Add recoveries to simple commander (#2792)

* fix Typo

* add recoveries

* add docs

* added demo using backup and spin

* rename isNavigationComplete to isTaskComplete

* rename cancelNav to cancelTask

* add prints

* fix premature exit

* rename NavResult to TaskResult

* fix readme order

* fix import order

* renaming

* renaming

nav2_simple_commander/README.md

@@ -19,10 +19,12 @@ The methods provided by the basic navigator are shown below, with inputs and exp
 | 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. |
-| getResult()				        | Gets final result of navigation task, to be called after `isNavComplete` returns `True`. Returns action server result object. |
+| spin(spin_dist, time_allowance)   | Requests the robot to performs an in-place rotation by a given angle.      |
+| backup(backup_dist, backup_speed, time_allowance) | Requests the robot to back up by a given distance.         |
+| cancelTask()                       | Cancel an ongoing task request.|
+| isTaskComplete()                   | Checks if task is complete yet, times out at `100ms`.  Returns `True` if completed and `False` if still going.                  |
+| getFeedback()                     | Gets feedback from task, returns action server feedback object. |
+| getResult()				        | Gets final result of task, to be called after `isTaskComplete` returns `True`. Returns action server result object. |
 | getPath(start, goal)              | Gets a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`.      |
 | getPathThroughPoses(start, goals) | Gets a path through a starting to a set of goals, a list of `PoseStamped`, `nav_msgs/Path`. |
 | changeMap(map_filepath)           | Requests a change from the current map to `map_filepath`'s yaml.           |
@@ -50,17 +52,17 @@ nav.setInitialPose(init_pose)
 nav.waitUntilNav2Active() # if autostarted, else use `lifecycleStartup()`
 ...
 nav.goToPose(goal_pose)
-while not nav.isNavComplete():
+while not nav.isTaskComplete():
 	feedback = nav.getFeedback()
 	if feedback.navigation_duration > 600:
-		nav.cancelNav()
+		nav.cancelTask()
 ...
 result = nav.getResult()
-if result == NavigationResult.SUCCEEDED:
+if result == TaskResult.SUCCEEDED:
     print('Goal succeeded!')
-elif result == NavigationResult.CANCELED:
+elif result == TaskResult.CANCELED:
     print('Goal was canceled!')
-elif result == NavigationResult.FAILED:
+elif result == TaskResult.FAILED:
     print('Goal failed!')
 ```
 

nav2_simple_commander/launch/recoveries_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='demo_recoveries',
+        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/demo_inspection.py

@@ -16,7 +16,7 @@
 from copy import deepcopy
 
 from geometry_msgs.msg import PoseStamped
-from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult
+from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
 import rclpy
 
 """
@@ -72,26 +72,25 @@ def main():
     # Do something during our route (e.x. AI to analyze stock information or upload to the cloud)
     # Simply the current waypoint ID for the demonstation
     i = 0
-    while not navigator.isNavComplete():
+    while not navigator.isTaskComplete():
         i += 1
         feedback = navigator.getFeedback()
         if feedback and i % 5 == 0:
             print('Executing current waypoint: ' +
                   str(feedback.current_waypoint + 1) + '/' + str(len(inspection_points)))
 
     result = navigator.getResult()
-    if result == NavigationResult.SUCCEEDED:
+    if result == TaskResult.SUCCEEDED:
         print('Inspection of shelves complete! Returning to start...')
-    elif result == NavigationResult.CANCELED:
+    elif result == TaskResult.CANCELED:
         print('Inspection of shelving was canceled. Returning to start...')
-        exit(1)
-    elif result == NavigationResult.FAILED:
+    elif result == TaskResult.FAILED:
         print('Inspection of shelving failed! Returning to start...')
 
     # go back to start
     initial_pose.header.stamp = navigator.get_clock().now().to_msg()
     navigator.goToPose(initial_pose)
-    while not navigator.isNavComplete():
+    while not navigator.isTaskComplete():
         pass
 
     exit(0)

nav2_simple_commander/nav2_simple_commander/demo_picking.py

@@ -14,7 +14,7 @@
 # limitations under the License.
 
 from geometry_msgs.msg import PoseStamped
-from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult
+from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
 
 import rclpy
 from rclpy.duration import Duration
@@ -82,7 +82,7 @@ def main():
     # (e.x. queue up future tasks or detect person for fine-tuned positioning)
     # Simply print information for workers on the robot's ETA for the demonstation
     i = 0
-    while not navigator.isNavComplete():
+    while not navigator.isTaskComplete():
         i += 1
         feedback = navigator.getFeedback()
         if feedback and i % 5 == 0:
@@ -92,7 +92,7 @@ def main():
                   + ' seconds.')
 
     result = navigator.getResult()
-    if result == NavigationResult.SUCCEEDED:
+    if result == TaskResult.SUCCEEDED:
         print('Got product from ' + request_item_location +
               '! Bringing product to shipping destination (' + request_destination + ')...')
         shipping_destination = PoseStamped()
@@ -104,16 +104,16 @@ def main():
         shipping_destination.pose.orientation.w = 0.0
         navigator.goToPose(shipping_destination)
 
-    elif result == NavigationResult.CANCELED:
+    elif result == TaskResult.CANCELED:
         print(f'Task at {request_item_location} was canceled. Returning to staging point...')
         initial_pose.header.stamp = navigator.get_clock().now().to_msg()
         navigator.goToPose(initial_pose)
 
-    elif result == NavigationResult.FAILED:
+    elif result == TaskResult.FAILED:
         print(f'Task at {request_item_location} failed!')
         exit(-1)
 
-    while not navigator.isNavComplete():
+    while not navigator.isTaskComplete():
         pass
 
     exit(0)

nav2_simple_commander/nav2_simple_commander/demo_recoveries.py

@@ -0,0 +1,105 @@
+#! /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, TaskResult
+import rclpy
+from rclpy.duration import Duration
+
+
+"""
+Basic recoveries demo. In this demonstration, the robot navigates
+to a dead-end where recoveries such as backup and spin are used
+to get out of it.
+"""
+
+
+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
+    navigator.waitUntilNav2Active()
+
+    goal_pose = PoseStamped()
+    goal_pose.header.frame_id = 'map'
+    goal_pose.header.stamp = navigator.get_clock().now().to_msg()
+    goal_pose.pose.position.x = 6.13
+    goal_pose.pose.position.y = 1.90
+    goal_pose.pose.orientation.w = 1.0
+
+    navigator.goToPose(goal_pose)
+
+    i = 0
+    while not navigator.isTaskComplete():
+        i += 1
+        feedback = navigator.getFeedback()
+        if feedback and i % 5 == 0:
+            print(
+                f'Estimated time of arrival to destination is: \
+                {Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9}'
+            )
+
+    # Robot hit a dead end, back it up
+    print('Robot hit a dead end, backing up...')
+    navigator.backup(backup_dist=0.5, backup_speed=0.1)
+
+    i = 0
+    while not navigator.isTaskComplete():
+        i += 1
+        feedback = navigator.getFeedback()
+        if feedback and i % 5 == 0:
+            print(f'Distance traveled: {feedback.distance_traveled}')
+
+    # Turn it around
+    print('Spinning robot around...')
+    navigator.spin(spin_dist=3.14)
+
+    i = 0
+    while not navigator.isTaskComplete():
+        i += 1
+        feedback = navigator.getFeedback()
+        if feedback and i % 5 == 0:
+            print(f'Spin angle traveled: {feedback.angular_distance_traveled}')
+
+    result = navigator.getResult()
+    if result == TaskResult.SUCCEEDED:
+        print('Dead end confirmed! Returning to start...')
+    elif result == TaskResult.CANCELED:
+        print('Recovery was canceled. Returning to start...')
+    elif result == TaskResult.FAILED:
+        print('Recovering from dead end failed! Returning to start...')
+
+    initial_pose.header.stamp = navigator.get_clock().now().to_msg()
+    navigator.goToPose(initial_pose)
+    while not navigator.isTaskComplete():
+        pass
+
+    exit(0)
+
+
+if __name__ == '__main__':
+    main()

nav2_simple_commander/nav2_simple_commander/demo_security.py

@@ -16,7 +16,7 @@
 from copy import deepcopy
 
 from geometry_msgs.msg import PoseStamped
-from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult
+from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
 
 import rclpy
 from rclpy.duration import Duration
@@ -75,7 +75,7 @@ def main():
         # Do something during our route (e.x. AI detection on camera images for anomalies)
         # Simply print ETA for the demonstation
         i = 0
-        while not navigator.isNavComplete():
+        while not navigator.isTaskComplete():
             i += 1
             feedback = navigator.getFeedback()
             if feedback and i % 5 == 0:
@@ -86,18 +86,18 @@ def main():
                 # Some failure mode, must stop since the robot is clearly stuck
                 if Duration.from_msg(feedback.navigation_time) > Duration(seconds=180.0):
                     print('Navigation has exceeded timeout of 180s, canceling request.')
-                    navigator.cancelNav()
+                    navigator.cancelTask()
 
         # If at end of route, reverse the route to restart
         security_route.reverse()
 
         result = navigator.getResult()
-        if result == NavigationResult.SUCCEEDED:
+        if result == TaskResult.SUCCEEDED:
             print('Route complete! Restarting...')
-        elif result == NavigationResult.CANCELED:
+        elif result == TaskResult.CANCELED:
             print('Security route was canceled, exiting.')
             exit(1)
-        elif result == NavigationResult.FAILED:
+        elif result == TaskResult.FAILED:
             print('Security route failed! Restarting from other side...')
 
     exit(0)

nav2_simple_commander/nav2_simple_commander/example_follow_path.py

@@ -14,7 +14,7 @@
 # limitations under the License.
 
 from geometry_msgs.msg import PoseStamped
-from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult
+from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
 import rclpy
 
 
@@ -56,7 +56,7 @@ def main():
     navigator.followPath(path)
 
     i = 0
-    while not navigator.isNavComplete():
+    while not navigator.isTaskComplete():
         ################################################
         #
         # Implement some code here for your application!
@@ -74,11 +74,11 @@ def main():
 
     # Do something depending on the return code
     result = navigator.getResult()
-    if result == NavigationResult.SUCCEEDED:
+    if result == TaskResult.SUCCEEDED:
         print('Goal succeeded!')
-    elif result == NavigationResult.CANCELED:
+    elif result == TaskResult.CANCELED:
         print('Goal was canceled!')
-    elif result == NavigationResult.FAILED:
+    elif result == TaskResult.FAILED:
         print('Goal failed!')
     else:
         print('Goal has an invalid return status!')