Skip to content

Commit fa1d091

Browse files
SteveMacenskiSteveMacenski
authored
Nav2 Simple (Python3) Commander Library (#2411)
* adding nav2_python_commander package * adding readme * launch files for the python commander examples * renaming to nav2_simple_commander * resolve review comments * fixing rosdep key * fixing up linters
1 parent 40f4c38 commit fa1d091

24 files changed

+2536
-0
lines changed

‎nav2_simple_commander/README.md‎

Lines changed: 111 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,111 @@
1+
# Nav2 Simple (Python3) Commander
2+
3+
## Overview
4+
5+
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.
6+
7+
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)).
8+
9+
![](media/readme.gif)
10+
11+
## API
12+
13+
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.
14+
15+
| Robot Navigator Method | Description |
16+
| --------------------------------- | -------------------------------------------------------------------------- |
17+
| setInitialPose(initial_pose) | Sets the initial pose (`PoseStamped`) of the robot to localization. |
18+
| goThroughPoses(poses) | Requests the robot to drive through a set of poses (list of `PoseStamped`).|
19+
| goToPose(pose) | Requests the robot to drive to a pose (`PoseStamped`). |
20+
| followWaypoints(poses) | Requests the robot to follow a set of waypoints (list of `PoseStamped`). This will execute the specific `TaskExecutor` at each pose. |
21+
| cancelNav() | Cancel an ongoing `goThroughPoses` `goToPose` or `followWaypoints` request.|
22+
| isNavComplete() | Checks if navigation is complete yet, times out at `100ms`. Returns `True` if completed and `False` if still going. |
23+
| getFeedback() | Gets feedback from navigation task, returns action server feedback object. |
24+
| getResult() | Gets final result of navigation task, to be called after `isNavComplete` returns `True`. Returns action server result object. |
25+
| getPath(start, goal) | Gets a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. |
26+
| getPathThroughPoses(start, goals) | Gets a path through a starting to a set of goals, a list of `PoseStamped`, `nav_msgs/Path`. |
27+
| changeMap(map_filepath) | Requests a change from the current map to `map_filepath`'s yaml. |
28+
| clearAllCostmaps() | Clears both the global and local costmaps. |
29+
| clearLocalCostmap() | Clears the local costmap. |
30+
| clearGlobalCostmap() | Clears the global costmap. |
31+
| getGlobalCostmap() | Returns the global costmap, `nav2_msgs/Costmap` |
32+
| getLocalCostmap() | Returns the local costmap, `nav2_msgs/Costmap` |
33+
| 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. |
34+
| 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. |
35+
| lifecycleShutdown() | Sends a request to all lifecycle management servers to shut them down. |
36+
37+
A general template for building applications is as follows:
38+
39+
``` python3
40+
41+
from nav2_simple_commander.robot_navigator import BasicNavigator
42+
import rclpy
43+
44+
rclpy.init()
45+
46+
nav = BasicNavigator()
47+
...
48+
nav.setInitialPose(init_pose)
49+
nav.waitUntilNav2Active() # if autostarted, else use `lifecycleStartup()`
50+
...
51+
nav.goToPose(goal_pose)
52+
while not nav.isNavComplete():
53+
feedback = nav.getFeedback()
54+
if feedback.navigation_duration > 600:
55+
nav.cancelNav()
56+
...
57+
result = nav.getResult()
58+
if result == NavigationResult.SUCCEEDED:
59+
print('Goal succeeded!')
60+
elif result == NavigationResult.CANCELED:
61+
print('Goal was canceled!')
62+
elif result == NavigationResult.FAILED:
63+
print('Goal failed!')
64+
```
65+
66+
## Usage of Demos and Examples
67+
68+
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).
69+
70+
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.
71+
72+
### Automatically
73+
74+
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`.
75+
76+
``` bash
77+
# Launch the launch file for the demo / example
78+
ros2 launch nav2_simple_commander demo_security_launch.py
79+
```
80+
81+
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.
82+
83+
### Manually
84+
85+
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.
86+
87+
``` bash
88+
# Terminal 1: launch your robot navigation and simulation (or physical robot). For example
89+
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
90+
91+
# Terminal 2: launch your autonomy / application demo or example. For example
92+
ros2 run nav2_simple_commander demo_security
93+
```
94+
95+
Then you should see the autonomy application running!
96+
97+
## Examples
98+
99+
The `nav2_simple_commander` has a few examples to highlight the API functions available to you as a user:
100+
101+
- `example_nav_to_pose.py` - Demonstrates the navigate to pose capabilities of the navigator, as well as a number of auxiliary methods.
102+
- `example_nav_through_poses.py` - Demonstrates the navigate through poses capabilities of the navigator, as well as a number of auxiliary methods.
103+
- `example_waypoint_follower.py` - Demonstrates the waypoint following capabilities of the navigator, as well as a number of auxiliary methods required.
104+
105+
## Demos
106+
107+
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:
108+
109+
- `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.
110+
- `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.
111+
- `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.

