├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include └── osmVisualizer │ ├── dilation.h │ └── osmVisualizer.hpp ├── launch ├── osm_visualizer.launch.py └── osm_visualizer.launch.xml ├── osms └── a.osm ├── package.xml └── src ├── occupancyGrid ├── occupancyGrip.cpp └── occupancyGrip2.cpp ├── osmVisualizer ├── osmVisualizer.cpp └── osm_visualizer_node.cpp └── pcdVisualizer └── pcdVisualizer.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Catkin custom files 51 | CATKIN_IGNORE 52 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(map_visualizer) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | find_package(ament_cmake REQUIRED) 19 | find_package(rclcpp REQUIRED) 20 | find_package(visualization_msgs REQUIRED) 21 | find_package(eigen3_cmake_module REQUIRED) 22 | find_package(Eigen3) 23 | find_package(lanelet2_core) 24 | find_package(lanelet2_io) 25 | find_package(lanelet2_maps) 26 | find_package(lanelet2_projection) 27 | find_package(lanelet2_routing) 28 | find_package(lanelet2_traffic_rules) 29 | find_package(lanelet2_validation) 30 | 31 | set(dep_osm "rclcpp" "visualization_msgs" "Eigen3" "lanelet2_core" "lanelet2_io" "lanelet2_maps" "lanelet2_validation" "lanelet2_routing" "lanelet2_projection" "lanelet2_traffic_rules") 32 | 33 | include_directories(include()) 34 | 35 | add_executable(osm_visualizer 36 | src/osmVisualizer/osmVisualizer.cpp 37 | src/osmVisualizer/osm_visualizer_node.cpp) 38 | 39 | ament_target_dependencies(osm_visualizer ${dep_osm}) 40 | 41 | target_include_directories(osm_visualizer PUBLIC 42 | $ 43 | $) 44 | 45 | install(TARGETS osm_visualizer 46 | DESTINATION lib/${PROJECT_NAME}) 47 | 48 | 49 | 50 | ########### pcd vis 51 | find_package(PCL REQUIRED QUIET COMPONENTS common features filters io segmentation surface) 52 | find_package(sensor_msgs REQUIRED) 53 | find_package(pcl_conversions REQUIRED) 54 | find_package(tf2 REQUIRED) 55 | find_package(tf2_geometry_msgs REQUIRED) 56 | find_package(tf2_ros REQUIRED) 57 | 58 | include_directories(include()) 59 | include_directories(${PCL_INCLUDE_DIRS}) 60 | 61 | add_executable(pcd_visualizer src/pcdVisualizer/pcdVisualizer.cpp) 62 | ament_target_dependencies(pcd_visualizer rclcpp sensor_msgs Eigen3 pcl_conversions tf2 tf2_geometry_msgs tf2_ros PCL) 63 | 64 | target_include_directories(pcd_visualizer PUBLIC 65 | $ 66 | $) 67 | 68 | target_link_libraries(pcd_visualizer ${PCL_LIBRARIES}) 69 | 70 | install(TARGETS pcd_visualizer 71 | DESTINATION lib/${PROJECT_NAME}) 72 | 73 | install( 74 | DIRECTORY launch 75 | DESTINATION share/${PROJECT_NAME} 76 | ) 77 | 78 | ### occupancy grid 79 | find_package(nav_msgs REQUIRED) 80 | 81 | 82 | include_directories(include()) 83 | 84 | add_executable(occupancy_pub src/occupancyGrid/occupancyGrip2.cpp) 85 | ament_target_dependencies(occupancy_pub rclcpp nav_msgs) 86 | 87 | target_include_directories(occupancy_pub PUBLIC 88 | $ 89 | $) 90 | 91 | target_link_libraries(occupancy_pub ${PCL_LIBRARIES}) 92 | 93 | install(TARGETS occupancy_pub 94 | DESTINATION lib/${PROJECT_NAME}) 95 | 96 | install( 97 | DIRECTORY launch 98 | DESTINATION share/${PROJECT_NAME} 99 | ) 100 | 101 | 102 | if(BUILD_TESTING) 103 | find_package(ament_lint_auto REQUIRED) 104 | # the following line skips the linter which checks for copyrights 105 | # uncomment the line when a copyright and license is not present in all source files 106 | #set(ament_cmake_copyright_FOUND TRUE) 107 | # the following line skips cpplint (only works in a git repo) 108 | # uncomment the line when this package is not in a git repo 109 | #set(ament_cmake_cpplint_FOUND TRUE) 110 | ament_lint_auto_find_test_dependencies() 111 | endif() 112 | 113 | ament_package() 114 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 mucahitayhan 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |

