From 5aad807bf53de39c92ba84c7d15b15638395a73b Mon Sep 17 00:00:00 2001 From: Rick van Osch Date: Fri, 13 May 2022 16:22:31 +0200 Subject: [PATCH 1/9] minor modifications to plugin such as parameterization of the used frame, topicname, origin etc --- include/gazebo_2Dmap_plugin.h | 5 ++++ src/gazebo_2Dmap_plugin.cpp | 44 +++++++++++++++++++++++++++++------ 2 files changed, 42 insertions(+), 7 deletions(-) diff --git a/include/gazebo_2Dmap_plugin.h b/include/gazebo_2Dmap_plugin.h index c331e21..e8f393c 100644 --- a/include/gazebo_2Dmap_plugin.h +++ b/include/gazebo_2Dmap_plugin.h @@ -105,12 +105,17 @@ class OccupancyMapFromWorld : public WorldPlugin { ros::Publisher map_pub_; nav_msgs::OccupancyGrid* occupancy_map_; std::string name_; + std::string frame_ = "odom"; + std::string topic_name_ = "map2d"; double map_resolution_; double map_height_; double map_size_x_; double map_size_y_; double init_robot_x_; double init_robot_y_; + double origin_x_ = 0.0; + double origin_y_ = 0.0; + bool origin_set_ = false; }; } // namespace gazebo diff --git a/src/gazebo_2Dmap_plugin.cpp b/src/gazebo_2Dmap_plugin.cpp index 0783b16..715e631 100644 --- a/src/gazebo_2Dmap_plugin.cpp +++ b/src/gazebo_2Dmap_plugin.cpp @@ -36,12 +36,18 @@ void OccupancyMapFromWorld::Load(physics::WorldPtr _parent, world_ = _parent; - map_pub_ = nh_.advertise("map2d", 1, true); + if(_sdf->HasElement("topic_name")) + topic_name_ = _sdf->GetElement("topic_name")->Get(); + + map_pub_ = nh_.advertise(topic_name_, 1, true); map_service_ = nh_.advertiseService( "gazebo_2Dmap_plugin/generate_map", &OccupancyMapFromWorld::ServiceCallback, this); map_resolution_ = 0.1; + if(_sdf->HasElement("frame")) + frame_ = _sdf->GetElement("frame")->Get(); + if(_sdf->HasElement("map_resolution")) map_resolution_ = _sdf->GetElement("map_resolution")->Get(); @@ -70,6 +76,14 @@ void OccupancyMapFromWorld::Load(physics::WorldPtr _parent, if(_sdf->HasElement("map_size_y")) map_size_y_ = _sdf->GetElement("map_size_y")->Get(); + if(_sdf->HasElement("origin_x")) + origin_set_ = true; + origin_x_ = _sdf->GetElement("origin_x")->Get(); + + if(_sdf->HasElement("origin_y")) + origin_set_ = true; + origin_y_ = _sdf->GetElement("origin_y")->Get(); + sdf::ElementPtr contactSensorSDF = _sdf->GetElement("contactSensor"); // std::string service_name = "world/get_octomap"; @@ -294,7 +308,7 @@ bool OccupancyMapFromWorld::index2cell(int index, unsigned int cell_size_x, void OccupancyMapFromWorld::CreateOccupancyMap() { //TODO map origin different from (0,0) - vector3d map_origin(0,0,map_height_); + vector3d map_origin(origin_x_,origin_y_,map_height_); unsigned int cells_size_x = map_size_x_ / map_resolution_; unsigned int cells_size_y = map_size_y_ / map_resolution_; @@ -304,18 +318,34 @@ void OccupancyMapFromWorld::CreateOccupancyMap() //all cells are initially unknown std::fill(occupancy_map_->data.begin(), occupancy_map_->data.end(), -1); occupancy_map_->header.stamp = ros::Time::now(); - occupancy_map_->header.frame_id = "odom"; //TODO map frame + occupancy_map_->header.frame_id = frame_; occupancy_map_->info.map_load_time = ros::Time(0); occupancy_map_->info.resolution = map_resolution_; occupancy_map_->info.width = cells_size_x; occupancy_map_->info.height = cells_size_y; #if GAZEBO_MAJOR_VERSION >= 9 - occupancy_map_->info.origin.position.x = map_origin.X() - map_size_x_ / 2; - occupancy_map_->info.origin.position.y = map_origin.Y() - map_size_y_ / 2; + if(origin_set_) + { + occupancy_map_->info.origin.position.x = map_origin.X(); + occupancy_map_->info.origin.position.y = map_origin.Y(); + } + else + { + occupancy_map_->info.origin.position.x = map_origin.X() - map_size_x_ / 2; + occupancy_map_->info.origin.position.y = map_origin.Y() - map_size_y_ / 2; + } occupancy_map_->info.origin.position.z = map_origin.Z(); #else - occupancy_map_->info.origin.position.x = map_origin.x - map_size_x_ / 2; - occupancy_map_->info.origin.position.y = map_origin.y - map_size_y_ / 2; + if(origin_set_) + { + occupancy_map_->info.origin.position.x = map_origin.X(); + occupancy_map_->info.origin.position.y = map_origin.Y(); + } + else + { + occupancy_map_->info.origin.position.x = map_origin.x - map_size_x_ / 2; + occupancy_map_->info.origin.position.y = map_origin.y - map_size_y_ / 2; + } occupancy_map_->info.origin.position.z = map_origin.z; #endif occupancy_map_->info.origin.orientation.w = 1; From d941e97aa93e80c305f079192e503aa346a437fb Mon Sep 17 00:00:00 2001 From: Rick van Osch Date: Mon, 16 May 2022 10:46:19 +0200 Subject: [PATCH 2/9] removed origin parameter that wasn't necessary --- include/gazebo_2Dmap_plugin.h | 3 --- src/gazebo_2Dmap_plugin.cpp | 36 ++++++----------------------------- 2 files changed, 6 insertions(+), 33 deletions(-) diff --git a/include/gazebo_2Dmap_plugin.h b/include/gazebo_2Dmap_plugin.h index e8f393c..512adf5 100644 --- a/include/gazebo_2Dmap_plugin.h +++ b/include/gazebo_2Dmap_plugin.h @@ -113,9 +113,6 @@ class OccupancyMapFromWorld : public WorldPlugin { double map_size_y_; double init_robot_x_; double init_robot_y_; - double origin_x_ = 0.0; - double origin_y_ = 0.0; - bool origin_set_ = false; }; } // namespace gazebo diff --git a/src/gazebo_2Dmap_plugin.cpp b/src/gazebo_2Dmap_plugin.cpp index 715e631..5602b33 100644 --- a/src/gazebo_2Dmap_plugin.cpp +++ b/src/gazebo_2Dmap_plugin.cpp @@ -75,15 +75,7 @@ void OccupancyMapFromWorld::Load(physics::WorldPtr _parent, if(_sdf->HasElement("map_size_y")) map_size_y_ = _sdf->GetElement("map_size_y")->Get(); - - if(_sdf->HasElement("origin_x")) - origin_set_ = true; - origin_x_ = _sdf->GetElement("origin_x")->Get(); - - if(_sdf->HasElement("origin_y")) - origin_set_ = true; - origin_y_ = _sdf->GetElement("origin_y")->Get(); - + sdf::ElementPtr contactSensorSDF = _sdf->GetElement("contactSensor"); // std::string service_name = "world/get_octomap"; @@ -308,7 +300,7 @@ bool OccupancyMapFromWorld::index2cell(int index, unsigned int cell_size_x, void OccupancyMapFromWorld::CreateOccupancyMap() { //TODO map origin different from (0,0) - vector3d map_origin(origin_x_,origin_y_,map_height_); + vector3d map_origin(0,0,map_height_); unsigned int cells_size_x = map_size_x_ / map_resolution_; unsigned int cells_size_y = map_size_y_ / map_resolution_; @@ -324,28 +316,12 @@ void OccupancyMapFromWorld::CreateOccupancyMap() occupancy_map_->info.width = cells_size_x; occupancy_map_->info.height = cells_size_y; #if GAZEBO_MAJOR_VERSION >= 9 - if(origin_set_) - { - occupancy_map_->info.origin.position.x = map_origin.X(); - occupancy_map_->info.origin.position.y = map_origin.Y(); - } - else - { - occupancy_map_->info.origin.position.x = map_origin.X() - map_size_x_ / 2; - occupancy_map_->info.origin.position.y = map_origin.Y() - map_size_y_ / 2; - } + occupancy_map_->info.origin.position.x = map_origin.X() - map_size_x_ / 2; + occupancy_map_->info.origin.position.y = map_origin.Y() - map_size_y_ / 2; occupancy_map_->info.origin.position.z = map_origin.Z(); #else - if(origin_set_) - { - occupancy_map_->info.origin.position.x = map_origin.X(); - occupancy_map_->info.origin.position.y = map_origin.Y(); - } - else - { - occupancy_map_->info.origin.position.x = map_origin.x - map_size_x_ / 2; - occupancy_map_->info.origin.position.y = map_origin.y - map_size_y_ / 2; - } + occupancy_map_->info.origin.position.x = map_origin.x - map_size_x_ / 2; + occupancy_map_->info.origin.position.y = map_origin.y - map_size_y_ / 2; occupancy_map_->info.origin.position.z = map_origin.z; #endif occupancy_map_->info.origin.orientation.w = 1; From c5081eac1fe74d5d6a2a3867c74b834b5fb9633e Mon Sep 17 00:00:00 2001 From: Rick van Osch Date: Mon, 16 May 2022 10:53:20 +0200 Subject: [PATCH 3/9] cleanup --- src/gazebo_2Dmap_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gazebo_2Dmap_plugin.cpp b/src/gazebo_2Dmap_plugin.cpp index 5602b33..24a0907 100644 --- a/src/gazebo_2Dmap_plugin.cpp +++ b/src/gazebo_2Dmap_plugin.cpp @@ -75,7 +75,7 @@ void OccupancyMapFromWorld::Load(physics::WorldPtr _parent, if(_sdf->HasElement("map_size_y")) map_size_y_ = _sdf->GetElement("map_size_y")->Get(); - + sdf::ElementPtr contactSensorSDF = _sdf->GetElement("contactSensor"); // std::string service_name = "world/get_octomap"; From 1e36be8daf749009f4a3f6db3cd5e9cd454ab1d2 Mon Sep 17 00:00:00 2001 From: Rick van Osch Date: Tue, 17 May 2022 11:05:31 +0200 Subject: [PATCH 4/9] added feature to force clear a certain area around the robot spawning point --- include/gazebo_2Dmap_plugin.h | 1 + src/gazebo_2Dmap_plugin.cpp | 34 +++++++++++++++++++++++++++++++++- 2 files changed, 34 insertions(+), 1 deletion(-) diff --git a/include/gazebo_2Dmap_plugin.h b/include/gazebo_2Dmap_plugin.h index 512adf5..578bde7 100644 --- a/include/gazebo_2Dmap_plugin.h +++ b/include/gazebo_2Dmap_plugin.h @@ -113,6 +113,7 @@ class OccupancyMapFromWorld : public WorldPlugin { double map_size_y_; double init_robot_x_; double init_robot_y_; + double robot_clearance_; }; } // namespace gazebo diff --git a/src/gazebo_2Dmap_plugin.cpp b/src/gazebo_2Dmap_plugin.cpp index 24a0907..3192500 100644 --- a/src/gazebo_2Dmap_plugin.cpp +++ b/src/gazebo_2Dmap_plugin.cpp @@ -76,6 +76,9 @@ void OccupancyMapFromWorld::Load(physics::WorldPtr _parent, if(_sdf->HasElement("map_size_y")) map_size_y_ = _sdf->GetElement("map_size_y")->Get(); + if(_sdf->HasElement("robot_clearance")) + robot_clearance_ = _sdf->GetElement("robot_clearance")->Get(); + sdf::ElementPtr contactSensorSDF = _sdf->GetElement("contactSensor"); // std::string service_name = "world/get_octomap"; @@ -349,7 +352,11 @@ void OccupancyMapFromWorld::CreateOccupancyMap() "map"); return; } - + int original_robot_index = map_index; + int origX = cell_x; + int origY = cell_y; + std::cout << "original robot index: " << +original_robot_index << std::endl; + std::cout << "origX: " << origX << " origY: " << origY << std::endl; std::vector wavefront; wavefront.push_back(map_index); @@ -407,6 +414,31 @@ void OccupancyMapFromWorld::CreateOccupancyMap() } } + // Clearing the area around the robot if clearance is set + if(robot_clearance_ > 0.0) + { + std::cout << "robot_clearance set: " << robot_clearance_ << std::endl; + int cellsInRange = robot_clearance_ / map_resolution_; + for(int xCell = -cellsInRange; xCell < cellsInRange; xCell++) + { + for(int yCell = -cellsInRange; yCell < cellsInRange; yCell++) + { + double xDelta = xCell * map_resolution_; + double yDelta = yCell * map_resolution_; + double distance = pow(xDelta, 2) + pow(yDelta, 2); + distance = sqrt(distance); + if(abs(distance) < robot_clearance_) + { + int newX = origX + xCell; + int newY = origY + yCell; + int map_idx = newX + (newY * cells_size_y); + occupancy_map_->data.at(map_idx) = 0; + } + + } + } + } + map_pub_.publish(*occupancy_map_); std::cout << "\rOccupancy Map generation completed " << std::endl; } From 8a66c520fa0fab3c89c169416b9b39dd0da08a08 Mon Sep 17 00:00:00 2001 From: Rick van Osch Date: Tue, 17 May 2022 11:51:11 +0200 Subject: [PATCH 5/9] cleanup and added documentation --- README.md | 5 ++++- src/gazebo_2Dmap_plugin.cpp | 2 -- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 062087a..6fabfb1 100644 --- a/README.md +++ b/README.md @@ -15,6 +15,9 @@ To include the plugin, add the following line in between the ` ` 10 0 0 + map + grid_map + 0.5 ``` @@ -36,4 +39,4 @@ The last map generated with the ```/gazebo_2Dmap_plugin/generate_map``` call is ## Hints * To identify the connected free space the robot would discover during mapping, the plugin performs a wavefront exploration along the occupancy grid starting from the origin of the gazebo world coordinate system. Please ensure that the corresponding cell is in the continuous free space. -* The plugin will map all objects in the world, including the robot. Remove all unwanted objects before creating the map. +* The plugin will map all objects in the world, including the robot. Remove all unwanted objects before creating the map or use the robot_clearance tag whilst including the plugin to force "clear" the area with a certain radius around the position given through init_robot_x and init_robot_y diff --git a/src/gazebo_2Dmap_plugin.cpp b/src/gazebo_2Dmap_plugin.cpp index 3192500..c05c35c 100644 --- a/src/gazebo_2Dmap_plugin.cpp +++ b/src/gazebo_2Dmap_plugin.cpp @@ -355,8 +355,6 @@ void OccupancyMapFromWorld::CreateOccupancyMap() int original_robot_index = map_index; int origX = cell_x; int origY = cell_y; - std::cout << "original robot index: " << +original_robot_index << std::endl; - std::cout << "origX: " << origX << " origY: " << origY << std::endl; std::vector wavefront; wavefront.push_back(map_index); From 5e139b4c8a3b6a78332919e9306e919f54ed0a04 Mon Sep 17 00:00:00 2001 From: Rick van Osch Date: Fri, 20 May 2022 10:40:18 +0200 Subject: [PATCH 6/9] fixed error in case of non-existing parameter in SDF --- src/gazebo_2Dmap_plugin.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/gazebo_2Dmap_plugin.cpp b/src/gazebo_2Dmap_plugin.cpp index c05c35c..babdd39 100644 --- a/src/gazebo_2Dmap_plugin.cpp +++ b/src/gazebo_2Dmap_plugin.cpp @@ -79,7 +79,8 @@ void OccupancyMapFromWorld::Load(physics::WorldPtr _parent, if(_sdf->HasElement("robot_clearance")) robot_clearance_ = _sdf->GetElement("robot_clearance")->Get(); - sdf::ElementPtr contactSensorSDF = _sdf->GetElement("contactSensor"); + if(_sdf->HasElement("contactSensor")) + sdf::ElementPtr contactSensorSDF = _sdf->GetElement("contactSensor"); // std::string service_name = "world/get_octomap"; // std::string octomap_pub_topic = "world/octomap"; From 2919c7612e209b25ef3e272c50a52741fa9bb662 Mon Sep 17 00:00:00 2001 From: whoutman Date: Mon, 23 May 2022 15:34:47 +0200 Subject: [PATCH 7/9] gazebo_ros_2Dmap_plugin -> gazebo_ros_2dmap_plugin --- CMakeLists.txt | 2 +- package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a6454c7..f5ce2bf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(gazebo_ros_2Dmap_plugin) +project(gazebo_ros_2dmap_plugin) find_package(catkin REQUIRED COMPONENTS gazebo_msgs diff --git a/package.xml b/package.xml index e8792a0..5f1bd40 100644 --- a/package.xml +++ b/package.xml @@ -1,6 +1,6 @@ - gazebo_ros_2Dmap_plugin + gazebo_ros_2dmap_plugin 0.0.0 Gazebo simulator plugin to automatically generate a 2D occupancy map from the simulated world at a given height. From 3ea5b7941ff25af6f688a96c871a2ada58ed192c Mon Sep 17 00:00:00 2001 From: whoutman Date: Mon, 23 May 2022 15:51:12 +0200 Subject: [PATCH 8/9] gazebo_ros_2Dmap_plugin -> gazebo_ros_2d_map_plugin --- CMakeLists.txt | 2 +- package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f5ce2bf..f02ac65 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(gazebo_ros_2dmap_plugin) +project(gazebo_ros_2d_map_plugin) find_package(catkin REQUIRED COMPONENTS gazebo_msgs diff --git a/package.xml b/package.xml index 5f1bd40..0e7a549 100644 --- a/package.xml +++ b/package.xml @@ -1,6 +1,6 @@ - gazebo_ros_2dmap_plugin + gazebo_ros_2d_map_plugin 0.0.0 Gazebo simulator plugin to automatically generate a 2D occupancy map from the simulated world at a given height. From 1d341c7328e7358697e45a4c1513306caa7969c2 Mon Sep 17 00:00:00 2001 From: whoutman Date: Mon, 23 May 2022 16:08:14 +0200 Subject: [PATCH 9/9] updated libname is CMakeLists.txt --- CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f02ac65..6d8ae7d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -26,9 +26,9 @@ include_directories( include ${catkin_INCLUDE_DIRS} ${GAZEBO_MSG_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ) -add_library(gazebo_2Dmap_plugin SHARED src/gazebo_2Dmap_plugin.cpp) -target_link_libraries(gazebo_2Dmap_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) +add_library(gazebo_2d_map_plugin SHARED src/gazebo_2Dmap_plugin.cpp) +target_link_libraries(gazebo_2d_map_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) -install(TARGETS gazebo_2Dmap_plugin +install(TARGETS gazebo_2d_map_plugin LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} )