From 2f5d1e67c67d5cb9341bd7fa7c19c25b3ccd886f Mon Sep 17 00:00:00 2001 From: Ekansh Sharma Date: Sat, 5 Feb 2022 01:49:03 +0100 Subject: [PATCH] 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 | 5 +- .../launch/follow_path_example_launch.py | 77 ++++++++++++++++ .../example_follow_path.py | 92 +++++++++++++++++++ .../nav2_simple_commander/robot_navigator.py | 24 +++++ nav2_simple_commander/setup.py | 1 + 5 files changed, 197 insertions(+), 2 deletions(-) create mode 100644 nav2_simple_commander/launch/follow_path_example_launch.py create mode 100644 nav2_simple_commander/nav2_simple_commander/example_follow_path.py diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index 707e372294..c44d4fea08 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -18,6 +18,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. | @@ -100,8 +101,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: diff --git a/nav2_simple_commander/launch/follow_path_example_launch.py b/nav2_simple_commander/launch/follow_path_example_launch.py new file mode 100644 index 0000000000..41624dff6f --- /dev/null +++ b/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 diff --git a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py new file mode 100644 index 0000000000..fe94f98798 --- /dev/null +++ b/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() diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 80c21cfb58..ce0ee8d01b 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/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.') diff --git a/nav2_simple_commander/setup.py b/nav2_simple_commander/setup.py index ca44856c5a..8fbdd00f11 100644 --- a/nav2_simple_commander/setup.py +++ b/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',