diff --git a/nav2_costmap_2d/src/costmap_2d_cloud.cpp b/nav2_costmap_2d/src/costmap_2d_cloud.cpp index 5aa78255e72..1119c508273 100644 --- a/nav2_costmap_2d/src/costmap_2d_cloud.cpp +++ b/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(); 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(); pointCloud2Helper(cloud, num_unknown, pcl_header, g_unknown); pub_unknown->publish(std::move(cloud)); diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py index 43e3e2df329..6adddd4405a 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server.py +++ b/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,12 +248,21 @@ 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' @@ -188,7 +270,8 @@ def test_docking_server(self): 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 diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/utils.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/utils.hpp index 062edbf4242..2bc65fae444 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/utils.hpp +++ b/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 + #include "geometry_msgs/msg/point_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "visualization_msgs/msg/marker.hpp" diff --git a/nav2_lifecycle_manager/test/test_bond.cpp b/nav2_lifecycle_manager/test/test_bond.cpp index 38c772e6b1b..fb0fa15f9b3 100644 --- a/nav2_lifecycle_manager/test/test_bond.cpp +++ b/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("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 diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index a838fae2dbd..0ed25f8f1b7 100644 --- a/nav2_util/include/nav2_util/lifecycle_node.hpp +++ b/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_{nullptr}; + std::shared_ptr bond_{nullptr}; double bond_heartbeat_period; rclcpp::TimerBase::SharedPtr autostart_timer_; }; diff --git a/nav2_util/src/lifecycle_node.cpp b/nav2_util/src/lifecycle_node.cpp index 49fdf51ec70..eb0b993476d 100644 --- a/nav2_util/src/lifecycle_node.cpp +++ b/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_ = std::make_shared( 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(); } } } diff --git a/tools/underlay.repos b/tools/underlay.repos index 005f11ec3f0..97de32c5e7d 100644 --- a/tools/underlay.repos +++ b/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