Add pause and reset services to hector_mapping (tu-darmstadt-ros-pkg#86)

* Add pause and reset services to Hector
AMR-Frederico · Mar 15, 2021 · 50eadc5 · 50eadc5
1 parent ebb464a
commit 50eadc5
Show file tree

Hide file tree

Showing 3 changed files with 52 additions and 2 deletions.
diff --git a/hector_mapping/CMakeLists.txt b/hector_mapping/CMakeLists.txt
@@ -1,7 +1,16 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(hector_mapping)
 
-find_package(catkin REQUIRED COMPONENTS roscpp nav_msgs visualization_msgs tf message_filters laser_geometry tf_conversions message_generation)
+find_package(catkin REQUIRED COMPONENTS 
+  roscpp
+  nav_msgs
+  visualization_msgs
+  tf
+  message_filters
+  laser_geometry
+  tf_conversions
+  message_generation
+  std_srvs)
 
 find_package(Boost REQUIRED COMPONENTS thread)
 

diff --git a/hector_mapping/src/HectorMappingRos.cpp b/hector_mapping/src/HectorMappingRos.cpp
@@ -50,6 +50,7 @@ HectorMappingRos::HectorMappingRos()
   , tfB_(0)
   , map__publish_thread_(0)
   , initial_pose_set_(false)
+  , pause_scan_processing_(false)
 {
   ros::NodeHandle private_nh_("~");
 
@@ -156,6 +157,10 @@ HectorMappingRos::HectorMappingRos()
       tmp.dynamicMapServiceServer_ = node_.advertiseService("dynamic_map", &HectorMappingRos::mapCallback, this);
     }
 
+    // Initialize services to reset map, and toggle scan pause
+    reset_map_service_ = node_.advertiseService("reset_map", &HectorMappingRos::resetMapCallback, this);
+    toggle_scan_processing_service_ = node_.advertiseService("pause_mapping", &HectorMappingRos::pauseMapCallback, this);
+
     setServiceGetMapData(tmp.map_, slamProcessor->getGridMap(i));
 
     if ( i== 0){
@@ -228,6 +233,10 @@ HectorMappingRos::~HectorMappingRos()
 
 void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan& scan)
 {
+  if (pause_scan_processing_) {
+    return;
+  }
+
   if (hectorDrawings)
   {
     hectorDrawings->setTime(scan.header.stamp);
@@ -372,6 +381,30 @@ bool HectorMappingRos::mapCallback(nav_msgs::GetMap::Request  &req,
   return true;
 }
 
+bool HectorMappingRos::resetMapCallback(std_srvs::Trigger::Request  &req,
+                                        std_srvs::Trigger::Response &res)
+{
+  ROS_INFO("HectorSM Reset map service called");
+  slamProcessor->reset();
+  return true;
+}
+
+bool HectorMappingRos::pauseMapCallback(std_srvs::SetBool::Request  &req,
+                                        std_srvs::SetBool::Response &res)
+{
+  if (req.data && !pause_scan_processing_)
+  {
+    ROS_INFO("HectorSM Mapping paused");
+  }
+  else if (!req.data && pause_scan_processing_)
+  {
+    ROS_INFO("HectorSM Mapping no longer paused");
+  }
+  pause_scan_processing_ = req.data;
+  res.success = true;
+  return true;
+}
+
 void HectorMappingRos::publishMap(MapPublisherContainer& mapPublisher, const hectorslam::GridMap& gridMap, ros::Time timestamp, MapLockerInterface* mapMutex)
 {
   nav_msgs::GetMap::Response& map_ (mapPublisher.map_);

diff --git a/hector_mapping/src/HectorMappingRos.h b/hector_mapping/src/HectorMappingRos.h
@@ -39,6 +39,9 @@
 #include "sensor_msgs/LaserScan.h"
 #include <std_msgs/String.h>
 
+#include <std_srvs/SetBool.h>
+#include <std_srvs/Trigger.h>
+
 #include "laser_geometry/laser_geometry.h"
 #include "nav_msgs/GetMap.h"
 
@@ -75,6 +78,8 @@ class HectorMappingRos
   void sysMsgCallback(const std_msgs::String& string);
 
   bool mapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
+  bool resetMapCallback(std_srvs::Trigger::Request  &req, std_srvs::Trigger::Response &res);
+  bool pauseMapCallback(std_srvs::SetBool::Request  &req, std_srvs::SetBool::Response &res);
 
   void publishMap(MapPublisherContainer& map_, const hectorslam::GridMap& gridMap, ros::Time timestamp, MapLockerInterface* mapMutex = 0);
 
@@ -115,6 +120,9 @@ class HectorMappingRos
   ros::Publisher odometryPublisher_;
   ros::Publisher scan_point_cloud_publisher_;
 
+  ros::ServiceServer reset_map_service_;
+  ros::ServiceServer toggle_scan_processing_service_;
+
   std::vector<MapPublisherContainer> mapPubContainer;
 
   tf::TransformListener tf_;
@@ -140,6 +148,7 @@ class HectorMappingRos
   bool initial_pose_set_;
   Eigen::Vector3f initial_pose_;
 
+  bool pause_scan_processing_;
 
   //-----------------------------------------------------------
   // Parameters
@@ -183,7 +192,6 @@ class HectorMappingRos
   bool p_map_with_known_poses_;
   bool p_timing_output_;
 
-
   float p_sqr_laser_min_dist_;
   float p_sqr_laser_max_dist_;
   float p_laser_z_min_value_;