Add pause and reset services to hector_mapping #86

marcelino-pensa · 2021-03-09T21:22:21Z

I have an application in which I need to pause/resume/reset hector_mapping. I made these modifications, and I believe that it might be useful to other users as well.

I noticed that it was possible to reset the map by publishing the string reset in the topic sys_msg_topic, but I needed it to be a service call instead of a topic publishing (I needed to block my code execution until the map was effectively reset).

I hope that these modifications are considered useful, and that this can merge to the main branch.

Note: I will add comments to my code changes to clarify everything I did
Note 2: This PR would provide a solution to the old issue in #37
Note 3: This PR is backward-compatible, as it doesn't change any behavior for code that interfaces with hector_mapping, it only adds new services that can be used when interfacing with it

closes #37

marcelino-pensa · 2021-03-09T21:23:15Z

hector_mapping/CMakeLists.txt

+find_package(catkin REQUIRED COMPONENTS 
+  roscpp
+  nav_msgs
+  visualization_msgs
+  tf
+  message_filters
+  laser_geometry
+  tf_conversions
+  message_generation
+  std_srvs)


I separated by one-per-line (for readability), but I can revert this if requested...
In addition, I added the package std_srvs so that I can add the service calls to this code

marcelino-pensa · 2021-03-09T21:23:34Z

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)


The node starts as "unpaused" (no changes in behavior)

marcelino-pensa · 2021-03-09T21:23:43Z

hector_mapping/src/HectorMappingRos.cpp

+    // Initialize services to reset map, and toggle scan pause
+    resetMapService_ = node_.advertiseService("reset_map", &HectorMappingRos::resetMapCallback, this);
+    toggleScanProcessingService_ = node_.advertiseService("pause_mapping", &HectorMappingRos::pauseMapCallback, this);


new services

marcelino-pensa · 2021-03-09T21:24:34Z

hector_mapping/src/HectorMappingRos.cpp

+  if (pause_scan_processing_) {
+    return;
+  }


if paused, then we don't process scanCallback

marcelino-pensa · 2021-03-09T21:25:04Z

hector_mapping/src/HectorMappingRos.cpp

+bool HectorMappingRos::resetMapCallback(std_srvs::Trigger::Request  &req,
+                                        std_srvs::Trigger::Response &res)
+{
+  ROS_INFO("HectorSM Reset map service called");
+  slamProcessor->reset();
+  return true;
+}


pretty much the same behavior as in sysMsgCallback

marcelino-pensa · 2021-03-09T21:25:52Z

hector_mapping/src/HectorMappingRos.cpp

+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;
+  return true;
+}


simple service callback that sets pause_scan_processing_ to whatever was requested in req

marcelino-pensa · 2021-03-09T21:29:16Z

hector_mapping/src/HectorMappingRos.h

+#include <std_srvs/SetBool.h>
+#include <std_srvs/Trigger.h>


new necessary includes

marcelino-pensa · 2021-03-09T21:29:27Z

hector_mapping/src/HectorMappingRos.h

+  bool resetMapCallback(std_srvs::Trigger::Request  &req, std_srvs::Trigger::Response &res);
+  bool pauseMapCallback(std_srvs::SetBool::Request  &req, std_srvs::SetBool::Response &res);


new service callbacks

marcelino-pensa · 2021-03-09T21:29:35Z

hector_mapping/src/HectorMappingRos.h

+  ros::ServiceServer resetMapService_;
+  ros::ServiceServer toggleScanProcessingService_;


ros service servers

Usually, we use snake_case for variable names, though, this is a bit inconsistent in this repo.

sounds good! that goes along with Google's Style Guide! I will make these variables snake_case, and maybe I'll spend sometime in the future to fix this whole class (in another PR).

marcelino-pensa · 2021-03-09T21:30:04Z

hector_mapping/src/HectorMappingRos.h

@@ -140,6 +148,7 @@ class HectorMappingRos
  bool initial_pose_set_;
  Eigen::Vector3f initial_pose_;

+  bool pause_scan_processing_;


class variable that determines whether we should process scans or not

hector_mapping/src/HectorMappingRos.cpp

StefanFabian · 2021-03-12T10:30:24Z

hector_mapping/src/HectorMappingRos.h

+  ros::ServiceServer resetMapService_;
+  ros::ServiceServer toggleScanProcessingService_;


