cccccccc
/
stretch_tutorials
mirror of https://github.com/hello-robot/stretch_tutorials.git
1
0
Fork 0
Code Issues 0 Projects 0 Releases 0 Wiki Activity
Browse Source

Merge pull request #4 from hello-robot/tutorial/nav2 

Tutorials for Nav2
pull/16/head
Chintan Desai 1 year ago
committed by GitHub
parent
a076441993 995f8bd980
commit
1a05442a3b
No known key found for this signature in database GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 194 additions and 31 deletions
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +1
    -1
      mkdocs.yml
  2. +143
    -0
      ros2/navigation_simple_commander.md
  3. +50
    -30
      ros2/navigation_stack.md

+ 1
- 1
mkdocs.yml View File

@ -179,7 +179,7 @@ nav:
- Deep Perception: ./ros2/deep_perception.md
# - PointCloud Transformation: ./ros2/example_11.md
# - ArUco Tag Locator: ./ros2/example_12.md
- Stretch Tool Share:
- Overview: ./stretch_tool_share/README.md
- Basics:

+ 143
- 0
ros2/navigation_simple_commander.md View File

@ -0,0 +1,143 @@
# Nav2 Stack Using Simple Commander Python API
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
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!
!!! 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
stretch_robot_stow.py
```
## 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.
First, execute the following command while passing the correct map YAML. Then, press the 'Startup' button:
```bash
ros2 launch stretch_navigation navigation.launch.py map:=${HELLO_ROBOT_FLEET}/maps/<map_name>.yaml
```
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).
<p align="center">
<img height=500 src="https://user-images.githubusercontent.com/97639181/206782270-e84b33c4-e155-468d-8a46-d926b88ba428.gif"/>
</p>
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.
```bash
cd ~/ament_ws/
colcon build
```
## See It In Action
Go ahead and execute the following command to run the demo and visualize the result in RViz. Be sure to pass the correct path to the map YAML:
Terminal 1:
```bash
ros2 launch stretch_navigation demo_security.launch.py map:=${HELLO_ROBOT_FLEET}/maps/<map_name>.yaml
```
<p align="center">
<img height=500 src="https://user-images.githubusercontent.com/97639181/214690474-25d2bbf5-74b2-4789-af7e-ef6b208b2275.gif"/>
</p>
## Code Breakdown
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.
```python
from stretch_navigation.robot_navigator import BasicNavigator, TaskResult
```
In the main method, we initialize the node and create an instance of the BasicNavigator class called navigator.
```python
def main():
rclpy.init()
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.
```python
security_route = [
[0.0, 0.0],
[1.057, 1.3551],
[1.5828, 5.0823],
[-0.5390, 5.6623],
[0.8975, 9.7033]]
```
<p align="center">
<img height=500 src="https://user-images.githubusercontent.com/97639181/206782439-775074c5-2401-4019-ade8-bf6c920ecc61.gif"/>
</p>
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
initial_pose = PoseStamped()
initial_pose.header.frame_id = 'map'
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
initial_pose.pose.position.x = 0.0
initial_pose.pose.position.y = 0.0
initial_pose.pose.orientation.z = 0.0
initial_pose.pose.orientation.w = 1.0
navigator.setInitialPose(initial_pose)
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.
```python
while rclpy.ok():
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[1:]:
pose.pose.position.x = pt[0]
pose.pose.position.y = pt[1]
route_poses.append(deepcopy(pose))
nav_start = navigator.get_clock().now()
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 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
i = 0
while not navigator.isTaskComplete():
i += 1
feedback = navigator.getFeedback()
if feedback and i % 5 == 0:
navigator.get_logger().info('Executing current waypoint: ' +
str(feedback.current_waypoint + 1) + '/' + str(len(route_poses)))
now = navigator.get_clock().now()
if now - nav_start > Duration(seconds=600.0):
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.
```python
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.
```python
result = navigator.getResult()
if result == TaskResult.SUCCEEDED:
navigator.get_logger().info('Route complete! Restarting...')
elif result == TaskResult.CANCELED:
navigator.get_logger().info('Security route was canceled, exiting.')
rclpy.shutdown()
elif result == TaskResult.FAILED:
navigator.get_logger().info('Security route failed! Restarting from other side...')
```
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.

+ 50
- 30
ros2/navigation_stack.md View File

