intraprocess bond::Bond::BondStatusCB use after free

nav2_costmap_2d/src/costmap_2d_cloud.cpp

@@ -193,15 +193,13 @@ void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid)
   pcl_header.frame_id = frame_id;
   pcl_header.stamp = stamp;
 
-  if (pub_marked->get_subscription_count() > 0)
-  {
+  if (pub_marked->get_subscription_count() > 0) {
     auto cloud = std::make_unique<sensor_msgs::msg::PointCloud2>();
     pointCloud2Helper(cloud, num_marked, pcl_header, g_marked);
     pub_marked->publish(std::move(cloud));
   }
 
-  if (pub_unknown->get_subscription_count() > 0)
-  {
+  if (pub_unknown->get_subscription_count() > 0) {
     auto cloud = std::make_unique<sensor_msgs::msg::PointCloud2>();
     pointCloud2Helper(cloud, num_unknown, pcl_header, g_unknown);
     pub_unknown->publish(std::move(cloud));

nav2_docking/opennav_docking/test/test_docking_server.py

@@ -17,29 +17,47 @@
 import unittest
 
 from action_msgs.msg import GoalStatus
+from ament_index_python.packages import get_package_prefix
 from geometry_msgs.msg import TransformStamped, Twist, TwistStamped
 from launch import LaunchDescription
+from launch.actions import (
+    LogInfo,
+    SetEnvironmentVariable,
+)
 from launch_ros.actions import Node
 import launch_testing
+import launch_testing.actions
+import launch_testing.asserts
+import launch_testing.markers
+import launch_testing.util
 from nav2_msgs.action import DockRobot, NavigateToPose, UndockRobot
 import pytest
 import rclpy
-from rclpy.action import ActionClient, ActionServer
+from rclpy.action.client import ActionClient
+from rclpy.action.server import ActionServer
+import rclpy.duration
+import rclpy.time
 from sensor_msgs.msg import BatteryState
-from tf2_ros import TransformBroadcaster
+from tf2_ros import Buffer, TransformBroadcaster, TransformListener
 
 
 # This test can be run standalone with:
 # python3 -u -m pytest test_docking_server.py -s
 
 @pytest.mark.rostest
+@launch_testing.markers.keep_alive
 def generate_test_description():
-
-    return LaunchDescription([
-        Node(
+    tmux_gdb_prefix = (
+        'tmux split-window '
+        + get_package_prefix('nav2_bringup')
+        + '/lib/nav2_bringup/gdb_tmux_splitwindow.sh'
+    )
+    logme = LogInfo(msg=f'tmux_gdb_prefix={tmux_gdb_prefix}')
+    docking_server = Node(
             package='opennav_docking',
             executable='opennav_docking',
             name='docking_server',
+            # arguments=['--ros-args', '--log-level', 'debug'],
             parameters=[{'wait_charge_timeout': 1.0,
                          'controller': {
                              'use_collision_detection': False,
@@ -56,17 +74,42 @@ def generate_test_description():
                              'pose': [10.0, 0.0, 0.0]
                          }}],
             output='screen',
-        ),
-        Node(
+        )
+    lifecycle_manager_navigation = Node(
             package='nav2_lifecycle_manager',
             executable='lifecycle_manager',
             name='lifecycle_manager_navigation',
             output='screen',
+            # prefix=tmux_gdb_prefix,
             parameters=[{'autostart': True},
                         {'node_names': ['docking_server']}]
-        ),
+        )
+
+    return LaunchDescription([
+        SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
+        SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
+        logme,
+        docking_server,
+        lifecycle_manager_navigation,
+        # In tests where all of the procs under tests terminate themselves, it's necessary
+        # to add a dummy process not under test to keep the launch alive. launch_test
+        # provides a simple launch action that does this:
+        launch_testing.util.KeepAliveProc(),
         launch_testing.actions.ReadyToTest(),
-    ])
+    ]), locals()
+
+
+@launch_testing.post_shutdown_test()
+class TestPostShutdown(unittest.TestCase):
+
+    def test_action_graceful_shutdown(self,
+                                      proc_info,
+                                      docking_server,
+                                      lifecycle_manager_navigation):
+        """Test graceful shutdown."""
+        launch_testing.asserts.assertExitCodes(proc_info, process=docking_server)
+        launch_testing.asserts.assertExitCodes(proc_info,
+                                               process=lifecycle_manager_navigation)
 
 
 class TestDockingServer(unittest.TestCase):
@@ -142,9 +185,39 @@ def nav_execute_callback(self, goal_handle):
         result.error_code = 0
         return result
 
+    def check_transform(self, timeout_sec, source_frame='odom', target_frame='base_link'):
+        """Try to look up the transform we're publishing."""
+        if self.transform_available:
+            return True
+
+        try:
+            # Wait for transform to become available with timeout
+            when = rclpy.time.Time()
+            transform = self.tf_buffer.lookup_transform(
+                target_frame,
+                source_frame,
+                when,
+                timeout=rclpy.duration.Duration(seconds=timeout_sec)
+            )
+
+            self.node.get_logger().info('Successfully found transform:')
+            self.node.get_logger().info(f'Translation: [{transform.transform.translation.x}, '
+                                        f'{transform.transform.translation.y}, '
+                                        f'{transform.transform.translation.z}]')
+            self.transform_available = True
+        except Exception as e:
+            self.node.get_logger().error(f'Error looking up transform: {str(e)}')
+            self.transform_available = False
+
+        return self.transform_available
+
     def test_docking_server(self):
         # Publish TF for odometry
         self.tf_broadcaster = TransformBroadcaster(self.node)
+        # Create tf buffer and listener
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self.node)
+        self.transform_available = False
 
         # Create a timer to run "control loop" at 20hz
         self.timer = self.node.create_timer(0.05, self.timer_callback)
@@ -175,20 +248,30 @@ def test_docking_server(self):
             'navigate_to_pose',
             self.nav_execute_callback)
 
-        # Spin once so that TF is published
-        rclpy.spin_once(self.node, timeout_sec=0.1)
+        # Publish transform
+        self.publish()
+
+        # Run for 2 seconds to allow tf to propogate
+        for i in range(20):
+            rclpy.spin_once(self.node, timeout_sec=0.1)
+            time.sleep(0.1)
+
+        assert self.check_transform(timeout_sec=1.0), 'transform not available'
 
         # Test docking action
         self.action_result = []
-        self.dock_action_client.wait_for_server(timeout_sec=5.0)
+        assert self.dock_action_client.wait_for_server(timeout_sec=5.0), \
+               'dock_robot service not available'
+
         goal = DockRobot.Goal()
         goal.use_dock_id = True
         goal.dock_id = 'test_dock'
         future = self.dock_action_client.send_goal_async(
             goal, feedback_callback=self.action_feedback_callback)
         rclpy.spin_until_future_complete(self.node, future)
         self.goal_handle = future.result()
-        assert self.goal_handle.accepted
+        assert self.goal_handle is not None, 'goal_handle should not be None'
+        assert self.goal_handle.accepted, 'goal not accepted'
         result_future_original = self.goal_handle.get_result_async()
 
         # Run for 2 seconds

nav2_graceful_controller/include/nav2_graceful_controller/utils.hpp

@@ -15,6 +15,8 @@
 #ifndef NAV2_GRACEFUL_CONTROLLER__UTILS_HPP_
 #define NAV2_GRACEFUL_CONTROLLER__UTILS_HPP_
 
+#include <memory>
+
 #include "geometry_msgs/msg/point_stamped.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "visualization_msgs/msg/marker.hpp"

nav2_lifecycle_manager/test/test_bond.cpp

@@ -90,7 +90,7 @@ class TestLifecycleNode : public nav2_util::LifecycleNode
 
   void breakBond()
   {
-    bond_->breakBond();
+    bond_.reset();
   }
 
   std::string getState()
@@ -170,7 +170,7 @@ TEST(LifecycleBondTest, POSITIVE)
 
 TEST(LifecycleBondTest, NEGATIVE)
 {
-  auto node = std::make_shared<rclcpp::Node>("lifecycle_manager_test_service_client");
+  auto node = nav2_util::generate_internal_node("lifecycle_manager_test_service_client");
   nav2_lifecycle_manager::LifecycleManagerClient client("lifecycle_manager_test", node);
 
   // create node, now without bond setup to connect to. Should fail because no bond

nav2_util/include/nav2_util/lifecycle_node.hpp

@@ -210,7 +210,7 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode
   void runCleanups();
 
   // Connection to tell that server is still up
-  std::unique_ptr<bond::Bond> bond_{nullptr};
+  std::shared_ptr<bond::Bond> bond_{nullptr};
   double bond_heartbeat_period;
   rclcpp::TimerBase::SharedPtr autostart_timer_;
 };

nav2_util/src/lifecycle_node.cpp

@@ -73,7 +73,7 @@ void LifecycleNode::createBond()
   if (bond_heartbeat_period > 0.0) {
     RCLCPP_INFO(get_logger(), "Creating bond (%s) to lifecycle manager.", this->get_name());
 
-    bond_ = std::make_unique<bond::Bond>(
+    bond_ = std::make_shared<bond::Bond>(
       std::string("bond"),
       this->get_name(),
       shared_from_this());
@@ -149,7 +149,7 @@ void LifecycleNode::destroyBond()
     RCLCPP_INFO(get_logger(), "Destroying bond (%s) to lifecycle manager.", this->get_name());
 
     if (bond_) {
-      bond_.reset();
+      bond_->breakBond();
     }
   }
 }

tools/underlay.repos

@@ -11,18 +11,18 @@ repositories:
   #   type: git
   #   url: https://github.com/ros-perception/vision_opencv.git
   #   version: rolling
-  # ros/bond_core:
-  #   type: git
-  #   url: https://github.com/ros/bond_core.git
-  #   version: ros2
+  ros/bond_core:
+    type: git
+    url: https://github.com/ewak/bond_core.git
+    version: feature/mw/memory_safe_subscription_callback
   # ros/diagnostics:
   #   type: git
   #   url: https://github.com/ros/diagnostics.git
-  #   version: ros2-devel   
+  #   version: ros2-devel
   # ros/geographic_info:
   #   type: git
   #   url: https://github.com/ros-geographic-info/geographic_info.git
-  #   version: ros2      
+  #   version: ros2
   # ompl/ompl:
   #   type: git
   #   url: https://github.com/ompl/ompl.git