Assisted teleop simple commander

.circleci/config.yml

@@ -33,12 +33,12 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v11\
+              - "<< parameters.key >>-v12\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v11\
+              - "<< parameters.key >>-v12\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -58,7 +58,7 @@ _commands:
       steps:
         - save_cache:
             name: Save Cache << parameters.key >>
-            key: "<< parameters.key >>-v11\
+            key: "<< parameters.key >>-v12\
               -{{ arch }}\
               -{{ .Branch }}\
               -{{ .Environment.CIRCLE_PR_NUMBER }}\

nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp

@@ -184,7 +184,7 @@ class TimedBehavior : public nav2_core::Behavior
   // onRun and cycle functions to execute a specific behavior
   void execute()
   {
-    RCLCPP_INFO(logger_, "Attempting %s", behavior_name_.c_str());
+    RCLCPP_INFO(logger_, "Running %s", behavior_name_.c_str());
 
     if (!enabled_) {
       RCLCPP_WARN(

nav2_simple_commander/launch/assisted_teleop_example_launch.py

@@ -0,0 +1,78 @@
+# Copyright (c) 2021 Samsung Research America
+# Copyright (c) 2022 Joshua Wallace
+#
+# 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_assisted_teleop',
+        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_assisted_teleop.py

@@ -0,0 +1,56 @@
+#! /usr/bin/env python3
+# Copyright 2022 Joshua Wallace
+#
+# 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 time import sleep
+
+from geometry_msgs.msg import PoseStamped
+from nav2_simple_commander.robot_navigator import BasicNavigator
+import rclpy
+
+"""
+Basic navigation demo to go to pose.
+"""
+
+
+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()
+
+    navigator.assistedTeleop(time_allowance=20)
+    while not navigator.isTaskComplete():
+        # Publish twist commands to be filtered by the assisted teleop action
+        sleep(0.2)
+        pass
+
+    navigator.lifecycleShutdown()
+    exit(0)
+
+
+if __name__ == '__main__':
+    main()

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 BackUp, Spin
+from nav2_msgs.action import AssistedTeleop, BackUp, Spin
 from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose
 from nav2_msgs.action import FollowPath, FollowWaypoints, NavigateThroughPoses, NavigateToPose
 from nav2_msgs.action import SmoothPath
@@ -75,6 +75,7 @@ def __init__(self):
         self.smoother_client = ActionClient(self, SmoothPath, 'smooth_path')
         self.spin_client = ActionClient(self, Spin, 'spin')
         self.backup_client = ActionClient(self, BackUp, 'backup')
+        self.assisted_teleop_client = ActionClient(self, AssistedTeleop, 'assisted_teleop')
         self.localization_pose_sub = self.create_subscription(PoseWithCovarianceStamped,
                                                               'amcl_pose',
                                                               self._amclPoseCallback,
@@ -222,6 +223,26 @@ 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 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):
+            self.info("'assisted_teleop' action server not available, waiting...")
+        goal_msg = AssistedTeleop.Goal()
+        goal_msg.time_allowance = Duration(sec=time_allowance)
+
+        self.info("Running 'assisted_teleop'....")
+        send_goal_future = \
+            self.assisted_teleop_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('Assisted Teleop request was rejected!')
+            return False
+
+        self.result_future = self.goal_handle.get_result_async()
+        return True
+
     def followPath(self, path, controller_id='', goal_checker_id=''):
         """Send a `FollowPath` action request."""
         self.debug("Waiting for 'FollowPath' action server")

nav2_simple_commander/setup.py

@@ -33,6 +33,7 @@
                 'demo_inspection = nav2_simple_commander.demo_inspection:main',
                 'demo_security = nav2_simple_commander.demo_security:main',
                 'demo_recoveries = nav2_simple_commander.demo_recoveries:main',
+                'example_assisted_teleop = nav2_simple_commander.example_assisted_teleop:main',
         ],
     },
 )