‎nav2_simple_commander/launch/inspection_demo_launch.py‎

Lines changed: 77 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,77 @@
1+
# Copyright (c) 2021 Samsung Research America
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
import os
16+
17+
from ament_index_python.packages import get_package_share_directory
18+
19+
from launch import LaunchDescription
20+
from launch.actions import ExecuteProcess, IncludeLaunchDescription
21+
from launch.launch_description_sources import PythonLaunchDescriptionSource
22+
from launch_ros.actions import Node
23+
24+
25+
def generate_launch_description():
26+
warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world')
27+
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
28+
python_commander_dir = get_package_share_directory('nav2_simple_commander')
29+
30+
map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml')
31+
world = os.path.join(python_commander_dir, 'warehouse.world')
32+
33+
# start the simulation
34+
start_gazebo_server_cmd = ExecuteProcess(
35+
cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world],
36+
cwd=[warehouse_dir], output='screen')
37+
38+
start_gazebo_client_cmd = ExecuteProcess(
39+
cmd=['gzclient'],
40+
cwd=[warehouse_dir], output='screen')
41+
42+
urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
43+
start_robot_state_publisher_cmd = Node(
44+
package='robot_state_publisher',
45+
executable='robot_state_publisher',
46+
name='robot_state_publisher',
47+
output='screen',
48+
arguments=[urdf])
49+
50+
# start the visualization
51+
rviz_cmd = IncludeLaunchDescription(
52+
PythonLaunchDescriptionSource(
53+
os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
54+
launch_arguments={'namespace': '',
55+
'use_namespace': 'False'}.items())
56+
57+
# start navigation
58+
bringup_cmd = IncludeLaunchDescription(
59+
PythonLaunchDescriptionSource(
60+
os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')),
61+
launch_arguments={'map': map_yaml_file}.items())
62+
63+
# start the demo autonomy task
64+
demo_cmd = Node(
65+
package='nav2_simple_commander',
66+
executable='demo_inspection',
67+
emulate_tty=True,
68+
output='screen')
69+
70+
ld = LaunchDescription()
71+
ld.add_action(start_gazebo_server_cmd)
72+
ld.add_action(start_gazebo_client_cmd)
73+
ld.add_action(start_robot_state_publisher_cmd)
74+
ld.add_action(rviz_cmd)
75+
ld.add_action(bringup_cmd)
76+
ld.add_action(demo_cmd)
77+
return ld

‎nav2_simple_commander/launch/nav_through_poses_example_launch.py‎

Lines changed: 77 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,77 @@
1+
# Copyright (c) 2021 Samsung Research America
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
import os
16+
17+
from ament_index_python.packages import get_package_share_directory
18+
19+
from launch import LaunchDescription
20+
from launch.actions import ExecuteProcess, IncludeLaunchDescription
21+
from launch.launch_description_sources import PythonLaunchDescriptionSource
22+
from launch_ros.actions import Node
23+
24+
25+
def generate_launch_description():
26+
warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world')
27+
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
28+
python_commander_dir = get_package_share_directory('nav2_simple_commander')
29+
30+
map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml')
31+
world = os.path.join(python_commander_dir, 'warehouse.world')
32+
33+
# start the simulation
34+
start_gazebo_server_cmd = ExecuteProcess(
35+
cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world],
36+
cwd=[warehouse_dir], output='screen')
37+
38+
start_gazebo_client_cmd = ExecuteProcess(
39+
cmd=['gzclient'],
40+
cwd=[warehouse_dir], output='screen')
41+
42+
urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
43+
start_robot_state_publisher_cmd = Node(
44+
package='robot_state_publisher',
45+
executable='robot_state_publisher',
46+
name='robot_state_publisher',
47+
output='screen',
48+
arguments=[urdf])
49+
50+
# start the visualization
51+
rviz_cmd = IncludeLaunchDescription(
52+
PythonLaunchDescriptionSource(
53+
os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
54+
launch_arguments={'namespace': '',
55+
'use_namespace': 'False'}.items())
56+
57+
# start navigation
58+
bringup_cmd = IncludeLaunchDescription(
59+
PythonLaunchDescriptionSource(
60+
os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')),
61+
launch_arguments={'map': map_yaml_file}.items())
62+
63+
# start the demo autonomy task
64+
demo_cmd = Node(
65+
package='nav2_simple_commander',
66+
executable='example_nav_through_poses',
67+
emulate_tty=True,
68+
output='screen')
69+
70+
ld = LaunchDescription()
71+
ld.add_action(start_gazebo_server_cmd)
72+
ld.add_action(start_gazebo_client_cmd)
73+
ld.add_action(start_robot_state_publisher_cmd)
74+
ld.add_action(rviz_cmd)
75+
ld.add_action(bringup_cmd)
76+
ld.add_action(demo_cmd)
77+
return ld

‎nav2_simple_commander/launch/nav_to_pose_example_launch.py‎

Lines changed: 77 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,77 @@
1+
# Copyright (c) 2021 Samsung Research America
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
import os
16+
17+
from ament_index_python.packages import get_package_share_directory
18+
19+
from launch import LaunchDescription
20+
from launch.actions import ExecuteProcess, IncludeLaunchDescription
21+
from launch.launch_description_sources import PythonLaunchDescriptionSource
22+
from launch_ros.actions import Node
23+
24+
25+
def generate_launch_description():
26+
warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world')
27+
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
28+
python_commander_dir = get_package_share_directory('nav2_simple_commander')
29+
30+
map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml')
31+
world = os.path.join(python_commander_dir, 'warehouse.world')
32+
33+
# start the simulation
34+
start_gazebo_server_cmd = ExecuteProcess(
35+
cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world],
36+
cwd=[warehouse_dir], output='screen')
37+
38+
start_gazebo_client_cmd = ExecuteProcess(
39+
cmd=['gzclient'],
40+
cwd=[warehouse_dir], output='screen')
41+
42+
urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
43+
start_robot_state_publisher_cmd = Node(
44+
package='robot_state_publisher',
45+
executable='robot_state_publisher',
46+
name='robot_state_publisher',
47+
output='screen',
48+
arguments=[urdf])
49+
50+
# start the visualization
51+
rviz_cmd = IncludeLaunchDescription(
52+
PythonLaunchDescriptionSource(
53+
os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
54+
launch_arguments={'namespace': '',
55+
'use_namespace': 'False'}.items())
56+
57+
# start navigation
58+
bringup_cmd = IncludeLaunchDescription(
59+
PythonLaunchDescriptionSource(
60+
os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')),
61+
launch_arguments={'map': map_yaml_file}.items())
62+
63+
# start the demo autonomy task
64+
demo_cmd = Node(
65+
package='nav2_simple_commander',
66+
executable='example_nav_to_pose',
67+
emulate_tty=True,
68+
output='screen')
69+
70+
ld = LaunchDescription()
71+
ld.add_action(start_gazebo_server_cmd)
72+
ld.add_action(start_gazebo_client_cmd)
73+
ld.add_action(start_robot_state_publisher_cmd)
74+
ld.add_action(rviz_cmd)
75+
ld.add_action(bringup_cmd)
76+
ld.add_action(demo_cmd)
77+
return ld

0 commit comments

Comments
 (0)