diff --git a/.circleci/config.yml b/.circleci/config.yml index b4028f653dc..ede7b367e94 100644 --- a/.circleci/config.yml +++ b/.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\ -\ @@ -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 }}\ diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index 7d38f381670..579a2fd0ce9 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/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( diff --git a/nav2_simple_commander/launch/assisted_teleop_example_launch.py b/nav2_simple_commander/launch/assisted_teleop_example_launch.py new file mode 100644 index 00000000000..59bd6ea6290 --- /dev/null +++ b/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 diff --git a/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py b/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py new file mode 100644 index 00000000000..564342e5417 --- /dev/null +++ b/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() diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 5807ca09b1e..59a8599b037 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 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") diff --git a/nav2_simple_commander/setup.py b/nav2_simple_commander/setup.py index ab19a26e473..c7b12a3fcfa 100644 --- a/nav2_simple_commander/setup.py +++ b/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', ], }, )