In this tutorial, we will work with Stretch to explore the Simple Commander Python API to enable autonomous navigation programatically. We will also demonstrate a security patrol routine for Stretch developed using this API. If you just landed here, it might be a good idea to first review the previous tutorial which covered mapping and navigation using RViz as an interface.
In this tutorial, we will work with Stretch to explore the Simple Commander Python API to enable autonomous navigation programatically. We will also demonstrate a security patrol routine for Stretch developed using this API. If you just landed here, it might be a good idea to first review the previous tutorial which covered mapping and navigation using RViz as an interface.
## The Simple Commander Python API
## The Simple Commander Python API
To develop complex behaviors with Stretch where navigation is just one aspect of the autonomy stack, we need to be able to plan and execute navigation routines as part of a bigger program. Luckily, the Nav2 stack exposes a Python API that abstracts the ROS layer and the Behavior Tree framework (more on that later!) from the user through a pre-configured library called the [robot navigator](https://github.com/hello-robot/stretch_ros2/blob/galactic/stretch_navigation/stretch_navigation/robot_navigator.py). This library defines a class called BasicNavigator which wraps the planner, controller and recovery action servers and exposes methods such as goToPose(), goToPoses() and followWaypoints() to execute navigation behaviors.
To develop complex behaviors with Stretch where navigation is just one aspect of the autonomy stack, we need to be able to plan and execute navigation routines as part of a bigger program. Luckily, the Nav2 stack exposes a Python API that abstracts the ROS layer and the Behavior Tree framework (more on that later!) from the user through a pre-configured library called the [robot navigator](https://github.com/hello-robot/stretch_ros2/blob/galactic/stretch_navigation/stretch_navigation/robot_navigator.py). This library defines a class called BasicNavigator which wraps the planner, controller and recovery action servers and exposes methods such as `goToPose()`, `goToPoses()` and `followWaypoints()` to execute navigation behaviors.
Let's first see the demo in action and then explore the code to understand how this works!
Let's first see the demo in action and then explore the code to understand how this works!
## Warning
We will not be using the arm for this demo. We recommend stowing the arm to avoid inadvertently bumping it into walls while the robot is navigating. Execute:
!!! warning
We will not be using the arm for this demo. We recommend stowing the arm to avoid inadvertently bumping it into walls while the robot is navigating.
Execute:
```bash
```bash
stretch_robot_stow.py
stretch_robot_stow.py
```
```
## Setup
## Setup
Let's set the patrol route up before you can execute this demo in your map. This requires reading the position of the robot at various locations in the map and entering the co-ordinates in the array called security_route in the [simple_commander_demo.py](https://github.com/hello-robot/stretch_ros2/blob/galactic/stretch_navigation/stretch_navigation/simple_commander_demo.py#L30) file.
Let's set the patrol route up before you can execute this demo in your map. This requires reading the position of the robot at various locations in the map and entering the co-ordinates in the array called `security_route` in the [simple_commander_demo.py](https://github.com/hello-robot/stretch_ros2/blob/galactic/stretch_navigation/stretch_navigation/simple_commander_demo.py#L30) file.
First, execute the following command and press the 'Startup' button:
First, execute the following command while passing the correct map YAML. Then, press the 'Startup' button:
Since, we expect the first point in the patrol route to be at the origin of the map, the first co-ordinates should be (0.0, 0.0). Next, to define the route, the easiest way to read the points in RViz is by setting the robot at random locations in the map using the '2D Pose Estimate' button in RViz as shown below. For each location, note the x, y co-ordinates in the position field of the base_footprint frame and add it to the security_route array.
Since we expect the first point in the patrol route to be at the origin of the map, the first co-ordinates should be (0.0, 0.0). Next, to define the route, the easiest way to define the waypoints in the `security_route` array is by setting the robot at random locations in the map using the '2D Pose Estimate' button in RViz as shown below. For each location, note the x, y co-ordinates in the position field of the base_footprint frame and add it to the `security_route` array in [simple_commander_demo.py](https://github.com/hello-robot/stretch_ros2/blob/galactic/stretch_navigation/stretch_navigation/simple_commander_demo.py#L30).
Finally, Press Ctrl+C to exit out of the navigation launch and save the simple_commander_demo.py file. Now, build the workspace to make the updated file available for the next launch command.
Finally, Press Ctrl+C to exit out of navigation and save the simple_commander_demo.py file. Now, build the workspace to make the updated file available for the next launch command.
Now, let's jump into the code to see how things work under the hood. Follow along in the [code]() to have a look at the entire script.
Now, let's jump into the code to see how things work under the hood. Follow along in the [code](https://github.com/hello-robot/stretch_ros2/blob/galactic/stretch_navigation/stretch_navigation/simple_commander_demo.py) to have a look at the entire script.
First, we import the BasicNavigator class from the robot_navigator library which comes standard with the Nav2 stack. This class wraps around the planner, controller and recovery action servers.
First, we import the `BasicNavigator` class from the robot_navigator library which comes standard with the Nav2 stack. This class wraps around the planner, controller and recovery action servers.
```python
```python
from stretch_navigation.robot_navigator import BasicNavigator, TaskResult
from stretch_navigation.robot_navigator import BasicNavigator, TaskResult
```
```
@ -58,7 +62,7 @@ def main():
navigator = BasicNavigator()
navigator = BasicNavigator()
```
```
Then, we set up a path for Stretch to patrol consisting of the co-ordinates in the map reference frame. These co-ordinates are specific to the map generated for this tutorial and would not be suitable for your robot. To define co-ordinates that work with your robot, first command the robot to at least three random locations in the map you have generated of your environment, then read the base_link x and y co-ordinates for each of them from the RViz TF plugin. Plug them in the security_route list. Keep in mind that for this demo, the robot is starting from the [0.0, 0.0] which is the origin of the map. This might not be the case for you.
Then, we set up a path for Stretch to patrol consisting of the co-ordinates in the map reference frame. These co-ordinates are specific to the map generated for this tutorial and would not be suitable for your robot. To define co-ordinates that work with your robot, first command the robot to at least three random locations in the map you have generated of your environment, then read the base_link x and y co-ordinates for each of them from the RViz TF plugin. Plug them in the `security_route` list. Keep in mind that for this demo, the robot is starting from the [0.0, 0.0] which is the origin of the map. This might not be the case for you.
```python
```python
security_route = [
security_route = [
[0.0, 0.0],
[0.0, 0.0],
@ -72,7 +76,7 @@ Then, we set up a path for Stretch to patrol consisting of the co-ordinates in t
Next, we set an initial pose for the robot which would help AMCL localize the robot by providing an initial estimate of the robot's location. For this, we pass a PoseStamped message in the map reference frame with the robot's pose to the setInitialPose() method. The Nav2 stack recommends this before starting the lifecycle nodes using the "startup" button in RViz. The waitUntilNav2Active() method waits until precisely this event.
Next, we set an initial pose for the robot which would help AMCL localize the robot by providing an initial estimate of the robot's location. For this, we pass a PoseStamped message in the map reference frame with the robot's pose to the `setInitialPose()` method. The Nav2 stack recommends this before starting the lifecycle nodes using the "Startup" button in RViz. The `waitUntilNav2Active()` method waits until precisely this event.
```python
```python
initial_pose = PoseStamped()
initial_pose = PoseStamped()
initial_pose.header.frame_id = 'map'
initial_pose.header.frame_id = 'map'
@ -86,7 +90,7 @@ Next, we set an initial pose for the robot which would help AMCL localize the ro
navigator.waitUntilNav2Active()
navigator.waitUntilNav2Active()
```
```
Once the nodes are active, the navigator is ready to receive pose goals either through the goToPose(), goToPoses() or followWaypoints() methods. For this demo, we will be using the followWaypoints() method which takes a list of poses as an argument. Since we intend for the robot to patrol the route indefinitely or until the node is killed (or the robot runs out of battery!), we wrap the method in an infinite while loop with rclpy.ok(). Then, we generate pose goals with the security_route list and append them to a new list called route_poses which is passed to the followWaypoints() method.
Once the nodes are active, the navigator is ready to receive pose goals either through the `goToPose()`, `goToPoses()` or `followWaypoints()` methods. For this demo, we will be using the `followWaypoints()` method which takes a list of poses as an argument. Since we intend for the robot to patrol the route indefinitely or until the node is killed (or the robot runs out of battery!), we wrap the method in an infinite while loop with `rclpy.ok()`. Then, we generate pose goals with the `security_route` list and append them to a new list called `route_poses` which is passed to the `followWaypoints()` method.
```python
```python
while rclpy.ok():
while rclpy.ok():
@ -104,7 +108,7 @@ Once the nodes are active, the navigator is ready to receive pose goals either t
navigator.followWaypoints(route_poses)
navigator.followWaypoints(route_poses)
```
```
Since we are utilizing an action server built into Nav2, it's possible to seek feedback on this long running task through the action interface. The isTaskComplete() method returns a boolean depending on whether the patrolling task is complete. For the follow waypoints action server, the feedback message tells us which waypoint is currently being executed through the feedback.current_waypoint attribute. It is possible to cancel a goal using the cancelTask() method if the robot gets stuck. For this demo, we have set the timeout at 600 seconds to sufficient time for the robot to succeed. However, if you wish to see it in action, you can reduce the timeout to 30 seconds.
Since we are utilizing an action server built into Nav2, it's possible to seek feedback on this long running task through the action interface. The `isTaskComplete()` method returns a boolean depending on whether the patrolling task is complete. For the follow waypoints action server, the feedback message tells us which waypoint is currently being executed through the `feedback.current_waypoint` attribute. It is possible to cancel a goal using the `cancelTask()` method if the robot gets stuck. For this demo, we have set the timeout at 600 seconds to allow sufficient time for the robot to succeed. However, if you wish to see it in action, you can reduce the timeout to 30 seconds.
```python
```python
i = 0
i = 0
while not navigator.isTaskComplete():
while not navigator.isTaskComplete():
@ -119,12 +123,12 @@ Since we are utilizing an action server built into Nav2, it's possible to seek f
navigator.cancelTask()
navigator.cancelTask()
```
```
Once the robot reaches the end of the route, we reverse the security_route list to generate the goal pose list that would be used by the followWaypoints() method in the next iteration of this loop.
Once the robot reaches the end of the route, we reverse the `security_route` list to generate the goal pose list that would be used by the `followWaypoints()` method in the next iteration of this loop.
```python
```python
security_route.reverse()
security_route.reverse()
```
```
Finally, after a leg of the patrol route is executed, we call the getResult() method to know whether the task succeeded, canceled or failed to log a message.
Finally, after a leg of the patrol route is executed, we call the `getResult()` method to know whether the task succeeded, canceled or failed to log a message.
```python
```python
result = navigator.getResult()
result = navigator.getResult()
if result == TaskResult.SUCCEEDED:
if result == TaskResult.SUCCEEDED:
@ -136,4 +140,4 @@ Finally, after a leg of the patrol route is executed, we call the getResult() me
navigator.get_logger().info('Security route failed! Restarting from other side...')
navigator.get_logger().info('Security route failed! Restarting from other side...')
```
```
That's it! Using the simple commander API is as simple as a python library that abstracts away the underlying ROS action layers. Be sure to follow more examples in the [nav2_simple_commander]() package if you want to work with a few other methods exposed by the library.
That's it! Using the Simple Commander API is as simple as that. Be sure to follow more examples in the [nav2_simple_commander](https://github.com/ros-planning/navigation2/tree/main/nav2_simple_commander) package if you wish to work with other useful methods exposed by the library.
Chintan Desai
1 year ago
GitHub