Feature to call controller action server to follow path (#2789)

* Added call to controller action server with a path to follow
++ Added a new function  in robot_navigator,py
++ Added a launch script to test  function
++ Updated setup.py to include demo_follow_path.py

* Code refactoring

* Code refactoring

* Code refactoring for consistency

* Updated README.md

* Resolved executable conflict in setup.py for example_follow_path.py

* Code refactoring with  ament_flake8

nav2_simple_commander/README.md

@@ -20,6 +20,7 @@ The methods provided by the basic navigator are shown below, with inputs and exp
 | goThroughPoses(poses)             | Requests the robot to drive through a set of poses (list of `PoseStamped`).|
 | goToPose(pose)                    | Requests the robot to drive to a pose (`PoseStamped`).                     |
 | followWaypoints(poses)            | Requests the robot to follow a set of waypoints (list of `PoseStamped`). This will execute the specific `TaskExecutor` at each pose.   |
+| followPath(path)                  | Requests the robot to follow a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`.     |
 | cancelNav()                       | Cancel an ongoing `goThroughPoses` `goToPose` or `followWaypoints` request.|
 | isNavComplete()                   | Checks if navigation is complete yet, times out at `100ms`.  Returns `True` if completed and `False` if still going.                  |
 | getFeedback()                     | Gets feedback from navigation task, returns action server feedback object. |
@@ -102,8 +103,8 @@ The `nav2_simple_commander` has a few examples to highlight the API functions av
 
 - `example_nav_to_pose.py` - Demonstrates the navigate to pose capabilities of the navigator, as well as a number of auxiliary methods.
 - `example_nav_through_poses.py` - Demonstrates the navigate through poses capabilities of the navigator, as well as a number of auxiliary methods.
-- `example_waypoint_follower.py` - Demonstrates the waypoint following capabilities of the navigator, as well as a number of auxiliary methods required.
-
+- `example_waypoint_follower.py` - Demonstrates the waypoint following capabilities of the navigator, as well as a number of auxiliary methods.
+- `example_follow_path.py` - Demonstrates the path following capabilities of the navigator, as well as a number of auxiliary methods.
 ## Demos
 
 The `nav2_simple_commander` has a few demonstrations to highlight a couple of simple autonomy applications you can build using the `nav2_simple_commander` API:

nav2_simple_commander/launch/follow_path_example_launch.py

@@ -0,0 +1,77 @@
+# Copyright (c) 2021 Samsung Research America
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world')
+    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+    python_commander_dir = get_package_share_directory('nav2_simple_commander')
+
+    map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml')
+    world = os.path.join(python_commander_dir, 'warehouse.world')
+
+    # start the simulation
+    start_gazebo_server_cmd = ExecuteProcess(
+        cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world],
+        cwd=[warehouse_dir], output='screen')
+
+    start_gazebo_client_cmd = ExecuteProcess(
+        cmd=['gzclient'],
+        cwd=[warehouse_dir], output='screen')
+
+    urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
+    start_robot_state_publisher_cmd = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='screen',
+        arguments=[urdf])
+
+    # start the visualization
+    rviz_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
+        launch_arguments={'namespace': '',
+                          'use_namespace': 'False'}.items())
+
+    # start navigation
+    bringup_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')),
+        launch_arguments={'map': map_yaml_file}.items())
+
+    # start the demo autonomy task
+    demo_cmd = Node(
+        package='nav2_simple_commander',
+        executable='example_follow_path',
+        emulate_tty=True,
+        output='screen')
+
+    ld = LaunchDescription()
+    ld.add_action(start_gazebo_server_cmd)
+    ld.add_action(start_gazebo_client_cmd)
+    ld.add_action(start_robot_state_publisher_cmd)
+    ld.add_action(rviz_cmd)
+    ld.add_action(bringup_cmd)
+    ld.add_action(demo_cmd)
+    return ld

nav2_simple_commander/nav2_simple_commander/example_follow_path.py

@@ -0,0 +1,92 @@
+#! /usr/bin/env python3
+# Copyright 2021 Samsung Research America
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from geometry_msgs.msg import PoseStamped
+from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult
+import rclpy
+
+
+"""
+Basic navigation demo to follow a given path
+"""
+
+
+def main():
+    rclpy.init()
+
+    navigator = BasicNavigator()
+
+    # Set our demo's initial pose
+    initial_pose = PoseStamped()
+    initial_pose.header.frame_id = 'map'
+    initial_pose.header.stamp = navigator.get_clock().now().to_msg()
+    initial_pose.pose.position.x = 3.45
+    initial_pose.pose.position.y = 2.15
+    initial_pose.pose.orientation.z = 1.0
+    initial_pose.pose.orientation.w = 0.0
+    navigator.setInitialPose(initial_pose)
+
+    # Wait for navigation to fully activate, since autostarting nav2
+    navigator.waitUntilNav2Active()
+
+    # Go to our demos first goal pose
+    goal_pose = PoseStamped()
+    goal_pose.header.frame_id = 'map'
+    goal_pose.header.stamp = navigator.get_clock().now().to_msg()
+    goal_pose.pose.position.x = -3.0
+    goal_pose.pose.position.y = -2.0
+    goal_pose.pose.orientation.w = 1.0
+
+    # Sanity check a valid path exists
+    path = navigator.getPath(initial_pose, goal_pose)
+
+    # Follow path
+    navigator.followPath(path)
+
+    i = 0
+    while not navigator.isNavComplete():
+        ################################################
+        #
+        # Implement some code here for your application!
+        #
+        ################################################
+
+        # Do something with the feedback
+        i += 1
+        feedback = navigator.getFeedback()
+        if feedback and i % 5 == 0:
+            print('Estimated distance remaining to goal position: ' +
+                  '{0:.3f}'.format(feedback.distance_to_goal) +
+                  '\nCurrent speed of the robot: ' +
+                  '{0:.3f}'.format(feedback.speed))
+
+    # Do something depending on the return code
+    result = navigator.getResult()
+    if result == NavigationResult.SUCCEEDED:
+        print('Goal succeeded!')
+    elif result == NavigationResult.CANCELED:
+        print('Goal was canceled!')
+    elif result == NavigationResult.FAILED:
+        print('Goal failed!')
+    else:
+        print('Goal has an invalid return status!')
+
+    navigator.lifecycleShutdown()
+
+    exit(0)
+
+
+if __name__ == '__main__':
+    main()

nav2_simple_commander/nav2_simple_commander/robot_navigator.py

@@ -23,6 +23,7 @@
 from lifecycle_msgs.srv import GetState
 from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose
 from nav2_msgs.action import FollowWaypoints, NavigateThroughPoses, NavigateToPose
+from nav2_msgs.action import FollowPath
 from nav2_msgs.srv import ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes
 
 import rclpy
@@ -62,6 +63,7 @@ def __init__(self):
                                                      'navigate_through_poses')
         self.nav_to_pose_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
         self.follow_waypoints_client = ActionClient(self, FollowWaypoints, 'follow_waypoints')
+        self.follow_path_client = ActionClient(self, FollowPath, 'follow_path')
         self.compute_path_to_pose_client = ActionClient(self, ComputePathToPose,
                                                         'compute_path_to_pose')
         self.compute_path_through_poses_client = ActionClient(self, ComputePathThroughPoses,
@@ -155,6 +157,28 @@ def followWaypoints(self, poses):
         self.result_future = self.goal_handle.get_result_async()
         return True
 
+    def followPath(self, path):
+        """Send a `FollowPath` action request."""
+        self.debug("Waiting for 'FollowPath' action server")
+        while not self.follow_path_client.wait_for_server(timeout_sec=1.0):
+            self.info("'FollowPath' action server not available, waiting...")
+
+        goal_msg = FollowPath.Goal()
+        goal_msg.path = path
+
+        self.info('Executing path...')
+        send_goal_future = self.follow_path_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('Follow path was rejected!')
+            return False
+
+        self.result_future = self.goal_handle.get_result_async()
+        return True
+
     def cancelNav(self):
         """Cancel pending navigation request of any type."""
         self.info('Canceling current goal.')

nav2_simple_commander/setup.py

@@ -28,6 +28,7 @@
                 'example_nav_to_pose = nav2_simple_commander.example_nav_to_pose:main',
                 'example_nav_through_poses = nav2_simple_commander.example_nav_through_poses:main',
                 'example_waypoint_follower = nav2_simple_commander.example_waypoint_follower:main',
+                'example_follow_path = nav2_simple_commander.example_follow_path:main',
                 'demo_picking = nav2_simple_commander.demo_picking:main',
                 'demo_inspection = nav2_simple_commander.demo_inspection:main',
                 'demo_security = nav2_simple_commander.demo_security:main',