Allows setting the frame and topic name through sdf (i.e. XML tags)

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
@@ -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}
 )

README.md

@@ -15,6 +15,9 @@ To include the plugin, add the following line in between the `<world> </world>`
     <map_size_y>10</map_size_y>          <!-- in meters, optional, default 10 -->
     <init_robot_x>0</init_robot_x>          <!-- x coordinate in meters, optional, default 0 -->
     <init_robot_y>0</init_robot_y>          <!-- y coordinate in meters, optional, default 0 -->
+    <frame>map</frame>                    <!-- frame in which the msg is made -->
+    <topic_name>grid_map</topic_name>       <!-- name of the topic the map should be published on -->
+    <robot_clearance>0.5</robot_clearance>  <!-- area around init_robot_x/y to be force-cleared -->
 </plugin>
 ```
 
@@ -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

include/gazebo_2Dmap_plugin.h

@@ -105,12 +105,15 @@ 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 robot_clearance_;
 };
 
 } // namespace gazebo

package.xml

@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <package>
-  <name>gazebo_ros_2Dmap_plugin</name>
+  <name>gazebo_ros_2d_map_plugin</name>
   <version>0.0.0</version>
   <description>Gazebo simulator plugin to automatically generate a 2D occupancy map from the simulated world at a given height.</description>
 

src/gazebo_2Dmap_plugin.cpp

@@ -36,12 +36,18 @@ void OccupancyMapFromWorld::Load(physics::WorldPtr _parent,
 
   world_ = _parent;
 
-  map_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>("map2d", 1, true);
+  if(_sdf->HasElement("topic_name"))
+    topic_name_ = _sdf->GetElement("topic_name")->Get<std::string>();
+
+  map_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>(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<std::string>();
+
   if(_sdf->HasElement("map_resolution"))
     map_resolution_ = _sdf->GetElement("map_resolution")->Get<double>();
 
@@ -70,7 +76,11 @@ void OccupancyMapFromWorld::Load(physics::WorldPtr _parent,
   if(_sdf->HasElement("map_size_y"))
     map_size_y_ = _sdf->GetElement("map_size_y")->Get<double>();
 
-  sdf::ElementPtr contactSensorSDF = _sdf->GetElement("contactSensor");
+  if(_sdf->HasElement("robot_clearance"))
+    robot_clearance_ = _sdf->GetElement("robot_clearance")->Get<double>();
+
+  if(_sdf->HasElement("contactSensor"))
+    sdf::ElementPtr contactSensorSDF = _sdf->GetElement("contactSensor");
 
   //  std::string service_name = "world/get_octomap";
   //  std::string octomap_pub_topic = "world/octomap";
@@ -304,7 +314,7 @@ 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;
@@ -343,7 +353,9 @@ void OccupancyMapFromWorld::CreateOccupancyMap()
                            "map");
     return;
   }
-
+  int original_robot_index = map_index;
+  int origX = cell_x;
+  int origY = cell_y;
   std::vector<unsigned int> wavefront;
   wavefront.push_back(map_index);
 
@@ -401,6 +413,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;
 }