@ -1,56 +1,76 @@
## Navigation Stack with Actual robot
# Nav2 Stack Using RViz
In this tutorial, we will explore the ROS 2 navigation stack using slam_toolbox for mapping an environment and the core Nav2 packages to navigate in the mapped environment. If you want to know more about teleoperating the mobile base or working with the RPlidar 2D scanner on Stretch, we recommend visiting the previous tutorials on [Teleoperating](https://docs.hello-robot.com/0.2/stretch-tutorials/ros2/example_1/) stretch and [Filtering Laser Scans](https://docs.hello-robot.com/0.2/stretch-tutorials/ros2/example_2/). These topics are a vital part of how Stretch's mobile base can be velocity controlled using Twist messages, and how the RPlidar's LaserScan messages enable [Obstacle Avoidance](https://docs.hello-robot.com/0.2/stretch-tutorials/ros2/obstacle_avoider/) for autonomous navigation.
Navigation is a key aspect of an autonomous agent because, often, to do anything meaningful, the agent needs to traverse an environment to reach a specific spot to perform a specific task. With a robot like Stretch, the task could be anything from delivering water or medicines for the elderly to performing a routine patrol of an establishment for security.
### NOTE
Nav 2 support for Stretch in ROS 2 is under active development. Please reach out to us if you want to use Nav 2 with Stretch in ROS 2.
Stretch's mobile base enables this capability and this tutorial will explore how we can autonomously plan and execute mobile base trajectories. Running this tutorial will require the robot to be untethered, so please ensure that the robot is adequetly charged.
Refer to the instructions below if you want to test this functionality in ROS 1.
## Mapping
The first step is to map the space that the robot will navigate in. The `offline_mapping.launch.py` file will enable you to do this. First run:
stretch_navigation provides the standard ROS navigation stack as two launch files. This package utilizes gmapping, move_base, and AMCL to drive the stretch RE1 around a mapped space. Running this code will require the robot to be untethered.
Then run the following commands to map the space that the robot will navigate in.
```bash
roslaunch stretch_navigation mapping.launch
ros2 launch stretch_navigation offline_mapping.launch.py
```
Rviz will show the robot and the map that is being constructed. With the terminal open, use the instructions printed by the teleop package to teleoperate the robot around the room. Avoid sharp turns and revisit previously visited spots to form loop closures.
Rviz will show the robot and the map that is being constructed. Now, use the xbox controller (see instructions below for using a keyboard) to teleoperate the robot around. To teleoperate the robot using the xbox controller, keep the front left (LB) button pressed while using the right joystick for translation and rotation.
Avoid sharp turns and revisit previously visited spots to form loop closures.
<p align="center">
<img height=600 src="https://raw.githubusercontent.com/hello-robot/stretch_tutorials/ROS2/images/mapping.gif"/>
<img src="https://user-images.githubusercontent.com/97639181/206605927-4ebe1003-6bcc-48d7-a159-ba492db92f3f.png"/>
</p>
In Rviz, once you see a map that has reconstructed the space well enough, you can run the following commands to save the map to `stretch_user/`.
<p align="center">
<img height=500 src="https://user-images.githubusercontent.com/97639181/206606439-a3e346d4-83d9-45ec-93cc-8804a2b9719c.gif"/>
</p>
In Rviz, once you see a map that has reconstructed the space well enough, open a new terminal and run the following commands to save the map to the `stretch_user/` directory.
```bash
mkdir -p ~/stretch_user/maps
rosrun map_server map_saver -f ${HELLO_FLEET_PATH}/maps/<map_name>
mkdir ${HELLO_FLEET_PATH}/maps
ros2 run nav2_map_server map_saver_cli -f ${HELLO_FLEET_PATH}/maps/<map_name>
```
The `<map_name>` does not include an extension. Map_saver will save two files as `<map_name>.pgm` and `<map_name>.yaml`.
!!! note
The `<map_name>` does not include an extension. The map_saver node will save two files as `<map_name>.pgm` and `<map_name>.yaml`.
!!! tip
For a quick sanity check, you can inspect the saved map using a pre-installed tool called Eye of Gnome (eog) by running the following command:
```bash
eog ${HELLO_FLEET_PATH}/maps/<map_name>.pgm
```
## Navigation
Next, with `<map_name>.yaml`, we can navigate the robot around the mapped space. Run:
```bash
roslaunch stretch_navigation navigation.launch map_yaml:=${HELLO_FLEET_PATH}/maps/<map_name>.yaml
ros2 launch stretch_navigation navigation.launch.py map:=${HELLO_FLEET_PATH}/maps/<map_name>.yaml
```
Rviz will show the robot in the previously mapped space, however, it's likely that the robot's location in the map does not match the robot's location in the real space. In the top bar of Rviz, use 2D Pose Estimate to lay an arrow down roughly where the robot is located in the real space. AMCL, the localization package, will better localize our pose once we give the robot a 2D Nav Goal. In the top bar of Rviz, use 2D Nav Goal to lay down an arrow where you'd like the robot to go. In the terminal, you'll see move_base go through the planning phases and then navigate the robot to the goal. If planning fails, the robot will begin a recovery behavior: spinning around 360 degrees in place.
A new RViz window should pop up with a `Startup` button in a menu at the bottom left of the window. Press the `Startup` button to kick-start all navigation related lifecycle nodes. Rviz will show the robot in the previously mapped space, however, it's likely that the robot's location in the map does not match the robot's location in the real space. To correct this, from the top bar of Rviz, use `2D Pose Estimate` to lay an arrow down roughly where the robot is located in the real space. This gives an initial estimate of the robot's location to AMCL, the localization package. AMCL will better localize the robot once we pass the robot a `2D Nav Goal`.
It is also possible to send 2D Pose Estimates and Nav Goals programatically. In your own launch file, you may include `navigation.launch` to bring up the navigation stack. Then, you can send `move_base_msgs::MoveBaseGoal` messages in order to navigate the robot programatically.
In the top bar of Rviz, use `2D Nav Goal` to lay down an arrow where you'd like the robot to navigate. In the terminal, you'll see Nav2 go through the planning phases and then navigate the robot to the goal. If planning fails, the robot will begin a recovery behavior - spinning around 180 degrees in place or backing up.
<p align="center">
<img height=500 src="https://user-images.githubusercontent.com/97639181/206606699-9f3b87b1-a7d1-4074-b68a-2e880fc576a3.gif"/>
</p>
<!-- ## Navigation Stack in Gazebo
To test Stretch Navigation in simulation there is a `mapping_gazebo.launch` and `navigation_gazebo.launch` files on the [feature/navigation_updates](https://github.com/hello-robot/stretch_ros/tree/feature/navigation_updates/stretch_navigation/launch) branch. Note that this branch works on ROS Melodic. Navigate to the branch by running the following
```
roscd stretch_navigation
git checkout feature/navigation_updates
```
Then bringup [Stretch in the willowgarage world](gazebo_basics.md) and in a new terminal run the following command to build a map of the Willow Garage world
```
roslaunch stretch_navigation mapping_gazebo.launch gazebo_visualize_lidar:=true gazebo_world:=worlds/willowgarage.world
```
!!! tip
If navigation fails or the robot becomes unresponsive to subsequent goals through RViz, you can still teleoperate the robot using Xbox controller.
## Note
The launch files expose the launch argument "teleop_type". By default, this argument is set to "joystick", which launches joystick teleop in the terminal with the Xbox controller that ships with Stretch. The Xbox controller utilizes a dead man's switch safety feature to avoid unintended movement of the robot. This is the switch located on the front left side of the controller marked "LB". Keep this switch pressed while translating or rotating the base using the joystick located on the right side of the Xbox controller.
If the Xbox controller is not available, the following commands will launch mapping or navigation, respectively, with keyboard teleop:
```bash
ros2 launch stretch_navigation offline_mapping.launch.py teleop_type:=keyboard
```
roslaunch stretch_navigation teleop_keyboard.launch
or
```bash
ros2 launch stretch_navigation navigation.launch.py teleop_type:=keyboard map:=${HELLO_FLEET_PATH}/maps/<map_name>.yaml
```
-->
## Simple Commander API
It is also possible to send 2D Pose Estimates and Nav Goals programatically. In your own launch file, you may include `navigation.launch` to bring up the navigation stack. Then, you can send pose goals using the Nav2 simple commander API in order to navigate the robot programatically. We will explore this in the next tutorial.

Write Preview
Loading…
Cancel
Save