Add reset mapping (and pose) service #87

marcelino-pensa · 2021-03-18T23:57:35Z

This PR introduces a new service (to rule them all) for managing hector in a single service call in terms of:

Resetting map
Resetting pose
Pause/unpause mapping

Note: This PR is backward-compatible, as it doesn't change any behavior for code that interfaces with hector_mapping, it only adds a new service that can be used when interfacing with it

closes #88

marcelino-pensa

left personal comments to aid in the review process

marcelino-pensa · 2021-03-18T23:57:51Z

hector_mapping/CMakeLists.txt

+## Generate services in the 'srv' folder
+add_service_files(
+  FILES
+  HectorManagement.srv


new service to manage hector processing

marcelino-pensa · 2021-03-18T23:58:44Z

hector_mapping/CMakeLists.txt

 generate_messages(
  DEPENDENCIES
+  geometry_msgs


the service above has geometry_msgs::Pose as an argument, so it needs to use geometry_msgs as a dependency

marcelino-pensa · 2021-03-18T23:59:17Z

hector_mapping/include/hector_slam_lib/util/UtilFunctions.h

+static double getYawFromQuat(const geometry_msgs::Quaternion &quat)
+{
+  return tf::getYaw(tf::Quaternion(quat.x, quat.y, quat.z, quat.w));
+}


added this new utility function that helps getting yaw from a quaternion... this is useful when resetting pose

marcelino-pensa · 2021-03-18T23:59:26Z

hector_mapping/src/HectorMappingRos.cpp

@@ -157,7 +157,8 @@ HectorMappingRos::HectorMappingRos()
      tmp.dynamicMapServiceServer_ = node_.advertiseService("dynamic_map", &HectorMappingRos::mapCallback, this);
    }

-    // Initialize services to reset map, and toggle scan pause
+    // Initialize services
+    hector_management_service_ = node_.advertiseService("hector_management", &HectorMappingRos::hectorManagementCallback, this);


new service declaration

marcelino-pensa · 2021-03-18T23:59:40Z

hector_mapping/src/HectorMappingRos.cpp

+                                                hector_mapping::HectorManagement::Response &res)
+{
+  // Pause/unpause
+  this->toggleMappingPause(req.pause);


call function to toggle mapping

marcelino-pensa · 2021-03-19T00:00:55Z

hector_mapping/src/HectorMappingRos.cpp

-    ROS_INFO("HectorSM Mapping no longer paused");
-  }
-  pause_scan_processing_ = req.data;
+  this->toggleMappingPause(req.data);


because this chunk of code would be identical in restartHectorCallback and pauseMapCallback, I created the function toggleMappingPause that has the very same chunk of code

marcelino-pensa · 2021-03-19T00:01:55Z

hector_mapping/src/HectorMappingRos.cpp

-  tf::poseMsgToTF(msg->pose.pose, pose);
-  initial_pose_ = Eigen::Vector3f(msg->pose.pose.position.x, msg->pose.pose.position.y, tf::getYaw(pose.getRotation()));
-  ROS_INFO("Setting initial pose with world coords x: %f y: %f yaw: %f", initial_pose_[0], initial_pose_[1], initial_pose_[2]);
+  this->resetPose(msg->pose.pose);


because this chunk of code would be identical in restartHectorCallback and initialPoseCallback, I created the function resetPose that contains that chunk of code

marcelino-pensa · 2021-03-19T00:02:29Z

hector_mapping/src/HectorMappingRos.cpp