map_visualizer package

2 | 3 | ## :dart: About ## 4 | This package provides the visualize the map in ROS2 using MarkerArray and Occupancygrid map types. 5 | There is two node osm_visualizer node and occupancy_pub node in this package. 6 | 7 | ## :rocket: Technologies ## 8 | 9 | The following tools were used in this project: 10 | 11 | - [ROS](https://www.ros.org/) Tested on ROS2 Foxy distro. 12 | 13 | ## :white_check_mark: Requirements ## 14 | Before starting :checkered_flag:, you need to have ROS2(Foxy) and install some ros2 packages. 15 | 16 | `sudo rosdep init` 17 | 18 | `rosdep update` 19 | 20 | `rosdep install --from-paths src -y --ignore-src` 21 | 22 | `sudo apt-get install ros--lanelet2` 23 | 24 | E.g: `sudo apt-get install ros-foxy-lanelet2` 25 | 26 | ## 1 - osm_visualizer node 27 | 28 | ### :sparkles: Features 29 | 30 | osm_visualizer node visualizes the .osm(OpenStreetMap) file into RViz MarkerArray. 31 | 32 | ### :checkered_flag: How to Run 33 | 34 | `ros2 run map_visualizer osm_visualizer --ros-args -p map_path:=path/to/map.osm` 35 | 36 | or `ros2 launch map_visualizer osm_visualizer.launch.xml` you need to change map path 37 | 38 | or `ros2 launch map_visualizer osm_visualizer.launch.py` you need to change map path 39 | 40 | ### Subscribed Topics 41 | 42 | - None 43 | 44 | ### Published Topics 45 | 46 | - /hd_map (visualization_msgs/msg/marker_array) : visualization messages for RViz 47 | - /array (std_msgs/msg/float64_multi_array) : the left and right bounds points of the map lanelet layers for occupancy_pub node 48 | 49 | ### Parameters 50 | 51 | | Name | Type | Description | Default value | 52 | | :--------------------- | :---------- | :------------------------------------------------------------------------------------------------- | :---------------------- | 53 | | map_path | std::string | lanelet2 map path | change on launch file | 54 | | enable_inc_path_points | bool | flag to increment points of left and rigth boundry's linestrings | true | 55 | | interval | std::string | interval to increment left and right boundry points with linear interpolation for occupancygrid map | 0.5 | 56 | 57 | 58 | --- 59 | 60 | ## 2 - occupancy_pub node 61 | 62 | ### :sparkles: Features 63 | 64 | occupancy_pub node turn the osm data provided by osm_visualizer node into nav_msgs/msg/occupancy_grid. 65 | 66 | ### :checkered_flag: How to Run 67 | 68 | `ros2 run map_visualizer occupancy_pub` 69 | 70 | ### Subscribed Topics 71 | 72 | - /array (std_msgs/msg/float64_multi_array) : the left and right bounds points of the map lanelet layers 73 | - /initialpose (geometry_msgs/msg/pose_with_covariance_stamped) : RViz initial pose message to calculate initial location on grid 74 | - /goal_pose (geometry_msgs/msg/pose_stamped) : RViz goal pose message to calculate goal location on grid 75 | 76 | ### Published Topics 77 | 78 | - /occupancy_grid (nav_msgs/msg/occupancy_grid) : occupancy grid messages for RViz 79 | - /obsx (std_msgs/msg/float64_multi_array) : x index of obstacles on grid for A* package 80 | - /obsy (std_msgs/msg/float64_multi_array) : y index of obstacles on grid for A* package 81 | - /gridsize (std_msgs/msg/float64_multi_array) : gridsize information message for A* package 82 | 83 | ### Parameters 84 | 85 | - None 86 | 87 | 88 | ## :memo: License 89 | 90 | This project is under license from MIT. For more details, see the [LICENSE](LICENSE) file. 91 | 92 | 93 |   94 | 95 | Back to top 96 | -------------------------------------------------------------------------------- /include/osmVisualizer/dilation.h: -------------------------------------------------------------------------------- 1 | // Function to apply dilation to the matrix 2 | #include 3 | #include 4 | std::vector> applyDilation(std::vector>& matrix) { 5 | int rows = matrix.size(); 6 | //if (rows == 0); 7 | int cols = matrix[0].size(); 8 | 9 | // Define the dilation kernel (3x3 with all 1s) 10 | std::vector> dilationKernel = { 11 | {1, 1, 1}, 12 | {1, 1, 1}, 13 | {1, 1, 1} 14 | }; 15 | 16 | // Create a copy of the original matrix to store the result 17 | std::vector> resultMatrix(rows, std::vector(cols, 0)); 18 | 19 | // Iterate through each element of the matrix 20 | for (int i = 0; i < rows; i++) { 21 | 22 | for (int j = 0; j < cols; j++) { 23 | 24 | // Apply the dilation operation 25 | if (matrix[i][j] == 1) { 26 | for (int ki = -1; ki <= 1; ki++) { 27 | for (int kj = -1; kj <= 1; kj++) { 28 | int ni = i + ki; 29 | int nj = j + kj; 30 | // Check boundaries 31 | if (ni >= 0 && ni < rows && nj >= 0 && nj < cols) { 32 | resultMatrix[ni][nj] = 1; 33 | } 34 | } 35 | } 36 | } 37 | } 38 | } 39 | 40 | // Copy the result matrix back to the original matrix 41 | matrix = resultMatrix; 42 | return matrix; 43 | } -------------------------------------------------------------------------------- /include/osmVisualizer/osmVisualizer.hpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "rclcpp/rclcpp.hpp" 7 | #include "visualization_msgs/msg/marker_array.hpp" 8 | #include "std_msgs/msg/float64_multi_array.hpp" 9 | 10 | // lanelet libraries 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using namespace std::chrono_literals; 19 | 20 | class OsmVisualizer : public rclcpp::Node 21 | { 22 | public: 23 | OsmVisualizer(); 24 | 25 | private: 26 | void timer_callback(); 27 | bool readParameters(); 28 | void writeToFile(const std_msgs::msg::Float64MultiArray& multi_array); 29 | void fill_marker(lanelet::LaneletMapPtr &t_map); 30 | void fill_array(lanelet::LaneletMapPtr &t_map); 31 | void fill_array_with_left_right(lanelet::LaneletMapPtr &t_map); 32 | void fill_min_max_values(const lanelet::Lanelet &li); 33 | double getDistance(const lanelet::ConstLanelet &ll , size_t i); 34 | rclcpp::TimerBase::SharedPtr timer_; 35 | rclcpp::Publisher::SharedPtr publisher_; 36 | rclcpp::Publisher::SharedPtr array_publisher_; 37 | 38 | std_msgs::msg::Float64MultiArray m_array; 39 | visualization_msgs::msg::MarkerArray m_marker_array; 40 | 41 | //params 42 | std::string map_path_; 43 | bool enable_inc_path_points_; 44 | double interval_; 45 | }; -------------------------------------------------------------------------------- /launch/osm_visualizer.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | 4 | def generate_launch_description(): 5 | return LaunchDescription([ 6 | Node( 7 | package="map_visualizer", 8 | executable="osm_visualizer", 9 | name="osm_visualizer", 10 | output="screen", 11 | emulate_tty=True, 12 | parameters=[ 13 | {"map_path": "/home/otonom/Downloads/a.osm"} 14 | ] 15 | ) 16 | ]) -------------------------------------------------------------------------------- /launch/osm_visualizer.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | map_visualizer 5 | 0.0.0 6 | TODO: Package description 7 | otonom 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | visualization_msgs 15 | rclcpp 16 | lanelet2_core 17 | lanelet2_io 18 | lanelet2_maps 19 | lanelet2_projection 20 | lanelet2_routing 21 | lanelet2_traffic_rules 22 | lanelet2_validation 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /src/occupancyGrid/occupancyGrip.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | class OccupancyGridPublisher : public rclcpp::Node { 7 | public: 8 | OccupancyGridPublisher() : Node("occupancy_grid_publisher") { 9 | osm_sub_ = this->create_subscription("/array", 10, std::bind(&OccupancyGridPublisher::float64MultiArrayCallback, this, std::placeholders::_1)); 10 | 11 | std::vector matrix_data = { 12 | 0, 100, 0, 0, 13 | 100, 0, 100, 100, 14 | 0, 0, 0, 100, 15 | 100, 100, 100, 0, 16 | 100,0,0,0 17 | }; 18 | 19 | int width = 4; // x 20 | int height = 5; // y 21 | 22 | occupancy_grid_msg.header.frame_id = "map"; 23 | occupancy_grid_msg.info.width = width; 24 | occupancy_grid_msg.info.height = height; 25 | occupancy_grid_msg.info.resolution = 1.0; 26 | occupancy_grid_msg.info.origin.position.x = 0.0; 27 | occupancy_grid_msg.info.origin.position.y = 0.0; 28 | occupancy_grid_msg.info.origin.position.z = 0.0; 29 | occupancy_grid_msg.data = matrix_data; 30 | 31 | occupancy_grid_publisher_ = this->create_publisher("occupancy_grid",10); 32 | timer_ = this->create_wall_timer(std::chrono::milliseconds(100),std::bind(&OccupancyGridPublisher::publishOccupancyGrid, this)); 33 | } 34 | 35 | private: 36 | void publishOccupancyGrid() 37 | { 38 | 39 | if(first_ && !array_data_x.empty()) 40 | { 41 | 42 | std::vectorint_vector_x = std::vector(array_data_x.begin(),array_data_x.end()); 43 | std::vectorint_vector_y = std::vector(array_data_y.begin(),array_data_y.end()); 44 | int min_x=*min_element(int_vector_x.begin(), int_vector_x.end()); 45 | int min_y=*min_element(int_vector_y.begin(), int_vector_y.end()); 46 | int max_x=*max_element(int_vector_x.begin(), int_vector_x.end()); 47 | int max_y=*max_element(int_vector_y.begin(), int_vector_y.end()); 48 | occupancy_grid_msg.header.frame_id = "map"; // Doldurmak istediğiniz frame_id'yi belirtin. 49 | int width = max_x - min_x; // max_x - min_x 50 | int height = max_y - min_y; // max_y - min_y 51 | occupancy_grid_msg.info.width = width; 52 | occupancy_grid_msg.info.height = height; 53 | occupancy_grid_msg.info.origin.position.x = min_x; 54 | occupancy_grid_msg.info.origin.position.y = min_y; 55 | occupancy_grid_msg.info.origin.position.z = 0.0; 56 | occupancy_grid_msg.info.resolution = 1.0; 57 | std::cout << width << std::endl; 58 | std::cout << height << std::endl; 59 | occupancy_grid_msg.data = createMatrixWithMod( int_vector_x , int_vector_y , width , height , min_x , min_y); 60 | first_ = false; 61 | } 62 | occupancy_grid_publisher_->publish(occupancy_grid_msg); 63 | } 64 | 65 | void float64MultiArrayCallback(const std_msgs::msg::Float64MultiArray::SharedPtr t_msg) 66 | { 67 | if(first) 68 | { 69 | for(int i = 0 ; i < t_msg->data.size()/2 ; i++) 70 | { 71 | array_data_x.push_back(round(t_msg->data[2*i])); 72 | array_data_y.push_back(round(t_msg->data[2*i+1])); 73 | } 74 | first = false; 75 | } 76 | } 77 | 78 | std::vector createMatrixWithMod(std::vector& int_vector_x, std::vector& int_vector_y, int& rows, int& columns, int& min_x, int& min_y) 79 | { 80 | std::cout << int_vector_y.size()<< std::endl; 81 | std::vector matrix_data(rows * columns, 0); // Initialize the matrix with zeros 82 | for (size_t i = 0; i < int_vector_x.size(); ++i) 83 | { 84 | int32_t x = int_vector_x[i] - min_x; 85 | int32_t y = int_vector_y[i] - min_y; 86 | 87 | if (x >= 0 && x < rows && y >= 0 && y < columns) { 88 | matrix_data[(y * rows) + x] = 100; 89 | } 90 | } 91 | return matrix_data; 92 | } 93 | 94 | rclcpp::Publisher::SharedPtr occupancy_grid_publisher_; 95 | rclcpp::Subscription::SharedPtr osm_sub_; 96 | 97 | rclcpp::TimerBase::SharedPtr timer_; 98 | nav_msgs::msg::OccupancyGrid occupancy_grid_msg; 99 | std::vector array_data_x; 100 | std::vector array_data_y; 101 | bool first_{true}; 102 | bool first{true}; 103 | }; 104 | 105 | int main(int argc, char **argv) { 106 | rclcpp::init(argc, argv); 107 | rclcpp::spin(std::make_shared()); 108 | rclcpp::shutdown(); 109 | return 0; 110 | } 111 | -------------------------------------------------------------------------------- /src/occupancyGrid/occupancyGrip2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "geometry_msgs/msg/pose_stamped.hpp" 6 | #include "geometry_msgs/msg/pose_with_covariance.hpp" 7 | #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" 8 | #include "../include/osmVisualizer/dilation.h" 9 | 10 | class OccupancyGridPublisher : public rclcpp::Node { 11 | public: 12 | OccupancyGridPublisher() : Node("occupancy_grid_publisher") { 13 | osm_sub_ = this->create_subscription("/array", 10, std::bind(&OccupancyGridPublisher::float64MultiArrayCallback, this, std::placeholders::_1)); 14 | occupancy_grid_publisher_ = this->create_publisher("occupancy_grid",10); 15 | obsx_publisher_ = this->create_publisher("obsx",10); 16 | obsy_publisher_ = this->create_publisher("obsy",10); 17 | grid_size_publisher = this->create_publisher("gridsize",10); 18 | goal_subscriber_ = this->create_subscription("/goal_pose", 10, std::bind(&OccupancyGridPublisher::goalCallback, this, std::placeholders::_1)); 19 | initial_subscriber_ = this->create_subscription("/initialpose", 10, std::bind(&OccupancyGridPublisher::initialCallback, this, std::placeholders::_1)); 20 | timer_ = this->create_wall_timer(std::chrono::milliseconds(100),std::bind(&OccupancyGridPublisher::publishOccupancyGrid, this)); 21 | } 22 | 23 | private: 24 | void publishOccupancyGrid() 25 | { 26 | if((first_ && !array_data_right_y.empty()&& flag1 && flag2) || prev_count_ != count_) 27 | { 28 | 29 | std::cout << "count:" << count_ << std::endl; 30 | 31 | std::vectorint_vector_left_x = std::vector(array_data_left_x.begin(),array_data_left_x.end()); 32 | std::vectorint_vector_left_y = std::vector(array_data_left_y.begin(),array_data_left_y.end()); 33 | std::vectorint_vector_right_x = std::vector(array_data_right_x.begin(),array_data_right_x.end()); 34 | std::vectorint_vector_right_y = std::vector(array_data_right_y.begin(),array_data_right_y.end()); 35 | 36 | int min_left_x=*min_element(int_vector_left_x.begin(), int_vector_left_x.end()); 37 | int min_left_y=*min_element(int_vector_left_y.begin(), int_vector_left_y.end()); 38 | int max_left_x=*max_element(int_vector_left_x.begin(), int_vector_left_x.end()); 39 | int max_left_y=*max_element(int_vector_left_y.begin(), int_vector_left_y.end()); 40 | int min_right_x=*min_element(int_vector_right_x.begin(), int_vector_right_x.end()); 41 | int min_right_y=*min_element(int_vector_right_y.begin(), int_vector_right_y.end()); 42 | int max_right_x=*max_element(int_vector_right_x.begin(), int_vector_right_x.end()); 43 | int max_right_y=*max_element(int_vector_right_y.begin(), int_vector_right_y.end()); 44 | 45 | int min_x = min_left_x < min_right_x ? min_left_x : min_right_x; 46 | int max_x = max_left_x > max_right_x ? max_left_x : max_right_x; 47 | int min_y = min_left_y < min_right_y ? min_left_y : min_right_y; 48 | int max_y = max_left_y > max_right_y ? max_left_y : max_right_y; 49 | 50 | int width = max_x - min_x; // max_x - min_x 51 | int height = max_y - min_y; // max_y - min_y 52 | 53 | std::vector int_vector_x; 54 | std::vector int_vector_y; 55 | for(size_t i = 0 ; i(k) / (num_points - 1); 70 | double x = x1 + t * (x2 - x1); 71 | double y = y1 + t * (y2 - y1); 72 | int_vector_x.push_back(static_cast(round(x))); 73 | int_vector_y.push_back(static_cast(round(y))); 74 | } 75 | } 76 | 77 | occupancy_grid_msg.header.frame_id = "map"; // Doldurmak istediğiniz frame_id'yi belirtin. 78 | occupancy_grid_msg.info.width = width; 79 | occupancy_grid_msg.info.height = height; 80 | occupancy_grid_msg.info.origin.position.x = min_x; 81 | occupancy_grid_msg.info.origin.position.y = min_y; 82 | occupancy_grid_msg.info.origin.position.z = 0.0; 83 | occupancy_grid_msg.info.resolution = 1.0; 84 | occupancy_grid_msg.data = createMatrixWithMod( int_vector_x , int_vector_y , width , height , min_x , min_y); 85 | first_ = false; 86 | double x_i = init_msg_.pose.pose.position.x - min_x; 87 | double y_i = init_msg_.pose.pose.position.y - min_y; 88 | double x_e = goal_msg_.pose.position.x - min_x; 89 | double y_e = goal_msg_.pose.position.y - min_y; 90 | grid_msg_.data = {static_cast(width) , static_cast(height),x_i,y_i,x_e,y_e,static_cast(min_x),static_cast(min_y)}; 91 | 92 | prev_count_ = count_; 93 | } 94 | obsx_publisher_->publish(x_msg_); 95 | obsy_publisher_->publish(y_msg_); 96 | grid_size_publisher->publish(grid_msg_); 97 | occupancy_grid_publisher_->publish(occupancy_grid_msg); 98 | 99 | } 100 | 101 | void goalCallback(const geometry_msgs::msg::PoseStamped::SharedPtr t_goal_msg) { 102 | goal_msg_ = *t_goal_msg; 103 | flag2=true; 104 | count_ ++; 105 | } 106 | 107 | void initialCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr t_init_msg) { 108 | init_msg_ = *t_init_msg; 109 | flag1=true; 110 | 111 | } 112 | 113 | void float64MultiArrayCallback(const std_msgs::msg::Float64MultiArray::SharedPtr t_msg) 114 | { 115 | if(first) 116 | { 117 | for(size_t i = 0 ; i < t_msg->data.size()/4 ; i++) 118 | { 119 | array_data_left_x.push_back(round(t_msg->data[4*i])); 120 | array_data_left_y.push_back(round(t_msg->data[4*i+1])); 121 | array_data_right_x.push_back(round(t_msg->data[4*i+2])); 122 | array_data_right_y.push_back(round(t_msg->data[4*i+3])); 123 | } 124 | first = false; 125 | } 126 | } 127 | 128 | 129 | std::vector createMatrixWithMod(std::vector& int_vector_x, std::vector& int_vector_y, int& rows, int& columns, int& min_x, int& min_y) 130 | { 131 | std::cout << int_vector_y.size()<< std::endl; 132 | std::vector matrix_data(rows * columns, 100); 133 | std::vector> dil_matrix(rows, std::vector(columns, 0)); 134 | for (size_t i = 0; i < int_vector_x.size(); ++i) 135 | { 136 | int32_t x = int_vector_x[i] - min_x; 137 | int32_t y = int_vector_y[i] - min_y; 138 | 139 | if (x >= 0 && x < rows && y >= 0 && y < columns) { 140 | matrix_data[(y * rows) + x] = 0; 141 | } 142 | } 143 | 144 | 145 | for( int i = 0 ; i < columns ; i++ ) 146 | { 147 | for( int j = 0 ; j < rows ; j++ ) 148 | { 149 | if(matrix_data[(i * rows) + j]==100) 150 | { 151 | x_msg_.data.push_back(j); 152 | y_msg_.data.push_back(i); 153 | } 154 | else 155 | { 156 | dil_matrix[j][i]=1; 157 | } 158 | } 159 | } 160 | //std::cout<<"dil matrix start: "< dil ; 165 | for( int i = 0 ; i < columns ; i++ ) 166 | { 167 | for( int j = 0 ; j < rows ; j++ ) 168 | { 169 | if (dil_matrix[j][i] == 1) 170 | { 171 | dil.push_back(0); 172 | //std::cout<<"dil matrix: "<::SharedPtr occupancy_grid_publisher_; 190 | rclcpp::Subscription::SharedPtr osm_sub_; 191 | rclcpp::Subscription::SharedPtr goal_subscriber_; 192 | rclcpp::Subscription::SharedPtr initial_subscriber_; 193 | 194 | rclcpp::Publisher::SharedPtr obsx_publisher_; 195 | rclcpp::Publisher::SharedPtr obsy_publisher_; 196 | rclcpp::Publisher::SharedPtr grid_size_publisher; 197 | std_msgs::msg::Float64MultiArray x_msg_; 198 | std_msgs::msg::Float64MultiArray y_msg_; 199 | std_msgs::msg::Float64MultiArray grid_msg_; 200 | geometry_msgs::msg::PoseStamped goal_msg_; 201 | geometry_msgs::msg::PoseWithCovarianceStamped init_msg_; 202 | bool flag1 =false; 203 | bool flag2 =false; 204 | int count_{0}; 205 | int prev_count_{0}; 206 | 207 | 208 | rclcpp::TimerBase::SharedPtr timer_; 209 | nav_msgs::msg::OccupancyGrid occupancy_grid_msg; 210 | std::vector array_data_left_x; 211 | std::vector array_data_left_y; 212 | std::vector array_data_right_x; 213 | std::vector array_data_right_y; 214 | bool first_{true}; 215 | bool first{true}; 216 | }; 217 | 218 | int main(int argc, char **argv) { 219 | rclcpp::init(argc, argv); 220 | rclcpp::spin(std::make_shared()); 221 | rclcpp::shutdown(); 222 | return 0; 223 | } 224 | -------------------------------------------------------------------------------- /src/osmVisualizer/osmVisualizer.cpp: -------------------------------------------------------------------------------- 1 | #include "../include/osmVisualizer/osmVisualizer.hpp" 2 | 3 | OsmVisualizer::OsmVisualizer() : Node("OsmVisualizer") 4 | { 5 | this->declare_parameter("map_path", "/home/atakan/Downloads/Town10.osm"); 6 | this->declare_parameter("enable_inc_path_points", true); 7 | this->declare_parameter("interval", 2.0); 8 | if (!readParameters()) 9 | rclcpp::shutdown(); 10 | 11 | publisher_ = this->create_publisher("/hd_map", 10); 12 | array_publisher_ = this->create_publisher("/array", 10); 13 | timer_ = this->create_wall_timer( 500ms, std::bind(&OsmVisualizer::timer_callback, this)); 14 | 15 | lanelet::LaneletMapPtr map = lanelet::load(map_path_, lanelet::projection::UtmProjector(lanelet::Origin({15, 30}))); 16 | for (auto point : map->pointLayer) 17 | { 18 | point.x() = point.attribute("local_x").asDouble().value(); 19 | point.y() = point.attribute("local_y").asDouble().value(); 20 | } 21 | 22 | fill_marker(map); 23 | fill_array_with_left_right(map); 24 | // writeToFile(m_array); 25 | } 26 | 27 | bool OsmVisualizer::readParameters() 28 | { 29 | if (!this->get_parameter("map_path", map_path_)) 30 | { 31 | std::cout << "Failed to read parameter 'map_path' " << std::endl; 32 | return false; 33 | } 34 | if (!this->get_parameter("enable_inc_path_points", enable_inc_path_points_)) 35 | { 36 | std::cout << "Failed to read parameter 'interval' to increase the path points" << std::endl; 37 | return false; 38 | } 39 | if (!this->get_parameter("interval", interval_)) 40 | { 41 | std::cout << "Failed to read parameter 'interval' to increase the path points" << std::endl; 42 | return false; 43 | } 44 | return true; 45 | } 46 | 47 | void OsmVisualizer::timer_callback() 48 | { 49 | // publisher_->get_subscription_count() > 0 && m_marker_array.markers.size() > 0 50 | // if( publisher_->get_subscription_count() > 0) 51 | { 52 | publisher_->publish(m_marker_array); 53 | array_publisher_->publish(m_array); 54 | } 55 | } 56 | 57 | void OsmVisualizer::fill_array(lanelet::LaneletMapPtr &t_map) 58 | { 59 | m_array.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); 60 | m_array.layout.dim[0].label = "rows"; 61 | m_array.layout.dim[0].size = 100000; 62 | m_array.layout.dim[0].stride = 100000*2; 63 | m_array.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); 64 | m_array.layout.dim[1].label = "cols"; 65 | m_array.layout.dim[1].size = 2; 66 | m_array.layout.dim[1].stride = 2; 67 | 68 | 69 | for (const auto &ll : t_map->laneletLayer) 70 | { 71 | for(size_t i = 0; i 2 && enable_inc_path_points_) 74 | { 75 | double dist = getDistance(ll,i); 76 | double interval = 1; 77 | int num_points = dist / interval; 78 | 79 | for(int k = 0 ; klaneletLayer.size(); 117 | m_array.layout.dim[0].stride = t_map->laneletLayer.size()*4; 118 | m_array.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); 119 | m_array.layout.dim[1].label = "cols"; 120 | m_array.layout.dim[1].size = 4; 121 | m_array.layout.dim[1].stride = 4; 122 | 123 | for (const auto &ll : t_map->laneletLayer) 124 | { 125 | std::vector bounds; 126 | bounds.push_back(ll.leftBound()); 127 | bounds.push_back(ll.rightBound()); 128 | 129 | size_t size = (bounds[0].size() < bounds[1].size()) ? bounds[0].size() : bounds[1].size(); 130 | for(size_t i = 0; i bounds; 152 | bounds.push_back(ll.leftBound()); 153 | bounds.push_back(ll.rightBound()); 154 | 155 | for (const auto &bound : bounds) 156 | { 157 | visualization_msgs::msg::Marker marker; 158 | marker.header.frame_id = "map"; 159 | marker.header.stamp = rclcpp::Clock{}.now(); 160 | marker.ns = "lanelet"; 161 | marker.id = i; 162 | i++; 163 | marker.type = visualization_msgs::msg::Marker::LINE_STRIP; 164 | marker.action = visualization_msgs::msg::Marker::ADD; 165 | marker.scale.x = 0.1; 166 | marker.color.a = 1.0; 167 | marker.color.r = 232; 168 | marker.color.g = 44; 169 | marker.color.b = 44; 170 | for (const auto &point : bound) 171 | { 172 | geometry_msgs::msg::Point p; 173 | p.x = point.x(); 174 | p.y = point.y(); 175 | p.z = 0; 176 | marker.points.push_back(p); 177 | } 178 | m_marker_array.markers.push_back(marker); 179 | } 180 | } 181 | -------------------------------------------------------------------------------- /src/osmVisualizer/osm_visualizer_node.cpp: -------------------------------------------------------------------------------- 1 | #include "../include/osmVisualizer/osmVisualizer.hpp" 2 | 3 | int main(int argc, char * argv[]) 4 | { 5 | rclcpp::init(argc, argv); 6 | rclcpp::spin(std::make_shared()); 7 | rclcpp::shutdown(); 8 | return 0; 9 | } -------------------------------------------------------------------------------- /src/pcdVisualizer/pcdVisualizer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include "rclcpp/rclcpp.hpp" 14 | #include "sensor_msgs/msg/point_cloud2.hpp" 15 | #include "sensor_msgs/msg/point_field.hpp" 16 | 17 | using namespace std::chrono_literals; 18 | 19 | class PcdtoPointcloud2 : public rclcpp::Node 20 | { 21 | public: 22 | PcdtoPointcloud2() 23 | : Node("pcd_to_ros") 24 | { 25 | 26 | pcl::PointCloud::Ptr cloud_(new pcl::PointCloud); 27 | pcl::io::loadPCDFile("/home/otonom/Downloads/Town10a.pcd", *cloud_); 28 | pcl::toROSMsg(*cloud_.get(),ros_pc2_); 29 | publisher_ = this->create_publisher("/pcd", 10); 30 | timer_ = this->create_wall_timer( 5000ms, std::bind(&PcdtoPointcloud2::timer_callback, this)); 31 | std::cout << "size: " << ros_pc2_.width * ros_pc2_.height << std::endl; 32 | ros_pc2_.header.frame_id = "map"; 33 | } 34 | 35 | private: 36 | void timer_callback() 37 | { 38 | if(publisher_->get_subscription_count() > 0 && m_first) 39 | { 40 | std::cout << "get_subscription_count: " << publisher_->get_subscription_count() << std::endl; 41 | ros_pc2_.header.stamp = this->get_clock()->now(); 42 | publisher_->publish(ros_pc2_); 43 | m_first = false; 44 | } 45 | } 46 | bool m_first{true}; 47 | sensor_msgs::msg::PointCloud2 ros_pc2_; 48 | rclcpp::TimerBase::SharedPtr timer_; 49 | rclcpp::Publisher::SharedPtr publisher_; 50 | }; 51 | 52 | int main(int argc, char * argv[]) 53 | { 54 | rclcpp::init(argc, argv); 55 | rclcpp::spin(std::make_shared()); 56 | rclcpp::shutdown(); 57 | return 0; 58 | } --------------------------------------------------------------------------------