Usually, we use snake_case for variable names, though, this is a bit inconsistent in this repo.

hector_mapping/src/HectorMappingRos.cpp

StefanFabian · 2021-03-12T10:36:31Z

Thank you for your PR!
Apart from some minor details, it looks good to me.

marcelino-pensa · 2021-03-12T16:50:47Z

@StefanFabian thank you very much for your review! I have addressed your concerns, and I hope that the PR is up to standards right now =)

hector_mapping/src/HectorMappingRos.cpp

StefanFabian · 2021-03-15T18:48:07Z

Thank you for your contribution!

marcelino-pensa · 2021-03-16T19:33:20Z

thanks for the prompt reviews, @StefanFabian! I have updated the wiki documentation to reflect the updates in this PR...

In the meantime, I wonder if it would be of interest to have a service call that handles Hector management in a single call (see the definition below). I have been using this in a fork of mine, but it might benefit other ROS hector users:

# Service definition for hector processing management
# -------------------------------------------------------------
# - - - - - Inputs - - - - -
# pause: whether hector should pause or resume mapping
# reset_map: if true, resets the whole map
# reset_pose: if true, reset hector's pose (only x, y, and yaw are captured)
# pose: if (reset_pose == true), then this pose is captured to reset hector's pose
# - - - - - Outputs - - - - - 
# success: boolean indicating whether the service call was successful
# -------------------------------------------------------------
bool pause
bool reset_map
bool reset_pose
geometry_msgs/Pose pose
---
bool success

jan0145 · 2021-07-09T18:11:00Z

This maybe a dumb question but I made all the changes and now I'm wondering what button to push or command to write to pause Hector slam. (I'm not too familiar with cpp).

marcelino-pensa · 2021-07-09T21:05:19Z

If you're okay with using the command line, then here are some commands that you can use:

Pause mapping:
rosservice call /pause_mapping "data: true"
Resume mapping:
rosservice call /pause_mapping "data: false"
Reset map:
rosservice call /reset_map "{}"
Restart map and set new initial pose for hector:

rosservice call /restart_mapping_with_new_pose "initial_pose:
  position:
    x: 0.0
    y: 0.0
    z: 0.0
  orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0"

marcelino-pensa · 2021-07-09T21:06:12Z

If you want to have it in your code, you could either incorporate system calls to the commands above, or (if you want to do it right) setup a ROS service client. Here is the ROS reference for that: http://wiki.ros.org/ROS/Tutorials/WritingServiceClient%28c%2B%2B%29

jan0145 · 2021-07-10T05:52:32Z

Thanks ! I’ll give it a try. Jan

…

On 9 Jul 2021, at 23:05, Marcelino Almeida ***@***.***> wrote: If you're okay with using the command line, then here are some commands that you can use: Pause mapping: rosservice call /pause_mapping "data: true" Resume mapping: rosservice call /pause_mapping "data: false" Reset map: rosservice call /reset_map "{}" Restart map and set new initial pose for hector: rosservice call /restart_mapping_with_new_pose "initial_pose: position: x: 0.0 y: 0.0 z: 0.0 orientation: x: 0.0 y: 0.0 z: 0.0 w: 1.0" — You are receiving this because you commented. Reply to this email directly, view it on GitHub <https://emea01.safelinks.protection.outlook.com/?url=https%3A%2F%2Fgithub.com%2Ftu-darmstadt-ros-pkg%2Fhector_slam%2Fpull%2F86%23issuecomment-877455674&data=04%7C01%7C%7C8d8ee08f3dfe43968dfe08d9431d495d%7C84df9e7fe9f640afb435aaaaaaaaaaaa%7C1%7C0%7C637614615324620482%7CUnknown%7CTWFpbGZsb3d8eyJWIjoiMC4wLjAwMDAiLCJQIjoiV2luMzIiLCJBTiI6Ik1haWwiLCJXVCI6Mn0%3D%7C1000&sdata=KFu8b1NiP7a1ff4lYMumiwSVoqKR%2FcfalHLzS3LDZpc%3D&reserved=0>, or unsubscribe <https://emea01.safelinks.protection.outlook.com/?url=https%3A%2F%2Fgithub.com%2Fnotifications%2Funsubscribe-auth%2FAC7WRB3CLOPGFLXVZRSR343TW5QBVANCNFSM4Y4QDYMQ&data=04%7C01%7C%7C8d8ee08f3dfe43968dfe08d9431d495d%7C84df9e7fe9f640afb435aaaaaaaaaaaa%7C1%7C0%7C637614615324630470%7CUnknown%7CTWFpbGZsb3d8eyJWIjoiMC4wLjAwMDAiLCJQIjoiV2luMzIiLCJBTiI6Ik1haWwiLCJXVCI6Mn0%3D%7C1000&sdata=xAKnuZkwiVSfLHMOdtDK%2BxBmYYXROlTPpwHJTa70Vis%3D&reserved=0>.