+{
+  // Pause/unpause
+  if (pause && !pause_scan_processing_)
+  {
+    ROS_INFO("[HectorSM]: Mapping paused");
+  }
+  else if (!pause && pause_scan_processing_)
+  {
+    ROS_INFO("[HectorSM]: Mapping no longer paused");
+  }
+  pause_scan_processing_ = pause;


this is the exact same code that was in pauseMapCallback, just brought it here

marcelino-pensa · 2021-03-19T00:03:56Z

hector_mapping/src/HectorMappingRos.cpp

+void HectorMappingRos::resetPose(const geometry_msgs::Pose &pose)
+{
+  initial_pose_set_ = true;
+  initial_pose_ = Eigen::Vector3f(pose.position.x, pose.position.y, util::getYawFromQuat(pose.orientation));
+  ROS_INFO("[HectorSM]: Setting initial pose with world coords x: %f y: %f yaw: %f",
+           initial_pose_[0], initial_pose_[1], initial_pose_[2]);
+}


this is an adaptation of what was in initialPoseCallback. The difference is that I made use of util::getYawFromQuat instead of declaring the following variable to extract yaw:

tf::Pose pose; tf::poseMsgToTF(msg->pose.pose, pose);

marcelino-pensa · 2021-03-19T00:05:36Z

hector_mapping/srv/HectorManagement.srv

@@ -0,0 +1,16 @@
+# Service definition for hector processing management


these comments make the service definition self-explanatory

StefanFabian · 2021-03-19T08:40:51Z

Thank you for your PR!
However, I think that pause and reset functionality are too semantically distinct for a shared service.
I also don't really see the advantage over calling the individual services separately?

marcelino-pensa · 2021-03-19T15:56:04Z

@StefanFabian thank you for your review!
I don't know if it will always be useful for other people, but it certainly is useful for my team! The reason for that is that we do the following in our pipeline:

1) pause for a while (while we don't need hector or in areas where we determine that hector does not work well
2) reset map -> reset pose -> unpause

So the combination of these functionalities is rather useful for unpausing than for pausing, since we will always reset map and reset pose when we unpause.
Of course, we could call three services in a row, but take a look at our interface:

bool HectorInterface(const bool &pause,
                                   const bool &reset_map,
                                   const bool &reset_pose,
                                   const geometry_msgs::Pose &pose) {
  hector_mapping::HectorManagement hector_management_msg;
  hector_management_msg.request.pause = pause;
  hector_management_msg.request.reset_map = reset_map;
  hector_management_msg.request.reset_pose = reset_pose;
  hector_management_msg.request.pose = pose;

  if (!hector_management_client_.call(hector_management_msg)) {
    ROS_WARN("Could not call hector management service");
    return false;
  }
  if (hector_management_msg.response.success == false) {
    ROS_WARN("Service \"%s\" failed to set hector management parameters",
             hector_management_client_.getService().c_str());
    return false;
  }
  ROS_INFO("Service \"%s\" set hector management parameters",
            hector_management_client_.getService().c_str());
  return true;
}

If we are to call three separate services, we would have to have three service clients, check whether the service call succeeded three times, and check the return value three times. It certainly is doable, just not as concise

StefanFabian · 2021-03-19T16:21:33Z

Hm, I would be okay with having a restart service that as a side-effect always unpauses.
I think having a service that resets the map and sets the initial pose would be a good addition since otherwise it is not guaranteed that the initial pose is set when the map resets since the message could get lost or arrive after the next scan.

marcelino-pensa · 2021-03-19T19:50:53Z

hector_mapping/src/HectorMappingRos.cpp

-    // 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);


my bad in the previous PR: I had placed this inside a for loop, so I'm bringing it out of that loop

marcelino-pensa · 2021-03-19T19:52:01Z

hector_mapping/src/HectorMappingRos.cpp

    setServiceGetMapData(tmp.map_, slamProcessor->getGridMap(i));

    if ( i== 0){
      mapPubContainer[i].mapMetadataPublisher_.publish(mapPubContainer[i].map_.map.info);
    }
  }

+  // Initialize services
+  reset_map_service_ = node_.advertiseService("reset_map", &HectorMappingRos::resetMapCallback, this);
+  restart_hector_service_ = node_.advertiseService("restart_hector", &HectorMappingRos::restartHectorCallback, this);


new service that:

resets the map

resets the initial pose

unpauses the execution

Any suggestions of a better name for the service? Or is restart_hector good enough?

