Add pose graph & node event publisher #809

hyunsehyun · 2025-10-17T05:26:44Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#803
Primary OS tested on	Ubuntu 22.04
Robotic platform tested on	Turtlebot3 Burger (gazebo simulation)

Description of contribution in a few bullet points

Loop-closure event publisher
- Registers a karto::MapperLoopClosureListener and publishes /slam_toolbox/loop_closure_event when Karto signals EndLoopClosure.
- The message carries only a timestamp.
New-node event publisher
- Publishes /slam_toolbox/new_node_event when a new node is generated.
Asynchronous pose-graph publish on loop closure and on new-node
- After emitting the loop-closure event, asynchronously publishes /slam_toolbox/pose_graph once the internal mapping mutex becomes available, instead of invoking it directly inside the callback.
- This ensures non-blocking and thread-safe behavior while keeping the publication nearly real-time.
- Also performs the same asynchronous publish when a new node is added.

Description of documentation updates required from your changes

Documentation updated

Future work that may be required in bullet points

N/A

Performance Metrics

Metric	Value / Observation
Pose-graph message size (50 nodes)	8.10 KB
Pose-graph message size (100 nodes)	15.88 KB
Pose-graph message size (150 nodes)	23.96 KB

1.mp4

Question for the Maintainer

I noticed that all other ROS distro branches (e.g., Foxy, Galactic, Rolling) use the lifecycle version,
so I made these changes on the humble_lifecycle branch for consistency.
Is this the correct branch to work on?

Also, could you please clarify why only the Humble version has two separate branches
(humble and humble_lifecycle), while the others maintain just a single lifecycle version?

SteveMacenski · 2025-10-21T00:48:03Z

This should be set to target ros2 so that rolling and all future distributions will obtain this change

SteveMacenski · 2025-10-21T00:49:24Z

msg/GraphNode.msg

@@ -0,0 +1,2 @@
+int32 node_id
+geometry_msgs/Pose2D pose


This is deprecated in ROS 2, please use a SE2 pose

We'll fix this.

SteveMacenski · 2025-10-21T00:49:30Z

msg/GraphEdge.msg

@@ -0,0 +1,4 @@
+int32 source_id
+int32 target_id
+geometry_msgs/Pose2D relative_pose


This is deprecated in ROS 2, please use a SE2 pose

We'll fix this.

SteveMacenski · 2025-10-21T00:49:48Z

msg/NewNodeEvent.msg

@@ -0,0 +1,3 @@
+int32 new_node_id
+builtin_interfaces/Time stamp
+geometry_msgs/Pose2D pose


This is deprecated in ROS 2, please use a SE2 pose

We'll fix this.

SteveMacenski · 2025-10-21T00:53:39Z

src/slam_toolbox_common.cpp

+}
+
+/*****************************************************************************/
+void SlamToolbox::poseGraphPublishTimerCallback()


I think you can have this done in the publishVisualizations callback like we have TF and the map occ grids themselves

@SteveMacenski As far as I understand, publishVisualizations() runs on a fixed interval (please correct me if I'm wrong). Since the pose_graph message can be quite large, we’d like to avoid publishing it periodically.

In our PR, we’re checking publish_pose_graph_requested_ flag inside poseGraphPublishTimerCallback, and only publish the pose_graph when the flag is set, for example after a loop closure event.
This allows us to publish the pose_graph only when a loop closure occurs. We chose to use this flag-based approach because occasionally we fail to lock the smapper_mutex_.

Please let me know if I’ve misunderstood your comment!

Oh, another option could be to check the flag inside publishVisualizations. That way, we wouldn't need additional poseGraphPublishTimerCallback. Is this what you meant?

Since the pose_graph message can be quite large, we’d like to avoid publishing it periodically.

I guess I'm suggesting checking on the atomic bool if we have an update to publish only if needed -- but to do that operation here instead of having the timer. Yes :-)

SteveMacenski · 2025-10-21T00:54:23Z

src/slam_toolbox_common.cpp

      boost::bind(&SlamToolbox::publishVisualizations, this)));

