diff --git a/CMakeLists.txt b/CMakeLists.txt
index a6454c7..6d8ae7d 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
@@ -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}
)
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/include/gazebo_2Dmap_plugin.h b/include/gazebo_2Dmap_plugin.h
index c331e21..578bde7 100644
--- a/include/gazebo_2Dmap_plugin.h
+++ b/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
diff --git a/package.xml b/package.xml
index e8792a0..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.
diff --git a/src/gazebo_2Dmap_plugin.cpp b/src/gazebo_2Dmap_plugin.cpp
index 0783b16..babdd39 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,7 +76,11 @@ 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");
+ if(_sdf->HasElement("robot_clearance"))
+ robot_clearance_ = _sdf->GetElement("robot_clearance")->Get();
+
+ 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 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;
}