marcelino-pensa · 2021-03-19T19:52:14Z

hector_mapping/src/HectorMappingRos.cpp

@@ -389,18 +390,28 @@ bool HectorMappingRos::resetMapCallback(std_srvs::Trigger::Request  &req,
  return true;
 }

+bool HectorMappingRos::restartHectorCallback(hector_mapping::SetInitialPose::Request  &req,


self-explanatory callback

marcelino-pensa · 2021-03-19T19:55:38Z

@StefanFabian I have implemented as per your request! Let me know if there is anything else you'd want changed. If approved, I'll move on to update the wiki

StefanFabian · 2021-03-20T12:04:12Z

Thank you for the update.
Looks good to me except for two minor changes.
The name of the service should make clear what is going to happen, so I'd prefer if it was called something along the lines of ResetMapping with a request member inital_pose.
I also don't get why some services (also in std_srvs) have a response member success since services always return a boolean indicating whether the request was successful.
Also, I don't think it makes sense to pass a bool as a const reference ;)

marcelino-pensa · 2021-03-22T20:28:50Z

@StefanFabian

I have updated the service name to restart_mapping_with_new_pose
I think that returning success = true is poor ROS design, as the success of the call will capture the returned value. I have seen software that never returns return true, some that always return return true even when success = false, some that never populate success, so it's a wild world out there. Since it sounds like you'd prefer not to have the success entry in the service response, I have removed it
Some developers are quite adamant that all function inputs are const & (for standardization), but I have removed the reference to match your preference

I hope these updates address all your concerns! Please let me know if you'd like to suggest any more improvements

StefanFabian · 2021-03-23T10:58:38Z

Looks like we had a misunderstanding. I meant the service msg SetInitialPose should be called something like ResetMapping since SetInitialPose doesn't tell me that it will restart as well.

I think that returning success = true is poor ROS design, as the success of the call will capture the returned value. I have seen software that never returns return true, some that always return return true even when success = false, some that never populate success, so it's a wild world out there. Since it sounds like you'd prefer not to have the success entry in the service response, I have removed it

There are use cases where it may make sense since a service call will also return false if it couldn't reach the service server but I think in our case, the user is only interested in whether the call succeeded or not.

Some developers are quite adamant that all function inputs are const & (for standardization), but I have removed the reference to match your preference

The example you're linked is just because of the template specialization. Const references are passed for better performance but in the case of simple value types, the reference is often larger than the value type so it makes no sense to not simply pass a copy.

marcelino-pensa · 2021-03-23T18:54:51Z

I have renamed the service definition, but I don't fully agree with your reasoning.
Using the name ResetMapping prevents the service definition from being used for other purposes.
For instance if you wanted to have a service reset_robot_pose that does not reset the map, you could use the same .srv definition without having to create another one.
As another example std_srvs::Trigger does not indicate that it will trigger a Hector map reset, but it can still be used in a service reset_map that uses a std_srvs::Trigger type.

In any case, let me know if you'd want any further updates in this PR, I'll be happy to do it =)

StefanFabian · 2021-03-24T10:13:15Z

I have renamed the service definition, but I don't fully agree with your reasoning.
Using the name ResetMapping prevents the service definition from being used for other purposes.

Yes, that is the intention. ROS messages and services should have a semantic meaning which means it is intended that you may have to define another message with the exact same members for a different purpose.
See for example sensor_msgs which has separate messages for Temperature and FluidPressure with the exact same structure.

Anyway, I have changed the bool parameter from const reference to pass by value and it's good to merge now.
Thank you for your work on this and for quickly addressing my concerns ;)

marcelino-pensa · 2021-03-24T15:31:05Z

Thank you very much, @StefanFabian . My bad on the boolean reference, I really thought I had pushed a change to remove the reference.
BTW, I have just updated the wiki with the new service type

* add service to reset the mapping with a new initial pose

marcelino-pensa added 2 commits March 18, 2021 18:53

add service for hector management

a25464b

