From 3992eb1e8663d312aeac685901b3283055aeeaeb Mon Sep 17 00:00:00 2001 From: Vortex-TH <3118243+Vortex-TH@users.noreply.github.com> Date: Wed, 20 Dec 2023 23:59:41 +0100 Subject: [PATCH] Add DriveOnHeading (#4022) * Add DriveOnHeading Signed-off-by: thass <3118243+Vortex-TH@users.noreply.github.com> * Fix style errors Signed-off-by: thass <3118243+Vortex-TH@users.noreply.github.com> --------- Signed-off-by: thass <3118243+Vortex-TH@users.noreply.github.com> --- .../nav2_simple_commander/robot_navigator.py | 29 ++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index a8ded664cd5..17d2f130e84 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -23,7 +23,7 @@ from geometry_msgs.msg import PoseStamped from geometry_msgs.msg import PoseWithCovarianceStamped from lifecycle_msgs.srv import GetState -from nav2_msgs.action import AssistedTeleop, BackUp, Spin +from nav2_msgs.action import AssistedTeleop, BackUp, DriveOnHeading, Spin from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose from nav2_msgs.action import ( FollowGPSWaypoints, @@ -88,6 +88,9 @@ def __init__(self, node_name='basic_navigator', namespace=''): self.smoother_client = ActionClient(self, SmoothPath, 'smooth_path') self.spin_client = ActionClient(self, Spin, 'spin') self.backup_client = ActionClient(self, BackUp, 'backup') + self.drive_on_heading_client = ActionClient( + self, DriveOnHeading, 'drive_on_heading' + ) self.assisted_teleop_client = ActionClient( self, AssistedTeleop, 'assisted_teleop' ) @@ -127,6 +130,7 @@ def destroy_node(self): self.smoother_client.destroy() self.spin_client.destroy() self.backup_client.destroy() + self.drive_on_heading_client.destroy() super().destroy_node() def setInitialPose(self, initial_pose): @@ -288,6 +292,29 @@ def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10): self.result_future = self.goal_handle.get_result_async() return True + def driveOnHeading(self, dist=0.15, speed=0.025, time_allowance=10): + self.debug("Waiting for 'DriveOnHeading' action server") + while not self.backup_client.wait_for_server(timeout_sec=1.0): + self.info("'DriveOnHeading' action server not available, waiting...") + goal_msg = DriveOnHeading.Goal() + goal_msg.target = Point(x=float(dist)) + goal_msg.speed = speed + goal_msg.time_allowance = Duration(sec=time_allowance) + + self.info(f'Drive {goal_msg.target.x} m on heading at {goal_msg.speed} m/s....') + send_goal_future = self.drive_on_heading_client.send_goal_async( + goal_msg, self._feedbackCallback + ) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error('Drive On Heading request was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + def assistedTeleop(self, time_allowance=30): self.debug("Wainting for 'assisted_teleop' action server") while not self.assisted_teleop_client.wait_for_server(timeout_sec=1.0):