From 3c1d40eb7eaf5d7e62419235ffa8204e7d2d2c4d Mon Sep 17 00:00:00 2001 From: Jin Date: Mon, 23 Oct 2017 10:27:04 +0800 Subject: [PATCH 1/5] Init map from static_map serivce when startup --- .../hector_slam_lib/map/GridMapLogOdds.h | 13 ++++ .../hector_slam_lib/map/OccGridMapBase.h | 8 +++ hector_mapping/launch/mapping_default.launch | 3 + hector_mapping/src/HectorMappingRos.cpp | 61 ++++++++++++++++--- hector_mapping/src/HectorMappingRos.h | 3 +- 5 files changed, 78 insertions(+), 10 deletions(-) diff --git a/hector_mapping/include/hector_slam_lib/map/GridMapLogOdds.h b/hector_mapping/include/hector_slam_lib/map/GridMapLogOdds.h index cfbbf7a8..fabb7e63 100644 --- a/hector_mapping/include/hector_slam_lib/map/GridMapLogOdds.h +++ b/hector_mapping/include/hector_slam_lib/map/GridMapLogOdds.h @@ -194,6 +194,19 @@ class GridMapLogOddsFunctions logOddsOccupied = probToLogOdds(factor); } + /** + * mark occupied cells as very occupied, and free cells very free + * @param cell The cell. + */ + void stablize(LogOddsCell& cell) const + { + if (cell.isOccupied()) { + cell.logOddsVal = 50.0f; + } else if (cell.isFree()) { + cell.logOddsVal = -50.0f; + } + } + protected: float probToLogOdds(float prob) diff --git a/hector_mapping/include/hector_slam_lib/map/OccGridMapBase.h b/hector_mapping/include/hector_slam_lib/map/OccGridMapBase.h index e3b439a2..64b13824 100644 --- a/hector_mapping/include/hector_slam_lib/map/OccGridMapBase.h +++ b/hector_mapping/include/hector_slam_lib/map/OccGridMapBase.h @@ -96,6 +96,14 @@ class OccGridMapBase return (this->getCell(index).isFree()); } + void stablize() + { + int size = this->getSizeX() * this->getSizeY(); + for (int i = 0; i < size; ++i) { + concreteGridFunctions.stablize(this->getCell(i)); + } + } + float getObstacleThreshold() const { ConcreteCellType temp; diff --git a/hector_mapping/launch/mapping_default.launch b/hector_mapping/launch/mapping_default.launch index 9af61bf9..976b4490 100644 --- a/hector_mapping/launch/mapping_default.launch +++ b/hector_mapping/launch/mapping_default.launch @@ -8,8 +8,11 @@ + + + diff --git a/hector_mapping/src/HectorMappingRos.cpp b/hector_mapping/src/HectorMappingRos.cpp index 906c2cde..efac9d53 100644 --- a/hector_mapping/src/HectorMappingRos.cpp +++ b/hector_mapping/src/HectorMappingRos.cpp @@ -91,6 +91,8 @@ HectorMappingRos::HectorMappingRos() private_nh_.param("output_timing", p_timing_output_,false); + private_nh_.param("use_static_map", p_use_static_map_, false); + private_nh_.param("map_pub_period", p_map_pub_period_, 2.0); double tmp = 0.0; @@ -123,7 +125,34 @@ HectorMappingRos::HectorMappingRos() odometryPublisher_ = node_.advertise("scanmatch_odom", 50); } - slamProcessor = new hectorslam::HectorSlamProcessor(static_cast(p_map_resolution_), p_map_size_, p_map_size_, Eigen::Vector2f(p_map_start_x_, p_map_start_y_), p_map_multi_res_levels_, hectorDrawings, debugInfoProvider); + slamProcessor = NULL; + if (p_use_static_map_) + { + ros::ServiceClient mapCli = node_.serviceClient("static_map"); + nav_msgs::GetMap srv; + ros::Duration waitTime(1.0); + int i; + for (i = 0; i < 3; i++) + { + if (mapCli.call(srv)) break; + waitTime.sleep(); + } + if (i == 3) + { + ROS_INFO("HectorSM init map by /static_map %s", "failed"); + } + else + { + setStaticMapData(srv.response.map); + ROS_INFO("init map by /static_map %s", "succeed"); + } + } + + if (!slamProcessor) + { + slamProcessor = new hectorslam::HectorSlamProcessor(static_cast(p_map_resolution_), p_map_size_, p_map_size_, Eigen::Vector2f(p_map_start_x_, p_map_start_y_), p_map_multi_res_levels_, hectorDrawings, debugInfoProvider); + } + slamProcessor->setUpdateFactorFree(p_update_factor_free_); slamProcessor->setUpdateFactorOccupied(p_update_factor_occupied_); slamProcessor->setMapUpdateMinDistDiff(p_map_update_distance_threshold_); @@ -378,7 +407,6 @@ void HectorMappingRos::publishMap(MapPublisherContainer& mapPublisher, const hec //only update map if it changed if (lastGetMapUpdateIndex != gridMap.getUpdateIndex()) { - int sizeX = gridMap.getSizeX(); int sizeY = gridMap.getSizeY(); @@ -487,7 +515,7 @@ bool HectorMappingRos::rosPointCloudToDataContainer(const sensor_msgs::PointClou void HectorMappingRos::setServiceGetMapData(nav_msgs::GetMap::Response& map_, const hectorslam::GridMap& gridMap) { Eigen::Vector2f mapOrigin (gridMap.getWorldCoords(Eigen::Vector2f::Zero())); - mapOrigin.array() -= gridMap.getCellLength()*0.5f; + // mapOrigin.array() -= gridMap.getCellLength()*0.5f; map_.map.info.origin.position.x = mapOrigin.x(); map_.map.info.origin.position.y = mapOrigin.y(); @@ -502,19 +530,36 @@ void HectorMappingRos::setServiceGetMapData(nav_msgs::GetMap::Response& map_, co map_.map.data.resize(map_.map.info.width * map_.map.info.height); } -/* void HectorMappingRos::setStaticMapData(const nav_msgs::OccupancyGrid& map) { float cell_length = map.info.resolution; - Eigen::Vector2f mapOrigin (map.info.origin.position.x + cell_length*0.5f, - map.info.origin.position.y + cell_length*0.5f); int map_size_x = map.info.width; int map_size_y = map.info.height; - slamProcessor = new hectorslam::HectorSlamProcessor(cell_length, map_size_x, map_size_y, Eigen::Vector2f(0.0f, 0.0f), 1, hectorDrawings, debugInfoProvider); + Eigen::Vector2f mapOrigin (-map.info.origin.position.x / (map_size_x*cell_length), + -map.info.origin.position.y / (map_size_y*cell_length)); + + delete slamProcessor; + slamProcessor = new hectorslam::HectorSlamProcessor(cell_length, map_size_x, map_size_y, mapOrigin, 1, hectorDrawings, debugInfoProvider); + + hectorslam::GridMap& gridMap = const_cast(slamProcessor->getGridMap(0)); + int size = map_size_x * map_size_y; + for (int i = 0; i < size; ++i) { + switch (map.data[i]) { + case 0: + gridMap.updateSetFree(i); + break; + case 100: + gridMap.updateSetOccupied(i); + break; + default: + // pass + break; + } + } + gridMap.stablize(); } -*/ void HectorMappingRos::publishMapLoop(double map_pub_period) diff --git a/hector_mapping/src/HectorMappingRos.h b/hector_mapping/src/HectorMappingRos.h index ad96f8b6..b68536eb 100644 --- a/hector_mapping/src/HectorMappingRos.h +++ b/hector_mapping/src/HectorMappingRos.h @@ -90,9 +90,7 @@ class HectorMappingRos void staticMapCallback(const nav_msgs::OccupancyGrid& map); void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg); - /* void setStaticMapData(const nav_msgs::OccupancyGrid& map); - */ protected: HectorDebugInfoProvider* debugInfoProvider; @@ -183,6 +181,7 @@ class HectorMappingRos bool p_map_with_known_poses_; bool p_timing_output_; + bool p_use_static_map_; float p_sqr_laser_min_dist_; float p_sqr_laser_max_dist_; From a5e19b5fd7866385faaf34cd69385b5f2fa2e9e7 Mon Sep 17 00:00:00 2001 From: Jin Date: Tue, 24 Oct 2017 16:43:16 +0800 Subject: [PATCH 2/5] load and publish multi map levels --- .../hector_slam_lib/map/OccGridMapBase.h | 15 +++++ hector_mapping/src/HectorMappingRos.cpp | 64 ++++++++++++------- hector_mapping/src/HectorMappingRos.h | 3 +- 3 files changed, 56 insertions(+), 26 deletions(-) diff --git a/hector_mapping/include/hector_slam_lib/map/OccGridMapBase.h b/hector_mapping/include/hector_slam_lib/map/OccGridMapBase.h index 64b13824..ae4b7fc0 100644 --- a/hector_mapping/include/hector_slam_lib/map/OccGridMapBase.h +++ b/hector_mapping/include/hector_slam_lib/map/OccGridMapBase.h @@ -71,6 +71,21 @@ class OccGridMapBase concreteGridFunctions.updateUnsetFree(this->getCell(index)); } + void updateSetOccupied(int xMap, int yMap) + { + concreteGridFunctions.updateSetOccupied(this->getCell(xMap, yMap)); + } + + void updateSetFree(int xMap, int yMap) + { + concreteGridFunctions.updateSetFree(this->getCell(xMap, yMap)); + } + + void updateUnsetFree(int xMap, int yMap) + { + concreteGridFunctions.updateUnsetFree(this->getCell(xMap, yMap)); + } + float getGridProbabilityMap(int index) const { return concreteGridFunctions.getGridProbability(this->getCell(index)); diff --git a/hector_mapping/src/HectorMappingRos.cpp b/hector_mapping/src/HectorMappingRos.cpp index efac9d53..2af3115b 100644 --- a/hector_mapping/src/HectorMappingRos.cpp +++ b/hector_mapping/src/HectorMappingRos.cpp @@ -46,7 +46,6 @@ HectorMappingRos::HectorMappingRos() : debugInfoProvider(0) , hectorDrawings(0) - , lastGetMapUpdateIndex(-100) , tfB_(0) , map__publish_thread_(0) , initial_pose_set_(false) @@ -159,7 +158,7 @@ HectorMappingRos::HectorMappingRos() slamProcessor->setMapUpdateMinAngleDiff(p_map_update_angle_threshold_); int mapLevels = slamProcessor->getMapLevels(); - mapLevels = 1; + //mapLevels = 1; for (int i = 0; i < mapLevels; ++i) { @@ -177,6 +176,7 @@ HectorMappingRos::HectorMappingRos() mapMetaTopicStr.append("_metadata"); MapPublisherContainer& tmp = mapPubContainer[i]; + tmp.lastGetMapUpdateIndex_ = -100; tmp.mapPublisher_ = node_.advertise(mapTopicStr, 1, true); tmp.mapMetadataPublisher_ = node_.advertise(mapMetaTopicStr, 1, true); @@ -187,9 +187,9 @@ HectorMappingRos::HectorMappingRos() setServiceGetMapData(tmp.map_, slamProcessor->getGridMap(i)); - if ( i== 0){ - mapPubContainer[i].mapMetadataPublisher_.publish(mapPubContainer[i].map_.map.info); - } + //if ( i== 0){ + mapPubContainer[i].mapMetadataPublisher_.publish(mapPubContainer[i].map_.map.info); + //} } ROS_INFO("HectorSM p_base_frame_: %s", p_base_frame_.c_str()); @@ -402,10 +402,12 @@ bool HectorMappingRos::mapCallback(nav_msgs::GetMap::Request &req, void HectorMappingRos::publishMap(MapPublisherContainer& mapPublisher, const hectorslam::GridMap& gridMap, ros::Time timestamp, MapLockerInterface* mapMutex) { + if (mapPublisher.mapPublisher_.getNumSubscribers() <= 0) return; + nav_msgs::GetMap::Response& map_ (mapPublisher.map_); //only update map if it changed - if (lastGetMapUpdateIndex != gridMap.getUpdateIndex()) + if (mapPublisher.lastGetMapUpdateIndex_ != gridMap.getUpdateIndex()) { int sizeX = gridMap.getSizeX(); int sizeY = gridMap.getSizeY(); @@ -432,9 +434,12 @@ void HectorMappingRos::publishMap(MapPublisherContainer& mapPublisher, const hec { data[i] = 100; } + else + { + } } - lastGetMapUpdateIndex = gridMap.getUpdateIndex(); + mapPublisher.lastGetMapUpdateIndex_ = gridMap.getUpdateIndex(); if (mapMutex) { @@ -541,24 +546,31 @@ void HectorMappingRos::setStaticMapData(const nav_msgs::OccupancyGrid& map) -map.info.origin.position.y / (map_size_y*cell_length)); delete slamProcessor; - slamProcessor = new hectorslam::HectorSlamProcessor(cell_length, map_size_x, map_size_y, mapOrigin, 1, hectorDrawings, debugInfoProvider); - - hectorslam::GridMap& gridMap = const_cast(slamProcessor->getGridMap(0)); - int size = map_size_x * map_size_y; - for (int i = 0; i < size; ++i) { - switch (map.data[i]) { - case 0: - gridMap.updateSetFree(i); - break; - case 100: - gridMap.updateSetOccupied(i); - break; - default: - // pass - break; + slamProcessor = new hectorslam::HectorSlamProcessor(cell_length, map_size_x, map_size_y, mapOrigin, p_map_multi_res_levels_, hectorDrawings, debugInfoProvider); + + float r = 1.0f; + for (int idx = 0; idx < slamProcessor->getMapLevels(); ++idx) + { + hectorslam::GridMap& gridMap = const_cast(slamProcessor->getGridMap(idx)); + for (int y = 0; y < map_size_y; ++y) + { + for (int x = 0; x < map_size_x; ++x) + { + int mapX = x * r + 0.5f, mapY = y * r + 0.5f; + if (!gridMap.hasGridValue(mapX, mapY)) continue; + int d = map.data[y * map_size_x + x]; + if (d == 0) { + gridMap.updateSetFree(mapX, mapY); + } else if (d == 100) { + gridMap.updateSetOccupied(mapX, mapY); + } else { + // pass + } + } } + gridMap.stablize(); + r /= 2.0f; } - gridMap.stablize(); } @@ -571,8 +583,12 @@ void HectorMappingRos::publishMapLoop(double map_pub_period) ros::Time mapTime (ros::Time::now()); //publishMap(mapPubContainer[2],slamProcessor->getGridMap(2), mapTime); //publishMap(mapPubContainer[1],slamProcessor->getGridMap(1), mapTime); - publishMap(mapPubContainer[0],slamProcessor->getGridMap(0), mapTime, slamProcessor->getMapMutex(0)); + // publishMap(mapPubContainer[0],slamProcessor->getGridMap(0), mapTime, slamProcessor->getMapMutex(0)); + for (int i = 0; i < slamProcessor->getMapLevels(); ++i) { + publishMap(mapPubContainer[i],slamProcessor->getGridMap(i), mapTime, slamProcessor->getMapMutex(i)); + } + //ros::WallDuration t2 = ros::WallTime::now() - t1; //std::cout << "time s: " << t2.toSec(); diff --git a/hector_mapping/src/HectorMappingRos.h b/hector_mapping/src/HectorMappingRos.h index b68536eb..874f4dfc 100644 --- a/hector_mapping/src/HectorMappingRos.h +++ b/hector_mapping/src/HectorMappingRos.h @@ -58,6 +58,7 @@ class HectorDebugInfoProvider; class MapPublisherContainer { public: + int lastGetMapUpdateIndex_; ros::Publisher mapPublisher_; ros::Publisher mapMetadataPublisher_; nav_msgs::GetMap::Response map_; @@ -96,8 +97,6 @@ class HectorMappingRos HectorDebugInfoProvider* debugInfoProvider; HectorDrawings* hectorDrawings; - int lastGetMapUpdateIndex; - ros::NodeHandle node_; ros::Subscriber scanSubscriber_; From 4bad569401f2929adf90d8327d0bb0095e383a62 Mon Sep 17 00:00:00 2001 From: Jin Date: Wed, 25 Oct 2017 10:34:22 +0800 Subject: [PATCH 3/5] Add a syscommand to reload static map added a mutex to protect the slamProcessor ptr in case of deleting it while accessing its contents. --- hector_mapping/src/HectorMappingRos.cpp | 61 ++++++++++++++++--------- hector_mapping/src/HectorMappingRos.h | 4 +- 2 files changed, 42 insertions(+), 23 deletions(-) diff --git a/hector_mapping/src/HectorMappingRos.cpp b/hector_mapping/src/HectorMappingRos.cpp index 2af3115b..7b9c7aec 100644 --- a/hector_mapping/src/HectorMappingRos.cpp +++ b/hector_mapping/src/HectorMappingRos.cpp @@ -127,24 +127,7 @@ HectorMappingRos::HectorMappingRos() slamProcessor = NULL; if (p_use_static_map_) { - ros::ServiceClient mapCli = node_.serviceClient("static_map"); - nav_msgs::GetMap srv; - ros::Duration waitTime(1.0); - int i; - for (i = 0; i < 3; i++) - { - if (mapCli.call(srv)) break; - waitTime.sleep(); - } - if (i == 3) - { - ROS_INFO("HectorSM init map by /static_map %s", "failed"); - } - else - { - setStaticMapData(srv.response.map); - ROS_INFO("init map by /static_map %s", "succeed"); - } + loadStaticMap(); } if (!slamProcessor) @@ -263,6 +246,7 @@ void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan& scan) } ros::WallTime startTime = ros::WallTime::now(); + boost::lock_guard locked(slamProcPtr_mutex_); if (!p_use_tf_scan_transformation_) { @@ -390,6 +374,13 @@ void HectorMappingRos::sysMsgCallback(const std_msgs::String& string) ROS_INFO("HectorSM reset"); slamProcessor->reset(); } + else if (string.data == "reload") + { + ROS_INFO("HectorSM reload static map"); + if (!loadStaticMap()) { + slamProcessor->reset(); + } + } } bool HectorMappingRos::mapCallback(nav_msgs::GetMap::Request &req, @@ -545,13 +536,12 @@ void HectorMappingRos::setStaticMapData(const nav_msgs::OccupancyGrid& map) Eigen::Vector2f mapOrigin (-map.info.origin.position.x / (map_size_x*cell_length), -map.info.origin.position.y / (map_size_y*cell_length)); - delete slamProcessor; - slamProcessor = new hectorslam::HectorSlamProcessor(cell_length, map_size_x, map_size_y, mapOrigin, p_map_multi_res_levels_, hectorDrawings, debugInfoProvider); + hectorslam::HectorSlamProcessor* ptr = new hectorslam::HectorSlamProcessor(cell_length, map_size_x, map_size_y, mapOrigin, p_map_multi_res_levels_, hectorDrawings, debugInfoProvider); float r = 1.0f; - for (int idx = 0; idx < slamProcessor->getMapLevels(); ++idx) + for (int idx = 0; idx < ptr->getMapLevels(); ++idx) { - hectorslam::GridMap& gridMap = const_cast(slamProcessor->getGridMap(idx)); + hectorslam::GridMap& gridMap = const_cast(ptr->getGridMap(idx)); for (int y = 0; y < map_size_y; ++y) { for (int x = 0; x < map_size_x; ++x) @@ -571,8 +561,34 @@ void HectorMappingRos::setStaticMapData(const nav_msgs::OccupancyGrid& map) gridMap.stablize(); r /= 2.0f; } + + boost::lock_guard locked(slamProcPtr_mutex_); + delete slamProcessor; + slamProcessor = ptr; } +bool HectorMappingRos::loadStaticMap() +{ + if (!p_use_static_map_) return false; + + ros::ServiceClient mapCli = node_.serviceClient("static_map"); + nav_msgs::GetMap srv; + ros::Duration waitTime(1.0); + for (int i = 0; i < 3; i++) + { + if (mapCli.call(srv)) goto succeed; + waitTime.sleep(); + } + +// fail: + ROS_INFO("HectorSM load static_map %s", "failed"); + return false; + +succeed: + setStaticMapData(srv.response.map); + ROS_INFO("HectorSM load static_map %s", "succeed"); + return true; +} void HectorMappingRos::publishMapLoop(double map_pub_period) { @@ -586,6 +602,7 @@ void HectorMappingRos::publishMapLoop(double map_pub_period) // publishMap(mapPubContainer[0],slamProcessor->getGridMap(0), mapTime, slamProcessor->getMapMutex(0)); for (int i = 0; i < slamProcessor->getMapLevels(); ++i) { + boost::lock_guard locked(slamProcPtr_mutex_); publishMap(mapPubContainer[i],slamProcessor->getGridMap(i), mapTime, slamProcessor->getMapMutex(i)); } diff --git a/hector_mapping/src/HectorMappingRos.h b/hector_mapping/src/HectorMappingRos.h index 874f4dfc..06f97e67 100644 --- a/hector_mapping/src/HectorMappingRos.h +++ b/hector_mapping/src/HectorMappingRos.h @@ -91,6 +91,7 @@ class HectorMappingRos void staticMapCallback(const nav_msgs::OccupancyGrid& map); void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg); + bool loadStaticMap(); void setStaticMapData(const nav_msgs::OccupancyGrid& map); protected: @@ -122,7 +123,8 @@ class HectorMappingRos tf::Transform map_to_odom_; boost::thread* map__publish_thread_; - + boost::mutex slamProcPtr_mutex_; + hectorslam::HectorSlamProcessor* slamProcessor; hectorslam::DataContainer laserScanContainer; From d19bd455103c57b7756153c9940c5f1124c2a454 Mon Sep 17 00:00:00 2001 From: Jin Date: Thu, 26 Oct 2017 16:27:48 +0800 Subject: [PATCH 4/5] use a read-write lock instead of the exclusive lock --- hector_mapping/src/HectorMappingRos.cpp | 9 +++++---- hector_mapping/src/HectorMappingRos.h | 2 +- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/hector_mapping/src/HectorMappingRos.cpp b/hector_mapping/src/HectorMappingRos.cpp index 7b9c7aec..87eb61c4 100644 --- a/hector_mapping/src/HectorMappingRos.cpp +++ b/hector_mapping/src/HectorMappingRos.cpp @@ -240,13 +240,14 @@ HectorMappingRos::~HectorMappingRos() void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan& scan) { + boost::shared_lock locked(slamProcPtr_mutex_); // read access (to the pointer) + if (hectorDrawings) { hectorDrawings->setTime(scan.header.stamp); } ros::WallTime startTime = ros::WallTime::now(); - boost::lock_guard locked(slamProcPtr_mutex_); if (!p_use_tf_scan_transformation_) { @@ -562,7 +563,7 @@ void HectorMappingRos::setStaticMapData(const nav_msgs::OccupancyGrid& map) r /= 2.0f; } - boost::lock_guard locked(slamProcPtr_mutex_); + boost::unique_lock locked(slamProcPtr_mutex_); // write access to the pointer delete slamProcessor; slamProcessor = ptr; } @@ -596,13 +597,13 @@ void HectorMappingRos::publishMapLoop(double map_pub_period) while(ros::ok()) { //ros::WallTime t1 = ros::WallTime::now(); - ros::Time mapTime (ros::Time::now()); //publishMap(mapPubContainer[2],slamProcessor->getGridMap(2), mapTime); //publishMap(mapPubContainer[1],slamProcessor->getGridMap(1), mapTime); // publishMap(mapPubContainer[0],slamProcessor->getGridMap(0), mapTime, slamProcessor->getMapMutex(0)); for (int i = 0; i < slamProcessor->getMapLevels(); ++i) { - boost::lock_guard locked(slamProcPtr_mutex_); + boost::shared_lock locked(slamProcPtr_mutex_); // read access + ros::Time mapTime (ros::Time::now()); publishMap(mapPubContainer[i],slamProcessor->getGridMap(i), mapTime, slamProcessor->getMapMutex(i)); } diff --git a/hector_mapping/src/HectorMappingRos.h b/hector_mapping/src/HectorMappingRos.h index 06f97e67..79e84100 100644 --- a/hector_mapping/src/HectorMappingRos.h +++ b/hector_mapping/src/HectorMappingRos.h @@ -123,7 +123,7 @@ class HectorMappingRos tf::Transform map_to_odom_; boost::thread* map__publish_thread_; - boost::mutex slamProcPtr_mutex_; + boost::shared_mutex slamProcPtr_mutex_; hectorslam::HectorSlamProcessor* slamProcessor; hectorslam::DataContainer laserScanContainer; From 086639581df6d98dffdd8c958836bb984817c76a Mon Sep 17 00:00:00 2001 From: Jin Date: Thu, 26 Oct 2017 10:37:19 +0800 Subject: [PATCH 5/5] modify command: 'reset' or 'reload' without args will reload the map, and 'reload x y a' will set initial pose after reload the map. --- hector_mapping/src/HectorMappingRos.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/hector_mapping/src/HectorMappingRos.cpp b/hector_mapping/src/HectorMappingRos.cpp index 87eb61c4..084b9a58 100644 --- a/hector_mapping/src/HectorMappingRos.cpp +++ b/hector_mapping/src/HectorMappingRos.cpp @@ -373,14 +373,22 @@ void HectorMappingRos::sysMsgCallback(const std_msgs::String& string) if (string.data == "reset") { ROS_INFO("HectorSM reset"); - slamProcessor->reset(); + boost::unique_lock locked(slamProcPtr_mutex_); // write access to the pointer + if (!loadStaticMap()) { + slamProcessor->reset(); + } } - else if (string.data == "reload") + else if (strncmp(string.data.c_str(), "reload", sizeof("reload")-1) == 0) { ROS_INFO("HectorSM reload static map"); + boost::unique_lock locked(slamProcPtr_mutex_); // write access to the pointer if (!loadStaticMap()) { slamProcessor->reset(); } + if (sscanf(string.data.c_str(), "reload %f %f %f", &initial_pose_[0], &initial_pose_[1], &initial_pose_[2]) == 3) { + initial_pose_set_ = true; + ROS_INFO("Setting initial pose with world coords x: %f y: %f yaw: %f", initial_pose_[0], initial_pose_[1], initial_pose_[2]); + } } } @@ -563,7 +571,6 @@ void HectorMappingRos::setStaticMapData(const nav_msgs::OccupancyGrid& map) r /= 2.0f; } - boost::unique_lock locked(slamProcPtr_mutex_); // write access to the pointer delete slamProcessor; slamProcessor = ptr; }