add newline to end of file

1b62bef

marcelino-pensa commented Mar 19, 2021

View reviewed changes

add service to restart hector

e97c9ee

marcelino-pensa commented Mar 19, 2021

View reviewed changes

delete unused variable

3cb9d5c

marcelino-pensa added 2 commits March 22, 2021 12:38

update service name

47a89e2

remove success boolean from service output

b1ceb93

Rename service type

115962f

marcelino-pensa and others added 4 commits March 23, 2021 14:21

change service definition entry from 'pose' to 'initial_pose'

5308799

update service entry in code

3332f13

No const reference for value type in cpp.

134d931

No const reference for value type in header.

bb0d478

StefanFabian changed the title ~~Add service for hector management~~ Add reset mapping (and pose) service Mar 24, 2021

StefanFabian merged commit c94cd29 into tu-darmstadt-ros-pkg:noetic-devel Mar 24, 2021

marcelino-pensa deleted the feature/ma-add-hector-management-service branch March 24, 2021 15:28

r1b4z01d pushed a commit to r1b4z01d/hector_slam that referenced this pull request Mar 5, 2022

Add reset mapping (and pose) service (tu-darmstadt-ros-pkg#87)

815d8fd

* add service to reset the mapping with a new initial pose

StefanFabian pushed a commit that referenced this pull request Mar 11, 2022

Add reset mapping (and pose) service (#87)

c83e8d7

* add service to reset the mapping with a new initial pose

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add reset mapping (and pose) service #87

Add reset mapping (and pose) service #87

marcelino-pensa commented Mar 18, 2021 •

edited

Loading

marcelino-pensa left a comment

marcelino-pensa Mar 18, 2021

marcelino-pensa Mar 18, 2021

marcelino-pensa Mar 18, 2021

marcelino-pensa Mar 18, 2021

marcelino-pensa Mar 18, 2021

marcelino-pensa Mar 19, 2021 •

edited

Loading

marcelino-pensa Mar 19, 2021 •

edited

Loading

marcelino-pensa Mar 19, 2021

marcelino-pensa Mar 19, 2021

marcelino-pensa Mar 19, 2021

StefanFabian commented Mar 19, 2021

marcelino-pensa commented Mar 19, 2021 •

edited

Loading

StefanFabian commented Mar 19, 2021

marcelino-pensa Mar 19, 2021

marcelino-pensa Mar 19, 2021

marcelino-pensa Mar 19, 2021

marcelino-pensa commented Mar 19, 2021

StefanFabian commented Mar 20, 2021

marcelino-pensa commented Mar 22, 2021

StefanFabian commented Mar 23, 2021

marcelino-pensa commented Mar 23, 2021

StefanFabian commented Mar 24, 2021

marcelino-pensa commented Mar 24, 2021

		@@ -0,0 +1,16 @@
		# Service definition for hector processing management

Add reset mapping (and pose) service #87

Add reset mapping (and pose) service #87

Conversation

marcelino-pensa commented Mar 18, 2021 • edited Loading

marcelino-pensa left a 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

marcelino-pensa Mar 19, 2021 • edited Loading

Choose a reason for hiding this comment

marcelino-pensa Mar 19, 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

StefanFabian commented Mar 19, 2021

marcelino-pensa commented Mar 19, 2021 • edited Loading

StefanFabian commented Mar 19, 2021

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

marcelino-pensa commented Mar 19, 2021

StefanFabian commented Mar 20, 2021

marcelino-pensa commented Mar 22, 2021

StefanFabian commented Mar 23, 2021

marcelino-pensa commented Mar 23, 2021

StefanFabian commented Mar 24, 2021

marcelino-pensa commented Mar 24, 2021

marcelino-pensa commented Mar 18, 2021 •

edited

Loading

marcelino-pensa Mar 19, 2021 •

edited

Loading

marcelino-pensa Mar 19, 2021 •

edited

Loading

marcelino-pensa commented Mar 19, 2021 •

edited

Loading