StefanFabian · 2021-07-10T10:06:32Z

If you want to have it in your code, you could either incorporate system calls to the commands above, or setup a ROS service client. Here is the ROS reference for that: http://wiki.ros.org/ROS/Tutorials/WritingServiceClient%28c%2B%2B%29

Please absolutely never use system calls in C++.

Add pause and reset services to Hector

b72b607

marcelino-pensa commented Mar 9, 2021

View reviewed changes

marcelino-pensa added 2 commits March 9, 2021 16:07

Populate results in pauseMapCallback

051a6bb

remove newline

21d7e98

marcelino-pensa commented Mar 9, 2021

View reviewed changes

hector_mapping/src/HectorMappingRos.cpp Outdated Show resolved Hide resolved

StefanFabian requested changes Mar 12, 2021

View reviewed changes

marcelino-pensa added 2 commits March 12, 2021 10:45

rename variables as snake_case

dd7ce1c

remove HectorSM from returned message

db83fc8

marcelino-pensa requested a review from StefanFabian March 12, 2021 16:49

StefanFabian reviewed Mar 15, 2021

View reviewed changes

hector_mapping/src/HectorMappingRos.cpp Outdated Show resolved Hide resolved

Do not return messages in pause service request

e892da4

marcelino-pensa requested a review from StefanFabian March 15, 2021 17:38

StefanFabian approved these changes Mar 15, 2021

View reviewed changes

StefanFabian merged commit 50eadc5 into tu-darmstadt-ros-pkg:noetic-devel Mar 15, 2021

marcelino-pensa deleted the feature/ma-add-pause-reset-services-hector branch March 15, 2021 19:56

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add pause and reset services to hector_mapping #86

Add pause and reset services to hector_mapping #86

marcelino-pensa commented Mar 9, 2021 •

edited

Loading

marcelino-pensa Mar 9, 2021

marcelino-pensa Mar 9, 2021

marcelino-pensa Mar 9, 2021

marcelino-pensa Mar 9, 2021

marcelino-pensa Mar 9, 2021

marcelino-pensa Mar 9, 2021

marcelino-pensa Mar 9, 2021

marcelino-pensa Mar 9, 2021

marcelino-pensa Mar 9, 2021

StefanFabian Mar 12, 2021

marcelino-pensa Mar 12, 2021

marcelino-pensa Mar 9, 2021

StefanFabian Mar 12, 2021

StefanFabian commented Mar 12, 2021

marcelino-pensa commented Mar 12, 2021

StefanFabian commented Mar 15, 2021

marcelino-pensa commented Mar 16, 2021

jan0145 commented Jul 9, 2021

marcelino-pensa commented Jul 9, 2021

marcelino-pensa commented Jul 9, 2021 •

edited

Loading

jan0145 commented Jul 10, 2021 via email

StefanFabian commented Jul 10, 2021

		bool resetMapCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
		bool pauseMapCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res);

		ros::ServiceServer resetMapService_;
		ros::ServiceServer toggleScanProcessingService_;

		#include <std_srvs/SetBool.h>
		#include <std_srvs/Trigger.h>

Add pause and reset services to hector_mapping #86

Add pause and reset services to hector_mapping #86

Conversation

marcelino-pensa commented Mar 9, 2021 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

StefanFabian commented Mar 12, 2021

marcelino-pensa commented Mar 12, 2021

StefanFabian commented Mar 15, 2021

marcelino-pensa commented Mar 16, 2021

jan0145 commented Jul 9, 2021

marcelino-pensa commented Jul 9, 2021

marcelino-pensa commented Jul 9, 2021 • edited Loading

jan0145 commented Jul 10, 2021 via email

StefanFabian commented Jul 10, 2021

marcelino-pensa commented Mar 9, 2021 •

edited

Loading

marcelino-pensa commented Jul 9, 2021 •

edited

Loading