diff --git a/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h b/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h
index 5809635a993909e01eb9ecb35a28dd2ba48adcc2..1947da82f3a60d2e3b31b6d03e0c62c646f28656 100644
--- a/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h
+++ b/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h
@@ -3,31 +3,32 @@
#include <ros/ros.h>
-#include <nav_msgs/OccupancyGrid.h>
-#include <nav_msgs/MapMetaData.h>
#include <homer_mapnav_msgs/ModifyMap.h>
+#include <nav_msgs/MapMetaData.h>
+#include <nav_msgs/OccupancyGrid.h>
#include <sstream>
/**
* @class MaskingManager
- * @brief Manages a map that can overwrite values in the SLAM map or store it in a separate layer
+ * @brief Manages a map that can overwrite values in the SLAM map or store it
+ * in a separate layer
* @author Malte Knauf, David Gossow
*/
-class MaskingManager
-{
- public:
-
+class MaskingManager {
+ public:
/** @brief The constructor. */
MaskingManager(nav_msgs::MapMetaData mapInfo);
/** @brief The destructor. */
virtual ~MaskingManager();
- void updateMapInfo(const nav_msgs::MapMetaData::ConstPtr mapInfo);
+ void updateMapInfo(const nav_msgs::MapMetaData &mapInfo);
- /** modifies either the masking layer or the slam layer (accordingly to the given map layer in the msg */
- nav_msgs::OccupancyGrid::ConstPtr modifyMap(homer_mapnav_msgs::ModifyMap::ConstPtr msg);
+ /** modifies either the masking layer or the slam layer (accordingly to the
+ * given map layer in the msg */
+ nav_msgs::OccupancyGrid::ConstPtr modifyMap(
+ homer_mapnav_msgs::ModifyMap::ConstPtr msg);
/** resets the masking map layer */
nav_msgs::OccupancyGrid::ConstPtr resetMap();
@@ -35,19 +36,18 @@ class MaskingManager
/** replaces the masking map layer */
void replaceMap(nav_msgs::OccupancyGrid map);
- private:
-
+ private:
/** stores the masking values in the dedicated masking map */
nav_msgs::OccupancyGrid m_MaskingMap;
/** stores the masking values that are afterwards sent to the slam map */
nav_msgs::OccupancyGrid m_SlamMap;
-
/** tools to draw masking polygons */
- void drawPolygon ( std::vector< geometry_msgs::Point > vertices, int value, int mapLayer );
- void drawLine ( std::vector<int> &data, int startX, int startY, int endX, int endY, int value );
- void fillPolygon ( std::vector<int> &data, int x, int y, int value );
+ void drawPolygon(std::vector<geometry_msgs::Point> vertices, int value,
+ int mapLayer);
+ void drawLine(std::vector<int> &data, int startX, int startY, int endX,
+ int endY, int value);
+ void fillPolygon(std::vector<int> &data, int x, int y, int value);
};
#endif
-
diff --git a/homer_map_manager/src/Managers/MaskingManager.cpp b/homer_map_manager/src/Managers/MaskingManager.cpp
index 7f72ac493df2ba4a32a379d97253582516eb59ee..4ee2ff60823e43a3abb3bb3bdf7e040225238b68 100644
--- a/homer_map_manager/src/Managers/MaskingManager.cpp
+++ b/homer_map_manager/src/Managers/MaskingManager.cpp
@@ -2,190 +2,181 @@
using namespace std;
-MaskingManager::MaskingManager(nav_msgs::MapMetaData mapInfo)
-{
+MaskingManager::MaskingManager(nav_msgs::MapMetaData mapInfo) {
m_MaskingMap.info = mapInfo;
- m_MaskingMap.data.resize(m_MaskingMap.info.width * m_MaskingMap.info.height);
- std::fill( m_MaskingMap.data.begin(), m_MaskingMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED );
+ m_MaskingMap.data.resize(m_MaskingMap.info.width *
+ m_MaskingMap.info.height);
+ std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(),
+ homer_mapnav_msgs::ModifyMap::NOT_MASKED);
m_SlamMap.info = mapInfo;
m_SlamMap.data.resize(m_SlamMap.info.width * m_SlamMap.info.height);
- std::fill( m_SlamMap.data.begin(), m_SlamMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED );
+ std::fill(m_SlamMap.data.begin(), m_SlamMap.data.end(),
+ homer_mapnav_msgs::ModifyMap::NOT_MASKED);
}
-MaskingManager::~MaskingManager()
-{}
+MaskingManager::~MaskingManager() {}
-void MaskingManager::updateMapInfo(const nav_msgs::MapMetaData::ConstPtr mapInfo)
-{
- if(m_SlamMap.info.width != mapInfo.width)
- {
+void MaskingManager::updateMapInfo(const nav_msgs::MapMetaData &mapInfo) {
+ if (m_SlamMap.info.width != mapInfo.width) {
m_SlamMap.info = mapInfo;
- std::vector<int> tmpData = m_MaskingMap.data;
-
+ int x_add_left =
+ (m_MaskingMap.info.origin.position.x - mapInfo.origin.position.x) /
+ mapInfo.resolution;
+ int y_add_up =
+ (m_MaskingMap.info.origin.position.y - mapInfo.origin.position.y) /
+ mapInfo.resolution;
+
+ std::vector<signed char> tmpData = m_MaskingMap.data;
+
m_MaskingMap.data.resize(mapInfo.width * mapInfo.height);
- std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(), 0);
-
- for(int x = 0; x < )
- {
- for(int y = 0;)
- {
+ std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(),
+ homer_mapnav_msgs::ModifyMap::NOT_MASKED);
+
+ for (int x = 0; x < m_MaskingMap.info.width; x++) {
+ for (int y = 0; y < m_MaskingMap.info.height; y++) {
+ int i = y * m_MaskingMap.info.width + x;
+ int j = (y + y_add_up) * mapInfo.width + x + x_add_left;
+ m_MaskingMap.data[j] = tmpData[i];
}
}
- m_MaskingMap.info = mapInfo;
+ m_MaskingMap.info = mapInfo;
}
}
-nav_msgs::OccupancyGrid::ConstPtr MaskingManager::modifyMap(homer_mapnav_msgs::ModifyMap::ConstPtr msg)
-{
- //reset SLAM mask map before each masking
- std::fill( m_SlamMap.data.begin(), m_SlamMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED );
+nav_msgs::OccupancyGrid::ConstPtr MaskingManager::modifyMap(
+ homer_mapnav_msgs::ModifyMap::ConstPtr msg) {
+ // reset SLAM mask map before each masking
+ std::fill(m_SlamMap.data.begin(), m_SlamMap.data.end(),
+ homer_mapnav_msgs::ModifyMap::NOT_MASKED);
drawPolygon(msg->region, msg->maskAction, msg->mapLayer);
nav_msgs::OccupancyGrid::ConstPtr ret;
- if(msg->mapLayer == 0)
- {
- ret = boost::make_shared<const::nav_msgs::OccupancyGrid>(m_SlamMap);
- }
- else
- {
- ret = boost::make_shared<const::nav_msgs::OccupancyGrid>(m_MaskingMap);
+ if (msg->mapLayer == 0) {
+ ret = boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_SlamMap);
+ } else {
+ ret = boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_MaskingMap);
}
return ret;
}
-nav_msgs::OccupancyGrid::ConstPtr MaskingManager::resetMap()
-{
- std::fill( m_MaskingMap.data.begin(), m_MaskingMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED );
- nav_msgs::OccupancyGrid::ConstPtr ret = boost::make_shared<const::nav_msgs::OccupancyGrid>(m_MaskingMap);
+nav_msgs::OccupancyGrid::ConstPtr MaskingManager::resetMap() {
+ std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(),
+ homer_mapnav_msgs::ModifyMap::NOT_MASKED);
+ nav_msgs::OccupancyGrid::ConstPtr ret =
+ boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_MaskingMap);
return ret;
}
-void MaskingManager::replaceMap(nav_msgs::OccupancyGrid map)
-{
- if(map.data.size() != 0)
+void MaskingManager::replaceMap(nav_msgs::OccupancyGrid map) {
+ if (map.data.size() != 0)
m_MaskingMap = map;
else
- std::fill( m_MaskingMap.data.begin(), m_MaskingMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED );
+ std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(),
+ homer_mapnav_msgs::ModifyMap::NOT_MASKED);
}
-void MaskingManager::drawPolygon ( vector< geometry_msgs::Point > vertices , int value , int mapLayer)
-{
- if ( vertices.size() == 0 )
- {
- ROS_INFO_STREAM( "No vertices given!" );
- return;
- }
- //make temp. map
+void MaskingManager::drawPolygon(vector<geometry_msgs::Point> vertices,
+ int value, int mapLayer) {
+ if (vertices.size() == 0) {
+ ROS_INFO_STREAM("No vertices given!");
+ return;
+ }
+ // make temp. map
std::vector<int> data(m_MaskingMap.info.width * m_MaskingMap.info.height);
- std::fill (data.begin(), data.end(), 0);
-
- //draw the lines surrounding the polygon
- for ( unsigned int i = 0; i < vertices.size(); i++ )
- {
- int i2 = ( i+1 ) % vertices.size();
- drawLine ( data, vertices[i].x, vertices[i].y, vertices[i2].x, vertices[i2].y, 1);
- }
- //calculate a point in the middle of the polygon
- float midX = 0;
- float midY = 0;
- for ( unsigned int i = 0; i < vertices.size(); i++ )
- {
- midX += vertices[i].x;
- midY += vertices[i].y;
- }
- midX /= vertices.size();
- midY /= vertices.size();
- //fill polygon
- fillPolygon ( data, (int)midX, (int)midY, 1 );
-
- //copy polygon to masking map or slam map (according to parameter mapLayer)
- for ( int i = 0; i < data.size(); i++ )
- {
- if ( data[i] != 0 )
- {
- switch(mapLayer)
- {
- case 0: //SLAM map
- m_SlamMap.data[i] = value;
- break;
- case 1: //Kinect Map. apply masking to masking map
- case 2: //masking map
- m_MaskingMap.data[i] = value;
- break;
+ std::fill(data.begin(), data.end(), 0);
+
+ // draw the lines surrounding the polygon
+ for (unsigned int i = 0; i < vertices.size(); i++) {
+ int i2 = (i + 1) % vertices.size();
+ drawLine(data, vertices[i].x, vertices[i].y, vertices[i2].x,
+ vertices[i2].y, 1);
+ }
+ // calculate a point in the middle of the polygon
+ float midX = 0;
+ float midY = 0;
+ for (unsigned int i = 0; i < vertices.size(); i++) {
+ midX += vertices[i].x;
+ midY += vertices[i].y;
+ }
+ midX /= vertices.size();
+ midY /= vertices.size();
+ // fill polygon
+ fillPolygon(data, (int)midX, (int)midY, 1);
+
+ // copy polygon to masking map or slam map (according to parameter mapLayer)
+ for (int i = 0; i < data.size(); i++) {
+ if (data[i] != 0) {
+ switch (mapLayer) {
+ case 0: // SLAM map
+ m_SlamMap.data[i] = value;
+ break;
+ case 1: // Kinect Map. apply masking to masking map
+ case 2: // masking map
+ m_MaskingMap.data[i] = value;
+ break;
}
}
}
}
-void MaskingManager::drawLine ( std::vector<int> &data, int startX, int startY, int endX, int endY, int value )
-{
- //bresenham algorithm
- int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
- // compute distances
- dx = endX - startX;
- dy = endY - startY;
-
- // compute increment
- if ( dx < 0 )
- {
- incx = -1;
- dx = -dx;
- }
- else
- {
- incx = dx ? 1 : 0;
- }
-
- if ( dy < 0 )
- {
- incy = -1;
- dy = -dy;
- }
- else
- {
- incy = dy ? 1 : 0;
- }
-
- // which distance is greater?
- dist = ( dx > dy ) ? dx : dy;
- // initializing
- x = startX;
- y = startY;
- xerr = dx;
- yerr = dy;
-
- // compute cells
- for ( t = 0; t < dist; t++ )
- {
- data[x + m_MaskingMap.info.width * y] = value;
-
- xerr += dx;
- yerr += dy;
- if ( xerr > dist )
- {
- xerr -= dist;
- x += incx;
+void MaskingManager::drawLine(std::vector<int> &data, int startX, int startY,
+ int endX, int endY, int value) {
+ // bresenham algorithm
+ int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
+ // compute distances
+ dx = endX - startX;
+ dy = endY - startY;
+
+ // compute increment
+ if (dx < 0) {
+ incx = -1;
+ dx = -dx;
+ } else {
+ incx = dx ? 1 : 0;
}
- if ( yerr > dist )
- {
- yerr -= dist;
- y += incy;
+
+ if (dy < 0) {
+ incy = -1;
+ dy = -dy;
+ } else {
+ incy = dy ? 1 : 0;
}
- }
-}
+ // which distance is greater?
+ dist = (dx > dy) ? dx : dy;
+ // initializing
+ x = startX;
+ y = startY;
+ xerr = dx;
+ yerr = dy;
+
+ // compute cells
+ for (t = 0; t < dist; t++) {
+ data[x + m_MaskingMap.info.width * y] = value;
+
+ xerr += dx;
+ yerr += dy;
+ if (xerr > dist) {
+ xerr -= dist;
+ x += incx;
+ }
+ if (yerr > dist) {
+ yerr -= dist;
+ y += incy;
+ }
+ }
+}
-void MaskingManager::fillPolygon ( std::vector<int> &data, int x, int y, int value )
-{
- int index = x + m_MaskingMap.info.width * y;
- if ( value != data[index] )
- {
- data[index] = value;
- fillPolygon ( data, x + 1, y, value );
- fillPolygon ( data, x - 1, y, value );
- fillPolygon ( data, x, y + 1, value );
- fillPolygon ( data, x, y - 1, value );
- }
+void MaskingManager::fillPolygon(std::vector<int> &data, int x, int y,
+ int value) {
+ int index = x + m_MaskingMap.info.width * y;
+ if (value != data[index]) {
+ data[index] = value;
+ fillPolygon(data, x + 1, y, value);
+ fillPolygon(data, x - 1, y, value);
+ fillPolygon(data, x, y + 1, value);
+ fillPolygon(data, x, y - 1, value);
+ }
}
diff --git a/homer_map_manager/src/map_manager_node.cpp b/homer_map_manager/src/map_manager_node.cpp
index 4e279befe589b7a96212bd279b5030ec378bef97..1060e1a32be2b3c084c417fde7bb0504bd5f30ea 100644
--- a/homer_map_manager/src/map_manager_node.cpp
+++ b/homer_map_manager/src/map_manager_node.cpp
@@ -1,16 +1,16 @@
#include <homer_map_manager/map_manager_node.h>
//#include <gperftools/profiler.h>
-MapManagerNode::MapManagerNode(ros::NodeHandle *nh)
-{
+MapManagerNode::MapManagerNode(ros::NodeHandle* nh) {
m_LastLaserTime = ros::Time::now();
int mapSize;
float resolution;
- ros::param::param("/homer_mapping/size", mapSize, (int) 35);
- ros::param::param("/homer_mapping/resolution", resolution, (float) 0.05);
- ros::param::param("/map_manager/roi_updates", m_roi_polling, (bool) false);
- ros::param::param("/map_manager/roi_polling_time", m_roi_polling_time, (float) 0.5);
+ ros::param::param("/homer_mapping/size", mapSize, (int)35);
+ ros::param::param("/homer_mapping/resolution", resolution, (float)0.05);
+ ros::param::param("/map_manager/roi_updates", m_roi_polling, (bool)false);
+ ros::param::param("/map_manager/roi_polling_time", m_roi_polling_time,
+ (float)0.5);
m_lastROIPoll = ros::Time::now();
m_MapManager = new MapManager(nh);
m_POIManager = new PoiManager(nh);
@@ -21,413 +21,436 @@ MapManagerNode::MapManagerNode(ros::NodeHandle *nh)
mapInfo.height = mapSize / resolution;
mapInfo.resolution = resolution;
mapInfo.origin.position.x = 0;
- mapInfo.origin.position.y = 0;
- mapInfo.origin.position.z = 0;
- mapInfo.origin.orientation.x = 0;
- mapInfo.origin.orientation.y = 0;
- mapInfo.origin.orientation.z = 0;
- mapInfo.origin.orientation.w = 1;
+ mapInfo.origin.position.y = 0;
+ mapInfo.origin.position.z = 0;
+ mapInfo.origin.orientation.x = 0;
+ mapInfo.origin.orientation.y = 0;
+ mapInfo.origin.orientation.z = 0;
+ mapInfo.origin.orientation.w = 1;
m_MaskingManager = new MaskingManager(mapInfo);
- //subscriptions of MapManagerModule
- m_RapidMapSubscriber = nh->subscribe("/rapid_mapping/map", 1, &MapManagerNode::callbackRapidMap, this);
- m_OctomapSubscriber = nh->subscribe("/projected_map", 1, &MapManagerNode::callbackOctomapMap, this);
- m_SLAMMapSubscriber = nh->subscribe("/homer_mapping/slam_map", 1, &MapManagerNode::callbackSLAMMap, this);
- m_SaveMapSubscriber = nh->subscribe("/map_manager/save_map", 1, &MapManagerNode::callbackSaveMap, this);
- m_LoadMapSubscriber = nh->subscribe("/map_manager/load_map", 1, &MapManagerNode::callbackLoadMap, this);
- m_MapVisibilitySubscriber = nh->subscribe("/map_manager/toggle_map_visibility", 1, &MapManagerNode::callbackMapVisibility, this);
- m_LaserScanSubscriber = nh->subscribe("/scan", 1, &MapManagerNode::callbackLaserScan, this);
- m_BackLaserScanSubscriber = nh->subscribe("/back_scan", 1, &MapManagerNode::callbackBackLaser, this);
- m_FrontLaserScanSubscriber = nh->subscribe("/front_scan", 1, &MapManagerNode::callbackFrontLaser, this);
- m_PoseSubscriber = nh->subscribe("/pose", 1, &MapManagerNode::poseCallback, this);
-
- //subscriptions of PoiManager
- m_AddPOISubscriber = nh->subscribe("/map_manager/add_POI", 20, &MapManagerNode::callbackAddPOI, this);
- m_ModifyPOISubscriber = nh->subscribe("/map_manager/modify_POI", 100, &MapManagerNode::callbackModifyPOI, this);
- m_DeletePOISubscriber = nh->subscribe("/map_manager/delete_POI", 100, &MapManagerNode::callbackDeletePOI, this);
- m_GetPOIsService = nh->advertiseService("/map_manager/get_pois", &MapManagerNode::getPOIsService, this);
-
- // Services for Map Handling
- m_SaveMapService = nh->advertiseService("/map_manager/save_map", &MapManagerNode::saveMapService, this);
- m_ResetMapService = nh->advertiseService("/map_manager/reset_map", &MapManagerNode::resetMapService, this);
- m_LoadMapService = nh->advertiseService("/map_manager/load_map", &MapManagerNode::loadMapService, this);
-
- //subscriptions of RoiManager
- m_AddROISubscriber = nh->subscribe("/map_manager/add_ROI", 20, &MapManagerNode::callbackAddROI, this);
- m_ModifyROISubscriber = nh->subscribe("/map_manager/modify_ROI", 100, &MapManagerNode::callbackModifyROI, this);
- m_DeleteROIByNameSubscriber = nh->subscribe("/map_manager/delete_ROI_by_name", 100, &MapManagerNode::callbackDeleteROIbyName, this);
- m_DeleteROIByIDSubscriber = nh->subscribe("/map_manager/delete_ROI_by_id", 100, &MapManagerNode::callbackDeleteROIbyID, this);
- m_GetROIsService = nh->advertiseService("/map_manager/get_rois", &MapManagerNode::getROIsService, this);
- m_GetROINameService = nh->advertiseService("/map_manager/get_roi_name", &MapManagerNode::getROINameService, this);
- m_PoiInsideROISService = nh->advertiseService("/map_manager/point_inside_rois", &MapManagerNode::pointInsideRoisService, this);
- if(m_roi_polling)
- {
- m_RoiPollPublisher = nh->advertise<homer_mapnav_msgs::RoiChange>("/map_manager/roi_change", 1);
- }
-
- //subscribtions of MaskingMapModule
- m_ModifyMapSubscriber = nh->subscribe("/map_manager/modify_map", 1, &MapManagerNode::callbackModifyMap, this);
- m_ResetMapsSubscriber = nh->subscribe("/map_manager/reset_maps", 1, &MapManagerNode::callbackResetMaps, this);
-
- //loaded map publisher
- m_LoadedMapPublisher = nh->advertise<nav_msgs::OccupancyGrid>("/map_manager/loaded_map", 1);
-
- //mask slam publisher
- m_MaskSlamMapPublisher = nh->advertise<nav_msgs::OccupancyGrid>("/map_manager/mask_slam", 1);
-
-
- //load map file from config if present
+ // subscriptions of MapManagerModule
+ m_RapidMapSubscriber = nh->subscribe(
+ "/rapid_mapping/map", 1, &MapManagerNode::callbackRapidMap, this);
+ m_OctomapSubscriber = nh->subscribe(
+ "/projected_map", 1, &MapManagerNode::callbackOctomapMap, this);
+ m_SLAMMapSubscriber = nh->subscribe("/homer_mapping/slam_map", 1,
+ &MapManagerNode::callbackSLAMMap, this);
+ m_SaveMapSubscriber = nh->subscribe("/map_manager/save_map", 1,
+ &MapManagerNode::callbackSaveMap, this);
+ m_LoadMapSubscriber = nh->subscribe("/map_manager/load_map", 1,
+ &MapManagerNode::callbackLoadMap, this);
+ m_MapVisibilitySubscriber =
+ nh->subscribe("/map_manager/toggle_map_visibility", 1,
+ &MapManagerNode::callbackMapVisibility, this);
+ m_LaserScanSubscriber =
+ nh->subscribe("/scan", 1, &MapManagerNode::callbackLaserScan, this);
+ m_BackLaserScanSubscriber = nh->subscribe(
+ "/back_scan", 1, &MapManagerNode::callbackBackLaser, this);
+ m_FrontLaserScanSubscriber = nh->subscribe(
+ "/front_scan", 1, &MapManagerNode::callbackFrontLaser, this);
+ m_PoseSubscriber =
+ nh->subscribe("/pose", 1, &MapManagerNode::poseCallback, this);
+
+ // subscriptions of PoiManager
+ m_AddPOISubscriber = nh->subscribe("/map_manager/add_POI", 20,
+ &MapManagerNode::callbackAddPOI, this);
+ m_ModifyPOISubscriber =
+ nh->subscribe("/map_manager/modify_POI", 100,
+ &MapManagerNode::callbackModifyPOI, this);
+ m_DeletePOISubscriber =
+ nh->subscribe("/map_manager/delete_POI", 100,
+ &MapManagerNode::callbackDeletePOI, this);
+ m_GetPOIsService = nh->advertiseService(
+ "/map_manager/get_pois", &MapManagerNode::getPOIsService, this);
+
+ // Services for Map Handling
+ m_SaveMapService = nh->advertiseService(
+ "/map_manager/save_map", &MapManagerNode::saveMapService, this);
+ m_ResetMapService = nh->advertiseService(
+ "/map_manager/reset_map", &MapManagerNode::resetMapService, this);
+ m_LoadMapService = nh->advertiseService(
+ "/map_manager/load_map", &MapManagerNode::loadMapService, this);
+
+ // subscriptions of RoiManager
+ m_AddROISubscriber = nh->subscribe("/map_manager/add_ROI", 20,
+ &MapManagerNode::callbackAddROI, this);
+ m_ModifyROISubscriber =
+ nh->subscribe("/map_manager/modify_ROI", 100,
+ &MapManagerNode::callbackModifyROI, this);
+ m_DeleteROIByNameSubscriber =
+ nh->subscribe("/map_manager/delete_ROI_by_name", 100,
+ &MapManagerNode::callbackDeleteROIbyName, this);
+ m_DeleteROIByIDSubscriber =
+ nh->subscribe("/map_manager/delete_ROI_by_id", 100,
+ &MapManagerNode::callbackDeleteROIbyID, this);
+ m_GetROIsService = nh->advertiseService(
+ "/map_manager/get_rois", &MapManagerNode::getROIsService, this);
+ m_GetROINameService = nh->advertiseService(
+ "/map_manager/get_roi_name", &MapManagerNode::getROINameService, this);
+ m_PoiInsideROISService =
+ nh->advertiseService("/map_manager/point_inside_rois",
+ &MapManagerNode::pointInsideRoisService, this);
+ if (m_roi_polling) {
+ m_RoiPollPublisher = nh->advertise<homer_mapnav_msgs::RoiChange>(
+ "/map_manager/roi_change", 1);
+ }
+
+ // subscribtions of MaskingMapModule
+ m_ModifyMapSubscriber = nh->subscribe(
+ "/map_manager/modify_map", 1, &MapManagerNode::callbackModifyMap, this);
+ m_ResetMapsSubscriber = nh->subscribe(
+ "/map_manager/reset_maps", 1, &MapManagerNode::callbackResetMaps, this);
+
+ // loaded map publisher
+ m_LoadedMapPublisher =
+ nh->advertise<nav_msgs::OccupancyGrid>("/map_manager/loaded_map", 1);
+
+ // mask slam publisher
+ m_MaskSlamMapPublisher =
+ nh->advertise<nav_msgs::OccupancyGrid>("/map_manager/mask_slam", 1);
+
+ // load map file from config if present
std::string mapfile;
- ros::param::get("/map_manager/default_mapfile", mapfile);
- if(mapfile != "")
- {
- std_msgs::String::Ptr mapfileMsg(new std_msgs::String);
- mapfileMsg->data= mapfile;
- callbackLoadMap ( mapfileMsg );
+ ros::param::get("/map_manager/default_mapfile", mapfile);
+ if (mapfile != "") {
+ std_msgs::String::Ptr mapfileMsg(new std_msgs::String);
+ mapfileMsg->data = mapfile;
+ callbackLoadMap(mapfileMsg);
}
- m_POIManager->broadcastPoiList();
- m_ROIManager->broadcastRoiList();
+ m_POIManager->broadcastPoiList();
+ m_ROIManager->broadcastRoiList();
}
-MapManagerNode::~MapManagerNode()
-{
- if(m_MapManager) delete m_MapManager;
- if(m_POIManager) delete m_POIManager;
- if(m_ROIManager) delete m_ROIManager;
- if(m_MaskingManager) delete m_MaskingManager;
+MapManagerNode::~MapManagerNode() {
+ if (m_MapManager) delete m_MapManager;
+ if (m_POIManager) delete m_POIManager;
+ if (m_ROIManager) delete m_ROIManager;
+ if (m_MaskingManager) delete m_MaskingManager;
}
-void MapManagerNode::callbackSLAMMap(const nav_msgs::OccupancyGrid::ConstPtr &msg)
-{
+void MapManagerNode::callbackSLAMMap(
+ const nav_msgs::OccupancyGrid::ConstPtr& msg) {
m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER, msg);
- m_MaskingManager->updateMapInfo(const nav_msgs::MapMetaData::ConstPtr msg->info);
+ m_MaskingManager->updateMapInfo(msg->info);
+}
+
+void MapManagerNode::callbackRapidMap(
+ const nav_msgs::OccupancyGrid::ConstPtr& msg) {
+ m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::RAPID_MAP, msg);
}
-void MapManagerNode::callbackRapidMap( const nav_msgs::OccupancyGrid::ConstPtr& msg)
-{
- m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::RAPID_MAP, msg);
+void MapManagerNode::callbackOctomapMap(
+ const nav_msgs::OccupancyGrid::ConstPtr& msg) {
+ m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::KINECT_LAYER,
+ msg);
+
+ // nav_msgs::OccupancyGrid mergedMap;
+ //
+ // int width = 701;
+ // int height = 701;
+ // float resolution = 0.05;
+ //
+ // int ByteSize = width * height;
+ //
+ // mergedMap.header.stamp = ros::Time::now();
+ // mergedMap.header.frame_id = "map";
+ // mergedMap.info.width = width;
+ // mergedMap.info.height = height;
+ // mergedMap.info.resolution = resolution;
+ // mergedMap.info.origin.position.x = -height*resolution/2;
+ // mergedMap.info.origin.position.y = -width*resolution/2;
+ // mergedMap.info.origin.orientation.w = 1.0;
+ // mergedMap.info.origin.orientation.x = 0.0;
+ // mergedMap.info.origin.orientation.y = 0.0;
+ // mergedMap.info.origin.orientation.z = 0.0;
+ // mergedMap.data.resize(ByteSize,-1);
+
+ // for ( int y = 0; y < msg->info.height; y++ )
+ // {
+ // for ( int x = 0; x < msg->info.width; x++ )
+ // {
+ // int i = x + y * msg->info.width;
+
+ //
+ // //if cell is occupied by kinect obstacle merge cell with merged
+ //map
+ // if(msg->data[i] ==
+ //homer_mapnav_msgs::ModifyMap::BLOCKED)
+ // {
+
+ // Eigen::Vector2i point(x,y);
+ // geometry_msgs::Point tmp = map_tools::fromMapCoords( point
+ //,msg->info.origin, msg->info.resolution);
+ // point = map_tools::toMapCoords(tmp , mergedMap.info.origin,
+ //mergedMap.info.resolution);
+ // int k = point.y() * mergedMap.info.width +
+ //point.x();
+ // mergedMap.data[k] =
+ //homer_mapnav_msgs::ModifyMap::DEPTH;
+ // }
+ // }
+ // }
+ //
+ //
+ // m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::KINECT_LAYER,
+ //boost::make_shared<nav_msgs::OccupancyGrid>(mergedMap));
}
-void MapManagerNode::callbackOctomapMap( const nav_msgs::OccupancyGrid::ConstPtr& msg)
-{
- m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::KINECT_LAYER, msg);
-
-// nav_msgs::OccupancyGrid mergedMap;
-//
-// int width = 701;
-// int height = 701;
-// float resolution = 0.05;
-//
-// int ByteSize = width * height;
-//
-// mergedMap.header.stamp = ros::Time::now();
-// mergedMap.header.frame_id = "map";
-// mergedMap.info.width = width;
-// mergedMap.info.height = height;
-// mergedMap.info.resolution = resolution;
-// mergedMap.info.origin.position.x = -height*resolution/2;
-// mergedMap.info.origin.position.y = -width*resolution/2;
-// mergedMap.info.origin.orientation.w = 1.0;
-// mergedMap.info.origin.orientation.x = 0.0;
-// mergedMap.info.origin.orientation.y = 0.0;
-// mergedMap.info.origin.orientation.z = 0.0;
-// mergedMap.data.resize(ByteSize,-1);
-
-// for ( int y = 0; y < msg->info.height; y++ )
-// {
-// for ( int x = 0; x < msg->info.width; x++ )
-// {
-// int i = x + y * msg->info.width;
-
-//
-// //if cell is occupied by kinect obstacle merge cell with merged map
-// if(msg->data[i] == homer_mapnav_msgs::ModifyMap::BLOCKED)
-// {
-
-// Eigen::Vector2i point(x,y);
-// geometry_msgs::Point tmp = map_tools::fromMapCoords( point ,msg->info.origin, msg->info.resolution);
-// point = map_tools::toMapCoords(tmp , mergedMap.info.origin, mergedMap.info.resolution);
-// int k = point.y() * mergedMap.info.width + point.x();
-// mergedMap.data[k] = homer_mapnav_msgs::ModifyMap::DEPTH;
-// }
-// }
-// }
-//
-//
-// m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::KINECT_LAYER, boost::make_shared<nav_msgs::OccupancyGrid>(mergedMap));
-}
-
-void MapManagerNode::callbackSaveMap(const std_msgs::String::ConstPtr &msg)
-{
+void MapManagerNode::callbackSaveMap(const std_msgs::String::ConstPtr& msg) {
MapGenerator map_saver(msg->data);
- nav_msgs::OccupancyGrid::ConstPtr SLAMMap = m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER);
- nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER);
- map_saver.save(SLAMMap, maskingMap, m_POIManager->getList(), m_ROIManager->getList() );
+ nav_msgs::OccupancyGrid::ConstPtr SLAMMap =
+ m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER);
+ nav_msgs::OccupancyGrid::ConstPtr maskingMap =
+ m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER);
+ map_saver.save(SLAMMap, maskingMap, m_POIManager->getList(),
+ m_ROIManager->getList());
}
-void MapManagerNode::callbackLoadMap(const std_msgs::String::ConstPtr &msg)
-{
+void MapManagerNode::callbackLoadMap(const std_msgs::String::ConstPtr& msg) {
m_MapManager->clearMapLayers();
- ROS_INFO_STREAM( "Loading map from file " << msg->data);
+ ROS_INFO_STREAM("Loading map from file " << msg->data);
bool success;
MapServer map_loader(msg->data, success);
- if(success)
- {
- ros::Rate poll_rate(10);
- while(m_LoadedMapPublisher.getNumSubscribers() == 0)
- {
- poll_rate.sleep();
- }
- m_LoadedMapPublisher.publish(map_loader.getSLAMMap());
- m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER, boost::make_shared<nav_msgs::OccupancyGrid>(map_loader.getSLAMMap()));
- nav_msgs::OccupancyGrid::ConstPtr maskingMap = boost::make_shared<nav_msgs::OccupancyGrid>(map_loader.getMaskingMap());
- m_MaskingManager->replaceMap(map_loader.getMaskingMap());
- if(maskingMap->data.size() != 0)
- {
- m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
- }
- m_POIManager->replacePOIList(map_loader.getPois());
- m_POIManager->broadcastPoiList();
- m_ROIManager->replaceROIList(map_loader.getRois());
- m_ROIManager->broadcastRoiList();
- }
- else
- {
- ROS_ERROR_STREAM("[MapManager] Could not open mapfile!!");
+ if (success) {
+ ros::Rate poll_rate(10);
+ while (m_LoadedMapPublisher.getNumSubscribers() == 0) {
+ poll_rate.sleep();
+ }
+ m_LoadedMapPublisher.publish(map_loader.getSLAMMap());
+ m_MapManager->updateMapLayer(
+ homer_mapnav_msgs::MapLayers::SLAM_LAYER,
+ boost::make_shared<nav_msgs::OccupancyGrid>(
+ map_loader.getSLAMMap()));
+ nav_msgs::OccupancyGrid::ConstPtr maskingMap =
+ boost::make_shared<nav_msgs::OccupancyGrid>(
+ map_loader.getMaskingMap());
+ m_MaskingManager->replaceMap(map_loader.getMaskingMap());
+ if (maskingMap->data.size() != 0) {
+ m_MapManager->updateMapLayer(
+ homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
+ }
+ m_POIManager->replacePOIList(map_loader.getPois());
+ m_POIManager->broadcastPoiList();
+ m_ROIManager->replaceROIList(map_loader.getRois());
+ m_ROIManager->broadcastRoiList();
+ } else {
+ ROS_ERROR_STREAM("[MapManager] Could not open mapfile!!");
}
}
-void MapManagerNode::callbackAddPOI(const homer_mapnav_msgs::PointOfInterest::ConstPtr &msg)
-{
- ROS_INFO_STREAM("callbackAddPOI");
+void MapManagerNode::callbackAddPOI(
+ const homer_mapnav_msgs::PointOfInterest::ConstPtr& msg) {
+ ROS_INFO_STREAM("callbackAddPOI");
m_POIManager->addPointOfInterest(msg);
}
-void MapManagerNode::callbackModifyPOI(const homer_mapnav_msgs::ModifyPOI::ConstPtr &msg)
-{
+void MapManagerNode::callbackModifyPOI(
+ const homer_mapnav_msgs::ModifyPOI::ConstPtr& msg) {
m_POIManager->modifyPointOfInterest(msg);
}
-void MapManagerNode::callbackDeletePOI(const std_msgs::String::ConstPtr &msg)
-{
+void MapManagerNode::callbackDeletePOI(const std_msgs::String::ConstPtr& msg) {
m_POIManager->deletePointOfInterest(msg->data);
}
-void MapManagerNode::callbackAddROI(const homer_mapnav_msgs::RegionOfInterest::ConstPtr &msg)
-{
+void MapManagerNode::callbackAddROI(
+ const homer_mapnav_msgs::RegionOfInterest::ConstPtr& msg) {
m_ROIManager->addRegionOfInterest(msg);
}
-void MapManagerNode::callbackModifyROI(const homer_mapnav_msgs::RegionOfInterest::ConstPtr &msg)
-{
+void MapManagerNode::callbackModifyROI(
+ const homer_mapnav_msgs::RegionOfInterest::ConstPtr& msg) {
m_ROIManager->modifyRegionOfInterest(msg);
}
-void MapManagerNode::callbackDeleteROIbyName(const std_msgs::String::ConstPtr &msg)
-{
- ROS_INFO_STREAM("Recieved /map_manager/delete_ROI_by_name");
- if(m_ROIManager->deleteRegionOfInterest(msg->data))
- {
- ROS_INFO_STREAM("ROI deleted.");
- }
- else
- {
- ROS_WARN_STREAM("Could not delete ROI.");
+void MapManagerNode::callbackDeleteROIbyName(
+ const std_msgs::String::ConstPtr& msg) {
+ ROS_INFO_STREAM("Recieved /map_manager/delete_ROI_by_name");
+ if (m_ROIManager->deleteRegionOfInterest(msg->data)) {
+ ROS_INFO_STREAM("ROI deleted.");
+ } else {
+ ROS_WARN_STREAM("Could not delete ROI.");
}
}
-void MapManagerNode::callbackDeleteROIbyID(const std_msgs::Int32::ConstPtr &msg)
-{
+void MapManagerNode::callbackDeleteROIbyID(
+ const std_msgs::Int32::ConstPtr& msg) {
m_ROIManager->deleteRegionOfInterest(msg->data);
}
-void MapManagerNode::callbackMapVisibility(const homer_mapnav_msgs::MapLayers::ConstPtr &msg)
-{
+void MapManagerNode::callbackMapVisibility(
+ const homer_mapnav_msgs::MapLayers::ConstPtr& msg) {
m_MapManager->toggleMapVisibility(msg->layer, msg->state);
}
-void MapManagerNode::callbackModifyMap(const homer_mapnav_msgs::ModifyMap::ConstPtr &msg)
-{
- nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MaskingManager->modifyMap(msg);
- if(msg->mapLayer == homer_mapnav_msgs::MapLayers::SLAM_LAYER || msg->maskAction == homer_mapnav_msgs::ModifyMap::HIGH_SENSITIV)
- {
+void MapManagerNode::callbackModifyMap(
+ const homer_mapnav_msgs::ModifyMap::ConstPtr& msg) {
+ nav_msgs::OccupancyGrid::ConstPtr maskingMap =
+ m_MaskingManager->modifyMap(msg);
+ if (msg->mapLayer == homer_mapnav_msgs::MapLayers::SLAM_LAYER ||
+ msg->maskAction == homer_mapnav_msgs::ModifyMap::HIGH_SENSITIV) {
m_MaskSlamMapPublisher.publish(maskingMap);
m_highSensitiveMap = maskingMap;
- }
- else
- {
- m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
+ } else {
+ m_MapManager->updateMapLayer(
+ homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
}
}
-void MapManagerNode::callbackResetMaps(const std_msgs::Empty::ConstPtr &msg)
-{
+void MapManagerNode::callbackResetMaps(const std_msgs::Empty::ConstPtr& msg) {
nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MaskingManager->resetMap();
- m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
+ m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER,
+ maskingMap);
}
-void MapManagerNode::callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr &msg)
-{
+void MapManagerNode::callbackLaserScan(
+ const sensor_msgs::LaserScan::ConstPtr& msg) {
m_MapManager->updateLaser(homer_mapnav_msgs::MapLayers::SICK_LAYER, msg);
}
-void MapManagerNode::callbackBackLaser(const sensor_msgs::LaserScan::ConstPtr &msg)
-{
+void MapManagerNode::callbackBackLaser(
+ const sensor_msgs::LaserScan::ConstPtr& msg) {
m_MapManager->updateLaser(homer_mapnav_msgs::MapLayers::HOKUYO_BACK, msg);
}
-void MapManagerNode::callbackFrontLaser(const sensor_msgs::LaserScan::ConstPtr &msg)
-{
- m_MapManager->updateLaser(homer_mapnav_msgs::MapLayers::HOKUYO_FRONT, msg);
+void MapManagerNode::callbackFrontLaser(
+ const sensor_msgs::LaserScan::ConstPtr& msg) {
+ m_MapManager->updateLaser(homer_mapnav_msgs::MapLayers::HOKUYO_FRONT, msg);
}
-bool MapManagerNode::getPOIsService(homer_mapnav_msgs::GetPointsOfInterest::Request& req,
- homer_mapnav_msgs::GetPointsOfInterest::Response& res)
-{
+bool MapManagerNode::getPOIsService(
+ homer_mapnav_msgs::GetPointsOfInterest::Request& req,
+ homer_mapnav_msgs::GetPointsOfInterest::Response& res) {
res.poi_list.pois = m_POIManager->getList();
return true;
}
-bool MapManagerNode::getROIsService(homer_mapnav_msgs::GetRegionsOfInterest::Request& req,
- homer_mapnav_msgs::GetRegionsOfInterest::Response& res)
-{
+bool MapManagerNode::getROIsService(
+ homer_mapnav_msgs::GetRegionsOfInterest::Request& req,
+ homer_mapnav_msgs::GetRegionsOfInterest::Response& res) {
res.roi_list.rois = m_ROIManager->getList();
return true;
}
-bool MapManagerNode::pointInsideRoisService(homer_mapnav_msgs::PointInsideRois::Request& req,
- homer_mapnav_msgs::PointInsideRois::Response& res)
-{
- res.rois = m_ROIManager->pointInsideRegionOfInterest(req.point);
- return true;
+bool MapManagerNode::pointInsideRoisService(
+ homer_mapnav_msgs::PointInsideRois::Request& req,
+ homer_mapnav_msgs::PointInsideRois::Response& res) {
+ res.rois = m_ROIManager->pointInsideRegionOfInterest(req.point);
+ return true;
}
-bool MapManagerNode::getROINameService(homer_mapnav_msgs::GetRegionOfInterestName::Request& req,
- homer_mapnav_msgs::GetRegionOfInterestName::Response& res)
-{
- res.name = m_ROIManager->getROIName(req.roi_id);
- return true;
+bool MapManagerNode::getROINameService(
+ homer_mapnav_msgs::GetRegionOfInterestName::Request& req,
+ homer_mapnav_msgs::GetRegionOfInterestName::Response& res) {
+ res.name = m_ROIManager->getROIName(req.roi_id);
+ return true;
}
-void MapManagerNode::poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
-{
- m_MapManager->updatePose(msg);
- if(msg->header.stamp - m_lastROIPoll > ros::Duration(m_roi_polling_time) && m_roi_polling)
- {
- m_lastROIPoll = msg->header.stamp;
- geometry_msgs::PointStamped posePoint;
- posePoint.header.frame_id="/map";
- posePoint.header.stamp = msg->header.stamp;
- posePoint.point = msg->pose.position;
- std::vector<homer_mapnav_msgs::RegionOfInterest> rois;
- rois = m_ROIManager->pointInsideRegionOfInterest(posePoint);
- bool found = false;
- for(int i = 0; i < m_ids.size(); i++)
- {
- found = false;
- for(int j = 0; j < rois.size(); j++)
- {
- if(m_ids[i] == rois[j].id)
- {
- rois.erase (rois.begin()+j);
- found = true;
- break;
- }
- }
- if(!found)
- {
- homer_mapnav_msgs::RoiChange change;
- change.id = m_ids[i];
- change.name = m_ROIManager->getROIName(m_ids[i]);
- change.action = false;
- m_RoiPollPublisher.publish(change);
- m_ids.erase(m_ids.begin()+i);
- i--;
- }
- }
- for(int i = 0; i < rois.size(); i++)
- {
- homer_mapnav_msgs::RoiChange change;
- change.id = rois[i].id;
- change.name = m_ROIManager->getROIName(change.id);
- change.action = true;
- m_RoiPollPublisher.publish(change);
- m_ids.push_back(rois[i].id);
- }
- }
+void MapManagerNode::poseCallback(
+ const geometry_msgs::PoseStamped::ConstPtr& msg) {
+ m_MapManager->updatePose(msg);
+ if (msg->header.stamp - m_lastROIPoll > ros::Duration(m_roi_polling_time) &&
+ m_roi_polling) {
+ m_lastROIPoll = msg->header.stamp;
+ geometry_msgs::PointStamped posePoint;
+ posePoint.header.frame_id = "/map";
+ posePoint.header.stamp = msg->header.stamp;
+ posePoint.point = msg->pose.position;
+ std::vector<homer_mapnav_msgs::RegionOfInterest> rois;
+ rois = m_ROIManager->pointInsideRegionOfInterest(posePoint);
+ bool found = false;
+ for (int i = 0; i < m_ids.size(); i++) {
+ found = false;
+ for (int j = 0; j < rois.size(); j++) {
+ if (m_ids[i] == rois[j].id) {
+ rois.erase(rois.begin() + j);
+ found = true;
+ break;
+ }
+ }
+ if (!found) {
+ homer_mapnav_msgs::RoiChange change;
+ change.id = m_ids[i];
+ change.name = m_ROIManager->getROIName(m_ids[i]);
+ change.action = false;
+ m_RoiPollPublisher.publish(change);
+ m_ids.erase(m_ids.begin() + i);
+ i--;
+ }
+ }
+ for (int i = 0; i < rois.size(); i++) {
+ homer_mapnav_msgs::RoiChange change;
+ change.id = rois[i].id;
+ change.name = m_ROIManager->getROIName(change.id);
+ change.action = true;
+ m_RoiPollPublisher.publish(change);
+ m_ids.push_back(rois[i].id);
+ }
+ }
}
-
bool MapManagerNode::saveMapService(homer_mapnav_msgs::SaveMap::Request& req,
- homer_mapnav_msgs::SaveMap::Response& res)
-{
- //ROS_INFO_STREAM("Saving map "<<req->folder);
+ homer_mapnav_msgs::SaveMap::Response& res) {
+ // ROS_INFO_STREAM("Saving map "<<req->folder);
MapGenerator map_saver(std::string(req.folder.data));
- nav_msgs::OccupancyGrid::ConstPtr SLAMMap = m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER);
- nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER);
- map_saver.save(SLAMMap, maskingMap, m_POIManager->getList(), m_ROIManager->getList() );
+ nav_msgs::OccupancyGrid::ConstPtr SLAMMap =
+ m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER);
+ nav_msgs::OccupancyGrid::ConstPtr maskingMap =
+ m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER);
+ map_saver.save(SLAMMap, maskingMap, m_POIManager->getList(),
+ m_ROIManager->getList());
}
bool MapManagerNode::loadMapService(homer_mapnav_msgs::LoadMap::Request& req,
- homer_mapnav_msgs::LoadMap::Response& res)
-{
-
- //load map file from config if present
+ homer_mapnav_msgs::LoadMap::Response& res) {
+ // load map file from config if present
std::string mapfile = std::string(req.filename.data);
- if(mapfile != "")
- {
- ROS_INFO_STREAM("Loading map with filename: "<<mapfile);
- std_msgs::String::Ptr mapfileMsg(new std_msgs::String);
- mapfileMsg->data= mapfile;
- callbackLoadMap ( mapfileMsg );
+ if (mapfile != "") {
+ ROS_INFO_STREAM("Loading map with filename: " << mapfile);
+ std_msgs::String::Ptr mapfileMsg(new std_msgs::String);
+ mapfileMsg->data = mapfile;
+ callbackLoadMap(mapfileMsg);
+ } else {
+ ROS_ERROR_STREAM("Map filename is empty. Could not load map");
}
- else
- {
- ROS_ERROR_STREAM("Map filename is empty. Could not load map");
- }
}
bool MapManagerNode::resetMapService(std_srvs::Empty::Request& req,
- std_srvs::Empty::Response& res)
-{
- ROS_INFO_STREAM("Resetting current map");
+ std_srvs::Empty::Response& res) {
+ ROS_INFO_STREAM("Resetting current map");
nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MaskingManager->resetMap();
- m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
+ m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER,
+ maskingMap);
}
-int main(int argc, char** argv)
-{
+int main(int argc, char** argv) {
ros::init(argc, argv, "map_manager");
ros::NodeHandle nh;
-/* char* pprofile = std::getenv("MAPMANAGER_PROFILE");
- if (pprofile)
- {
- ProfilerStart(pprofile);
- }
-*/
+ /* char* pprofile = std::getenv("MAPMANAGER_PROFILE");
+ if (pprofile)
+ {
+ ProfilerStart(pprofile);
+ }
+ */
MapManagerNode node(&nh);
ros::Rate loop_rate(10);
- while (ros::ok())
- {
- try
- {
+ while (ros::ok()) {
+ try {
ros::spinOnce();
loop_rate.sleep();
+ } catch (exception& e) {
+ std::cout << "Exception in main loop" << e.what() << std::endl;
}
- catch (exception &e)
+ }
+ /* if (pprofile)
{
- std::cout<<"Exception in main loop"<<e.what()<<std::endl;
+ ProfilerStop();
}
- }
-/* if (pprofile)
- {
- ProfilerStop();
- }
-*/
+ */
return 0;
}
-