From 6e8320c438292d04ffd76a5398b9e9e67e7d5360 Mon Sep 17 00:00:00 2001 From: X-lanni Date: Fri, 8 Aug 2025 15:51:22 +0800 Subject: [PATCH] feat(nav2) add simple commander api --- agv_pro_navigation2/CMakeLists.txt | 2 +- agv_pro_navigation2/package.xml | 2 +- .../scripts/example_nav_through_poses.py | 101 ++++++++++++++++ .../scripts/example_nav_to_pose.py | 97 ++++++++++++++++ .../scripts/example_waypoint_follower.py | 108 ++++++++++++++++++ 5 files changed, 308 insertions(+), 2 deletions(-) create mode 100644 agv_pro_navigation2/scripts/example_nav_through_poses.py create mode 100644 agv_pro_navigation2/scripts/example_nav_to_pose.py create mode 100644 agv_pro_navigation2/scripts/example_waypoint_follower.py diff --git a/agv_pro_navigation2/CMakeLists.txt b/agv_pro_navigation2/CMakeLists.txt index a23addb..b885e3b 100644 --- a/agv_pro_navigation2/CMakeLists.txt +++ b/agv_pro_navigation2/CMakeLists.txt @@ -24,7 +24,7 @@ if(BUILD_TESTING) endif() install( - DIRECTORY launch map param rviz + DIRECTORY launch map param rviz scripts DESTINATION share/${PROJECT_NAME} ) diff --git a/agv_pro_navigation2/package.xml b/agv_pro_navigation2/package.xml index ee224d0..089919d 100644 --- a/agv_pro_navigation2/package.xml +++ b/agv_pro_navigation2/package.xml @@ -2,7 +2,7 @@ agv_pro_navigation2 - 1.0.0 + 1.0.1 ROS2 launch scripts for navigation2 lanni BSD-3-Clause license diff --git a/agv_pro_navigation2/scripts/example_nav_through_poses.py b/agv_pro_navigation2/scripts/example_nav_through_poses.py new file mode 100644 index 0000000..3f0835b --- /dev/null +++ b/agv_pro_navigation2/scripts/example_nav_through_poses.py @@ -0,0 +1,101 @@ +#! /usr/bin/env python3 + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +import rclpy +from rclpy.duration import Duration + + +""" +Basic navigation demo to go to poses. +""" + +def set_initial_pose(navigator: BasicNavigator, x: float, y: float, oz: float, ow: float): + """ + Set the initial pose of the robot for AMCL localization. + + Args: + navigator (BasicNavigator): The navigator instance controlling the robot. + x (float): Initial X position in the map frame. + y (float): Initial Y position in the map frame. + oz (float): Orientation Z component (quaternion). + ow (float): Orientation W component (quaternion). + """ + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = x + initial_pose.pose.position.y = y + initial_pose.pose.orientation.z = oz + initial_pose.pose.orientation.w = ow + navigator.setInitialPose(initial_pose) + +def create_pose(navigator: BasicNavigator, x, y, z, w): + pose = PoseStamped() + pose.header.frame_id = 'map' + pose.header.stamp = navigator.get_clock().now().to_msg() + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.orientation.z = z + pose.pose.orientation.w = w + return pose + +def nav_through_pose(navigator: BasicNavigator, goal_poses, verbose: bool = False) -> bool: + + nav_start = navigator.get_clock().now() + navigator.goThroughPoses(goal_poses) + + while not navigator.isTaskComplete(): + feedback = navigator.getFeedback() + if feedback and verbose: + remaining = Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9 + print(f"Estimated time of arrival: {remaining:.0f} seconds") + + # Do something depending on the return code + result = navigator.getResult() + if result == TaskResult.SUCCEEDED: + print('Goal succeeded!') + return True + elif result == TaskResult.CANCELED: + print('Goal was canceled!') + elif result == TaskResult.FAILED: + print('Goal failed!') + else: + print('Goal has an invalid return status!') + return False + +if __name__ == '__main__': + rclpy.init() + + navigator = BasicNavigator() + + # Set robot initial pose + # set_initial_pose(navigator, x=-1.9248794317245483, y=-0.5366987586021423, oz=-1.8463129131030735e-06, ow=0.9999999999982956)\ + + # Wait for navigation to fully activate, since autostarting nav2 + # navigator.waitUntilNav2Active() + + way1_goals = [ + [0.5062479972839355, -0.5562516450881958, -0.011363976322727884, 0.9999354279362925], + [1.7874977588653564, -0.6250066757202148, 0.7002746726771927, 0.7138735061667792], + [1.3625057935714722, 1.5999948978424072, 0.9999809382375775, 0.006174395637980891], + [-1.4687445163726807, 1.487505555152893, -0.9025521753719631, 0.43058050435584877] + ] + way2_goals = [ + [0.5062479972839355, -0.5562516450881958, -0.011363976322727884, 0.9999354279362925], + [1.7874977588653564, -0.6250066757202148, 0.7002746726771927, 0.7138735061667792], + [1.3625057935714722, 1.5999948978424072, 0.9999809382375775, 0.006174395637980891], + [-1.4687445163726807, 1.487505555152893, -0.9025521753719631, 0.43058050435584877] + ] + + goal_poses_1 = [create_pose(navigator, *g) for g in way1_goals] + goal_poses_2 = [create_pose(navigator, *g) for g in way2_goals] + + result1 = nav_through_pose(navigator, goal_poses_1, verbose=True) + print(f"First segment navigation result: {result1}") + + if result1 ==True: + result2 = nav_through_pose(navigator, goal_poses_2, verbose=True) + print(f"Second segment navigation result: {result2}") + + rclpy.shutdown() \ No newline at end of file diff --git a/agv_pro_navigation2/scripts/example_nav_to_pose.py b/agv_pro_navigation2/scripts/example_nav_to_pose.py new file mode 100644 index 0000000..0911b09 --- /dev/null +++ b/agv_pro_navigation2/scripts/example_nav_to_pose.py @@ -0,0 +1,97 @@ +#! /usr/bin/env python3 + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +import rclpy +from rclpy.duration import Duration + +""" +Basic navigation demo to go to pose. +""" + +def set_initial_pose(navigator: BasicNavigator, x: float, y: float, oz: float, ow: float): + """ + Set the initial pose of the robot for AMCL localization. + + Args: + navigator (BasicNavigator): The navigator instance controlling the robot. + x (float): Initial X position in the map frame. + y (float): Initial Y position in the map frame. + oz (float): Orientation Z component (quaternion). + ow (float): Orientation W component (quaternion). + """ + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = x + initial_pose.pose.position.y = y + initial_pose.pose.orientation.z = oz + initial_pose.pose.orientation.w = ow + navigator.setInitialPose(initial_pose) + + +def navigate_to_goal(navigator: BasicNavigator, x: float, y: float, oz: float, ow: float, verbose: bool = False) -> bool: + """ + Navigate the robot to a target goal pose. + + Args: + navigator (BasicNavigator): The navigator instance controlling the robot. + x (float): Goal X position in the map frame. + y (float): Goal Y position in the map frame. + oz (float): Orientation Z component (quaternion). + ow (float): Orientation W component (quaternion). + verbose (bool, optional): If True, prints navigation feedback such as estimated arrival time. Default is False. + + Returns: + bool: True if navigation succeeded, False otherwise. + """ + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'map' + goal_pose.header.stamp = navigator.get_clock().now().to_msg() + goal_pose.pose.position.x = x + goal_pose.pose.position.y = y + goal_pose.pose.orientation.z = oz + goal_pose.pose.orientation.w = ow + + navigator.goToPose(goal_pose) + + while not navigator.isTaskComplete(): + feedback = navigator.getFeedback() + if feedback and verbose: + remaining = Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9 + print(f"Estimated time of arrival: {remaining:.0f} seconds") + + result = navigator.getResult() + if result == TaskResult.SUCCEEDED: + print('Goal succeeded!') + return True + elif result == TaskResult.CANCELED: + print('Goal was canceled!') + elif result == TaskResult.FAILED: + print('Goal failed!') + else: + print('Goal has an invalid return status!') + return False + +if __name__ == '__main__': + rclpy.init() + navigator = BasicNavigator() + + # Set robot initial pose + # set_initial_pose(navigator, x=-1.9248794317245483, y=-0.5366987586021423, oz=-1.8463129131030735e-06, ow=0.9999999999982956) + + # Wait for navigation to fully activate, since autostarting nav2 + # navigator.waitUntilNav2Active() + + goal_A = [1.6766083240509033,0.37930558800697327,-0.03491306994337919, 0.9993903529387947] + goal_B = [-0.5062443017959595,1.559376835823059,0.6869307039904945,0.7267229237578264] + + x_goal, y_goal, orientation_z, orientation_w = goal_A + success = navigate_to_goal(navigator, x_goal, y_goal, orientation_z, orientation_w) + print("Navigation result:", success) + + x_goal, y_goal, orientation_z, orientation_w = goal_B + success = navigate_to_goal(navigator, x_goal, y_goal, orientation_z, orientation_w) + print("Navigation result:", success) + + rclpy.shutdown() diff --git a/agv_pro_navigation2/scripts/example_waypoint_follower.py b/agv_pro_navigation2/scripts/example_waypoint_follower.py new file mode 100644 index 0000000..9dd77d3 --- /dev/null +++ b/agv_pro_navigation2/scripts/example_waypoint_follower.py @@ -0,0 +1,108 @@ +#! /usr/bin/env python3 + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +import rclpy +from rclpy.duration import Duration + +""" +Basic navigation demo to go to poses. +""" + +def set_initial_pose(navigator: BasicNavigator, x: float, y: float, oz: float, ow: float): + """ + Set the initial pose of the robot for AMCL localization. + + Args: + navigator (BasicNavigator): The navigator instance controlling the robot. + x (float): Initial X position in the map frame. + y (float): Initial Y position in the map frame. + oz (float): Orientation Z component (quaternion). + ow (float): Orientation W component (quaternion). + """ + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = x + initial_pose.pose.position.y = y + initial_pose.pose.orientation.z = oz + initial_pose.pose.orientation.w = ow + navigator.setInitialPose(initial_pose) + +def create_pose(navigator: BasicNavigator, x, y, z, w): + pose = PoseStamped() + pose.header.frame_id = 'map' + pose.header.stamp = navigator.get_clock().now().to_msg() + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.orientation.z = z + pose.pose.orientation.w = w + return pose + +def nav_waypoint_follower(navigator: BasicNavigator, goal_poses, verbose: bool = False) -> bool: + + nav_start = navigator.get_clock().now() + navigator.followWaypoints(goal_poses) + + i = 0 + while not navigator.isTaskComplete(): + # Do something with the feedback + i = i + 1 + feedback = navigator.getFeedback() + if (feedback and i % 5) and verbose == 0: + print('Executing current waypoint: ' + + str(feedback.current_waypoint + 1) + '/' + str(len(goal_poses))) + now = navigator.get_clock().now() + + # Some navigation timeout to demo cancellation + if now - nav_start > Duration(seconds=600.0): + navigator.cancelTask() + + # Do something depending on the return code + result = navigator.getResult() + if result == TaskResult.SUCCEEDED: + print('Goal succeeded!') + return True + elif result == TaskResult.CANCELED: + print('Goal was canceled!') + elif result == TaskResult.FAILED: + print('Goal failed!') + else: + print('Goal has an invalid return status!') + return False + +if __name__ == '__main__': + rclpy.init() + + navigator = BasicNavigator() + + # Set robot initial pose + # set_initial_pose(navigator, x=-1.9248794317245483, y=-0.5366987586021423, oz=-1.8463129131030735e-06, ow=0.9999999999982956)\ + + # Wait for navigation to fully activate, since autostarting nav2 + # navigator.waitUntilNav2Active() + + way1_goals = [ + [0.5062479972839355, -0.5562516450881958, -0.011363976322727884, 0.9999354279362925], + [1.7874977588653564, -0.6250066757202148, 0.7002746726771927, 0.7138735061667792], + [1.3625057935714722, 1.5999948978424072, 0.9999809382375775, 0.006174395637980891], + [-1.4687445163726807, 1.487505555152893, -0.9025521753719631, 0.43058050435584877] + ] + way2_goals = [ + [0.5062479972839355, -0.5562516450881958, -0.011363976322727884, 0.9999354279362925], + [1.7874977588653564, -0.6250066757202148, 0.7002746726771927, 0.7138735061667792], + [1.3625057935714722, 1.5999948978424072, 0.9999809382375775, 0.006174395637980891], + [-1.4687445163726807, 1.487505555152893, -0.9025521753719631, 0.43058050435584877] + ] + + goal_poses_1 = [create_pose(navigator, *g) for g in way1_goals] + goal_poses_2 = [create_pose(navigator, *g) for g in way2_goals] + + result1 = nav_waypoint_follower(navigator, goal_poses_1, verbose=False) + print(f"First segment navigation result: {result1}") + + if result1 ==True: + result2 = nav_waypoint_follower(navigator, goal_poses_2, verbose=False) + print(f"Second segment navigation result: {result2}") + + rclpy.shutdown() \ No newline at end of file