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/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):