diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md new file mode 100644 index 0000000000..707e372294 --- /dev/null +++ b/nav2_simple_commander/README.md @@ -0,0 +1,111 @@ +# Nav2 Simple (Python3) Commander + +## Overview + +The goal of this package is to provide a "navigation as a library" capability to Python3 users. We provide an API that handles all the ROS2-y and Action Server-y things for you such that you can focus on building an application leveraging the capabilities of Nav2. We also provide you with demos and examples of API usage to build common basic capabilities in autonomous mobile robotics. + +This was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) at [Samsung Research](https://www.sra.samsung.com/), with initial prototypes being prepared for the Keynote at the [2021 ROS Developers Day](https://www.theconstructsim.com/ros-developers-day-2021/) conference (code can be found [here](https://github.com/SteveMacenski/nav2_rosdevday_2021)). + +![](media/readme.gif) + +## API + +The methods provided by the basic navigator are shown below, with inputs and expected returns. If a server fails, it may throw an exception or return a `None` object, so please be sure to properly wrap your navigation calls in try/catch and check results for `None` type. + +| Robot Navigator Method | Description | +| --------------------------------- | -------------------------------------------------------------------------- | +| setInitialPose(initial_pose) | Sets the initial pose (`PoseStamped`) of the robot to localization. | +| 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. | +| 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. | +| 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. | +| clearAllCostmaps() | Clears both the global and local costmaps. | +| clearLocalCostmap() | Clears the local costmap. | +| clearGlobalCostmap() | Clears the global costmap. | +| getGlobalCostmap() | Returns the global costmap, `nav2_msgs/Costmap` | +| getLocalCostmap() | Returns the local costmap, `nav2_msgs/Costmap` | +| waitUntilNav2Active() | Blocks until Nav2 is completely online and lifecycle nodes are in the active state. To be used in conjunction with autostart or external lifecycle bringup. | +| lifecycleStartup() | Sends a request to all lifecycle management servers to bring them into the active state, to be used if autostart is `false` and you want this program to control Nav2's lifecycle. | +| lifecycleShutdown() | Sends a request to all lifecycle management servers to shut them down. | + +A general template for building applications is as follows: + +``` python3 + +from nav2_simple_commander.robot_navigator import BasicNavigator +import rclpy + +rclpy.init() + +nav = BasicNavigator() +... +nav.setInitialPose(init_pose) +nav.waitUntilNav2Active() # if autostarted, else use `lifecycleStartup()` +... +nav.goToPose(goal_pose) +while not nav.isNavComplete(): + feedback = nav.getFeedback() + if feedback.navigation_duration > 600: + nav.cancelNav() +... +result = nav.getResult() +if result == NavigationResult.SUCCEEDED: + print('Goal succeeded!') +elif result == NavigationResult.CANCELED: + print('Goal was canceled!') +elif result == NavigationResult.FAILED: + print('Goal failed!') +``` + +## Usage of Demos and Examples + +Make sure to install the `aws_robomaker_small_warehouse_world` package or build it in your local workspace alongside Nav2. It can be found [here](https://github.com/aws-robotics/aws-robomaker-small-warehouse-world). The demonstrations, examples, and launch files assume you're working with this gazebo world (such that the hard-programmed shelf locations and routes highlighting the API are meaningful). + +Make sure you have set the model directory of turtlebot3 simulation and aws warehouse world to the `GAZEBO_MODEL_PATH`. There are 2 main ways to run the demos of the `nav2_simple_commander` API. + +### Automatically + +The main benefit of this is automatically showing the above demonstrations in a single command for the default robot model and world. This will make use of Nav2's default robot and parameters set out in the main simulation launch file in `nav2_bringup`. + +``` bash +# Launch the launch file for the demo / example +ros2 launch nav2_simple_commander demo_security_launch.py +``` + +This will bring up the robot in the AWS Warehouse in a reasonable position, launch the autonomy script, and complete some task to demonstrate the `nav2_simple_commander` API. + +### Manually + +The main benefit of this is to be able to launch alternative robot models or different navigation configurations than the default for a specific technology demonstation. As long as Nav2 and the simulation (or physical robot) is running, the simple python commander examples / demos don't care what the robot is or how it got there. Since the examples / demos do contain hard-programmed item locations or routes, you should still utilize the AWS Warehouse. Obviously these are easy to update if you wish to adapt these examples / demos to another environment. + +``` bash +# Terminal 1: launch your robot navigation and simulation (or physical robot). For example +ros2 launch nav2_bringup tb3_simulation_launch.py world:=/path/to/aws_robomaker_small_warehouse_world/.world map:=/path/to/aws_robomaker_small_warehouse_world/.yaml + +# Terminal 2: launch your autonomy / application demo or example. For example +ros2 run nav2_simple_commander demo_security +``` + +Then you should see the autonomy application running! + +## Examples + +The `nav2_simple_commander` has a few examples to highlight the API functions available to you as a user: + +- `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. + +## 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: + +- `demo_security.py` - A simple security robot application, showing how to have a robot follow a security route using Navigate Through Poses to do a patrol route, indefinitely. +- `demo_picking.py` - A simple item picking application, showing how to have a robot drive to a specific shelf in a warehouse to either pick an item or have a person place an item into a basket and deliver it to a destination for shipping using Navigate To Pose. +- `demo_inspection.py` - A simple shelf inspection application, showing how to use the Waypoint Follower and task executors to take pictures, RFID scans, etc of shelves to analyze the current shelf statuses and locate items in the warehouse. diff --git a/nav2_simple_commander/launch/inspection_demo_launch.py b/nav2_simple_commander/launch/inspection_demo_launch.py new file mode 100644 index 0000000000..3cfbd1a2c3 --- /dev/null +++ b/nav2_simple_commander/launch/inspection_demo_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_inspection', + 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/launch/nav_through_poses_example_launch.py b/nav2_simple_commander/launch/nav_through_poses_example_launch.py new file mode 100644 index 0000000000..11f39b9a37 --- /dev/null +++ b/nav2_simple_commander/launch/nav_through_poses_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_nav_through_poses', + 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/launch/nav_to_pose_example_launch.py b/nav2_simple_commander/launch/nav_to_pose_example_launch.py new file mode 100644 index 0000000000..7d019884fe --- /dev/null +++ b/nav2_simple_commander/launch/nav_to_pose_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_nav_to_pose', + 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/launch/picking_demo_launch.py b/nav2_simple_commander/launch/picking_demo_launch.py new file mode 100644 index 0000000000..5978570243 --- /dev/null +++ b/nav2_simple_commander/launch/picking_demo_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_picking', + 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/launch/security_demo_launch.py b/nav2_simple_commander/launch/security_demo_launch.py new file mode 100644 index 0000000000..55ef0fe65a --- /dev/null +++ b/nav2_simple_commander/launch/security_demo_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_security', + 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/launch/warehouse.world b/nav2_simple_commander/launch/warehouse.world new file mode 100644 index 0000000000..639965a031 --- /dev/null +++ b/nav2_simple_commander/launch/warehouse.world @@ -0,0 +1,682 @@ + + + + + 0 0 -9.8 + + 0.001 + 1 + 1000 + + + + + + + + model://aws_robomaker_warehouse_ShelfF_01 + + -5.795143 -0.956635 0 0 0 0 + + + + + model://aws_robomaker_warehouse_WallB_01 + + 0.0 0.0 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfE_01 + + 4.73156 0.57943 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfE_01 + + 4.73156 -4.827049 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfE_01 + + 4.73156 -8.6651 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfD_01 + + 4.73156 -1.242668 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfD_01 + + 4.73156 -3.038551 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfD_01 + + 4.73156 -6.750542 0 0 0 0 + + + + + + + model://aws_robomaker_warehouse_GroundB_01 + + 0.0 0.0 -0.090092 0 0 0 + + + + + model://aws_robomaker_warehouse_Lamp_01 + + 0 0 -4 0 0 0 + + + + + + + model://aws_robomaker_warehouse_Bucket_01 + + 0.433449 9.631706 0 0 0 -1.563161 + + + + + model://aws_robomaker_warehouse_Bucket_01 + + -1.8321 -6.3752 0 0 0 -1.563161 + + + + + model://aws_robomaker_warehouse_Bucket_01 + + 0.433449 8.59 0 0 0 -1.563161 + + + + + model://aws_robomaker_warehouse_ClutteringA_01 + + 5.708138 8.616844 -0.017477 0 0 0 + + + + + model://aws_robomaker_warehouse_ClutteringA_01 + + 3.408638 8.616844 -0.017477 0 0 0 + + + + + model://aws_robomaker_warehouse_ClutteringA_01 + + -1.491287 5.222435 -0.017477 0 0 -1.583185 + + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 3.324959 3.822449 -0.012064 0 0 1.563871 + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 5.54171 3.816475 -0.015663 0 0 -1.583191 + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 5.384239 6.137154 0 0 0 3.150000 + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 3.236 6.137154 0 0 0 3.150000 + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + -1.573677 2.301994 -0.015663 0 0 -3.133191 + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + -1.2196 9.407 -0.015663 0 0 1.563871 + + + + + model://aws_robomaker_warehouse_ClutteringD_01 + + -1.634682 -7.811813 -0.319559 0 0 0 + + + + + model://aws_robomaker_warehouse_TrashCanC_01 + + -1.592441 7.715420 0 0 0 0 + + + + + + model://aws_robomaker_warehouse_PalletJackB_01 + + -0.276098 -9.481944 0.023266 0 0 0 + + + + + 0 0 9 0 0 0 + 0.5 0.5 0.5 1 + 0.2 0.2 0.2 1 + + 80 + 0.3 + 0.01 + 0.001 + + 1 + 0.1 0.1 -1 + + + + + -4.70385 10.895 16.2659 -0 0.921795 -1.12701 + orbit + perspective + + + + + 3.45 2.15 0.01 0.0 0.0 3.14 + + + + + + + -0.064 0 0.048 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 1.0 + + + + -0.064 0 0.048 0 0 0 + + + 0.265 0.265 0.089 + + + + + + -0.064 0 0 0 0 0 + + + model://turtlebot3_waffle/meshes/waffle_base.dae + 0.001 0.001 0.001 + + + + + + + + true + 200 + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + + false + + + ~/out:=imu + + + + + + + + -0.052 0 0.111 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.125 + + + + -0.052 0 0.111 0 0 0 + + + 0.0508 + 0.055 + + + + + + -0.064 0 0.121 0 0 0 + + + model://turtlebot3_waffle/meshes/lds.dae + 0.001 0.001 0.001 + + + + + + true + true + -0.064 0 0.121 0 0 0 + 5 + + + + 360 + 1.000000 + 0.000000 + 6.280000 + + + + 0.120000 + 3.5 + 0.015000 + + + gaussian + 0.0 + 0.01 + + + + + + ~/out:=scan + + sensor_msgs/LaserScan + base_scan + + + + + + + + 0.0 0.144 0.023 -1.57 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.1 + + + + 0.0 0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + 0.0 0.144 0.023 0 0 0 + + + model://turtlebot3_waffle/meshes/tire.dae + 0.001 0.001 0.001 + + + + + + + + + 0.0 -0.144 0.023 -1.57 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.1 + + + + 0.0 -0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + 0.0 -0.144 0.023 0 0 0 + + + model://turtlebot3_waffle/meshes/tire.dae + 0.001 0.001 0.001 + + + + + + + -0.177 -0.064 -0.004 0 0 0 + + 0.001 + + 0.00001 + 0.000 + 0.000 + 0.00001 + 0.000 + 0.00001 + + + + + + 0.005000 + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + -0.177 0.064 -0.004 0 0 0 + + 0.001 + + 0.00001 + 0.000 + 0.000 + 0.00001 + 0.000 + 0.00001 + + + + + + 0.005000 + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + base_footprint + base_link + 0.0 0.0 0.010 0 0 0 + + + + base_link + wheel_left_link + 0.0 0.144 0.023 -1.57 0 0 + + 0 0 1 + + + + + base_link + wheel_right_link + 0.0 -0.144 0.023 -1.57 0 0 + + 0 0 1 + + + + + base_link + caster_back_right_link + + + + base_link + caster_back_left_link + + + + base_link + base_scan + -0.064 0 0.121 0 0 0 + + 0 0 1 + + + + + + + + + + 30 + + + wheel_left_joint + wheel_right_joint + + + 0.287 + 0.066 + + + 20 + 1.0 + + cmd_vel + + + true + true + false + + odom + odom + base_footprint + + + + + + + ~/out:=joint_states + + 30 + wheel_left_joint + wheel_right_joint + + + + + + diff --git a/nav2_simple_commander/launch/waypoint_follower_example_launch.py b/nav2_simple_commander/launch/waypoint_follower_example_launch.py new file mode 100644 index 0000000000..eddcc0d4b5 --- /dev/null +++ b/nav2_simple_commander/launch/waypoint_follower_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_waypoint_follower', + 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/media/readme.gif b/nav2_simple_commander/media/readme.gif new file mode 100644 index 0000000000..d3a925a252 Binary files /dev/null and b/nav2_simple_commander/media/readme.gif differ diff --git a/nav2_simple_commander/nav2_simple_commander/__init__.py b/nav2_simple_commander/nav2_simple_commander/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py new file mode 100644 index 0000000000..111e54dc9d --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py @@ -0,0 +1,101 @@ +#! /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 copy import deepcopy + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +import rclpy + +""" +Basic stock inspection demo. In this demonstration, the expectation +is that there are cameras or RFID sensors mounted on the robots +collecting information about stock quantity and location. +""" + + +def main(): + rclpy.init() + + navigator = BasicNavigator() + + # Inspection route, probably read in from a file for a real application + # from either a map or drive and repeat. + inspection_route = [ + [3.461, -0.450], + [5.531, -0.450], + [3.461, -2.200], + [5.531, -2.200], + [3.661, -4.121], + [5.431, -4.121], + [3.661, -5.850], + [5.431, -5.800]] + + # 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() + + # Send our route + inspection_points = [] + inspection_pose = PoseStamped() + inspection_pose.header.frame_id = 'map' + inspection_pose.header.stamp = navigator.get_clock().now().to_msg() + inspection_pose.pose.orientation.z = 1.0 + inspection_pose.pose.orientation.w = 0.0 + for pt in inspection_route: + inspection_pose.pose.position.x = pt[0] + inspection_pose.pose.position.y = pt[1] + inspection_points.append(deepcopy(inspection_pose)) + navigator.followWaypoints(inspection_points) + + # 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(): + 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: + print('Inspection of shelves complete! Returning to start...') + elif result == NavigationResult.CANCELED: + print('Inspection of shelving was canceled. Returning to start...') + exit(1) + elif result == NavigationResult.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(): + pass + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/demo_picking.py b/nav2_simple_commander/nav2_simple_commander/demo_picking.py new file mode 100644 index 0000000000..8dbf0f7b8e --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/demo_picking.py @@ -0,0 +1,123 @@ +#! /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 +from rclpy.duration import Duration + + +# Shelf positions for picking +shelf_positions = { + 'shelf_A': [-3.829, -7.604], + 'shelf_B': [-3.791, -3.287], + 'shelf_C': [-3.791, 1.254], + 'shelf_D': [-3.24, 5.861]} + +# Shipping destination for picked products +shipping_destinations = { + 'recycling': [-0.205, 7.403], + 'pallet_jack7': [-0.073, -8.497], + 'conveyer_432': [6.217, 2.153], + 'frieght_bay_3': [-6.349, 9.147]} + +""" +Basic item picking demo. In this demonstration, the expectation +is that there is a person at the item shelf to put the item on the robot +and at the pallet jack to remove it +(probably with some kind of button for 'got item, robot go do next task'). +""" + + +def main(): + # Recieved virtual request for picking item at Shelf A and bring to + # worker at the pallet jack 7 for shipping. This request would + # contain the shelf ID ("shelf_A") and shipping destination ("pallet_jack7") + #################### + request_item_location = 'shelf_C' + request_destination = 'pallet_jack7' + #################### + + 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() + + shelf_item_pose = PoseStamped() + shelf_item_pose.header.frame_id = 'map' + shelf_item_pose.header.stamp = navigator.get_clock().now().to_msg() + shelf_item_pose.pose.position.x = shelf_positions[request_item_location][0] + shelf_item_pose.pose.position.y = shelf_positions[request_item_location][1] + shelf_item_pose.pose.orientation.z = 1.0 + shelf_item_pose.pose.orientation.w = 0.0 + print('Received request for item picking at ' + request_item_location + '.') + navigator.goToPose(shelf_item_pose) + + # Do something during our route + # (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(): + i += 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print('Estimated time of arrival at ' + request_item_location + + ' for worker: ' + '{0:.0f}'.format( + Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9) + + ' seconds.') + + result = navigator.getResult() + if result == NavigationResult.SUCCEEDED: + print('Got product from ' + request_item_location + + '! Bringing product to shipping destination (' + request_destination + ')...') + shipping_destination = PoseStamped() + shipping_destination.header.frame_id = 'map' + shipping_destination.header.stamp = navigator.get_clock().now().to_msg() + shipping_destination.pose.position.x = shipping_destinations[request_destination][0] + shipping_destination.pose.position.y = shipping_destinations[request_destination][1] + shipping_destination.pose.orientation.z = 1.0 + shipping_destination.pose.orientation.w = 0.0 + navigator.goToPose(shipping_destination) + + elif result == NavigationResult.CANCELED: + print('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: + print('Task at ' + request_item_location + ' failed!') + exit(-1) + + while not navigator.isNavComplete(): + pass + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/demo_security.py b/nav2_simple_commander/nav2_simple_commander/demo_security.py new file mode 100644 index 0000000000..8180bc36b4 --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/demo_security.py @@ -0,0 +1,107 @@ +#! /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 copy import deepcopy + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult + +import rclpy +from rclpy.duration import Duration + + +""" +Basic security route patrol demo. In this demonstration, the expectation +is that there are security cameras mounted on the robots recording or being +watched live by security staff. +""" + + +def main(): + rclpy.init() + + navigator = BasicNavigator() + + # Security route, probably read in from a file for a real application + # from either a map or drive and repeat. + security_route = [ + [1.792, 2.144], + [1.792, -5.44], + [1.792, -9.427], + [-3.665, -9.427], + [-3.665, -4.303], + [-3.665, 2.330], + [-3.665, 9.283]] + + # 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() + + # Do security route until dead + while rclpy.ok(): + # Send our route + route_poses = [] + pose = PoseStamped() + pose.header.frame_id = 'map' + pose.header.stamp = navigator.get_clock().now().to_msg() + pose.pose.orientation.w = 1.0 + for pt in security_route: + pose.pose.position.x = pt[0] + pose.pose.position.y = pt[1] + route_poses.append(deepcopy(pose)) + navigator.goThroughPoses(route_poses) + + # 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(): + i += 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print('Estimated time to complete current route: ' + '{0:.0f}'.format( + Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9) + + ' seconds.') + + # 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() + + # If at end of route, reverse the route to restart + security_route.reverse() + + result = navigator.getResult() + if result == NavigationResult.SUCCEEDED: + print('Route complete! Restarting...') + elif result == NavigationResult.CANCELED: + print('Security route was canceled, exiting.') + exit(1) + elif result == NavigationResult.FAILED: + print('Security route failed! Restarting from other side...') + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py new file mode 100644 index 0000000000..f4523b8974 --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py @@ -0,0 +1,139 @@ +#! /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 +from rclpy.duration import Duration + +""" +Basic navigation demo to go to poses. +""" + + +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) + + # Activate navigation, if not autostarted. This should be called after setInitialPose() + # or this will initialize at the origin of the map and update the costmap with bogus readings. + # If autostart, you should `waitUntilNav2Active()` instead. + # navigator.lifecycleStartup() + + # Wait for navigation to fully activate, since autostarting nav2 + navigator.waitUntilNav2Active() + + # If desired, you can change or load the map as well + # navigator.changeMap('/path/to/map.yaml') + + # You may use the navigator to clear or obtain costmaps + # navigator.clearAllCostmaps() # also have clearLocalCostmap() and clearGlobalCostmap() + # global_costmap = navigator.getGlobalCostmap() + # local_costmap = navigator.getLocalCostmap() + + # set our demo's goal poses + goal_poses = [] + goal_pose1 = PoseStamped() + goal_pose1.header.frame_id = 'map' + goal_pose1.header.stamp = navigator.get_clock().now().to_msg() + goal_pose1.pose.position.x = 1.5 + goal_pose1.pose.position.y = 0.55 + goal_pose1.pose.orientation.w = 0.707 + goal_pose1.pose.orientation.z = 0.707 + goal_poses.append(goal_pose1) + + # additional goals can be appended + goal_pose2 = PoseStamped() + goal_pose2.header.frame_id = 'map' + goal_pose2.header.stamp = navigator.get_clock().now().to_msg() + goal_pose2.pose.position.x = 1.5 + goal_pose2.pose.position.y = -3.75 + goal_pose2.pose.orientation.w = 0.707 + goal_pose2.pose.orientation.z = 0.707 + goal_poses.append(goal_pose2) + goal_pose3 = PoseStamped() + goal_pose3.header.frame_id = 'map' + goal_pose3.header.stamp = navigator.get_clock().now().to_msg() + goal_pose3.pose.position.x = -3.6 + goal_pose3.pose.position.y = -4.75 + goal_pose3.pose.orientation.w = 0.707 + goal_pose3.pose.orientation.z = 0.707 + goal_poses.append(goal_pose3) + + # sanity check a valid path exists + # path = navigator.getPathThroughPoses(initial_pose, goal_poses) + + navigator.goThroughPoses(goal_poses) + + i = 0 + while not navigator.isNavComplete(): + ################################################ + # + # Implement some code here for your application! + # + ################################################ + + # Do something with the feedback + i = i + 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print('Estimated time of arrival: ' + '{0:.0f}'.format( + Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9) + + ' seconds.') + + # Some navigation timeout to demo cancellation + if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0): + navigator.cancelNav() + + # Some navigation request change to demo preemption + if Duration.from_msg(feedback.navigation_time) > Duration(seconds=35.0): + goal_pose4 = PoseStamped() + goal_pose4.header.frame_id = 'map' + goal_pose4.header.stamp = navigator.get_clock().now().to_msg() + goal_pose4.pose.position.x = -5.0 + goal_pose4.pose.position.y = -4.75 + goal_pose4.pose.orientation.w = 0.707 + goal_pose4.pose.orientation.z = 0.707 + navigator.goThroughPoses([goal_pose4]) + + # 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/example_nav_to_pose.py b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py new file mode 100644 index 0000000000..cd302e418c --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py @@ -0,0 +1,112 @@ +#! /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 +from rclpy.duration import Duration + +""" +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) + + # Activate navigation, if not autostarted. This should be called after setInitialPose() + # or this will initialize at the origin of the map and update the costmap with bogus readings. + # If autostart, you should `waitUntilNav2Active()` instead. + # navigator.lifecycleStartup() + + # Wait for navigation to fully activate, since autostarting nav2 + navigator.waitUntilNav2Active() + + # If desired, you can change or load the map as well + # navigator.changeMap('/path/to/map.yaml') + + # You may use the navigator to clear or obtain costmaps + # navigator.clearAllCostmaps() # also have clearLocalCostmap() and clearGlobalCostmap() + # global_costmap = navigator.getGlobalCostmap() + # local_costmap = navigator.getLocalCostmap() + + # 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 = -2.0 + goal_pose.pose.position.y = -0.5 + goal_pose.pose.orientation.w = 1.0 + + # sanity check a valid path exists + # path = navigator.getPath(initial_pose, goal_pose) + + navigator.goToPose(goal_pose) + + i = 0 + while not navigator.isNavComplete(): + ################################################ + # + # Implement some code here for your application! + # + ################################################ + + # Do something with the feedback + i = i + 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print('Estimated time of arrival: ' + '{0:.0f}'.format( + Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9) + + ' seconds.') + + # Some navigation timeout to demo cancellation + if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0): + navigator.cancelNav() + + # Some navigation request change to demo preemption + if Duration.from_msg(feedback.navigation_time) > Duration(seconds=18.0): + goal_pose.pose.position.x = -3.0 + navigator.goToPose(goal_pose) + + # 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/example_waypoint_follower.py b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py new file mode 100644 index 0000000000..8ed0fc3d16 --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py @@ -0,0 +1,142 @@ +#! /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 +from rclpy.duration import Duration + +""" +Basic navigation demo to go to poses. +""" + + +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) + + # Activate navigation, if not autostarted. This should be called after setInitialPose() + # or this will initialize at the origin of the map and update the costmap with bogus readings. + # If autostart, you should `waitUntilNav2Active()` instead. + # navigator.lifecycleStartup() + + # Wait for navigation to fully activate, since autostarting nav2 + navigator.waitUntilNav2Active() + + # If desired, you can change or load the map as well + # navigator.changeMap('/path/to/map.yaml') + + # You may use the navigator to clear or obtain costmaps + # navigator.clearAllCostmaps() # also have clearLocalCostmap() and clearGlobalCostmap() + # global_costmap = navigator.getGlobalCostmap() + # local_costmap = navigator.getLocalCostmap() + + # set our demo's goal poses to follow + goal_poses = [] + goal_pose1 = PoseStamped() + goal_pose1.header.frame_id = 'map' + goal_pose1.header.stamp = navigator.get_clock().now().to_msg() + goal_pose1.pose.position.x = 1.5 + goal_pose1.pose.position.y = 0.55 + goal_pose1.pose.orientation.w = 0.707 + goal_pose1.pose.orientation.z = 0.707 + goal_poses.append(goal_pose1) + + # additional goals can be appended + goal_pose2 = PoseStamped() + goal_pose2.header.frame_id = 'map' + goal_pose2.header.stamp = navigator.get_clock().now().to_msg() + goal_pose2.pose.position.x = 1.5 + goal_pose2.pose.position.y = -3.75 + goal_pose2.pose.orientation.w = 0.707 + goal_pose2.pose.orientation.z = 0.707 + goal_poses.append(goal_pose2) + goal_pose3 = PoseStamped() + goal_pose3.header.frame_id = 'map' + goal_pose3.header.stamp = navigator.get_clock().now().to_msg() + goal_pose3.pose.position.x = -3.6 + goal_pose3.pose.position.y = -4.75 + goal_pose3.pose.orientation.w = 0.707 + goal_pose3.pose.orientation.z = 0.707 + goal_poses.append(goal_pose3) + + # sanity check a valid path exists + # path = navigator.getPath(initial_pose, goal_pose1) + + nav_start = navigator.get_clock().now() + navigator.followWaypoints(goal_poses) + + i = 0 + while not navigator.isNavComplete(): + ################################################ + # + # Implement some code here for your application! + # + ################################################ + + # Do something with the feedback + i = i + 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print('Executing current waypoint: ' + + str(feedback.current_waypoint + 1) + '/' + str(len(goal_poses))) + now = navigator.get_clock().now() + + # Some navigation timeout to demo cancellation + if now - nav_start > Duration(seconds=600.0): + navigator.cancelNav() + + # Some follow waypoints request change to demo preemption + if now - nav_start > Duration(seconds=35.0): + goal_pose4 = PoseStamped() + goal_pose4.header.frame_id = 'map' + goal_pose4.header.stamp = now.to_msg() + goal_pose4.pose.position.x = -5.0 + goal_pose4.pose.position.y = -4.75 + goal_pose4.pose.orientation.w = 0.707 + goal_pose4.pose.orientation.z = 0.707 + goal_poses = [goal_pose4] + nav_start = now + navigator.followWaypoints(goal_poses) + + # 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 new file mode 100644 index 0000000000..20428fdf1b --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -0,0 +1,421 @@ +#! /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 enum import Enum +import time + +from action_msgs.msg import GoalStatus +from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseWithCovarianceStamped +from lifecycle_msgs.srv import GetState +from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose +from nav2_msgs.action import FollowWaypoints, NavigateThroughPoses, NavigateToPose +from nav2_msgs.srv import ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes + +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy +from rclpy.qos import QoSProfile, QoSReliabilityPolicy + + +class NavigationResult(Enum): + UKNOWN = 0 + SUCCEEDED = 1 + CANCELED = 2 + FAILED = 3 + + +class BasicNavigator(Node): + + def __init__(self): + super().__init__(node_name='basic_navigator') + self.initial_pose = PoseStamped() + self.initial_pose.header.frame_id = 'map' + self.goal_handle = None + self.result_future = None + self.feedback = None + self.status = None + + amcl_pose_qos = QoSProfile( + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1) + + self.initial_pose_received = False + self.nav_through_poses_client = ActionClient(self, + NavigateThroughPoses, + 'navigate_through_poses') + self.nav_to_pose_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') + self.follow_waypoints_client = ActionClient(self, FollowWaypoints, 'follow_waypoints') + self.compute_path_to_pose_client = ActionClient(self, ComputePathToPose, + 'compute_path_to_pose') + self.compute_path_through_poses_client = ActionClient(self, ComputePathThroughPoses, + 'compute_path_through_poses') + self.localization_pose_sub = self.create_subscription(PoseWithCovarianceStamped, + 'amcl_pose', + self._amclPoseCallback, + amcl_pose_qos) + self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, + 'initialpose', + 10) + self.change_maps_srv = self.create_client(LoadMap, '/map_server/load_map') + self.clear_costmap_global_srv = self.create_client( + ClearEntireCostmap, '/global_costmap/clear_entirely_global_costmap') + self.clear_costmap_local_srv = self.create_client( + ClearEntireCostmap, '/local_costmap/clear_entirely_local_costmap') + self.get_costmap_global_srv = self.create_client(GetCostmap, '/global_costmap/get_costmap') + self.get_costmap_local_srv = self.create_client(GetCostmap, '/local_costmap/get_costmap') + + def setInitialPose(self, initial_pose): + """Set the initial pose to the localization system.""" + self.initial_pose_received = False + self.initial_pose = initial_pose + self._setInitialPose() + + def goThroughPoses(self, poses): + """Send a `NavThroughPoses` action request.""" + self.debug("Waiting for 'NavigateThroughPoses' action server") + while not self.nav_through_poses_client.wait_for_server(timeout_sec=1.0): + self.info("'NavigateThroughPoses' action server not available, waiting...") + + goal_msg = NavigateThroughPoses.Goal() + goal_msg.poses = poses + + self.info('Navigating with ' + str(len(goal_msg.poses)) + ' goals.' + '...') + send_goal_future = self.nav_through_poses_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('Goal with ' + str(len(poses)) + ' poses was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + + def goToPose(self, pose): + """Send a `NavToPose` action request.""" + self.debug("Waiting for 'NavigateToPose' action server") + while not self.nav_to_pose_client.wait_for_server(timeout_sec=1.0): + self.info("'NavigateToPose' action server not available, waiting...") + + goal_msg = NavigateToPose.Goal() + goal_msg.pose = pose + + self.info('Navigating to goal: ' + str(pose.pose.position.x) + ' ' + + str(pose.pose.position.y) + '...') + send_goal_future = self.nav_to_pose_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('Goal to ' + str(pose.pose.position.x) + ' ' + + str(pose.pose.position.y) + ' was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + + def followWaypoints(self, poses): + """Send a `FollowWaypoints` action request.""" + self.debug("Waiting for 'FollowWaypoints' action server") + while not self.follow_waypoints_client.wait_for_server(timeout_sec=1.0): + self.info("'FollowWaypoints' action server not available, waiting...") + + goal_msg = FollowWaypoints.Goal() + goal_msg.poses = poses + + self.info('Following ' + str(len(goal_msg.poses)) + ' goals.' + '...') + send_goal_future = self.follow_waypoints_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('Following ' + str(len(poses)) + ' waypoints request 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.') + if self.result_future: + future = self.goal_handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, future) + return + + def isNavComplete(self): + """Check if the navigation request of any type is complete yet.""" + if not self.result_future: + # task was cancelled or completed + return True + rclpy.spin_until_future_complete(self, self.result_future, timeout_sec=0.10) + if self.result_future.result(): + self.status = self.result_future.result().status + if self.status != GoalStatus.STATUS_SUCCEEDED: + self.debug('Goal with failed with status code: {0}'.format(self.status)) + return True + else: + # Timed out, still processing, not complete yet + return False + + self.debug('Goal succeeded!') + return True + + def getFeedback(self): + """Get the pending action feedback message.""" + return self.feedback + + def getResult(self): + """Get the pending action result message.""" + if self.status == GoalStatus.STATUS_SUCCEEDED: + return NavigationResult.SUCCEEDED + elif self.status == GoalStatus.STATUS_ABORTED: + return NavigationResult.FAILED + elif self.status == GoalStatus.STATUS_CANCELED: + return NavigationResult.CANCELED + else: + return NavigationResult.UNKNOWN + + def waitUntilNav2Active(self): + """Block until the full navigation system is up and running.""" + self._waitForNodeToActivate('amcl') + self._waitForInitialPose() + self._waitForNodeToActivate('bt_navigator') + self.info('Nav2 is ready for use!') + return + + def getPath(self, start, goal): + """Send a `ComputePathToPose` action request.""" + self.debug("Waiting for 'ComputePathToPose' action server") + while not self.compute_path_to_pose_client.wait_for_server(timeout_sec=1.0): + self.info("'ComputePathToPose' action server not available, waiting...") + + goal_msg = ComputePathToPose.Goal() + goal_msg.goal = goal + goal_msg.start = start + + self.info('Getting path...') + send_goal_future = self.compute_path_to_pose_client.send_goal_async(goal_msg) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error('Get path was rejected!') + return None + + self.result_future = self.goal_handle.get_result_async() + rclpy.spin_until_future_complete(self, self.result_future) + self.status = self.result_future.result().status + if self.status != GoalStatus.STATUS_SUCCEEDED: + self.warn('Getting path failed with status code: {0}'.format(self.status)) + return None + + return self.result_future.result().result.path + + def getPathThroughPoses(self, start, goals): + """Send a `ComputePathThroughPoses` action request.""" + self.debug("Waiting for 'ComputePathThroughPoses' action server") + while not self.compute_path_through_poses_client.wait_for_server(timeout_sec=1.0): + self.info("'ComputePathThroughPoses' action server not available, waiting...") + + goal_msg = ComputePathThroughPoses.Goal() + goal_msg.goals = goals + goal_msg.start = start + + self.info('Getting path...') + send_goal_future = self.compute_path_through_poses_client.send_goal_async(goal_msg) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error('Get path was rejected!') + return None + + self.result_future = self.goal_handle.get_result_async() + rclpy.spin_until_future_complete(self, self.result_future) + self.status = self.result_future.result().status + if self.status != GoalStatus.STATUS_SUCCEEDED: + self.warn('Getting path failed with status code: {0}'.format(self.status)) + return None + + return self.result_future.result().result.path + + def changeMap(self, map_filepath): + """Change the current static map in the map server.""" + while not self.change_maps_srv.wait_for_service(timeout_sec=1.0): + self.info('change map service not available, waiting...') + req = LoadMap.Request() + req.map_url = map_filepath + future = self.change_maps_srv.call_async(req) + rclpy.spin_until_future_complete(self, future) + status = future.result().result + if status != LoadMap.Response().RESULT_SUCCESS: + self.error('Change map request failed!') + else: + self.info('Change map request was successful!') + return + + def clearAllCostmaps(self): + """Clear all costmaps.""" + self.clearLocalCostmap() + self.clearGlobalCostmap() + return + + def clearLocalCostmap(self): + """Clear local costmap.""" + while not self.clear_costmap_local_srv.wait_for_service(timeout_sec=1.0): + self.info('Clear local costmaps service not available, waiting...') + req = ClearEntireCostmap.Request() + future = self.clear_costmap_local_srv.call_async(req) + rclpy.spin_until_future_complete(self, future) + return + + def clearGlobalCostmap(self): + """Clear global costmap.""" + while not self.clear_costmap_global_srv.wait_for_service(timeout_sec=1.0): + self.info('Clear global costmaps service not available, waiting...') + req = ClearEntireCostmap.Request() + future = self.clear_costmap_global_srv.call_async(req) + rclpy.spin_until_future_complete(self, future) + return + + def getGlobalCostmap(self): + """Get the global costmap.""" + while not self.get_costmap_global_srv.wait_for_service(timeout_sec=1.0): + self.info('Get global costmaps service not available, waiting...') + req = GetCostmap.Request() + future = self.get_costmap_global_srv.call_async(req) + rclpy.spin_until_future_complete(self, future) + return future.result().map + + def getLocalCostmap(self): + """Get the local costmap.""" + while not self.get_costmap_local_srv.wait_for_service(timeout_sec=1.0): + self.info('Get local costmaps service not available, waiting...') + req = GetCostmap.Request() + future = self.get_costmap_local_srv.call_async(req) + rclpy.spin_until_future_complete(self, future) + return future.result().map + + def lifecycleStartup(self): + """Startup nav2 lifecycle system.""" + self.info('Starting up lifecycle nodes based on lifecycle_manager.') + for srv_name, srv_type in self.get_service_names_and_types(): + if srv_type[0] == 'nav2_msgs/srv/ManageLifecycleNodes': + self.info('Starting up ' + srv_name) + mgr_client = self.create_client(ManageLifecycleNodes, srv_name) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info(srv_name + ' service not available, waiting...') + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().STARTUP + future = mgr_client.call_async(req) + + # starting up requires a full map->odom->base_link TF tree + # so if we're not successful, try forwarding the initial pose + while True: + rclpy.spin_until_future_complete(self, future, timeout_sec=0.10) + if not future: + self._waitForInitialPose() + else: + break + self.info('Nav2 is ready for use!') + return + + def lifecycleShutdown(self): + """Shutdown nav2 lifecycle system.""" + self.info('Shutting down lifecycle nodes based on lifecycle_manager.') + for srv_name, srv_type in self.get_service_names_and_types(): + if srv_type[0] == 'nav2_msgs/srv/ManageLifecycleNodes': + self.info('Shutting down ' + srv_name) + mgr_client = self.create_client(ManageLifecycleNodes, srv_name) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info(srv_name + ' service not available, waiting...') + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + future.result() + return + + def _waitForNodeToActivate(self, node_name): + # Waits for the node within the tester namespace to become active + self.debug('Waiting for ' + node_name + ' to become active..') + node_service = node_name + '/get_state' + state_client = self.create_client(GetState, node_service) + while not state_client.wait_for_service(timeout_sec=1.0): + self.info(node_service + ' service not available, waiting...') + + req = GetState.Request() + state = 'unknown' + while state != 'active': + self.debug('Getting ' + node_name + ' state...') + future = state_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result() is not None: + state = future.result().current_state.label + self.debug('Result of get_state: %s' % state) + time.sleep(2) + return + + def _waitForInitialPose(self): + while not self.initial_pose_received: + self.info('Setting initial pose') + self._setInitialPose() + self.info('Waiting for amcl_pose to be received') + rclpy.spin_once(self, timeout_sec=1.0) + return + + def _amclPoseCallback(self, msg): + self.debug('Received amcl pose') + self.initial_pose_received = True + return + + def _feedbackCallback(self, msg): + self.debug('Received action feedback message') + self.feedback = msg.feedback + return + + def _setInitialPose(self): + msg = PoseWithCovarianceStamped() + msg.pose.pose = self.initial_pose.pose + msg.header.frame_id = self.initial_pose.header.frame_id + msg.header.stamp = self.initial_pose.header.stamp + self.info('Publishing Initial Pose') + self.initial_pose_pub.publish(msg) + return + + def info(self, msg): + self.get_logger().info(msg) + return + + def warn(self, msg): + self.get_logger().warn(msg) + return + + def error(self, msg): + self.get_logger().error(msg) + return + + def debug(self, msg): + self.get_logger().debug(msg) + return diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml new file mode 100644 index 0000000000..a30175bf7d --- /dev/null +++ b/nav2_simple_commander/package.xml @@ -0,0 +1,25 @@ + + + + nav2_simple_commander + 1.0.0 + An importable library for writing mobile robot applications in python3 + steve + Apache-2.0 + + python-enum34 + rclpy + geometry_msgs + nav2_msgs + action_msgs + lifecycle_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/nav2_simple_commander/resource/nav2_simple_commander b/nav2_simple_commander/resource/nav2_simple_commander new file mode 100644 index 0000000000..e69de29bb2 diff --git a/nav2_simple_commander/setup.cfg b/nav2_simple_commander/setup.cfg new file mode 100644 index 0000000000..2d91d90392 --- /dev/null +++ b/nav2_simple_commander/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/nav2_simple_commander +[install] +install-scripts=$base/lib/nav2_simple_commander diff --git a/nav2_simple_commander/setup.py b/nav2_simple_commander/setup.py new file mode 100644 index 0000000000..ca44856c5a --- /dev/null +++ b/nav2_simple_commander/setup.py @@ -0,0 +1,36 @@ +from glob import glob +import os + +from setuptools import setup + + +package_name = 'nav2_simple_commander' + +setup( + name=package_name, + version='1.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name), glob('launch/*')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='steve', + maintainer_email='stevenmacenski@gmail.com', + description='An importable library for writing mobile robot applications in python3', + license='Apache-2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + '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', + 'demo_picking = nav2_simple_commander.demo_picking:main', + 'demo_inspection = nav2_simple_commander.demo_inspection:main', + 'demo_security = nav2_simple_commander.demo_security:main', + ], + }, +) diff --git a/nav2_simple_commander/test/test_copyright.py b/nav2_simple_commander/test/test_copyright.py new file mode 100644 index 0000000000..cc8ff03f79 --- /dev/null +++ b/nav2_simple_commander/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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 ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/nav2_simple_commander/test/test_flake8.py b/nav2_simple_commander/test/test_flake8.py new file mode 100644 index 0000000000..27ee1078ff --- /dev/null +++ b/nav2_simple_commander/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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 ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/nav2_simple_commander/test/test_pep257.py b/nav2_simple_commander/test/test_pep257.py new file mode 100644 index 0000000000..b234a3840f --- /dev/null +++ b/nav2_simple_commander/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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 ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings'