Skip to content
Snippets Groups Projects
Commit 60972339 authored by Florian Polster's avatar Florian Polster
Browse files

masking manager dynamic map

parent 8da12a60
No related branches found
No related tags found
1 merge request!2Feature/dynamic map size
This commit is part of merge request !2. Comments created here will be created in the context of that merge request.
Hide whitespace changes
Inline Side-by-side
homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h
+ 17
− 17
View file @ 60972339
...@@ -3,31 +3,32 @@ ...@@ -3,31 +3,32 @@
#include <ros/ros.h> #include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/MapMetaData.h>
#include <homer_mapnav_msgs/ModifyMap.h> #include <homer_mapnav_msgs/ModifyMap.h>
#include <nav_msgs/MapMetaData.h>
#include <nav_msgs/OccupancyGrid.h>
#include <sstream> #include <sstream>
/** /**
* @class MaskingManager * @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 * @author Malte Knauf, David Gossow
*/ */
class MaskingManager class MaskingManager {
{ public:
public:
/** @brief The constructor. */ /** @brief The constructor. */
MaskingManager(nav_msgs::MapMetaData mapInfo); MaskingManager(nav_msgs::MapMetaData mapInfo);
/** @brief The destructor. */ /** @brief The destructor. */
virtual ~MaskingManager(); 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 */ /** modifies either the masking layer or the slam layer (accordingly to the
nav_msgs::OccupancyGrid::ConstPtr modifyMap(homer_mapnav_msgs::ModifyMap::ConstPtr msg); * given map layer in the msg */
nav_msgs::OccupancyGrid::ConstPtr modifyMap(
homer_mapnav_msgs::ModifyMap::ConstPtr msg);
/** resets the masking map layer */ /** resets the masking map layer */
nav_msgs::OccupancyGrid::ConstPtr resetMap(); nav_msgs::OccupancyGrid::ConstPtr resetMap();
...@@ -35,19 +36,18 @@ class MaskingManager ...@@ -35,19 +36,18 @@ class MaskingManager
/** replaces the masking map layer */ /** replaces the masking map layer */
void replaceMap(nav_msgs::OccupancyGrid map); void replaceMap(nav_msgs::OccupancyGrid map);
private: private:
/** stores the masking values in the dedicated masking map */ /** stores the masking values in the dedicated masking map */
nav_msgs::OccupancyGrid m_MaskingMap; nav_msgs::OccupancyGrid m_MaskingMap;
/** stores the masking values that are afterwards sent to the slam map */ /** stores the masking values that are afterwards sent to the slam map */
nav_msgs::OccupancyGrid m_SlamMap; nav_msgs::OccupancyGrid m_SlamMap;
/** tools to draw masking polygons */ /** tools to draw masking polygons */
void drawPolygon ( std::vector< geometry_msgs::Point > vertices, int value, int mapLayer ); void drawPolygon(std::vector<geometry_msgs::Point> vertices, int value,
void drawLine ( std::vector<int> &data, int startX, int startY, int endX, int endY, int value ); int mapLayer);
void fillPolygon ( std::vector<int> &data, int x, int y, int value ); 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 #endif
homer_map_manager/src/Managers/MaskingManager.cpp
+ 138
− 147
View file @ 60972339
...@@ -2,190 +2,181 @@ ...@@ -2,190 +2,181 @@
using namespace std; using namespace std;
MaskingManager::MaskingManager(nav_msgs::MapMetaData mapInfo) MaskingManager::MaskingManager(nav_msgs::MapMetaData mapInfo) {
{
m_MaskingMap.info = mapInfo; m_MaskingMap.info = mapInfo;
m_MaskingMap.data.resize(m_MaskingMap.info.width * m_MaskingMap.info.height); m_MaskingMap.data.resize(m_MaskingMap.info.width *
std::fill( m_MaskingMap.data.begin(), m_MaskingMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED ); 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.info = mapInfo;
m_SlamMap.data.resize(m_SlamMap.info.width * m_SlamMap.info.height); 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) void MaskingManager::updateMapInfo(const nav_msgs::MapMetaData &mapInfo) {
{ if (m_SlamMap.info.width != mapInfo.width) {
if(m_SlamMap.info.width != mapInfo.width)
{
m_SlamMap.info = mapInfo; 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); m_MaskingMap.data.resize(mapInfo.width * mapInfo.height);
std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(), 0); std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(),
homer_mapnav_msgs::ModifyMap::NOT_MASKED);
for(int x = 0; x < )
{ for (int x = 0; x < m_MaskingMap.info.width; x++) {
for(int y = 0;) 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) nav_msgs::OccupancyGrid::ConstPtr MaskingManager::modifyMap(
{ homer_mapnav_msgs::ModifyMap::ConstPtr msg) {
//reset SLAM mask map before each masking // reset SLAM mask map before each masking
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);
drawPolygon(msg->region, msg->maskAction, msg->mapLayer); drawPolygon(msg->region, msg->maskAction, msg->mapLayer);
nav_msgs::OccupancyGrid::ConstPtr ret; nav_msgs::OccupancyGrid::ConstPtr ret;
if(msg->mapLayer == 0) if (msg->mapLayer == 0) {
{ ret = boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_SlamMap);
ret = boost::make_shared<const::nav_msgs::OccupancyGrid>(m_SlamMap); } else {
} ret = boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_MaskingMap);
else
{
ret = boost::make_shared<const::nav_msgs::OccupancyGrid>(m_MaskingMap);
} }
return ret; return ret;
} }
nav_msgs::OccupancyGrid::ConstPtr MaskingManager::resetMap() nav_msgs::OccupancyGrid::ConstPtr MaskingManager::resetMap() {
{ std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(),
std::fill( m_MaskingMap.data.begin(), m_MaskingMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED ); homer_mapnav_msgs::ModifyMap::NOT_MASKED);
nav_msgs::OccupancyGrid::ConstPtr ret = boost::make_shared<const::nav_msgs::OccupancyGrid>(m_MaskingMap); nav_msgs::OccupancyGrid::ConstPtr ret =
boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_MaskingMap);
return ret; return ret;
} }
void MaskingManager::replaceMap(nav_msgs::OccupancyGrid map) void MaskingManager::replaceMap(nav_msgs::OccupancyGrid map) {
{ if (map.data.size() != 0)
if(map.data.size() != 0)
m_MaskingMap = map; m_MaskingMap = map;
else 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) void MaskingManager::drawPolygon(vector<geometry_msgs::Point> vertices,
{ int value, int mapLayer) {
if ( vertices.size() == 0 ) if (vertices.size() == 0) {
{ ROS_INFO_STREAM("No vertices given!");
ROS_INFO_STREAM( "No vertices given!" ); return;
return; }
} // make temp. map
//make temp. map
std::vector<int> data(m_MaskingMap.info.width * m_MaskingMap.info.height); std::vector<int> data(m_MaskingMap.info.width * m_MaskingMap.info.height);
std::fill (data.begin(), data.end(), 0); std::fill(data.begin(), data.end(), 0);
//draw the lines surrounding the polygon // draw the lines surrounding the polygon
for ( unsigned int i = 0; i < vertices.size(); i++ ) for (unsigned int i = 0; i < vertices.size(); i++) {
{ int i2 = (i + 1) % vertices.size();
int i2 = ( i+1 ) % vertices.size(); drawLine(data, vertices[i].x, vertices[i].y, vertices[i2].x,
drawLine ( data, vertices[i].x, vertices[i].y, vertices[i2].x, vertices[i2].y, 1); vertices[i2].y, 1);
} }
//calculate a point in the middle of the polygon // calculate a point in the middle of the polygon
float midX = 0; float midX = 0;
float midY = 0; float midY = 0;
for ( unsigned int i = 0; i < vertices.size(); i++ ) for (unsigned int i = 0; i < vertices.size(); i++) {
{ midX += vertices[i].x;
midX += vertices[i].x; midY += vertices[i].y;
midY += vertices[i].y; }
} midX /= vertices.size();
midX /= vertices.size(); midY /= vertices.size();
midY /= vertices.size(); // fill polygon
//fill polygon fillPolygon(data, (int)midX, (int)midY, 1);
fillPolygon ( data, (int)midX, (int)midY, 1 );
// copy polygon to masking map or slam map (according to parameter mapLayer)
//copy polygon to masking map or slam map (according to parameter mapLayer) for (int i = 0; i < data.size(); i++) {
for ( int i = 0; i < data.size(); i++ ) if (data[i] != 0) {
{ switch (mapLayer) {
if ( data[i] != 0 ) case 0: // SLAM map
{ m_SlamMap.data[i] = value;
switch(mapLayer) break;
{ case 1: // Kinect Map. apply masking to masking map
case 0: //SLAM map case 2: // masking map
m_SlamMap.data[i] = value; m_MaskingMap.data[i] = value;
break; 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 ) void MaskingManager::drawLine(std::vector<int> &data, int startX, int startY,
{ int endX, int endY, int value) {
//bresenham algorithm // bresenham algorithm
int x, y, t, dist, xerr, yerr, dx, dy, incx, incy; int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
// compute distances // compute distances
dx = endX - startX; dx = endX - startX;
dy = endY - startY; dy = endY - startY;
// compute increment // compute increment
if ( dx < 0 ) if (dx < 0) {
{ incx = -1;
incx = -1; dx = -dx;
dx = -dx; } else {
} incx = dx ? 1 : 0;
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;
} }
if ( yerr > dist )
{ if (dy < 0) {
yerr -= dist; incy = -1;
y += incy; 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 ) void MaskingManager::fillPolygon(std::vector<int> &data, int x, int y,
{ int value) {
int index = x + m_MaskingMap.info.width * y; int index = x + m_MaskingMap.info.width * y;
if ( value != data[index] ) if (value != data[index]) {
{ data[index] = value;
data[index] = value; fillPolygon(data, x + 1, y, value);
fillPolygon ( data, x + 1, y, 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 ); fillPolygon(data, x, y - 1, value);
fillPolygon ( data, x, y - 1, value ); }
}
} }
homer_map_manager/src/map_manager_node.cpp
+ 335
− 312
View file @ 60972339
#include <homer_map_manager/map_manager_node.h> #include <homer_map_manager/map_manager_node.h>
//#include <gperftools/profiler.h> //#include <gperftools/profiler.h>
MapManagerNode::MapManagerNode(ros::NodeHandle *nh) MapManagerNode::MapManagerNode(ros::NodeHandle* nh) {
{
m_LastLaserTime = ros::Time::now(); m_LastLaserTime = ros::Time::now();
int mapSize; int mapSize;
float resolution; float resolution;
ros::param::param("/homer_mapping/size", mapSize, (int) 35); ros::param::param("/homer_mapping/size", mapSize, (int)35);
ros::param::param("/homer_mapping/resolution", resolution, (float) 0.05); 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_updates", m_roi_polling, (bool)false);
ros::param::param("/map_manager/roi_polling_time", m_roi_polling_time, (float) 0.5); ros::param::param("/map_manager/roi_polling_time", m_roi_polling_time,
(float)0.5);
m_lastROIPoll = ros::Time::now(); m_lastROIPoll = ros::Time::now();
m_MapManager = new MapManager(nh); m_MapManager = new MapManager(nh);
m_POIManager = new PoiManager(nh); m_POIManager = new PoiManager(nh);
...@@ -21,413 +21,436 @@ MapManagerNode::MapManagerNode(ros::NodeHandle *nh) ...@@ -21,413 +21,436 @@ MapManagerNode::MapManagerNode(ros::NodeHandle *nh)
mapInfo.height = mapSize / resolution; mapInfo.height = mapSize / resolution;
mapInfo.resolution = resolution; mapInfo.resolution = resolution;
mapInfo.origin.position.x = 0; mapInfo.origin.position.x = 0;
mapInfo.origin.position.y = 0; mapInfo.origin.position.y = 0;
mapInfo.origin.position.z = 0; mapInfo.origin.position.z = 0;
mapInfo.origin.orientation.x = 0; mapInfo.origin.orientation.x = 0;
mapInfo.origin.orientation.y = 0; mapInfo.origin.orientation.y = 0;
mapInfo.origin.orientation.z = 0; mapInfo.origin.orientation.z = 0;
mapInfo.origin.orientation.w = 1; mapInfo.origin.orientation.w = 1;
m_MaskingManager = new MaskingManager(mapInfo); m_MaskingManager = new MaskingManager(mapInfo);
//subscriptions of MapManagerModule // subscriptions of MapManagerModule
m_RapidMapSubscriber = nh->subscribe("/rapid_mapping/map", 1, &MapManagerNode::callbackRapidMap, this); m_RapidMapSubscriber = nh->subscribe(
m_OctomapSubscriber = nh->subscribe("/projected_map", 1, &MapManagerNode::callbackOctomapMap, this); "/rapid_mapping/map", 1, &MapManagerNode::callbackRapidMap, this);
m_SLAMMapSubscriber = nh->subscribe("/homer_mapping/slam_map", 1, &MapManagerNode::callbackSLAMMap, this); m_OctomapSubscriber = nh->subscribe(
m_SaveMapSubscriber = nh->subscribe("/map_manager/save_map", 1, &MapManagerNode::callbackSaveMap, this); "/projected_map", 1, &MapManagerNode::callbackOctomapMap, this);
m_LoadMapSubscriber = nh->subscribe("/map_manager/load_map", 1, &MapManagerNode::callbackLoadMap, this); m_SLAMMapSubscriber = nh->subscribe("/homer_mapping/slam_map", 1,
m_MapVisibilitySubscriber = nh->subscribe("/map_manager/toggle_map_visibility", 1, &MapManagerNode::callbackMapVisibility, this); &MapManagerNode::callbackSLAMMap, this);
m_LaserScanSubscriber = nh->subscribe("/scan", 1, &MapManagerNode::callbackLaserScan, this); m_SaveMapSubscriber = nh->subscribe("/map_manager/save_map", 1,
m_BackLaserScanSubscriber = nh->subscribe("/back_scan", 1, &MapManagerNode::callbackBackLaser, this); &MapManagerNode::callbackSaveMap, this);
m_FrontLaserScanSubscriber = nh->subscribe("/front_scan", 1, &MapManagerNode::callbackFrontLaser, this); m_LoadMapSubscriber = nh->subscribe("/map_manager/load_map", 1,
m_PoseSubscriber = nh->subscribe("/pose", 1, &MapManagerNode::poseCallback, this); &MapManagerNode::callbackLoadMap, this);
m_MapVisibilitySubscriber =
//subscriptions of PoiManager nh->subscribe("/map_manager/toggle_map_visibility", 1,
m_AddPOISubscriber = nh->subscribe("/map_manager/add_POI", 20, &MapManagerNode::callbackAddPOI, this); &MapManagerNode::callbackMapVisibility, this);
m_ModifyPOISubscriber = nh->subscribe("/map_manager/modify_POI", 100, &MapManagerNode::callbackModifyPOI, this); m_LaserScanSubscriber =
m_DeletePOISubscriber = nh->subscribe("/map_manager/delete_POI", 100, &MapManagerNode::callbackDeletePOI, this); nh->subscribe("/scan", 1, &MapManagerNode::callbackLaserScan, this);
m_GetPOIsService = nh->advertiseService("/map_manager/get_pois", &MapManagerNode::getPOIsService, this); m_BackLaserScanSubscriber = nh->subscribe(
"/back_scan", 1, &MapManagerNode::callbackBackLaser, this);
// Services for Map Handling m_FrontLaserScanSubscriber = nh->subscribe(
m_SaveMapService = nh->advertiseService("/map_manager/save_map", &MapManagerNode::saveMapService, this); "/front_scan", 1, &MapManagerNode::callbackFrontLaser, this);
m_ResetMapService = nh->advertiseService("/map_manager/reset_map", &MapManagerNode::resetMapService, this); m_PoseSubscriber =
m_LoadMapService = nh->advertiseService("/map_manager/load_map", &MapManagerNode::loadMapService, this); nh->subscribe("/pose", 1, &MapManagerNode::poseCallback, this);
//subscriptions of RoiManager // subscriptions of PoiManager
m_AddROISubscriber = nh->subscribe("/map_manager/add_ROI", 20, &MapManagerNode::callbackAddROI, this); m_AddPOISubscriber = nh->subscribe("/map_manager/add_POI", 20,
m_ModifyROISubscriber = nh->subscribe("/map_manager/modify_ROI", 100, &MapManagerNode::callbackModifyROI, this); &MapManagerNode::callbackAddPOI, this);
m_DeleteROIByNameSubscriber = nh->subscribe("/map_manager/delete_ROI_by_name", 100, &MapManagerNode::callbackDeleteROIbyName, this); m_ModifyPOISubscriber =
m_DeleteROIByIDSubscriber = nh->subscribe("/map_manager/delete_ROI_by_id", 100, &MapManagerNode::callbackDeleteROIbyID, this); nh->subscribe("/map_manager/modify_POI", 100,
m_GetROIsService = nh->advertiseService("/map_manager/get_rois", &MapManagerNode::getROIsService, this); &MapManagerNode::callbackModifyPOI, this);
m_GetROINameService = nh->advertiseService("/map_manager/get_roi_name", &MapManagerNode::getROINameService, this); m_DeletePOISubscriber =
m_PoiInsideROISService = nh->advertiseService("/map_manager/point_inside_rois", &MapManagerNode::pointInsideRoisService, this); nh->subscribe("/map_manager/delete_POI", 100,
if(m_roi_polling) &MapManagerNode::callbackDeletePOI, this);
{ m_GetPOIsService = nh->advertiseService(
m_RoiPollPublisher = nh->advertise<homer_mapnav_msgs::RoiChange>("/map_manager/roi_change", 1); "/map_manager/get_pois", &MapManagerNode::getPOIsService, this);
}
// Services for Map Handling
//subscribtions of MaskingMapModule m_SaveMapService = nh->advertiseService(
m_ModifyMapSubscriber = nh->subscribe("/map_manager/modify_map", 1, &MapManagerNode::callbackModifyMap, this); "/map_manager/save_map", &MapManagerNode::saveMapService, this);
m_ResetMapsSubscriber = nh->subscribe("/map_manager/reset_maps", 1, &MapManagerNode::callbackResetMaps, this); m_ResetMapService = nh->advertiseService(
"/map_manager/reset_map", &MapManagerNode::resetMapService, this);
//loaded map publisher m_LoadMapService = nh->advertiseService(
m_LoadedMapPublisher = nh->advertise<nav_msgs::OccupancyGrid>("/map_manager/loaded_map", 1); "/map_manager/load_map", &MapManagerNode::loadMapService, this);
//mask slam publisher // subscriptions of RoiManager
m_MaskSlamMapPublisher = nh->advertise<nav_msgs::OccupancyGrid>("/map_manager/mask_slam", 1); m_AddROISubscriber = nh->subscribe("/map_manager/add_ROI", 20,
&MapManagerNode::callbackAddROI, this);
m_ModifyROISubscriber =
//load map file from config if present 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; std::string mapfile;
ros::param::get("/map_manager/default_mapfile", mapfile); ros::param::get("/map_manager/default_mapfile", mapfile);
if(mapfile != "") if (mapfile != "") {
{ std_msgs::String::Ptr mapfileMsg(new std_msgs::String);
std_msgs::String::Ptr mapfileMsg(new std_msgs::String); mapfileMsg->data = mapfile;
mapfileMsg->data= mapfile; callbackLoadMap(mapfileMsg);
callbackLoadMap ( mapfileMsg );
} }
m_POIManager->broadcastPoiList(); m_POIManager->broadcastPoiList();
m_ROIManager->broadcastRoiList(); m_ROIManager->broadcastRoiList();
} }
MapManagerNode::~MapManagerNode() MapManagerNode::~MapManagerNode() {
{ if (m_MapManager) delete m_MapManager;
if(m_MapManager) delete m_MapManager; if (m_POIManager) delete m_POIManager;
if(m_POIManager) delete m_POIManager; if (m_ROIManager) delete m_ROIManager;
if(m_ROIManager) delete m_ROIManager; if (m_MaskingManager) delete m_MaskingManager;
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_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) void MapManagerNode::callbackOctomapMap(
{ const nav_msgs::OccupancyGrid::ConstPtr& msg) {
m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::RAPID_MAP, 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) void MapManagerNode::callbackSaveMap(const std_msgs::String::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)
{
MapGenerator map_saver(msg->data); MapGenerator map_saver(msg->data);
nav_msgs::OccupancyGrid::ConstPtr SLAMMap = m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER); nav_msgs::OccupancyGrid::ConstPtr SLAMMap =
nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER); m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER);
map_saver.save(SLAMMap, maskingMap, m_POIManager->getList(), m_ROIManager->getList() ); 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(); m_MapManager->clearMapLayers();
ROS_INFO_STREAM( "Loading map from file " << msg->data); ROS_INFO_STREAM("Loading map from file " << msg->data);
bool success; bool success;
MapServer map_loader(msg->data, success); MapServer map_loader(msg->data, success);
if(success) if (success) {
{ ros::Rate poll_rate(10);
ros::Rate poll_rate(10); while (m_LoadedMapPublisher.getNumSubscribers() == 0) {
while(m_LoadedMapPublisher.getNumSubscribers() == 0) poll_rate.sleep();
{ }
poll_rate.sleep(); m_LoadedMapPublisher.publish(map_loader.getSLAMMap());
} m_MapManager->updateMapLayer(
m_LoadedMapPublisher.publish(map_loader.getSLAMMap()); homer_mapnav_msgs::MapLayers::SLAM_LAYER,
m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER, boost::make_shared<nav_msgs::OccupancyGrid>(map_loader.getSLAMMap())); boost::make_shared<nav_msgs::OccupancyGrid>(
nav_msgs::OccupancyGrid::ConstPtr maskingMap = boost::make_shared<nav_msgs::OccupancyGrid>(map_loader.getMaskingMap()); map_loader.getSLAMMap()));
m_MaskingManager->replaceMap(map_loader.getMaskingMap()); nav_msgs::OccupancyGrid::ConstPtr maskingMap =
if(maskingMap->data.size() != 0) boost::make_shared<nav_msgs::OccupancyGrid>(
{ map_loader.getMaskingMap());
m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap); m_MaskingManager->replaceMap(map_loader.getMaskingMap());
} if (maskingMap->data.size() != 0) {
m_POIManager->replacePOIList(map_loader.getPois()); m_MapManager->updateMapLayer(
m_POIManager->broadcastPoiList(); homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
m_ROIManager->replaceROIList(map_loader.getRois()); }
m_ROIManager->broadcastRoiList(); m_POIManager->replacePOIList(map_loader.getPois());
} m_POIManager->broadcastPoiList();
else m_ROIManager->replaceROIList(map_loader.getRois());
{ m_ROIManager->broadcastRoiList();
ROS_ERROR_STREAM("[MapManager] Could not open mapfile!!"); } else {
ROS_ERROR_STREAM("[MapManager] Could not open mapfile!!");
} }
} }
void MapManagerNode::callbackAddPOI(const homer_mapnav_msgs::PointOfInterest::ConstPtr &msg) void MapManagerNode::callbackAddPOI(
{ const homer_mapnav_msgs::PointOfInterest::ConstPtr& msg) {
ROS_INFO_STREAM("callbackAddPOI"); ROS_INFO_STREAM("callbackAddPOI");
m_POIManager->addPointOfInterest(msg); 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); 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); 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); 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); m_ROIManager->modifyRegionOfInterest(msg);
} }
void MapManagerNode::callbackDeleteROIbyName(const std_msgs::String::ConstPtr &msg) void MapManagerNode::callbackDeleteROIbyName(
{ const std_msgs::String::ConstPtr& msg) {
ROS_INFO_STREAM("Recieved /map_manager/delete_ROI_by_name"); ROS_INFO_STREAM("Recieved /map_manager/delete_ROI_by_name");
if(m_ROIManager->deleteRegionOfInterest(msg->data)) if (m_ROIManager->deleteRegionOfInterest(msg->data)) {
{ ROS_INFO_STREAM("ROI deleted.");
ROS_INFO_STREAM("ROI deleted."); } else {
} ROS_WARN_STREAM("Could not delete ROI.");
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); 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); m_MapManager->toggleMapVisibility(msg->layer, msg->state);
} }
void MapManagerNode::callbackModifyMap(const homer_mapnav_msgs::ModifyMap::ConstPtr &msg) void MapManagerNode::callbackModifyMap(
{ const homer_mapnav_msgs::ModifyMap::ConstPtr& msg) {
nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MaskingManager->modifyMap(msg); nav_msgs::OccupancyGrid::ConstPtr maskingMap =
if(msg->mapLayer == homer_mapnav_msgs::MapLayers::SLAM_LAYER || msg->maskAction == homer_mapnav_msgs::ModifyMap::HIGH_SENSITIV) 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_MaskSlamMapPublisher.publish(maskingMap);
m_highSensitiveMap = maskingMap; m_highSensitiveMap = maskingMap;
} } else {
else m_MapManager->updateMapLayer(
{ homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
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(); 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); 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); m_MapManager->updateLaser(homer_mapnav_msgs::MapLayers::HOKUYO_BACK, msg);
} }
void MapManagerNode::callbackFrontLaser(const sensor_msgs::LaserScan::ConstPtr &msg) void MapManagerNode::callbackFrontLaser(
{ const sensor_msgs::LaserScan::ConstPtr& msg) {
m_MapManager->updateLaser(homer_mapnav_msgs::MapLayers::HOKUYO_FRONT, msg); m_MapManager->updateLaser(homer_mapnav_msgs::MapLayers::HOKUYO_FRONT, msg);
} }
bool MapManagerNode::getPOIsService(homer_mapnav_msgs::GetPointsOfInterest::Request& req, bool MapManagerNode::getPOIsService(
homer_mapnav_msgs::GetPointsOfInterest::Response& res) homer_mapnav_msgs::GetPointsOfInterest::Request& req,
{ homer_mapnav_msgs::GetPointsOfInterest::Response& res) {
res.poi_list.pois = m_POIManager->getList(); res.poi_list.pois = m_POIManager->getList();
return true; return true;
} }
bool MapManagerNode::getROIsService(homer_mapnav_msgs::GetRegionsOfInterest::Request& req, bool MapManagerNode::getROIsService(
homer_mapnav_msgs::GetRegionsOfInterest::Response& res) homer_mapnav_msgs::GetRegionsOfInterest::Request& req,
{ homer_mapnav_msgs::GetRegionsOfInterest::Response& res) {
res.roi_list.rois = m_ROIManager->getList(); res.roi_list.rois = m_ROIManager->getList();
return true; return true;
} }
bool MapManagerNode::pointInsideRoisService(homer_mapnav_msgs::PointInsideRois::Request& req, bool MapManagerNode::pointInsideRoisService(
homer_mapnav_msgs::PointInsideRois::Response& res) homer_mapnav_msgs::PointInsideRois::Request& req,
{ homer_mapnav_msgs::PointInsideRois::Response& res) {
res.rois = m_ROIManager->pointInsideRegionOfInterest(req.point); res.rois = m_ROIManager->pointInsideRegionOfInterest(req.point);
return true; return true;
} }
bool MapManagerNode::getROINameService(homer_mapnav_msgs::GetRegionOfInterestName::Request& req, bool MapManagerNode::getROINameService(
homer_mapnav_msgs::GetRegionOfInterestName::Response& res) homer_mapnav_msgs::GetRegionOfInterestName::Request& req,
{ homer_mapnav_msgs::GetRegionOfInterestName::Response& res) {
res.name = m_ROIManager->getROIName(req.roi_id); res.name = m_ROIManager->getROIName(req.roi_id);
return true; return true;
} }
void MapManagerNode::poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) void MapManagerNode::poseCallback(
{ const geometry_msgs::PoseStamped::ConstPtr& msg) {
m_MapManager->updatePose(msg); m_MapManager->updatePose(msg);
if(msg->header.stamp - m_lastROIPoll > ros::Duration(m_roi_polling_time) && m_roi_polling) if (msg->header.stamp - m_lastROIPoll > ros::Duration(m_roi_polling_time) &&
{ m_roi_polling) {
m_lastROIPoll = msg->header.stamp; m_lastROIPoll = msg->header.stamp;
geometry_msgs::PointStamped posePoint; geometry_msgs::PointStamped posePoint;
posePoint.header.frame_id="/map"; posePoint.header.frame_id = "/map";
posePoint.header.stamp = msg->header.stamp; posePoint.header.stamp = msg->header.stamp;
posePoint.point = msg->pose.position; posePoint.point = msg->pose.position;
std::vector<homer_mapnav_msgs::RegionOfInterest> rois; std::vector<homer_mapnav_msgs::RegionOfInterest> rois;
rois = m_ROIManager->pointInsideRegionOfInterest(posePoint); rois = m_ROIManager->pointInsideRegionOfInterest(posePoint);
bool found = false; bool found = false;
for(int i = 0; i < m_ids.size(); i++) for (int i = 0; i < m_ids.size(); i++) {
{ found = false;
found = false; for (int j = 0; j < rois.size(); j++) {
for(int j = 0; j < rois.size(); j++) if (m_ids[i] == rois[j].id) {
{ rois.erase(rois.begin() + j);
if(m_ids[i] == rois[j].id) found = true;
{ break;
rois.erase (rois.begin()+j); }
found = true; }
break; if (!found) {
} homer_mapnav_msgs::RoiChange change;
} change.id = m_ids[i];
if(!found) change.name = m_ROIManager->getROIName(m_ids[i]);
{ change.action = false;
homer_mapnav_msgs::RoiChange change; m_RoiPollPublisher.publish(change);
change.id = m_ids[i]; m_ids.erase(m_ids.begin() + i);
change.name = m_ROIManager->getROIName(m_ids[i]); i--;
change.action = false; }
m_RoiPollPublisher.publish(change); }
m_ids.erase(m_ids.begin()+i); for (int i = 0; i < rois.size(); i++) {
i--; homer_mapnav_msgs::RoiChange change;
} change.id = rois[i].id;
} change.name = m_ROIManager->getROIName(change.id);
for(int i = 0; i < rois.size(); i++) change.action = true;
{ m_RoiPollPublisher.publish(change);
homer_mapnav_msgs::RoiChange change; m_ids.push_back(rois[i].id);
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, bool MapManagerNode::saveMapService(homer_mapnav_msgs::SaveMap::Request& req,
homer_mapnav_msgs::SaveMap::Response& res) homer_mapnav_msgs::SaveMap::Response& res) {
{ // ROS_INFO_STREAM("Saving map "<<req->folder);
//ROS_INFO_STREAM("Saving map "<<req->folder);
MapGenerator map_saver(std::string(req.folder.data)); 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 SLAMMap =
nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER); m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER);
map_saver.save(SLAMMap, maskingMap, m_POIManager->getList(), m_ROIManager->getList() ); 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, bool MapManagerNode::loadMapService(homer_mapnav_msgs::LoadMap::Request& req,
homer_mapnav_msgs::LoadMap::Response& res) homer_mapnav_msgs::LoadMap::Response& res) {
{ // load map file from config if present
//load map file from config if present
std::string mapfile = std::string(req.filename.data); std::string mapfile = std::string(req.filename.data);
if(mapfile != "") if (mapfile != "") {
{ ROS_INFO_STREAM("Loading map with filename: " << mapfile);
ROS_INFO_STREAM("Loading map with filename: "<<mapfile); std_msgs::String::Ptr mapfileMsg(new std_msgs::String);
std_msgs::String::Ptr mapfileMsg(new std_msgs::String); mapfileMsg->data = mapfile;
mapfileMsg->data= mapfile; callbackLoadMap(mapfileMsg);
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, bool MapManagerNode::resetMapService(std_srvs::Empty::Request& req,
std_srvs::Empty::Response& res) std_srvs::Empty::Response& res) {
{ ROS_INFO_STREAM("Resetting current map");
ROS_INFO_STREAM("Resetting current map");
nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MaskingManager->resetMap(); 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::init(argc, argv, "map_manager");
ros::NodeHandle nh; ros::NodeHandle nh;
/* char* pprofile = std::getenv("MAPMANAGER_PROFILE"); /* char* pprofile = std::getenv("MAPMANAGER_PROFILE");
if (pprofile) if (pprofile)
{ {
ProfilerStart(pprofile); ProfilerStart(pprofile);
} }
*/ */
MapManagerNode node(&nh); MapManagerNode node(&nh);
ros::Rate loop_rate(10); ros::Rate loop_rate(10);
while (ros::ok()) while (ros::ok()) {
{ try {
try
{
ros::spinOnce(); ros::spinOnce();
loop_rate.sleep(); 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; return 0;
} }
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment