diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index dafec791147..2412e90a54d 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -33,6 +33,7 @@ jobs: - "nav2_bringup" - "nav2_collision_monitor" - "nav2_costmap_2d" + - "opennav_docking" steps: - uses: actions/checkout@v4 diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py index 560ad02e6f1..e96f38bb37f 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server.py +++ b/nav2_docking/opennav_docking/test/test_docking_server.py @@ -30,7 +30,7 @@ import pytest import rclpy from rclpy.action.client import ActionClient -from rclpy.action.server import ActionServer +from rclpy.action.server import ActionServer, ServerGoalHandle from sensor_msgs.msg import BatteryState from tf2_ros import TransformBroadcaster @@ -45,7 +45,7 @@ @pytest.mark.rostest # @pytest.mark.flaky # @pytest.mark.flaky(max_runs=5, min_passes=3) -def generate_test_description(): +def generate_test_description() -> LaunchDescription: return LaunchDescription([ # SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), @@ -86,14 +86,14 @@ def generate_test_description(): class TestDockingServer(unittest.TestCase): @classmethod - def setUpClass(cls): + def setUpClass(cls) -> None: rclpy.init() @classmethod - def tearDownClass(cls): + def tearDownClass(cls) -> None: rclpy.shutdown() - def setUp(self): + def setUp(self) -> None: # Create a ROS node for tests # Latest odom -> base_link self.x = 0.0 @@ -105,14 +105,14 @@ def setUp(self): self.command = Twist() self.node = rclpy.create_node('test_docking_server') - def tearDown(self): + def tearDown(self) -> None: self.node.destroy_node() - def command_velocity_callback(self, msg): - self.node.get_logger().info('Command: %f %f' % (msg.twist.linear.x, msg.twist.angular.z)) + def command_velocity_callback(self, msg: TwistStamped) -> None: + self.node.get_logger().info(f'Command: {msg.twist.linear.x:f} {msg.twist.angular.z:f}') self.command = msg.twist - def timer_callback(self): + def timer_callback(self) -> None: # Propagate command period = 0.05 self.x += cos(self.theta) * self.command.linear.x * period @@ -121,7 +121,7 @@ def timer_callback(self): # Need to publish updated TF self.publish() - def publish(self): + def publish(self) -> None: # Publish base->odom transform t = TransformStamped() t.header.stamp = self.node.get_clock().now().to_msg() @@ -140,7 +140,7 @@ def publish(self): b.current = -1.0 self.battery_state_pub.publish(b) - def action_feedback_callback(self, msg): + def action_feedback_callback(self, msg: DockRobot.Feedback) -> None: # Force the docking action to run a full recovery loop and then # make contact with the dock (based on pose of robot) before # we report that the robot is charging @@ -148,12 +148,16 @@ def action_feedback_callback(self, msg): msg.feedback.state == msg.feedback.WAIT_FOR_CHARGE: self.is_charging = True - def nav_execute_callback(self, goal_handle): + def nav_execute_callback( + self, + goal_handle: ServerGoalHandle[NavigateToPose.Goal, + NavigateToPose.Result, NavigateToPose.Feedback] + ) -> NavigateToPose.Result: goal = goal_handle.request self.x = goal.pose.pose.position.x - 0.05 self.y = goal.pose.pose.position.y + 0.05 self.theta = 2.0 * acos(goal.pose.pose.orientation.w) - self.node.get_logger().info('Navigating to %f %f %f' % (self.x, self.y, self.theta)) + self.node.get_logger().info(f'Navigating to {self.x:f} {self.y:f} {self.theta:f}') goal_handle.succeed() self.publish() @@ -162,7 +166,7 @@ def nav_execute_callback(self, goal_handle): result.error_msg = '' return result - def test_docking_server(self): + def test_docking_server(self) -> None: # Publish TF for odometry self.tf_broadcaster = TransformBroadcaster(self.node) @@ -239,8 +243,10 @@ def test_docking_server(self): self.action_result.append(result_future_original.result()) # First is aborted due to preemption - self.assertEqual(self.action_result[0].status, GoalStatus.STATUS_ABORTED) - self.assertFalse(self.action_result[0].result.success) + self.assertIsNotNone(self.action_result[0]) + if self.action_result[0] is not None: + self.assertEqual(self.action_result[0].status, GoalStatus.STATUS_ABORTED) + self.assertFalse(self.action_result[0].result.success) self.node.get_logger().info('Goal preempted') @@ -250,8 +256,10 @@ def test_docking_server(self): time.sleep(0.1) # Second is aborted due to preemption during main loop (takes down all actions) - self.assertEqual(self.action_result[1].status, GoalStatus.STATUS_ABORTED) - self.assertFalse(self.action_result[1].result.success) + self.assertIsNotNone(self.action_result[1]) + if self.action_result[1] is not None: + self.assertEqual(self.action_result[1].status, GoalStatus.STATUS_ABORTED) + self.assertFalse(self.action_result[1].result.success) # Resend the goal self.node.get_logger().info('Sending goal again') @@ -265,9 +273,11 @@ def test_docking_server(self): rclpy.spin_until_future_complete(self.node, result_future) self.action_result.append(result_future.result()) - self.assertEqual(self.action_result[2].status, GoalStatus.STATUS_SUCCEEDED) - self.assertTrue(self.action_result[2].result.success) - self.assertEqual(self.action_result[2].result.num_retries, 1) + self.assertIsNotNone(self.action_result[2]) + if self.action_result[2] is not None: + self.assertEqual(self.action_result[2].status, GoalStatus.STATUS_SUCCEEDED) + self.assertTrue(self.action_result[2].result.success) + self.assertEqual(self.action_result[2].result.num_retries, 1) # Test undocking action self.is_charging = False @@ -283,13 +293,15 @@ def test_docking_server(self): rclpy.spin_until_future_complete(self.node, result_future) self.action_result.append(result_future.result()) - self.assertEqual(self.action_result[3].status, GoalStatus.STATUS_SUCCEEDED) - self.assertTrue(self.action_result[3].result.success) + self.assertIsNotNone(self.action_result[3]) + if self.action_result[3] is not None: + self.assertEqual(self.action_result[3].status, GoalStatus.STATUS_SUCCEEDED) + self.assertTrue(self.action_result[3].result.success) @launch_testing.post_shutdown_test() class TestProcessOutput(unittest.TestCase): - def test_exit_code(self, proc_info): + def test_exit_code(self, proc_info: launch_testing.ProcInfoHandler) -> None: # Check that all processes in the launch exit with code 0 launch_testing.asserts.assertExitCodes(proc_info) diff --git a/nav2_msgs/action/__init__.py b/nav2_msgs/action/__init__.py new file mode 100644 index 00000000000..a4036af8cda --- /dev/null +++ b/nav2_msgs/action/__init__.py @@ -0,0 +1,35 @@ +from nav2_msgs.action._assisted_teleop import AssistedTeleop +from nav2_msgs.action._back_up import BackUp +from nav2_msgs.action._compute_path_through_poses import ComputePathThroughPoses +from nav2_msgs.action._compute_path_to_pose import ComputePathToPose +from nav2_msgs.action._dock_robot import DockRobot +from nav2_msgs.action._drive_on_heading import DriveOnHeading +from nav2_msgs.action._dummy_behavior import DummyBehavior +from nav2_msgs.action._follow_gps_waypoints import FollowGPSWaypoints +from nav2_msgs.action._follow_path import FollowPath +from nav2_msgs.action._follow_waypoints import FollowWaypoints +from nav2_msgs.action._navigate_through_poses import NavigateThroughPoses +from nav2_msgs.action._navigate_to_pose import NavigateToPose +from nav2_msgs.action._smooth_path import SmoothPath +from nav2_msgs.action._spin import Spin +from nav2_msgs.action._undock_robot import UndockRobot +from nav2_msgs.action._wait import Wait + +__all__ = [ + 'AssistedTeleop', + 'BackUp', + 'ComputePathThroughPoses', + 'ComputePathToPose', + 'DockRobot', + 'DriveOnHeading', + 'DummyBehavior', + 'FollowGPSWaypoints', + 'FollowPath', + 'FollowWaypoints', + 'NavigateThroughPoses', + 'NavigateToPose', + 'SmoothPath', + 'Spin', + 'UndockRobot', + 'Wait', +] diff --git a/nav2_msgs/msg/__init__.py b/nav2_msgs/msg/__init__.py new file mode 100644 index 00000000000..a6e8fcd4219 --- /dev/null +++ b/nav2_msgs/msg/__init__.py @@ -0,0 +1,29 @@ +from nav2_msgs.msg._behavior_tree_log import BehaviorTreeLog +from nav2_msgs.msg._behavior_tree_status_change import BehaviorTreeStatusChange +from nav2_msgs.msg._collision_detector_state import CollisionDetectorState +from nav2_msgs.msg._collision_monitor_state import CollisionMonitorState +from nav2_msgs.msg._costmap import Costmap +from nav2_msgs.msg._costmap_filter_info import CostmapFilterInfo +from nav2_msgs.msg._costmap_meta_data import CostmapMetaData +from nav2_msgs.msg._costmap_update import CostmapUpdate +from nav2_msgs.msg._missed_waypoint import MissedWaypoint +from nav2_msgs.msg._particle import Particle +from nav2_msgs.msg._particle_cloud import ParticleCloud +from nav2_msgs.msg._speed_limit import SpeedLimit +from nav2_msgs.msg._voxel_grid import VoxelGrid + +__all__ = [ + 'BehaviorTreeLog', + 'BehaviorTreeStatusChange', + 'CollisionDetectorState', + 'CollisionMonitorState', + 'Costmap', + 'CostmapFilterInfo', + 'CostmapMetaData', + 'CostmapUpdate', + 'MissedWaypoint', + 'Particle', + 'ParticleCloud', + 'SpeedLimit', + 'VoxelGrid', +] diff --git a/nav2_msgs/srv/__init__.py b/nav2_msgs/srv/__init__.py new file mode 100644 index 00000000000..51658c3f3b8 --- /dev/null +++ b/nav2_msgs/srv/__init__.py @@ -0,0 +1,25 @@ +from nav2_msgs.srv._clear_costmap_around_robot import ClearCostmapAroundRobot +from nav2_msgs.srv._clear_costmap_except_region import ClearCostmapExceptRegion +from nav2_msgs.srv._clear_entire_costmap import ClearEntireCostmap +from nav2_msgs.srv._get_costmap import GetCostmap +from nav2_msgs.srv._get_costs import GetCosts +from nav2_msgs.srv._is_path_valid import IsPathValid +from nav2_msgs.srv._load_map import LoadMap +from nav2_msgs.srv._manage_lifecycle_nodes import ManageLifecycleNodes +from nav2_msgs.srv._reload_dock_database import ReloadDockDatabase +from nav2_msgs.srv._save_map import SaveMap +from nav2_msgs.srv._set_initial_pose import SetInitialPose + +__all__ = [ + 'ClearCostmapAroundRobot', + 'ClearCostmapExceptRegion', + 'ClearEntireCostmap', + 'GetCostmap', + 'GetCosts', + 'IsPathValid', + 'LoadMap', + 'ManageLifecycleNodes', + 'ReloadDockDatabase', + 'SaveMap', + 'SetInitialPose', +] diff --git a/tools/pyproject.toml b/tools/pyproject.toml index 08693d6a353..9f88cd9de93 100644 --- a/tools/pyproject.toml +++ b/tools/pyproject.toml @@ -24,7 +24,12 @@ module = [ "launch.*", "ament_index_python.*", "nav2_common.*", + "nav2_msgs.*", "launch_testing.*", + "action_msgs.*", + "geometry_msgs.*", + "sensor_msgs.*", + "tf2_ros.*", ] ignore_missing_imports = true