Skip to content
Snippets Groups Projects
Commit 320b71d6 authored by Projekt Robbie's avatar Projekt Robbie
Browse files

map manager crash

parent 5b8a8165
No related branches found
No related tags found
No related merge requests found
...@@ -65,8 +65,23 @@ nav_msgs::OccupancyGrid::ConstPtr MaskingManager::modifyMap( ...@@ -65,8 +65,23 @@ nav_msgs::OccupancyGrid::ConstPtr MaskingManager::modifyMap(
} }
nav_msgs::OccupancyGrid::ConstPtr MaskingManager::resetMap() { nav_msgs::OccupancyGrid::ConstPtr MaskingManager::resetMap() {
m_MaskingMap.info.width = 1;
m_MaskingMap.info.height = 1;
m_MaskingMap.info.resolution = 1;
m_MaskingMap.info.origin.position.x = 0;
m_MaskingMap.info.origin.position.y = 0;
m_MaskingMap.data.resize(m_MaskingMap.info.width *
m_MaskingMap.info.height);
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);
m_SlamMap.info = m_MaskingMap.info;
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);
nav_msgs::OccupancyGrid::ConstPtr ret = nav_msgs::OccupancyGrid::ConstPtr ret =
boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_MaskingMap); boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_MaskingMap);
return ret; return ret;
......
...@@ -4,10 +4,8 @@ ...@@ -4,10 +4,8 @@
MapManagerNode::MapManagerNode(ros::NodeHandle* nh) { MapManagerNode::MapManagerNode(ros::NodeHandle* nh) {
m_LastLaserTime = ros::Time::now(); m_LastLaserTime = ros::Time::now();
int mapSize; int mapSize = 1;
float resolution; float resolution = 1;
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_updates", m_roi_polling, (bool)false);
ros::param::param("/map_manager/roi_polling_time", m_roi_polling_time, ros::param::param("/map_manager/roi_polling_time", m_roi_polling_time,
(float)0.5); (float)0.5);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment