diff --git a/homer_map_manager/CMakeLists.txt b/homer_map_manager/CMakeLists.txt index c659d0ab71d24a20af827740223eeada98e16713..ce519e8fbfeeab7b3b28762b32c0a102624597fc 100644 --- a/homer_map_manager/CMakeLists.txt +++ b/homer_map_manager/CMakeLists.txt @@ -5,6 +5,8 @@ find_package(catkin REQUIRED COMPONENTS roscpp roslib tf homer_mapnav_msgs homer find_package( Eigen3 REQUIRED ) +set(CMAKE_BUILD_TYPE Release) + find_package(OpenMP) if (OPENMP_FOUND) set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") diff --git a/homer_map_manager/include/homer_map_manager/Managers/MapManager.h b/homer_map_manager/include/homer_map_manager/Managers/MapManager.h index 3d82a8820a0a295e521b83ac9bf8c96deeb0606f..d5a74ce4be03bccc050bb037035a18b37e859814 100644 --- a/homer_map_manager/include/homer_map_manager/Managers/MapManager.h +++ b/homer_map_manager/include/homer_map_manager/Managers/MapManager.h @@ -60,12 +60,6 @@ class MapManager * @brief clearMapLayers Clear all map layers */ void clearMapLayers(); - - /** getters */ - double getHeight() { return m_Height; } - double getWidth() { return m_Width; } - double getResolution() { return m_Resolution; } - geometry_msgs::Pose getOrigin() { return m_Origin; } void updateLaser(int layer, const sensor_msgs::LaserScan::ConstPtr& msg ); @@ -94,11 +88,7 @@ class MapManager std::map<int, bool> m_MapVisibility; //sizes of the last slam map - double m_Height; - double m_Width; - double m_Resolution; bool m_got_transform; - geometry_msgs::Pose m_Origin; geometry_msgs::PoseStamped::ConstPtr m_pose; tf::StampedTransform m_sick_transform; tf::StampedTransform m_hokuyo_transform; 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 a372cf639d0c18d0a1125f750008f9521a0fae73..a7b3c7c98afa23f4b9271d81e5e55277d14d9991 100644 --- a/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h +++ b/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h @@ -3,28 +3,32 @@ #include <ros/ros.h> -#include <nav_msgs/OccupancyGrid.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(int mapSize, float resolution); + MaskingManager(nav_msgs::MapMetaData mapInfo); /** @brief The destructor. */ virtual ~MaskingManager(); - /** 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); + nav_msgs::OccupancyGrid::ConstPtr 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); /** resets the masking map layer */ nav_msgs::OccupancyGrid::ConstPtr resetMap(); @@ -32,22 +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; - /** sizes of the masking map layer */ - int m_Width, m_Height; - float m_CellSize; - /** 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/MapManager.cpp b/homer_map_manager/src/Managers/MapManager.cpp index b6bbf506e1fbe29e74a707bd0b10e05269e4ac41..6368ff308a455808f137071db5a2c37935cc1f99 100644 --- a/homer_map_manager/src/Managers/MapManager.cpp +++ b/homer_map_manager/src/Managers/MapManager.cpp @@ -23,10 +23,6 @@ MapManager::MapManager(ros::NodeHandle* nh) { m_MapVisibility[m_laser_layers[i]] = true; } - - m_Height = -1; - m_Width = -1; - m_Resolution = -1; try { @@ -57,13 +53,8 @@ MapManager::~MapManager() void MapManager::updateMapLayer(int type, nav_msgs::OccupancyGrid::ConstPtr layer) { m_MapLayers[type] = layer; - //if slam map update map sizes if(type == homer_mapnav_msgs::MapLayers::SLAM_LAYER) { - m_Height = layer->info.height; - m_Width = layer->info.width; - m_Resolution = layer->info.resolution; - m_Origin = layer->info.origin; sendMergedMap(); } } diff --git a/homer_map_manager/src/Managers/MaskingManager.cpp b/homer_map_manager/src/Managers/MaskingManager.cpp index 1b3af14db71a07f09e56a1b2449f32e0540bf32b..2ee1dd73257851f42fad2d8cd7a7b211dc0e4c7f 100644 --- a/homer_map_manager/src/Managers/MaskingManager.cpp +++ b/homer_map_manager/src/Managers/MaskingManager.cpp @@ -2,184 +2,184 @@ using namespace std; -MaskingManager::MaskingManager(int mapSize, float resolution) -{ - m_CellSize = resolution; - m_Width = mapSize / m_CellSize + 1; - m_Height = mapSize / m_CellSize + 1; - ROS_INFO_STREAM( "Creating " << m_Width << " x " << m_Height << " map." ); - m_MaskingMap.info.resolution = m_CellSize; - m_MaskingMap.info.height = m_Height; - m_MaskingMap.info.width = m_Width; - m_MaskingMap.info.origin.position.x = -m_Height * resolution / 2; - m_MaskingMap.info.origin.position.y = -m_Width * resolution / 2; - m_MaskingMap.data.resize(m_Width * m_Height); - std::fill( m_MaskingMap.data.begin(), m_MaskingMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED ); - - m_SlamMap.info.resolution = m_CellSize; - m_SlamMap.info.height = m_Height; - m_SlamMap.info.width = m_Width; - m_SlamMap.info.origin.position.x = -m_Height * resolution / 2; - m_SlamMap.info.origin.position.y = -m_Width * resolution / 2; - m_SlamMap.data.resize(m_Width * m_Height); - std::fill( m_SlamMap.data.begin(), m_SlamMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED ); +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_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); } -MaskingManager::~MaskingManager() -{} +MaskingManager::~MaskingManager() {} -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::updateMapInfo(const nav_msgs::MapMetaData &mapInfo) { + if (m_SlamMap.info.width < mapInfo.width || m_SlamMap.info.height < mapInfo.height) { + m_SlamMap.info = mapInfo; + + int x_add_left = + (m_MaskingMap.info.origin.position.x - mapInfo.origin.position.x + 0.025) / + mapInfo.resolution; + int y_add_up = + (m_MaskingMap.info.origin.position.y - mapInfo.origin.position.y + 0.025) / + mapInfo.resolution; + ROS_INFO_STREAM("x add "<<x_add_left<<" y add "<<y_add_up); + + 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(), + 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; + } + return boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_MaskingMap); +} + +nav_msgs::OccupancyGrid::ConstPtr MaskingManager::modifyMap( + homer_mapnav_msgs::ModifyMap::ConstPtr msg) { + // reset SLAM mask map before each masking + m_SlamMap.data.resize(m_SlamMap.info.height * m_SlamMap.info.width); + 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 - std::vector<int> data(m_Width * m_Height); - for ( int i = 0; i < data.size(); i++ ) - { - data[i] = 0; +void MaskingManager::drawPolygon(vector<geometry_msgs::Point> vertices, + int value, int mapLayer) { + if (vertices.size() == 0) { + ROS_INFO_STREAM("No vertices given!"); + return; } - - //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; + // 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; } } } } -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_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_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 59bca4741e4c1462c7f7b7592a12736a7c424924..185dce9f41d63066ad349dc532d4319fbf3cabc1 100644 --- a/homer_map_manager/src/map_manager_node.cpp +++ b/homer_map_manager/src/map_manager_node.cpp @@ -1,419 +1,457 @@ #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); m_ROIManager = new RoiManager(nh); - m_MaskingManager = new MaskingManager(mapSize, resolution); - - //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 + + nav_msgs::MapMetaData mapInfo; + mapInfo.width = mapSize / resolution; + 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; + + 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 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) { + nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MaskingManager->updateMapInfo(msg->info); + m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap); m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER, msg); } -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; } - diff --git a/homer_mapping/config/homer_mapping.yaml b/homer_mapping/config/homer_mapping.yaml index a7e2bf69bc4aef3b76f4219b5d5069596db6195f..cf75bf3086ec610299d00aca3ac5e921ce1a23db 100644 --- a/homer_mapping/config/homer_mapping.yaml +++ b/homer_mapping/config/homer_mapping.yaml @@ -1,9 +1,8 @@ -/homer_mapping/size: 40 # size of one edge of the map in m. map is quadratic -/homer_mapping/resolution: 0.05 # m meter per cell -/homer_mapping/max_laser: 20.0 # m max range for including range into map +/homer_mapping/size: 4 # size of one edge of the map in m. map is quadratic +/homer_mapping/resolution: 0.04 # m meter per cell #map config values -/homer_mapping/backside_checking: false # Enable checking to avoid matching front- and backside of obstacles, e.g. walls. Useful when creating high resolution maps +/homer_mapping/backside_checking: true # Enable checking to avoid matching front- and backside of obstacles, e.g. walls. Useful when creating high resolution maps /homer_mapping/obstacle_borders: true # Leaves a small border around obstacles unchanged when inserting a laser scan. Improves stability of generated map /homer_mapping/measure_sampling_step: 0.1 # m Minimum distance in m between two samples for probability calculation @@ -11,20 +10,20 @@ /particlefilter/error_values/rotation_error_rotating: 10.0 # percent /particlefilter/error_values/rotation_error_translating: 2.0 # degrees per meter -/particlefilter/error_values/translation_error_translating: 10.0 # percent -/particlefilter/error_values/translation_error_rotating: 0.05 # m per degree -/particlefilter/error_values/move_jitter_while_turning: 0.1 # m per degree +/particlefilter/error_values/translation_error_translating: 1.0 # percent +/particlefilter/error_values/translation_error_rotating: 0.09 # m per degree +/particlefilter/error_values/move_jitter_while_turning: 0.05 # m per degree /particlefilter/hyper_slamfilter/particlefilter_num: 1 /particlefilter/particle_num: 1000 -/particlefilter/max_rotation_per_second: 0.4 # maximal rotation in radiants if mapping is performed. if rotation is bigger, mapping is interrupted +/particlefilter/max_rotation_per_second: 0.1 # maximal rotation in radiants if mapping is performed. if rotation is bigger, mapping is interrupted /particlefilter/wait_time: 0.05 # minimum time to wait between two slam steps in seconds #the map is only updated when the robot has turned a minimal angle, has moved a minimal distance or a maximal time has passed /particlefilter/update_min_move_angle: 2 # degrees /particlefilter/update_min_move_dist: 0.05 # m -/particlefilter/max_update_interval: 0.5 # sec +/particlefilter/max_update_interval: 0.2 # sec /selflocalization/scatter_var_xy: 0.1 # m /selflocalization/scatter_var_theta: 0.2 # radiants diff --git a/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h b/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h index 0235e8604a89f3ea651bd3a1815eb393e0213cc3..63f51b978d6c4d74dab3ad59095d7aeb6b20acd6 100644 --- a/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h +++ b/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h @@ -13,6 +13,7 @@ #include <homer_nav_libs/Math/Box2D.h> #include <nav_msgs/OccupancyGrid.h> +#include <nav_msgs/MapMetaData.h> #include <tf/transform_listener.h> #include <sensor_msgs/LaserScan.h> @@ -91,7 +92,7 @@ class OccupancyMap { /** * Constructor for a loaded map. */ - OccupancyMap(float*& occupancyProbability, geometry_msgs::Pose origin, float resolution, int pixelSize, Box2D<int> exploredRegion); + OccupancyMap(float*& occupancyProbability, geometry_msgs::Pose origin, float resolution, int width, int height, Box2D<int> exploredRegion); /** * Copy constructor, copies all members inclusive the arrays that lie behind the pointers. @@ -170,7 +171,7 @@ class OccupancyMap { float computeScore( Pose robotPose, std::vector<MeasurePoint> measurePoints ); /** - * @return QImage of size m_PixelSize, m_PixelSize with values of m_OccupancyProbability scaled to 0-254 + * @return QImage of size m_metaData.width, m_metaData.height with values of m_OccupancyProbability scaled to 0-254 */ QImage getProbabilityQImage(int trancparencyThreshold, bool showInaccessible) const; @@ -187,9 +188,9 @@ class OccupancyMap { * @param[out] data vector containing occupancy probabilities. 0 = free, 100 = occupied, -1 = NOT_KNOWN * @param[out] width Width of data array * @param[out] height Height of data array - * @param[out] resolution Resolution of the map (m_Resolution) + * @param[out] resolution Resolution of the map (m_metaData.resolution) */ - void getOccupancyProbabilityImage(vector<int8_t> &data, int& width, int& height, float &resolution); + void getOccupancyProbabilityImage(vector<int8_t> &data, nav_msgs::MapMetaData& metaData); /** * This method marks free the position of the robot (according to its dimensions). @@ -219,6 +220,8 @@ class OccupancyMap { */ void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr &msg); + void changeMapSize(int x_add_left, int y_add_up, int x_add_right, int y_add_down ); + protected: @@ -297,22 +300,14 @@ class OccupancyMap { */ void cleanUp(); - /** - * Stores the size of one map pixel in m. - */ - float m_Resolution; + /** + * Stores the metadata of the map + */ + nav_msgs::MapMetaData m_metaData; - /** - * Stores the origin of the map - */ - geometry_msgs::Pose m_Origin; - /** - * Stores the width of the map in cell numbers. - */ - int m_PixelSize; /** - * Stores the size of the map arrays, i.e. m_PixelSize * m_PixelSize + * Stores the size of the map arrays, i.e. m_metaData.width * m_metaData.height * for faster computation. */ unsigned m_ByteSize; @@ -329,9 +324,6 @@ class OccupancyMap { // Counts how often a pixel is hit by a measurement that says the pixel is occupied. unsigned short* m_OccupancyCount; - // Counts how often a cell is marked as inaccessible via markInaccessible() - unsigned char* m_InaccessibleCount; - // Used for setting flags for cells, that have been modified during the current update. unsigned char* m_CurrentChanges; @@ -341,10 +333,6 @@ class OccupancyMap { /** * Store values from config files. */ - // maximum valid range of one laser measurement - float m_LaserMaxRange; - //minimum valid range of one laser measurement - float m_LaserMinRange; //minimum range classified as free in case of errorneous laser measurement float m_FreeReadingDistance; //enables checking to avoid matching front- and backside of an obstacle, e.g. wall @@ -357,9 +345,6 @@ class OccupancyMap { //bool to reset high_sensitive Areas on the next iteration bool m_reset_high; - //position of the laser scaner in base_link frame - geometry_msgs::Point m_LaserPos; - /** * Defines a bounding box around the changes in the current map. */ diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h b/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h index a712fb390b06c70a7a311674f4ca6492fdb32a6e..89026615dcd140e77e3fb11159c6ebce03f500d0 100644 --- a/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h +++ b/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h @@ -312,6 +312,8 @@ class SlamFilter : public ParticleFilter<SlamParticle> { */ ros::Time m_LastUpdateTime; + ros::Time m_LastMoveTime; + /** * Calculates the square of given input f * @param f input diff --git a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp index 7318e38aaf7e129b1acb2d75445a503f81e4d994..515128a22715094ec3338dcd8a4bbde13989cdfb 100644 --- a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp +++ b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp @@ -37,18 +37,35 @@ const int LOADED_MEASURECOUNT = 10; OccupancyMap::OccupancyMap() { + float mapSize, resolution; + ros::param::get("/homer_mapping/size", mapSize); + ros::param::get("/homer_mapping/resolution", resolution); + + //add one safety pixel + m_metaData.resolution = resolution; + m_metaData.width = mapSize / m_metaData.resolution + 1; + m_metaData.height = mapSize / m_metaData.resolution + 1; + m_ByteSize = m_metaData.width * m_metaData.height; + + m_metaData.origin.position.x = - (m_metaData.width * m_metaData.resolution / 2.0); + m_metaData.origin.position.y = - (m_metaData.height * m_metaData.resolution / 2.0); + m_metaData.origin.orientation.w = 1.0; + m_metaData.origin.orientation.x = 0.0; + m_metaData.origin.orientation.y = 0.0; + m_metaData.origin.orientation.z = 0.0; initMembers(); } -OccupancyMap::OccupancyMap(float *&occupancyProbability, geometry_msgs::Pose origin, float resolution, int pixelSize, Box2D<int> exploredRegion) +OccupancyMap::OccupancyMap(float *&occupancyProbability, geometry_msgs::Pose origin, float resolution, int width, int height, Box2D<int> exploredRegion) { + m_metaData.origin = origin; + m_metaData.resolution = resolution; + m_metaData.width = width; + m_metaData.height = height; + m_ByteSize = m_metaData.width * m_metaData.height; initMembers(); - m_Origin = origin; - m_Resolution = resolution; - m_PixelSize = pixelSize; - m_ByteSize = pixelSize * pixelSize; m_ExploredRegion = exploredRegion; m_ChangedRegion = exploredRegion; @@ -74,10 +91,7 @@ OccupancyMap::OccupancyMap ( const OccupancyMap& occupancyMap ) m_MeasurementCount = 0; m_OccupancyCount = 0; m_CurrentChanges = 0; - m_InaccessibleCount = 0; m_HighSensitive = 0; - m_LaserMaxRange = 0; - m_LaserMinRange = 0; *this = occupancyMap; } @@ -89,21 +103,6 @@ OccupancyMap::~OccupancyMap() void OccupancyMap::initMembers() { - float mapSize; - ros::param::get("/homer_mapping/size", mapSize); - ros::param::get("/homer_mapping/resolution", m_Resolution); - ros::param::param("/homer_mapping/max_laser", m_LaserMaxRange, (float) 8.0); - - //add one safety pixel - m_PixelSize = mapSize / m_Resolution + 1; - m_ByteSize = m_PixelSize * m_PixelSize; - - m_Origin.position.x = -m_PixelSize*m_Resolution/2; - m_Origin.position.y = -m_PixelSize*m_Resolution/2; - m_Origin.orientation.w = 1.0; - m_Origin.orientation.x = 0.0; - m_Origin.orientation.y = 0.0; - m_Origin.orientation.z = 0.0; ros::param::get("/homer_mapping/backside_checking", m_BacksideChecking); ros::param::get("/homer_mapping/obstacle_borders", m_ObstacleBorders); @@ -114,7 +113,6 @@ void OccupancyMap::initMembers() m_MeasurementCount = new unsigned short[m_ByteSize]; m_OccupancyCount = new unsigned short[m_ByteSize]; m_CurrentChanges = new unsigned char[m_ByteSize]; - m_InaccessibleCount = new unsigned char[m_ByteSize]; m_HighSensitive = new unsigned short[m_ByteSize]; for ( unsigned i=0; i<m_ByteSize; i++ ) { @@ -122,11 +120,10 @@ void OccupancyMap::initMembers() m_OccupancyCount[i]=0; m_MeasurementCount[i]=0; m_CurrentChanges[i]=NO_CHANGE; - m_InaccessibleCount[i]=0; m_HighSensitive[i] = 0; } - m_ExploredRegion=Box2D<int> ( m_PixelSize/2.1, m_PixelSize/2.1, m_PixelSize/1.9, m_PixelSize/1.9 ); + m_ExploredRegion=Box2D<int> ( m_metaData.width/2.1, m_metaData.height/2.1, m_metaData.width/1.9, m_metaData.height/1.9 ); maximizeChangedRegion(); try @@ -134,8 +131,11 @@ void OccupancyMap::initMembers() bool got_transform = m_tfListener.waitForTransform("/base_link","/laser", ros::Time(0), ros::Duration(1)); while(!got_transform); { - ROS_ERROR_STREAM("need transformation from base_link to laser!"); got_transform = m_tfListener.waitForTransform("/base_link","/laser", ros::Time(0), ros::Duration(1)); + if(!got_transform) + { + ROS_ERROR_STREAM("need transformation from base_link to laser!"); + } } m_tfListener.lookupTransform("/base_link", "/laser", ros::Time(0), m_laserTransform); @@ -152,9 +152,9 @@ OccupancyMap& OccupancyMap::operator= ( const OccupancyMap & occupancyMap ) // free allocated memory cleanUp(); - m_Resolution = occupancyMap.m_Resolution; + m_metaData = occupancyMap.m_metaData; + m_ExploredRegion = occupancyMap.m_ExploredRegion; - m_PixelSize = occupancyMap.m_PixelSize; m_ByteSize = occupancyMap.m_ByteSize; ros::param::get("/homer_mapping/backside_checking", m_BacksideChecking); @@ -164,7 +164,6 @@ OccupancyMap& OccupancyMap::operator= ( const OccupancyMap & occupancyMap ) m_MeasurementCount = new unsigned short[m_ByteSize]; m_OccupancyCount = new unsigned short[m_ByteSize]; m_CurrentChanges = new unsigned char[m_ByteSize]; - m_InaccessibleCount = new unsigned char[m_ByteSize]; m_HighSensitive = new unsigned short[m_ByteSize]; // copy array data @@ -172,30 +171,93 @@ OccupancyMap& OccupancyMap::operator= ( const OccupancyMap & occupancyMap ) memcpy ( m_MeasurementCount, occupancyMap.m_MeasurementCount, m_ByteSize * sizeof ( *m_MeasurementCount ) ); memcpy ( m_OccupancyCount, occupancyMap.m_OccupancyCount, m_ByteSize * sizeof ( *m_OccupancyCount ) ); memcpy ( m_CurrentChanges, occupancyMap.m_CurrentChanges, m_ByteSize * sizeof ( *m_CurrentChanges ) ); - memcpy ( m_InaccessibleCount, occupancyMap.m_InaccessibleCount, m_ByteSize * sizeof ( *m_InaccessibleCount ) ); memcpy ( m_HighSensitive, occupancyMap.m_HighSensitive, m_ByteSize * sizeof ( *m_HighSensitive) ); return *this; } +void OccupancyMap::changeMapSize( int x_add_left, int y_add_up, int x_add_right, int y_add_down) +{ + int new_width = m_metaData.width + x_add_left + x_add_right; + int new_height = m_metaData.height + y_add_up + y_add_down; + + m_ByteSize = new_width * new_height; + // allocate all arrays + float* OccupancyProbability = new float[m_ByteSize]; + unsigned short* MeasurementCount = new unsigned short[m_ByteSize]; + unsigned short* OccupancyCount = new unsigned short[m_ByteSize]; + unsigned char* CurrentChanges = new unsigned char[m_ByteSize]; + unsigned short* HighSensitive = new unsigned short[m_ByteSize]; + + for ( unsigned i=0; i<m_ByteSize; i++ ) + { + OccupancyProbability[i]=UNKNOWN_LIKELIHOOD; + OccupancyCount[i]=0; + MeasurementCount[i]=0; + CurrentChanges[i]=NO_CHANGE; + HighSensitive[i] = 0; + } + + for(int y = 0; y < m_metaData.height; y++) + { + for(int x = 0; x < m_metaData.width; x++) + { + int i = y * m_metaData.width + x; + int in = (y + y_add_up) * new_width + (x + x_add_left); + OccupancyProbability[in] = m_OccupancyProbability[i]; + MeasurementCount[in] = m_MeasurementCount[i]; + OccupancyCount[in] = m_OccupancyCount[i]; + CurrentChanges[in] = m_CurrentChanges[i]; + HighSensitive[in] = m_HighSensitive[i]; + } + } + + m_ExploredRegion.setMinX( m_ExploredRegion.minX() + x_add_left); + m_ExploredRegion.setMaxX( m_ExploredRegion.maxX() + x_add_left); + m_ExploredRegion.setMinY( m_ExploredRegion.minY() + y_add_up); + m_ExploredRegion.setMaxY( m_ExploredRegion.maxY() + y_add_up); + + m_ChangedRegion.setMinX( m_ChangedRegion.minX() + x_add_left); + m_ChangedRegion.setMaxX( m_ChangedRegion.maxX() + x_add_left); + m_ChangedRegion.setMinY( m_ChangedRegion.minY() + y_add_up); + m_ChangedRegion.setMaxY( m_ChangedRegion.maxY() + y_add_up); + + + m_metaData.width = new_width; + m_metaData.height = new_height; + m_metaData.origin.position.x -= (x_add_left ) * m_metaData.resolution; + m_metaData.origin.position.y -= (y_add_up ) * m_metaData.resolution; + + cleanUp(); + + m_OccupancyProbability = OccupancyProbability; + m_MeasurementCount = MeasurementCount; + m_OccupancyCount = OccupancyCount; + m_CurrentChanges = CurrentChanges; + m_HighSensitive = HighSensitive; + + + +} + int OccupancyMap::width() const { - return m_PixelSize; + return m_metaData.width; } int OccupancyMap::height() const { - return m_PixelSize; + return m_metaData.height; } float OccupancyMap::getOccupancyProbability ( Eigen::Vector2i p ) { - unsigned offset = m_PixelSize * p.y() + p.x(); - if ( offset > unsigned ( m_ByteSize ) ) + if(p.y() >= m_metaData.height || p.x() >= m_metaData.width) { - return UNKNOWN_LIKELIHOOD; + return UNKNOWN_LIKELIHOOD; } + unsigned offset = m_metaData.width * p.y() + p.x(); return m_OccupancyProbability[ offset ]; } @@ -209,12 +271,22 @@ void OccupancyMap::computeOccupancyProbabilities() { for ( int y = m_ChangedRegion.minY(); y <= m_ChangedRegion.maxY(); y++ ) { - int yOffset = m_PixelSize * y; + int yOffset = m_metaData.width * y; for ( int x = m_ChangedRegion.minX(); x <= m_ChangedRegion.maxX(); x++ ) { int i = x + yOffset; if ( m_MeasurementCount[i] > 0 ) { + //int maxCount = 100; //TODO param + //if(m_MeasurementCount[i] > maxCount * 2 -1) + //{ + //int scalingFactor = m_MeasurementCount[i] / maxCount; + //if ( scalingFactor != 0 ) + //{ + //m_MeasurementCount[i] /= scalingFactor; + //m_OccupancyCount[i] /= scalingFactor; + //} + //} m_OccupancyProbability[i] = m_OccupancyCount[i] / static_cast<float> ( m_MeasurementCount[i] ); if (m_HighSensitive[i] == 1) { @@ -250,14 +322,6 @@ void OccupancyMap::insertLaserData ( sensor_msgs::LaserScan::ConstPtr laserData, { m_latestMapTransform = transform; markRobotPositionFree(); - ros::Time stamp = laserData->header.stamp; - - //m_LaserMaxRange = laserData->range_max; - m_LaserMinRange = laserData->range_min; - - - m_LaserPos.x = m_laserTransform.getOrigin().getX(); - m_LaserPos.y = m_laserTransform.getOrigin().getY(); std::vector<RangeMeasurement> ranges; ranges.reserve ( laserData->ranges.size() ); @@ -267,11 +331,12 @@ void OccupancyMap::insertLaserData ( sensor_msgs::LaserScan::ConstPtr laserData, float lastValidRange=m_FreeReadingDistance; RangeMeasurement rangeMeasurement; - rangeMeasurement.sensorPos = m_LaserPos; + rangeMeasurement.sensorPos.x = m_laserTransform.getOrigin().getX(); + rangeMeasurement.sensorPos.y = m_laserTransform.getOrigin().getY(); for ( unsigned int i = 0; i < laserData->ranges.size(); i++ ) { - if ( ( laserData->ranges[i] >= m_LaserMinRange ) && ( laserData->ranges[i] <= m_LaserMaxRange ) ) + if ( ( laserData->ranges[i] >= laserData->range_min ) && ( laserData->ranges[i] <= laserData->range_max ) ) { //if we're at the end of an errorneous segment, interpolate //between last valid point and current point @@ -279,56 +344,43 @@ void OccupancyMap::insertLaserData ( sensor_msgs::LaserScan::ConstPtr laserData, { //smaller of the two ranges belonging to end points float range = Math::min ( lastValidRange, laserData->ranges[i] ); - range -= m_Resolution * 2; - + range -= m_metaData.resolution * 2; if ( range < m_FreeReadingDistance ) { range = m_FreeReadingDistance; } else - if ( range > m_LaserMaxRange ) + if ( range > laserData->range_max ) { - range = m_LaserMaxRange; + range = laserData->range_max; } - //choose smaller range for ( unsigned j=lastValidIndex+1; j<i; j++ ) { float alpha = laserData->angle_min + j * laserData->angle_increment; - geometry_msgs::Point point; tf::Vector3 pin; tf::Vector3 pout; pin.setX( cos(alpha) * range); pin.setY( sin(alpha) * range); pin.setZ(0); - pout = m_laserTransform * pin; - - point.x = pout.x(); - point.y = pout.y(); - - rangeMeasurement.endPos = point; + rangeMeasurement.endPos.x = pout.x(); + rangeMeasurement.endPos.y = pout.y(); rangeMeasurement.free = true; ranges.push_back ( rangeMeasurement ); } } float alpha = laserData->angle_min + i * laserData->angle_increment; - geometry_msgs::Point point; tf::Vector3 pin; tf::Vector3 pout; pin.setX( cos(alpha) * laserData->ranges[i]); pin.setY( sin(alpha) * laserData->ranges[i]); pin.setZ(0); - pout = m_laserTransform * pin; - - point.x = pout.x(); - point.y = pout.y(); - - rangeMeasurement.endPos = point; + rangeMeasurement.endPos.x = pout.x(); + rangeMeasurement.endPos.y = pout.y(); rangeMeasurement.free = false; ranges.push_back ( rangeMeasurement ); - errorFound=false; lastValidIndex=i; lastValidRange=laserData->ranges[i]; @@ -339,113 +391,145 @@ void OccupancyMap::insertLaserData ( sensor_msgs::LaserScan::ConstPtr laserData, } } -// if ( errorFound ) -// { -// for ( unsigned j=lastValidIndex+1; j<laserData->ranges.size(); j++ ) -// { -// rangeMeasurement.endPos = map_tools::laser_range_to_point(m_FreeReadingDistance, j, laserData->angle_min, laserData->angle_increment, m_tfListener, laserData->header.frame_id, "/base_link"); // - -// rangeMeasurement.free = true; -// ranges.push_back ( rangeMeasurement ); -// } -// } - insertRanges ( ranges , laserData->header.stamp); + } void OccupancyMap::insertRanges ( vector<RangeMeasurement> ranges, ros::Time stamp ) { + if(ranges.size() < 1) + { + return; + } clearChanges(); Eigen::Vector2i lastEndPixel; + int need_x_left = 0; + int need_x_right = 0; + int need_y_up = 0; + int need_y_down = 0; - //paint safety borders - if ( m_ObstacleBorders ) - { - for ( unsigned i=0; i<ranges.size(); i++ ) - { - geometry_msgs::Point endPosWorld = map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform); - Eigen::Vector2i endPixel = map_tools::toMapCoords(endPosWorld, m_Origin, m_Resolution); + std::vector<Eigen::Vector2i> map_pixel; - for ( int y=endPixel.y()-1; y <= endPixel.y() +1; y++ ) - { - for ( int x=endPixel.x()-1; x <= endPixel.x() +1; x++ ) - { - unsigned offset=x+m_PixelSize*y; - if ( offset < unsigned ( m_ByteSize ) ) - { - m_CurrentChanges[ offset ] = SAFETY_BORDER; - } - } - } - } - } - //paint safety ranges - for ( unsigned i=0; i<ranges.size(); i++ ) + for(int i = 0; i < ranges.size(); i++) { - geometry_msgs::Point startPosWorld = map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform); - Eigen::Vector2i startPixel = map_tools::toMapCoords(startPosWorld, m_Origin, m_Resolution); - geometry_msgs::Point endPos; - endPos.x = ranges[i].endPos.x * 4; - endPos.y = ranges[i].endPos.y * 4; - - geometry_msgs::Point endPosWorld = map_tools::transformPoint(endPos, m_latestMapTransform); - Eigen::Vector2i endPixel = map_tools::toMapCoords(endPosWorld, m_Origin, m_Resolution); - - - if(endPixel.x() < 0) endPixel.x() = 0; - if(endPixel.y() < 0) endPixel.y() = 0; - if(endPixel.x() >= m_PixelSize) endPixel.x() = m_PixelSize - 1; - if(endPixel.y() >= m_PixelSize) endPixel.y() = m_PixelSize - 1; - - drawLine ( m_CurrentChanges, startPixel, endPixel, SAFETY_BORDER ); + geometry_msgs::Point endPosWorld = map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform); + map_pixel.push_back(map_tools::toMapCoords(endPosWorld, m_metaData.origin, m_metaData.resolution)); } + geometry_msgs::Point sensorPosWorld = map_tools::transformPoint(ranges[0].sensorPos, m_latestMapTransform); + Eigen::Vector2i sensorPixel = map_tools::toMapCoords(sensorPosWorld, m_metaData.origin, m_metaData.resolution); + m_ChangedRegion.enclose ( sensorPixel.x(), sensorPixel.y() ); + + ////paint safety borders + //if ( m_ObstacleBorders ) + //{ + //for ( unsigned i=0; i<ranges.size(); i++ ) + //{ + //Eigen::Vector2i endPixel = map_pixel[i]; + + //for ( int y=endPixel.y()-1; y <= endPixel.y() +1; y++ ) + //{ + //if(y > m_metaData.height) continue; + //for ( int x=endPixel.x()-1; x <= endPixel.x() +1; x++ ) + //{ + //if(x > m_metaData.width) continue; + //unsigned offset=x+m_metaData.width*y; + //if ( offset < unsigned ( m_ByteSize ) ) + //{ + //m_CurrentChanges[ offset ] = SAFETY_BORDER; + //} + //} + //} + //} + //} + ////paint safety ranges + //for ( unsigned i=0; i<ranges.size(); i++ ) + //{ + //Eigen::Vector2i startPixel = map_pixel[i]; + //geometry_msgs::Point endPos; + //endPos.x = ranges[i].endPos.x * 4; + //endPos.y = ranges[i].endPos.y * 4; + + //geometry_msgs::Point endPosWorld = map_tools::transformPoint(endPos, m_latestMapTransform); + //Eigen::Vector2i endPixel = map_tools::toMapCoords(endPosWorld, m_metaData.origin, m_metaData.resolution); + + + //if(endPixel.x() < 0) endPixel.x() = 0; + //if(endPixel.y() < 0) endPixel.y() = 0; + //if(endPixel.x() >= m_metaData.width) endPixel.x() = m_metaData.width - 1; + //if(endPixel.y() >= m_metaData.height) endPixel.y() = m_metaData.height - 1; + + //drawLine ( m_CurrentChanges, startPixel, endPixel, SAFETY_BORDER ); + //} //paint end pixels for ( unsigned i=0; i<ranges.size(); i++ ) { - if ( !ranges[i].free ) - { - geometry_msgs::Point endPosWorld = map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform); - Eigen::Vector2i endPixel = map_tools::toMapCoords(endPosWorld, m_Origin, m_Resolution); - - if ( endPixel != lastEndPixel ) - { - unsigned offset = endPixel.x() + m_PixelSize * endPixel.y(); - if ( offset < m_ByteSize ) - { - m_CurrentChanges[ offset ] = ::OCCUPIED; - } - } - lastEndPixel=endPixel; - } - } - - //paint free ranges - geometry_msgs::Point sensorPosWorld = map_tools::transformPoint(ranges[0].sensorPos, m_latestMapTransform); - Eigen::Vector2i sensorPixel = map_tools::toMapCoords(sensorPosWorld, m_Origin, m_Resolution); - - for ( unsigned i=0; i<ranges.size(); i++ ) - { - geometry_msgs::Point endPosWorld = map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform); - Eigen::Vector2i endPixel = map_tools::toMapCoords(endPosWorld, m_Origin, m_Resolution); - - m_ChangedRegion.enclose ( sensorPixel.x(), sensorPixel.y() ); - m_ChangedRegion.enclose ( endPixel.x(), endPixel.y() ); - - if ( endPixel != lastEndPixel ) - { - drawLine ( m_CurrentChanges, sensorPixel, endPixel, ::FREE ); - } + Eigen::Vector2i endPixel = map_pixel[i]; + + if ( endPixel != lastEndPixel ) + { + bool outside = false; + if(endPixel.x() >= m_metaData.width) + { + need_x_right = std::max((int)( endPixel.x() - m_metaData.width + 10), need_x_right); + outside = true; + } + if(endPixel.x() < 0) + { + need_x_left = std::max((int)(-endPixel.x()) + 10, need_x_left); + outside = true; + } + if(endPixel.y() >= m_metaData.height) + { + need_y_down = std::max((int)(endPixel.y() - m_metaData.height) + 10, need_y_down); + outside = true; + } + if(endPixel.y() < 0) + { + need_y_up = std::max((int)(- endPixel.y()) + 10, need_y_up); + outside = true; + } + if(outside) + { + continue; + } + m_ChangedRegion.enclose ( endPixel.x(), endPixel.y() ); + //paint free ranges + drawLine ( m_CurrentChanges, sensorPixel, endPixel, ::FREE ); - lastEndPixel=endPixel; + if ( !ranges[i].free ) + { + unsigned offset = endPixel.x() + m_metaData.width * endPixel.y(); + m_CurrentChanges[ offset ] = ::OCCUPIED; + } + } + lastEndPixel=endPixel; } - m_ChangedRegion.clip ( Box2D<int> ( 0,0,m_PixelSize-1,m_PixelSize-1 ) ); + m_ChangedRegion.clip ( Box2D<int> ( 0,0,m_metaData.width-1,m_metaData.height-1 ) ); m_ExploredRegion.enclose ( m_ChangedRegion ); applyChanges(); computeOccupancyProbabilities(); + if(need_x_left + need_x_right + need_y_down + need_y_up > 0) + { + //keep square aspect ration till homer_gui can handle other maps + //int need_x = need_x_left + need_x_right; + //int need_y = need_y_up + need_y_down; + //if(need_x > need_y) + //{ + //need_y_down += need_x - need_y; + //} + //else if (need_y > need_x) + //{ + //need_x_right += need_y - need_x; + //} + + ROS_INFO_STREAM("new map size!"); + ROS_INFO_STREAM(" "<<need_x_left<<" "<<need_y_up<<" "<<need_x_right<<" "<<need_y_down); + changeMapSize(need_x_left, need_y_up, need_x_right, need_y_down); + } } double OccupancyMap::contrastFromProbability ( int8_t prob ) @@ -473,7 +557,7 @@ double OccupancyMap::evaluateByContrast() { for ( int x = m_ExploredRegion.minX(); x <= m_ExploredRegion.maxX(); x++ ) { - int i = x + y * m_PixelSize; + int i = x + y * m_metaData.width; if ( m_MeasurementCount [i] > 1 ) { int prob = m_OccupancyProbability[i] * 100; @@ -501,16 +585,13 @@ vector<MeasurePoint> OccupancyMap::getMeasurePoints (sensor_msgs::LaserScanConst double minDist = m_MeasureSamplingStep; - //m_LaserMaxRange = laserData->range_max; - m_LaserMinRange = laserData->range_min; - Point2D lastHitPos; Point2D lastUsedHitPos; //extract points for measuring for ( unsigned int i=0; i < laserData->ranges.size(); i++ ) { - if ( laserData->ranges[i] <= m_LaserMaxRange && laserData->ranges[i] >= m_LaserMinRange ) + if ( laserData->ranges[i] <= laserData->range_max && laserData->ranges[i] >= laserData->range_min ) { float alpha = laserData->angle_min + i * laserData->angle_increment; tf::Vector3 pin; @@ -528,7 +609,7 @@ vector<MeasurePoint> OccupancyMap::getMeasurePoints (sensor_msgs::LaserScanConst MeasurePoint p; //preserve borders of segments if ( ( i!=0 ) && - ( lastUsedHitPos.distance ( lastHitPos ) > m_Resolution*0.5 ) && + ( lastUsedHitPos.distance ( lastHitPos ) > m_metaData.resolution*0.5 ) && ( hitPos.distance ( lastHitPos ) >= minDist*1.5 ) ) { p.hitPos = lastHitPos; @@ -579,7 +660,7 @@ vector<MeasurePoint> OccupancyMap::getMeasurePoints (sensor_msgs::LaserScanConst CVec2 normal = diff.rotate ( Math::Pi * 0.5 ); normal.normalize(); - normal *= m_Resolution * sqrt ( 2.0 ) * 10.0; + normal *= m_metaData.resolution * sqrt ( 2.0 ) * 10.0; result[i].frontPos = result[i].hitPos + normal; } @@ -609,8 +690,8 @@ float OccupancyMap::computeScore ( Pose robotPose, std::vector<MeasurePoint> mea hitPos.x = x; hitPos.y = y; - Eigen::Vector2i hitPixel = map_tools::toMapCoords(hitPos, m_Origin, m_Resolution); - hitOffset = hitPixel.x() + m_PixelSize*hitPixel.y(); + Eigen::Vector2i hitPixel = map_tools::toMapCoords(hitPos, m_metaData.origin, m_metaData.resolution); + hitOffset = hitPixel.x() + m_metaData.width*hitPixel.y(); //avoid multiple measuring of same pixel or unknown pixel if ( ( hitOffset == lastOffset ) || ( hitOffset >= unsigned ( m_ByteSize ) ) || ( m_MeasurementCount[hitOffset] == 0 ) ) @@ -627,8 +708,8 @@ float OccupancyMap::computeScore ( Pose robotPose, std::vector<MeasurePoint> mea frontPos.x = x; frontPos.y = y; - Eigen::Vector2i frontPixel = map_tools::toMapCoords(frontPos, m_Origin, m_Resolution); - frontOffset = frontPixel.x() + m_PixelSize*frontPixel.y(); + Eigen::Vector2i frontPixel = map_tools::toMapCoords(frontPos, m_metaData.origin, m_metaData.resolution); + frontOffset = frontPixel.x() + m_metaData.width*frontPixel.y(); if ( ( frontOffset >= unsigned ( m_ByteSize ) ) || ( m_MeasurementCount[frontOffset] == 0 ) ) { @@ -691,10 +772,13 @@ void OccupancyMap::drawLine ( DataT* data, Eigen::Vector2i& startPixel, Eigen::V // compute cells for ( t = 0; t < dist; t++ ) { - int index = x + m_PixelSize * y; + int index = x + m_metaData.width * y; // set flag to free if no flag is set // (do not overwrite occupied cells) - if(index < 0) continue; + if(index < 0 || index > m_ByteSize) + { + continue; + } if ( data[index] == NO_CHANGE ) { data[index] = value; @@ -721,10 +805,10 @@ void OccupancyMap::drawLine ( DataT* data, Eigen::Vector2i& startPixel, Eigen::V void OccupancyMap::applyChanges() { - for ( int y = m_ChangedRegion.minY(); y <= m_ChangedRegion.maxY(); y++ ) + for ( int y = m_ChangedRegion.minY()+1; y <= m_ChangedRegion.maxY()-1; y++ ) { - int yOffset = m_PixelSize * y; - for ( int x = m_ChangedRegion.minX(); x <= m_ChangedRegion.maxX(); x++ ) + int yOffset = m_metaData.width * y; + for ( int x = m_ChangedRegion.minX()+1; x <= m_ChangedRegion.maxX()-1; x++ ) { int i = x + yOffset; if ( ( m_CurrentChanges[i] == ::FREE || m_CurrentChanges[i] == ::OCCUPIED ) && m_MeasurementCount[i] < SHRT_MAX ) @@ -733,53 +817,72 @@ void OccupancyMap::applyChanges() } if ( m_CurrentChanges[i] == ::OCCUPIED && m_OccupancyCount[i] < USHRT_MAX ) { - m_OccupancyCount[i]++; + + //if(m_MeasurementCount[x + m_metaData.width * (y+1)] > 1) + //m_MeasurementCount[x + m_metaData.width * (y+1)]++; + //if(m_MeasurementCount[x + m_metaData.width * (y-1)] > 1) + //m_MeasurementCount[x + m_metaData.width * (y-1)]++; + //if(m_MeasurementCount[i-1] > 1) + //m_MeasurementCount[i-1]++; + //if(m_MeasurementCount[i+1] > 1) + //m_MeasurementCount[i+1]++; + m_OccupancyCount[i]++ ; } } } + for ( int y = m_ChangedRegion.minY()+1; y <= m_ChangedRegion.maxY()-1; y++ ) + { + int yOffset = m_metaData.width * y; + for ( int x = m_ChangedRegion.minX()+1; x <= m_ChangedRegion.maxX()-1; x++ ) + { + int i = x + yOffset; + if(m_OccupancyCount[i]> m_MeasurementCount[i]) + m_OccupancyCount[i]=m_MeasurementCount[i]; +}} } void OccupancyMap::clearChanges() { m_ChangedRegion.expand ( 2 ); - m_ChangedRegion.clip ( Box2D<int> ( 0,0,m_PixelSize-1,m_PixelSize-1 ) ); + m_ChangedRegion.clip ( Box2D<int> ( 0,0,m_metaData.width-1,m_metaData.height-1 ) ); for ( int y = m_ChangedRegion.minY(); y <= m_ChangedRegion.maxY(); y++ ) { - int yOffset = m_PixelSize * y; + int yOffset = m_metaData.width * y; for ( int x = m_ChangedRegion.minX(); x <= m_ChangedRegion.maxX(); x++ ) { int i = x + yOffset; m_CurrentChanges[i] = NO_CHANGE; } } - m_ChangedRegion=Box2D<int> ( m_PixelSize - 1, m_PixelSize - 1, 0, 0 ); + m_ChangedRegion=Box2D<int> ( m_metaData.width - 1, m_metaData.height - 1, 0, 0 ); } void OccupancyMap::incrementMeasurementCount ( Eigen::Vector2i p ) { - unsigned index = p.x() + m_PixelSize * p.y(); - if ( index < m_ByteSize ) - { - if ( m_CurrentChanges[index] == NO_CHANGE && m_MeasurementCount[index] < USHRT_MAX ) - { - m_CurrentChanges[index] = ::FREE; - m_MeasurementCount[index]++; - } - } - else - { - ROS_ERROR( "Index out of bounds: x = %d, y = %d", p.x(), p.y() ); - } + if(p.x() >= m_metaData.width || p.y() >= m_metaData.height) + { + return; + } + unsigned index = p.x() + m_metaData.width * p.y(); + if ( m_CurrentChanges[index] == NO_CHANGE && m_MeasurementCount[index] < USHRT_MAX ) + { + m_CurrentChanges[index] = ::FREE; + m_MeasurementCount[index]++; + } } void OccupancyMap::incrementOccupancyCount ( Eigen::Vector2i p ) { - int index = p.x() + m_PixelSize * p.y(); - if ( ( m_CurrentChanges[index] == NO_CHANGE || m_CurrentChanges[index] == ::FREE ) && m_MeasurementCount[index] < USHRT_MAX ) - { - m_CurrentChanges[index] = ::OCCUPIED; - m_OccupancyCount[index]++; - } + if(p.x() >= m_metaData.width || p.y() >= m_metaData.height) + { + return; + } + unsigned index = p.x() + m_metaData.width * p.y(); + if ( ( m_CurrentChanges[index] == NO_CHANGE || m_CurrentChanges[index] == ::FREE ) && m_MeasurementCount[index] < USHRT_MAX ) + { + m_CurrentChanges[index] = ::OCCUPIED; + m_OccupancyCount[index]++; + } } void OccupancyMap::scaleDownCounts ( int maxCount ) @@ -790,7 +893,6 @@ void OccupancyMap::scaleDownCounts ( int maxCount ) ROS_WARN("WARNING: argument maxCount is choosen to small, resetting map."); memset ( m_MeasurementCount, 0, m_ByteSize ); memset ( m_OccupancyCount, 0, m_ByteSize ); - memset ( m_InaccessibleCount, 0, m_ByteSize ); } else { @@ -801,11 +903,6 @@ void OccupancyMap::scaleDownCounts ( int maxCount ) { m_MeasurementCount[i] /= scalingFactor; m_OccupancyCount[i] /= scalingFactor; - m_InaccessibleCount[i] /= scalingFactor; - } - if ( m_InaccessibleCount[i] > maxCount ) - { - m_InaccessibleCount[i] = maxCount; } } } @@ -822,14 +919,14 @@ void OccupancyMap::markRobotPositionFree() point.y = 0; point.z = 0; geometry_msgs::Point endPosWorld = map_tools::transformPoint(point, m_latestMapTransform); - Eigen::Vector2i robotPixel = map_tools::toMapCoords(endPosWorld, m_Origin, m_Resolution); + Eigen::Vector2i robotPixel = map_tools::toMapCoords(endPosWorld, m_metaData.origin, m_metaData.resolution); - int width = 0.4 / m_Resolution; + int width = 0.35 / m_metaData.resolution; for ( int i = robotPixel.y() - width; i <= robotPixel.y() + width; i++ ) { for ( int j = robotPixel.x() - width; j <= robotPixel.x() + width; j++ ) { - incrementMeasurementCount ( Eigen::Vector2i ( i, j ) ); + incrementMeasurementCount ( Eigen::Vector2i ( j, i ) ); } } Box2D<int> robotBox ( robotPixel.x()-width, robotPixel.y()-width, robotPixel.x() +width, robotPixel.y() +width ); @@ -840,12 +937,12 @@ void OccupancyMap::markRobotPositionFree() QImage OccupancyMap::getProbabilityQImage ( int trancparencyThreshold, bool showInaccessible ) const { - QImage retImage ( m_PixelSize, m_PixelSize, QImage::Format_RGB32 ); - for ( int y = 0; y < m_PixelSize; y++ ) + QImage retImage ( m_metaData.width, m_metaData.height, QImage::Format_RGB32 ); + for ( int y = 0; y < m_metaData.height; y++ ) { - for ( int x = 0; x < m_PixelSize; x++ ) + for ( int x = 0; x < m_metaData.width; x++ ) { - int index = x + y * m_PixelSize; + int index = x + y * m_metaData.width; int value = UNKNOWN; if ( m_MeasurementCount[index] > 0 ) { @@ -855,26 +952,20 @@ QImage OccupancyMap::getProbabilityQImage ( int trancparencyThreshold, bool show value = static_cast<int> ( ( 0.75 + 0.025 * m_MeasurementCount[index] ) * ( 1.0 - m_OccupancyProbability[index] ) * 255 ); } } - if ( showInaccessible && m_InaccessibleCount[index] >= 2 ) - { - value = 0; - } retImage.setPixel ( x, y, qRgb ( value, value, value ) ); } } return retImage; } -void OccupancyMap::getOccupancyProbabilityImage ( vector<int8_t>& data, int& width, int& height, float& resolution ) +void OccupancyMap::getOccupancyProbabilityImage ( vector<int8_t>& data, nav_msgs::MapMetaData& metaData) { - width = m_PixelSize; - height = m_PixelSize; - resolution = m_Resolution; - data.resize(m_PixelSize * m_PixelSize); + metaData = m_metaData; + data.resize(m_metaData.width * m_metaData.height); std::fill(data.begin(), data.end(), (int8_t)NOT_SEEN_YET); //note: linker error without strange cast from int8_t to int8_t for ( int y = m_ExploredRegion.minY(); y <= m_ExploredRegion.maxY(); y++ ) { - int yOffset = m_PixelSize * y; + int yOffset = m_metaData.width * y; for ( int x = m_ExploredRegion.minX(); x <= m_ExploredRegion.maxX(); x++ ) { int i = x + yOffset; @@ -882,12 +973,6 @@ void OccupancyMap::getOccupancyProbabilityImage ( vector<int8_t>& data, int& wid { continue; } - // set inaccessible points to black - if ( m_InaccessibleCount[i] >= 2 ) - { - data[i] = 99; - continue; - } if(m_OccupancyProbability[i] == UNKNOWN_LIKELIHOOD) { data[i] = NOT_SEEN_YET; @@ -966,10 +1051,6 @@ void OccupancyMap::cleanUp() { delete[] m_CurrentChanges; } - if ( m_InaccessibleCount ) - { - delete[] m_InaccessibleCount; - } if ( m_HighSensitive ) { delete[] m_HighSensitive; diff --git a/homer_mapping/src/ParticleFilter/SlamFilter.cpp b/homer_mapping/src/ParticleFilter/SlamFilter.cpp index 628ae187df2b2ea8d1bb131a3ac8c12083de87b6..1e58a7278480b6d6b9054463e2d50bdcb08f905a 100644 --- a/homer_mapping/src/ParticleFilter/SlamFilter.cpp +++ b/homer_mapping/src/ParticleFilter/SlamFilter.cpp @@ -4,7 +4,7 @@ // minimum move for translation in m const float MIN_MOVE_DISTANCE2 = 0.001 * 0.001; // minimum turn in radiants -const float MIN_TURN_DISTANCE2 = 0.001 * 0.001; +const float MIN_TURN_DISTANCE2 = 0.01 * 0.01; const float M_2PI = 2 * M_PI; @@ -50,6 +50,7 @@ SlamFilter::SlamFilter ( int particleNum ) : ParticleFilter<SlamParticle> ( part m_EffectiveParticleNum = m_ParticleNum; m_LastUpdateTime = ros::Time(0); + m_LastMoveTime = ros::Time::now(); } SlamFilter::SlamFilter ( SlamFilter& slamFilter ) : ParticleFilter<SlamParticle> ( slamFilter.m_ParticleNum ) @@ -315,24 +316,29 @@ void SlamFilter::filter (Pose currentPose, sensor_msgs::LaserScanConstPtr laserD Transformation2D trans = m_CurrentPoseOdometry - m_ReferencePoseOdometry; - // do not resample if move to small - if ( sqr ( trans.x() ) + sqr ( trans.y() ) < MIN_MOVE_DISTANCE2 && sqr ( trans.theta() ) < MIN_TURN_DISTANCE2 ) + // do not resample if move to small and last move is min 0.5 sec away + if ( sqr ( trans.x() ) + sqr ( trans.y() ) < MIN_MOVE_DISTANCE2 && sqr ( trans.theta() ) < MIN_TURN_DISTANCE2 && (ros::Time::now() - m_LastMoveTime).toSec() > 1.0 ) + //if(false) { - ROS_DEBUG_STREAM( "Move too small, will not resample." ); - if ( m_EffectiveParticleNum < m_ParticleNum / 10 ) - { - resample(); - ROS_INFO_STREAM( "Particles too scattered, resampling." ); - // filter steps + ROS_DEBUG_STREAM( "Move too small, will not resample." ); + if ( m_EffectiveParticleNum < m_ParticleNum / 10 ) + { + resample(); + ROS_INFO_STREAM( "Particles too scattered, resampling." ); + // filter steps drift(); measure(); normalize(); sort ( 0, m_ParticleNum - 1 ); - } + } } else { + if(!( sqr ( trans.x() ) + sqr ( trans.y() ) < MIN_MOVE_DISTANCE2 && sqr ( trans.theta() ) < MIN_TURN_DISTANCE2 )) + { + m_LastMoveTime = ros::Time::now(); + } resample(); // filter steps drift(); @@ -365,17 +371,19 @@ void SlamFilter::filter (Pose currentPose, sensor_msgs::LaserScanConstPtr laserD else { thetaPerSecond = trans.theta() / elapsedSeconds; - } + } + float poseVarianceX, poseVarianceY; + getPoseVariances(50, poseVarianceX, poseVarianceY); - m_LastUpdatePose = likeliestPose; - m_LastUpdateTime = measurementTime; - if ( std::fabs(thetaPerSecond) < m_MaxRotationPerSecond ) + if ( std::fabs(thetaPerSecond) < m_MaxRotationPerSecond && poseVarianceX < 0.05 && poseVarianceY < 0.05 ) { updateMap(); + m_LastUpdatePose = likeliestPose; + m_LastUpdateTime = measurementTime; } else { - ROS_DEBUG_STREAM( "No mapping performed, rotation angle too big." ); + ROS_WARN_STREAM( "No mapping performed, rotation angle too big." ); } } else diff --git a/homer_mapping/src/slam_node.cpp b/homer_mapping/src/slam_node.cpp index c80692b22b0c147b9c4fbad1a0407681a3589ff4..0588db58fa5fe631ac087723dc03016ed494a910 100644 --- a/homer_mapping/src/slam_node.cpp +++ b/homer_mapping/src/slam_node.cpp @@ -113,34 +113,23 @@ void SlamNode::callbackResetHigh(const std_msgs::Empty::ConstPtr& msg) void SlamNode::sendMapDataMessage(ros::Time mapTime) { std::vector<int8_t> mapData; - int width, height; - float resolution; + nav_msgs::MapMetaData metaData; OccupancyMap* occMap = m_HyperSlamFilter->getBestSlamFilter()->getLikeliestMap(); - occMap->getOccupancyProbabilityImage (mapData, width, height, resolution); + occMap->getOccupancyProbabilityImage (mapData, metaData); - if ( width != height ) - { - ROS_ERROR_STREAM("ERROR: Map is not quadratic! can not send map!"); - } - else + //if ( width != height ) + //{ + //ROS_ERROR_STREAM("ERROR: Map is not quadratic! can not send map!"); + //} + //else { nav_msgs::OccupancyGrid mapMsg; std_msgs::Header header; header.stamp = mapTime; header.frame_id = "map"; mapMsg.header = header; - nav_msgs::MapMetaData mapMetaData; - mapMetaData.width = width; - mapMetaData.height = height; - mapMetaData.resolution = resolution; - mapMetaData.origin.position.x = -height*resolution/2; - mapMetaData.origin.position.y = -width*resolution/2; - mapMetaData.origin.orientation.w = 1.0; - mapMetaData.origin.orientation.x = 0.0; - mapMetaData.origin.orientation.y = 0.0; - mapMetaData.origin.orientation.z = 0.0; - mapMsg.info = mapMetaData; + mapMsg.info = metaData; mapMsg.data = mapData; m_SLAMMapPublisher.publish(mapMsg); @@ -323,10 +312,10 @@ void SlamNode::callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr &msg) float res = msg->info.resolution; int height = msg->info.height; // cell size int width = msg->info.width; //cell size - if(height!=width) { - ROS_ERROR("Height != width in loaded map"); - return; - } + //if(height!=width) { + //ROS_ERROR("Height != width in loaded map"); + //return; + //} //convert map vector from ros format to robbie probability array float* map = new float[msg->data.size()]; @@ -359,7 +348,7 @@ void SlamNode::callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr &msg) } } Box2D<int> exploredRegion = Box2D<int> ( minX, minY, maxX, maxY ); - OccupancyMap* occMap = new OccupancyMap(map, msg->info.origin, res, width, exploredRegion); + OccupancyMap* occMap = new OccupancyMap(map, msg->info.origin, res, width, height, exploredRegion); m_HyperSlamFilter->setOccupancyMap( occMap ); m_HyperSlamFilter->setMapping( false ); //is this already done by gui message? ROS_INFO_STREAM( "Replacing occupancy map" ); diff --git a/homer_navigation/CMakeLists.txt b/homer_navigation/CMakeLists.txt index 0d5bda2ddbb6e746e60c794acf7e937b5a698917..a763a4028feaa42a284eb30d82cf4558a628d7f0 100644 --- a/homer_navigation/CMakeLists.txt +++ b/homer_navigation/CMakeLists.txt @@ -16,7 +16,8 @@ find_package(catkin REQUIRED COMPONENTS find_package(Eigen3 REQUIRED) -set(CMAKE_BUILD_TYPE Release) +#set(CMAKE_BUILD_TYPE Release) +set(CMAKE_BUILD_TYPE Debug) catkin_package( INCLUDE_DIRS include diff --git a/homer_navigation/launch/homer_navigation.launch b/homer_navigation/launch/homer_navigation.launch index db217818f217c26ead905d64d0bb2e7523d1a821..baa57eecae5383863adaae142ff65746babd7075 100644 --- a/homer_navigation/launch/homer_navigation.launch +++ b/homer_navigation/launch/homer_navigation.launch @@ -1,4 +1,4 @@ <launch> <rosparam command="load" file="$(find homer_navigation)/config/homer_navigation.yaml"/> - <node ns="/homer_navigation" name="homer_navigation" pkg="homer_navigation" type="homer_navigation" output="screen"/> + <node ns="/homer_navigation" name="homer_navigation" pkg="homer_navigation" type="homer_navigation" output="screen" launch-prefix="gdb"/> </launch> diff --git a/homer_navigation/src/homer_navigation_node.cpp b/homer_navigation/src/homer_navigation_node.cpp index 3c5d45c5c9968770d99a2ad9d9ecac2f7e23d439..c5de4ecce49f98698f240ff55adf9231a9a6560e 100644 --- a/homer_navigation/src/homer_navigation_node.cpp +++ b/homer_navigation/src/homer_navigation_node.cpp @@ -208,8 +208,6 @@ void HomerNavigationNode::calculatePath() { m_path_reaches_target = true; return; } - m_explorer->setOccupancyMap(m_width, m_height, m_origin, - &(*m_last_map_data)[0]); m_explorer->setStart( map_tools::toMapCoords(m_robot_pose.position, m_origin, m_resolution)); @@ -271,8 +269,6 @@ void HomerNavigationNode::startNavigation() { maskMap(); } - m_explorer->setOccupancyMap(m_width, m_height, m_origin, - &(*m_last_map_data)[0]); // check if there still exists a path to the original target if (m_avoided_collision && m_initial_path_reaches_target && @@ -865,7 +861,7 @@ void HomerNavigationNode::maskMap() { ROS_INFO_STREAM("max in m: " << map_tools::fromMapCoords( safe_planning_box.max(), m_origin, m_resolution)); for (size_t x = 0; x < m_width; x++) { - for (size_t y = 0; y < m_width; y++) { + for (size_t y = 0; y < m_height; y++){ if (!safe_planning_box.contains(Eigen::Vector2i(x, y))) { m_last_map_data->at(y * m_width + x) = -1; } @@ -905,6 +901,8 @@ void HomerNavigationNode::mapCallback( m_width = msg->info.width; m_height = msg->info.height; m_resolution = msg->info.resolution; + m_explorer->setOccupancyMap(m_width, m_height, m_origin, + &(*m_last_map_data)[0]); switch (m_MainMachine.state()) { case AWAITING_PATHPLANNING_MAP: