From 492b274e522cc46ebaf88d360594eb8c8bd7de6f Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Fri, 15 Nov 2024 15:27:21 +0800 Subject: [PATCH] Move tests Signed-off-by: Aaron Chong --- README.md | 8 +- codecov.yaml | 12 - .../free_fleet_adapter/nav2_robot_adapter.py | 69 +++-- .../integration/test_nav2_robot_adapter.py | 243 +++++++++++++++++- .../nav2_send_navigate_to_pose.py | 4 +- 5 files changed, 291 insertions(+), 45 deletions(-) diff --git a/README.md b/README.md index d3ce7f3f..6498348c 100644 --- a/README.md +++ b/README.md @@ -124,7 +124,7 @@ zenohd # If using released standalaone binaries # cd PATH_TO_EXTRACTED_ZENOH_ROUTER -# ./zenohd +# ./zenohd ``` Start `zenoh-bridge-ros2dds` with the appropriate zenoh client configuration, @@ -141,7 +141,7 @@ Listen to transforms over `zenoh`, ```bash source ~/ff_ws/install/setup.bash -ros2 run free_fleet_examples get_tf.py \ +ros2 run free_fleet_examples nav2_get_tf.py \ --namespace turtlebot3_1 ``` @@ -149,7 +149,7 @@ Start a `navigate_to_pose` action over `zenoh`, using example values, ```bash source ~/ff_ws/install/setup.bash -ros2 run free_fleet_examples send_navigate_to_pose.py \ +ros2 run free_fleet_examples nav2_send_navigate_to_pose.py \ --frame-id map \ --namespace turtlebot3_1 \ -x 1.808 \ @@ -214,7 +214,7 @@ zenohd # If using released standalaone binaries # cd PATH_TO_EXTRACTED_ZENOH_ROUTER -# ./zenohd +# ./zenohd ``` Start `zenoh-bridge-ros2dds` with the appropriate zenoh client configuration, diff --git a/codecov.yaml b/codecov.yaml index 3b1bb711..ddc5d27e 100644 --- a/codecov.yaml +++ b/codecov.yaml @@ -1,16 +1,4 @@ -# coverage: -# precision: 2 -# round: down -# range: "35...100" -# status: -# project: -# default: -# informational: true -# patch: off fixes: - "ros_ws/src/*/free_fleet/free_fleet/::" - "ros_ws/src/*/free_fleet/free_fleet_adapter/::" - "ros_ws/src/*/free_fleet/free_fleet_examples/::" -# comment: -# layout: "diff, flags, files" -# behavior: default diff --git a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py index c2512638..d4902dad 100644 --- a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py @@ -171,11 +171,8 @@ def pose(self) -> Annotated[list[float], 3] | None: ] return robot_pose - def _make_random_goal_id(self): - return np.random.randint(0, 255, size=(16)).astype('uint8').tolist() - def _is_navigation_done(self) -> bool: - if self.nav_goal_id is None: + if self.execution is None or self.nav_goal_id is None: return True req = NavigateToPose_GetResult_Request(goal_id=self.nav_goal_id) @@ -214,7 +211,17 @@ def _is_navigation_done(self) -> bool: f'{type(e)}: {e}') continue + def _check_update_handle_initialization(self): + if self.update_handle is None: + error_message = \ + f'Failed to update robot {self.name}, robot adapter has not ' \ + 'yet been initialized with a fleet update handle.' + self.node.get_logger().error(error_message) + raise RuntimeError(error_message) + def update(self, state: rmf_easy.RobotState): + self._check_update_handle_initialization() + activity_identifier = None if self.execution: # TODO(ac): use an enum to record what type of execution it is, @@ -229,37 +236,31 @@ def update(self, state: rmf_easy.RobotState): self.update_handle.update(state, activity_identifier) - def navigate( + def _handle_navigate_to_pose( self, - destination: rmf_easy.Destination, - execution: rmf_easy.CommandExecution + map_name: str, + x: float, + y: float, + z: float, + yaw: float ): - self.execution = execution - self.node.get_logger().info( - f'Commanding [{self.name}] to navigate to {destination.position} ' - f'on map [{destination.map}]' - ) - - if destination.map != self.map: + if map_name != self.map: # TODO(ac): test this map related replanning behavior self.replan_counts += 1 self.node.get_logger().error( - f'Destination is on map [{destination.map}], while robot ' - f'[{self.name}] is on map [{self.map}], replan count ' - f'[{self.replan_counts}]' + f'Destination is on map [{map_name}], while robot [{self.name}] is ' + f'on map [{self.map}], replan count [{self.replan_counts}]' ) + + self._check_update_handle_initialization() self.update_handle.more().replan() return time_now = self.node.get_clock().now().seconds_nanoseconds() stamp = Time(sec=time_now[0], nanosec=time_now[1]) header = Header(stamp=stamp, frame_id='map') - position = GeometryMsgs_Point( - x=destination.position[0], - y=destination.position[1], - z=0 - ) - quat = quaternion_from_euler(0, 0, destination.position[2]) + position = GeometryMsgs_Point(x=x, y=y, z=z) + quat = quaternion_from_euler(0, 0, yaw) orientation = GeometryMsgs_Quaternion( x=quat[0], y=quat[1], @@ -269,7 +270,8 @@ def navigate( pose = GeometryMsgs_Pose(position=position, orientation=orientation) pose_stamped = GeometryMsgs_PoseStamped(header=header, pose=pose) - nav_goal_id = self._make_random_goal_id() + nav_goal_id = \ + np.random.randint(0, 255, size=(16)).astype('uint8').tolist() req = NavigateToPose_SendGoal_Request( goal_id=nav_goal_id, pose=pose_stamped, @@ -298,6 +300,7 @@ def navigate( f'Navigation goal {nav_goal_id} was rejected, replan ' f'count [{self.replan_counts}]' ) + self._check_update_handle_initialization() self.update_handle.more().replan() self.nav_goal_id = None return @@ -308,6 +311,24 @@ def navigate( ) continue + def navigate( + self, + destination: rmf_easy.Destination, + execution: rmf_easy.CommandExecution + ): + self.execution = execution + self.node.get_logger().info( + f'Commanding [{self.name}] to navigate to {destination.position} ' + f'on map [{destination.map}]' + ) + self._handle_navigate_to_pose( + destination.map, + destination.position[0], + destination.position[1], + 0.0, + destination.position[2] + ) + def stop(self, activity: ActivityIdentifier): if self.execution is None: return diff --git a/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py b/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py index c08ae678..1f5550e3 100644 --- a/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py +++ b/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py @@ -14,11 +14,13 @@ # See the License for the specific language governing permissions and # limitations under the License. +import math import time import unittest from free_fleet_adapter.nav2_robot_adapter import Nav2RobotAdapter import rclpy +import rmf_adapter.easy_full_control as rmf_easy from tf2_ros import Buffer import zenoh @@ -38,7 +40,7 @@ def tearDownClass(cls): cls.zenoh_session.close() rclpy.shutdown() - def test_robot_does_not_exist(self): + def test_non_existent_robot_pose(self): tf_buffer = Buffer() robot_adapter = Nav2RobotAdapter( @@ -63,7 +65,7 @@ def test_robot_does_not_exist(self): assert not robot_exists - def test_robot_exists(self): + def test_robot_pose(self): tf_buffer = Buffer() robot_adapter = Nav2RobotAdapter( @@ -88,4 +90,239 @@ def test_robot_exists(self): assert robot_exists - # def test_robot_navigate(): + def test_robot_battery_soc(self): + tf_buffer = Buffer() + + robot_adapter = Nav2RobotAdapter( + name='turtlebot3_1', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + battery_soc = robot_adapter.battery_soc() + assert math.isclose(battery_soc, 1.0) + + def test_robot_unable_to_update(self): + tf_buffer = Buffer() + + robot_adapter = Nav2RobotAdapter( + name='turtlebot3_1', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + able_to_update = False + try: + robot_adapter.update( + rmf_easy.RobotState( + 'L1', + [0.0, 0.0, 0.0], + 1.0 + ) + ) + able_to_update = True + except RuntimeError: + able_to_update = False + assert not able_to_update + + def test_idle_robot_navigate_is_done(self): + tf_buffer = Buffer() + + robot_adapter = Nav2RobotAdapter( + name='turtlebot3_1', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + assert robot_adapter._is_navigation_done() + + def test_robot_stop_without_command(self): + tf_buffer = Buffer() + + robot_adapter = Nav2RobotAdapter( + name='turtlebot3_1', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + assert robot_adapter.execution is None + robot_adapter.stop() + assert robot_adapter.execution is None + assert robot_adapter._is_navigation_done() + + def test_robot_handle_navigate_to_invalid_map(self): + tf_buffer = Buffer() + + robot_adapter = Nav2RobotAdapter( + name='turtlebot3_1', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + able_to_handle_navigate = False + try: + robot_adapter._handle_navigate_to_pose( + 'invalid_map', + 0.0, + 1.0, + 2.0, + 0.0 + ) + able_to_handle_navigate = True + except RuntimeError: + able_to_handle_navigate = False + assert not able_to_handle_navigate + + def test_robot_handle_navigate_to_invalid_pose(self): + tf_buffer = Buffer() + + robot_adapter = Nav2RobotAdapter( + name='turtlebot3_1', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + able_to_handle_navigate = False + try: + robot_adapter._handle_navigate_to_pose( + 'L1', + 1000000, + 1000000, + 0.0, + 0.0 + ) + able_to_handle_navigate = True + except RuntimeError: + able_to_handle_navigate = False + assert not able_to_handle_navigate + + def test_robot_handle_navigate_to_pose(self): + tf_buffer = Buffer() + + robot_adapter = Nav2RobotAdapter( + name='turtlebot3_1', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + able_to_handle_navigate = False + try: + robot_adapter._handle_navigate_to_pose( + 'L1', + -0.395, + -0.606, + 0.0, + 0.0 + ) + able_to_handle_navigate = True + except RuntimeError: + able_to_handle_navigate = False + assert able_to_handle_navigate + + time.sleep(1) + assert not robot_adapter._is_navigation_done() + time.sleep(10) + assert robot_adapter._is_navigation_done() + + def test_robot_stop_navigate(self): + tf_buffer = Buffer() + + robot_adapter = Nav2RobotAdapter( + name='turtlebot3_1', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + able_to_handle_navigate = False + try: + robot_adapter._handle_navigate_to_pose( + 'L1', + 1.808, + 0.503, + 0.0, + 0.0 + ) + able_to_handle_navigate = True + except RuntimeError: + able_to_handle_navigate = False + assert able_to_handle_navigate + + time.sleep(1) + assert not robot_adapter._is_navigation_done() + time.sleep(1) + robot_adapter.stop(robot_adapter.execution.identifier) + time.sleep(1) + assert robot_adapter._is_navigation_done() + + def test_robot_execute_unknown_action(self): + tf_buffer = Buffer() + + robot_adapter = Nav2RobotAdapter( + name='turtlebot3_1', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + able_to_execute_action = False + try: + robot_adapter.execute_action( + 'unknown_category', + {}, + None + ) + able_to_execute_action = True + except RuntimeError: + able_to_execute_action = False + assert not able_to_execute_action diff --git a/free_fleet_examples/free_fleet_examples/nav2_send_navigate_to_pose.py b/free_fleet_examples/free_fleet_examples/nav2_send_navigate_to_pose.py index cbbd3b46..daec76f5 100755 --- a/free_fleet_examples/free_fleet_examples/nav2_send_navigate_to_pose.py +++ b/free_fleet_examples/free_fleet_examples/nav2_send_navigate_to_pose.py @@ -132,13 +132,13 @@ def main(argv=sys.argv): ) # print("Result: {0}".format(rep.sequence)) print(f'Result: {rep.status}') - if rep.status == GoalStatus.STATUS_ABORTED: + if rep.status == GoalStatus.STATUS_ABORTED.value: print( 'Received (ERROR: "Plan aborted by ' 'planner_server")' ) break - if rep.status == GoalStatus.STATUS_SUCCEEDED: + if rep.status == GoalStatus.STATUS_SUCCEEDED.value: break except Exception as e: print(e)