+  // Create pose graph publish timer (10Hz)
+  pose_graph_timer_ = this->create_wall_timer(


That would remove this timer

SteveMacenski · 2025-10-21T00:55:37Z

src/slam_toolbox_common.cpp


    publishPose(range_scan->GetCorrectedPose(), covariance, scan->header.stamp);
+    publishNewNodeEvent(range_scan);
+    requestPoseGraphPublish();


Do you think this should happen on each new node or only when a loop closure happens? This could become quite a large thing to publish every ~second as this grows to a very large space

I think just doing the new node is enough. The full graph should wait until its needed to update other info.

If someone is subscribed to the graph and new node topics, then if a new node comes in, they can add to their graph without reinitializing everything since the rest of the graph is stable. We should only force a graph update if other non-new-nodes have changed things

I think just doing the new node is enough. The full graph should wait until its needed to update other info.

@SteveMacenski We were publishing the full pose_graph on every new node event because I thought other users might expect the topic to be updated more frequently in general cases.
However, I agree with your point. The full graph takes up a lot of space, and incremental updates using new node events alone should be sufficient. Also, handling general cases is beyond the scope of this PR.

We'll remove the full pose_graph publication from every new node event, and update the README to clarify that the pose_graph topic is only published on loop closures.

SteveMacenski · 2025-10-21T00:56:32Z

src/slam_toolbox_common.cpp

+      node_msg.pose.x = lrs->GetCorrectedPose().GetX();
+      node_msg.pose.y = lrs->GetCorrectedPose().GetY();
+      node_msg.pose.theta = lrs->GetCorrectedPose().GetHeading();
+      msg.nodes.push_back(node_msg);


If you know the size of mapper_vertices then you can reserve the size so we don't have a ton of copies from resizing

We'll fix this!

SteveMacenski · 2025-10-21T00:56:41Z

src/slam_toolbox_common.cpp

+      }
+    }
+
+    msg.edges.push_back(edge_msg);


If you know the size of mapper_edges then you can reserve the size so we don't have a ton of copies from resizing

We'll fix this!

SteveMacenski · 2025-10-21T00:57:05Z

src/slam_toolbox_common.cpp

+    msg.edges.push_back(edge_msg);
+  }
+
+  pose_graph_pub_->publish(msg);


msg should be constructed as a unique pointer and here we can std::move() to publish. Trust me, for large messages it makes a difference in performance.

We'll fix this.
I'm not very familiar with C++, so thanks for pointing that out. Really helpful!

SteveMacenski · 2025-10-21T00:58:50Z

src/slam_toolbox_common.cpp

+void SlamToolbox::loopClosureCallback()
+/*****************************************************************************/
+{
+  requestPoseGraphPublish();


You could just register this callback as the loopClosureCallback if this is all that this does

We'll fix this.

darthegg · 2025-10-22T07:01:29Z

This should be set to target ros2 so that rolling and all future distributions will obtain this change

@SteveMacenski Sorry to bother you, but could you please explain the branching policy a bit more?
Is ros2 branch intended for rolling and future ROS2 distros?
I see that ros2 is currently the default branch, and under active branches there are also jazzy, kilted, and humble.

Should I open separate PRs for each of ros2, jazzy, kilted, and humble?

SteveMacenski · 2025-10-22T17:56:18Z

humble_lifecycle is a branch off of Humble which enables lifecycle nodes which could not be backported into Humble itself since it would break existing users / applications. Enough people wanted it that I created this branch for that feature to live, but its not the main Humble branch nor is this distributed in binaries.

ros2 is the main branch (as true in rclcpp / other repos) -- though sometimes you may see it called rolling (just kind of depends on when that repo was made, we adjusted naming conventions a few times). All new work should be in ros2/rolling so that all future distributions have that contribution in it. Then and separately, we can backport either automatically or manually to existing distributions as long as it does not break ABI/API stability so that existing distributions can have that feature as well.

For now, ros2 only. Once we're done, we can assess if we can backport to various branches -- but almost certainly we cannot for Humble.

darthegg · 2025-10-23T04:32:39Z

@SteveMacenski Thanks for your detailed and kind reply! We'll make the updates based on your comments soon.

Add pose graph & node event publisher

16abb83

SteveMacenski reviewed Oct 21, 2025

View reviewed changes

Add pose graph & node event publisher #809

Are you sure you want to change the base?

Add pose graph & node event publisher #809

Uh oh!

Conversation

hyunsehyun commented Oct 17, 2025

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Future work that may be required in bullet points

Performance Metrics

Question for the Maintainer

Uh oh!

SteveMacenski commented Oct 21, 2025

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

darthegg Oct 22, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

darthegg Oct 22, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

darthegg commented Oct 22, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

SteveMacenski commented Oct 22, 2025

Uh oh!

darthegg commented Oct 23, 2025

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

darthegg Oct 22, 2025 •

edited

Loading

darthegg Oct 22, 2025 •

edited

Loading

darthegg commented Oct 22, 2025 •

edited

Loading