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

Add ROS 2 tutorial Readme files

pull/14/head
hello-chintan 2 years ago
parent
8b60d0643b
commit
b73e9cd47f
12 changed files with 1266 additions and 0 deletions
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +144
    -0
      ros2/example_1.md
  2. +177
    -0
      ros2/example_2.md
  3. +155
    -0
      ros2/example_3.md
  4. +157
    -0
      ros2/example_4.md
  5. +283
    -0
      ros2/follow_joint_trajectory.md
  6. +33
    -0
      ros2/gazebo_basics.md
  7. +25
    -0
      ros2/getting_started.md
  8. +41
    -0
      ros2/internal_state_of_stretch.md
  9. +54
    -0
      ros2/moveit_basics.md
  10. +56
    -0
      ros2/navigation_stack.md
  11. +38
    -0
      ros2/rviz_basics.md
  12. +103
    -0
      ros2/teleoperating_stretch.md

+ 144
- 0
ros2/example_1.md View File

@ -0,0 +1,144 @@
## Example 1
<p align="center">
<img src="images/move_stretch.gif"/>
</p>
The goal of this example is to give you an enhanced understanding of how to control the mobile base by sending `Twist` messages to a Stretch robot.
```bash
ros2 launch stretch_core stretch_driver.launch.py
```
To drive the robot in circles with the move node, type the following in a new terminal.
```bash
ros2 run stetch_ros_tutorials move
```
To stop the node from sending twist messages, type **Ctrl** + **c**.
### The Code
Below is the code which will send *Twist* messages to drive the robot in circles.
```python
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
class Move(Node):
def __init__(self):
super().__init__('stretch_base_move')
self.publisher_ = self.create_publisher(Twist, '/stretch/cmd_vel', 10)
self.get_logger().info("Starting to move in circle...")
timer_period = 0.5 # seconds
self.timer = self.create_timer(timer_period, self.move_around)
def move_around(self):
command = Twist()
command.linear.x = 0.0
command.linear.y = 0.0
command.linear.z = 0.0
command.angular.x = 0.0
command.angular.y = 0.0
command.angular.z = 0.5
self.publisher_.publish(command)
def main(args=None):
rclpy.init(args=args)
base_motion = Move()
rclpy.spin(base_motion)
base_motion.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
```
### The Code Explained
Now let's break the code down.
```python
#!/usr/bin/env python3
```
Every Python ROS [Node](http://wiki.ros.org/Nodes) will have this declaration at the top. The first line makes sure your script is executed as a Python script.
```python
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
```
You need to import rclpy if you are writing a ROS 2 Node. The geometry_msgs.msg import is so that we can send velocity commands to the robot.
```python
class Move(Node):
def __init__(self):
super().__init__('stretch_base_move')
self.publisher_ = self.create_publisher(Twist, '/stretch/cmd_vel', 10)
```
This section of code defines the talker's interface to the rest of ROS. self.publisher_ = self.create_publisher(Twist, '/stretch/cmd_vel', 10) declares that your node is publishing to the /stretch/cmd_vel topic using the message type Twist. The queue_size argument limits the amount of queued messages if any subscriber is not receiving them fast enough.
```Python
timer_period = 0.5 # seconds
self.timer = self.create_timer(timer_period, self.move_around)
```
We create a timer with a period of 0.5 seconds. This timer ensures that the function move_around is called every 0.5 seconds. This ensures a constant rate of 2Hz for the execution loop.
```Python
command = Twist()
```
Make a Twist message. We're going to set all of the elements, since we
can't depend on them defaulting to safe values.
```python
command.linear.x = 0.0
command.linear.y = 0.0
command.linear.z = 0.0
```
A Twist has three linear velocities (in meters per second), along each of the axes. For Stretch, it will only pay attention to the x velocity, since it can't directly move in the y direction or the z direction. We set the linear velocities to 0.
```python
command.angular.x = 0.0
command.angular.y = 0.0
command.angular.z = 0.5
```
A *Twist* also has three rotational velocities (in radians per second).
The Stretch will only respond to rotations around the z (vertical) axis. We set this to a non-zero value.
```python
self.publisher_.publish(command)
```
Publish the Twist commands in the previously defined topic name */stretch/cmd_vel*.
```Python
def main(args=None):
rclpy.init(args=args)
base_motion = Move()
rclpy.spin(base_motion)
base_motion.destroy_node()
rclpy.shutdown()
```
The next line, rclpy.init(args=args), is very important as it tells ROS to initialize the node. Until rclpy has this information, it cannot start communicating with the ROS Master. In this case, your node will take on the name 'stretch_base_move'. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/". We then create an instance called base_motion of the class Move(). This is then spun using the spin function in rclpy to call the callback functions, in our case the timer that ensures the move_around function is called at a steady rate of 2Hz.
<!-- ## Move Stretch in Simulation
<p align="center">
<img src="images/move.gif"/>
</p>
Using your preferred text editor, modify the topic name of the published *Twist* messages. Please review the edit in the **move.py** script below.
```python
self.pub = rospy.Publisher('/stretch_diff_drive_controller/cmd_vel', Twist, queue_size=1)
```
After saving the edited node, bringup [Stretch in the empty world simulation](gazebo_basics.md). To drive the robot with the node, type the follwing in a new terminal
```
cd catkin_ws/src/stretch_ros_turotials/src/
python3 move.py
``` -->
To stop the node from sending twist messages, type **Ctrl** + **c**.
**Next Example:** [Example 2](example_2.md)

+ 177
- 0
ros2/example_2.md View File

@ -0,0 +1,177 @@
## Example 2
The aim of this example is to provide instruction on how to filter scan messages.
<!-- TODO: Update the links -->
For robots with laser scanners, ROS provides a special Message type in the [sensor_msgs](http://wiki.ros.org/sensor_msgs) package called [LaserScan](http://docs.ros.org/en/api/sensor_msgs/html/msg/LaserScan.html) to hold information about a given scan. Let's take a look at the message specification itself:
```
#
# Laser scans angles are measured counter clockwise,
# with Stretch's LiDAR having both angle_min and angle_max facing forward
# (very closely along the x-axis) of the device frame
#
Header header
float32 angle_min # start angle of the scan [rad]
float32 angle_max # end angle of the scan [rad]
float32 angle_increment # angular distance between measurements [rad]
float32 time_increment # time between measurements [seconds]
float32 scan_time # time between scans [seconds]
float32 range_min # minimum range value [m]
float32 range_max # maximum range value [m]
float32[] ranges # range data [m] (Note: values < range_min or > range_max should be discarded)
float32[] intensities # intensity data [device-specific units]
```
The above message tells you everything you need to know about a scan. Most importantly, you have the angle of each hit and its distance (range) from the scanner. If you want to work with raw range data, then the above message is all you need. There is also an image below that illustrates the components of the message type.
<p align="center">
<img src="images/lidar.png"/>
</p>
For a Stretch robot the start angle of the scan, `angle_min`, and
end angle, `angle_max`, are closely located along the x-axis of Stretch's frame. `angle_min` and `angle_max` are set at **-3.1416** and **3.1416**, respectively. This is illustrated by the images below.
<p align="center">
<img height=500 src="images/stretch_axes.png"/>
<img height=500 src="images/scan_angles.png"/>
</p>
Knowing the orientation of the LiDAR allows us to filter the scan values for a desired range. In this case, we are only considering the scan ranges in front of the stretch robot.
First, open a terminal and run the stretch driver launch file.
```bash
ros2 launch stretch_core stretch_driver.launch.py
```
Then in a new terminal run the rplidar launch file from `stretch_core`.
```bash
ros2 launch stretch_core rplidar.launch.py
```
To filter the lidar scans for ranges that are directly in front of Stretch (width of 1 meter) run the scan filter node by typing the following in a new terminal.
```bash
ros2 run stretch_ros_tutorials scan_filter
```
Then run the following command to bring up a simple RViz configuration of the Stretch robot.
```bash
ros2 run rviz2 rviz2 -d `ros2 pkg prefix stretch_calibration`/rviz/stretch_simple_test.rviz
```
Change the topic name from the LaserScan display from */scan* to */filter_scan*.
<p align="center">
<img height=600 src="images/scanfilter.gif"/>
</p>
### The Code
```python
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from numpy import linspace, inf
from math import sin
from sensor_msgs.msg import LaserScan
class ScanFilter(Node):
def __init__(self):
super().__init__('stretch_scan_filter')
self.pub = self.create_publisher(LaserScan, '/filtered_scan', 10)
self.sub = self.create_subscription(LaserScan, '/scan', self.scan_filter_callback, 10)
self.width = 1
self.extent = self.width / 2.0
self.get_logger().info("Publishing the filtered_scan topic. Use RViz to visualize.")
def scan_filter_callback(self,msg):
angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
points = [r * sin(theta) if (theta < -2.5 or theta > 2.5) else inf for r,theta in zip(msg.ranges, angles)]
new_ranges = [r if abs(y) < self.extent else inf for r,y in zip(msg.ranges, points)]
msg.ranges = new_ranges
self.pub.publish(msg)
def main(args=None):
rclpy.init(args=args)
scan_filter = ScanFilter()
rclpy.spin(scan_filter)
scan_filter.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
```
### The Code Explained
Now let's break the code down.
```python
#!/usr/bin/env python3
```
Every Python ROS [Node](http://wiki.ros.org/Nodes) will have this declaration at the top. The first line makes sure your script is executed as a Python script.
```python
import rclpy
from rclpy.node import Node
from numpy import linspace, inf
from math import sin
from sensor_msgs.msg import LaserScan
```
You need to import rclpy if you are writing a ROS Node. There are functions from numpy and math that are required within this code, that's why linspace, inf, and sin are imported. The sensor_msgs.msg import is so that we can subscribe and publish LaserScan messages.
```python
self.width = 1
self.extent = self.width / 2.0
```
We're going to assume that the robot is pointing up the x-axis, so that any points with y coordinates further than half of the defined width (1 meter) from the axis are not considered.
```python
self.sub = self.create_subscription(LaserScan, '/scan', self.scan_filter_callback, 10)
```
Set up a subscriber. We're going to subscribe to the topic "/scan", looking for LaserScan messages. When a message comes in, ROS is going to pass it to the function "callback" automatically.
```python
self.pub = self.create_publisher(LaserScan, '/filtered_scan', 10)
```
This declares that your node is publishing to the *filtered_scan* topic using the message type LaserScan. This lets any nodes listening on *filtered_scan* that we are going to publish data on that topic.
```python
angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
```
This line of code utilizes linspace to compute each angle of the subscribed scan.
```python
points = [r * sin(theta) if (theta < -2.5 or theta > 2.5) else inf for r,theta in zip(msg.ranges, angles)]
```
Here we are computing the y coordinates of the ranges that are **below -2.5** and **above 2.5** radians of the scan angles. These limits are sufficient for considering scan ranges in front of Stretch, but these values can be altered to your preference.
```python
new_ranges = [r if abs(y) < self.extent else inf for r,y in zip(msg.ranges, points)]
```
If the absolute value of a point's y-coordinate is under *self.extent* then keep the range, otherwise use inf, which means "no return".
```python
msg.ranges = new_ranges
self.pub.publish(msg)
```
Substitute in the new ranges in the original message, and republish it.
```python
def main(args=None):
rclpy.init(args=args)
scan_filter = ScanFilter()
```
The next line, rclpy.init_node initializes the node. In this case, your node will take on the name 'stretch_scan_filter'. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/".
Setup Scanfilter class with `scan_filter = Scanfilter()`
```python
rclpy.spin(scan_filter)
```
Give control to ROS. This will allow the callback to be called whenever new
messages come in. If we don't put this line in, then the node will not work,
and ROS will not process any messages.
**Next Example:** [Example 3](example_3.md)

+ 155
- 0
ros2/example_3.md View File

@ -0,0 +1,155 @@
## Example 3
The aim of example 3 is to combine the two previous examples and have Stretch utilize its laser scan data to avoid collision with objects as it drives forward.
```bash
ros2 launch stretch_core stretch_driver.launch.py
```
Then in a new terminal type the following to activate the LiDAR sensor.
```bash
ros2 launch stretch_core rplidar.launch.py
```
To activate the avoider node, type the following in a new terminal.
```bash
ros2 run stretch_ros_tutorials avoider
```
To stop the node from sending twist messages, type **Ctrl** + **c** in the terminal running the avoider node.
<p align="center">
<img height=600 src="images/avoider.gif"/>
</p>
### The Code
```python
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from numpy import linspace, inf, tanh
from math import sin
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
class Avoider(Node):
def __init__(self):
super().__init__('stretch_avoider')
self.width = 1
self.extent = self.width / 2.0
self.distance = 0.5
self.twist = Twist()
self.twist.linear.x = 0.0
self.twist.linear.y = 0.0
self.twist.linear.z = 0.0
self.twist.angular.x = 0.0
self.twist.angular.y = 0.0
self.twist.angular.z = 0.0
self.publisher_ = self.create_publisher(Twist, '/stretch/cmd_vel', 1)
self.subscriber_ = self.create_subscription(LaserScan, '/scan', self.set_speed, 10)
def set_speed(self, msg):
angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
points = [r * sin(theta) if (theta < -2.5 or theta > 2.5) else inf for r,theta in zip(msg.ranges, angles)]
new_ranges = [r if abs(y) < self.extent else inf for r,y in zip(msg.ranges, points)]
error = min(new_ranges) - self.distance
self.twist.linear.x = tanh(error) if (error > 0.05 or error < -0.05) else 0
self.publisher_.publish(self.twist)
def main(args=None):
rclpy.init(args=args)
avoider = Avoider()
rclpy.spin(avoider)
avoider.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
```
### The Code Explained
Now let's break the code down.
```python
#!/usr/bin/env python3
```
Every Python ROS [Node](http://wiki.ros.org/Nodes) will have this declaration at the top. The first line makes sure your script is executed as a Python script.
```python
import rclpy
from rclpy.node import Node
from numpy import linspace, inf, tanh
from math import sin
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
```
You need to import rclpy if you are writing a ROS Node. There are functions from numpy and math that are required within this code, thus linspace, inf, tanh, and sin are imported. The sensor_msgs.msg import is so that we can subscribe to LaserScan messages. The geometry_msgs.msg import is so that we can send velocity commands to the robot.
```python
self.publisher_ = self.create_publisher(Twist, '/stretch/cmd_vel', 1)
```
This declares that your node is publishing to the /stretch/cmd_vel topic using the message type Twist.
```python
self.subscriber_ = self.create_subscription(LaserScan, '/scan', self.set_speed, 10)
```
Set up a subscriber. We're going to subscribe to the topic "/scan", looking for LaserScan messages. When a message comes in, ROS is going to pass it to the function "set_speed" automatically.
```python
self.width = 1
self.extent = self.width / 2.0
self.distance = 0.5
```
*self.width* is the width of the laser scan we want in front of Stretch. Since Stretch's front is pointing in the x-axis, any points with y coordinates further than half of the defined width (*self.extent*) from the x-axis are not considered. *self.distance* defines the stopping distance from an object.
```python
self.twist = Twist()
self.twist.linear.x = 0.0
self.twist.linear.y = 0.0
self.twist.linear.z = 0.0
self.twist.angular.x = 0.0
self.twist.angular.y = 0.0
self.twist.angular.z = 0.0
```
Allocate a Twist to use, and set everything to zero. We're going to do this when the class is initiating. Redefining this within the callback function, `set_speed()` can be more computationally taxing.
```python
angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
points = [r * sin(theta) if (theta < -2.5 or theta > 2.5) else inf for r,theta in zip(msg.ranges, angles)]
new_ranges = [r if abs(y) < self.extent else inf for r,y in zip(msg.ranges, points)]
```
This line of code utilizes linspace to compute each angle of the subscribed scan. Here we compute the y coordinates of the ranges that are below -2.5 and above 2.5 radians of the scan angles. These limits are sufficient for considering scan ranges in front of Stretch, but these values can be altered to your preference. If the absolute value of a point's y-coordinate is under self.extent then keep the range, otherwise use inf, which means "no return".
```python
error = min(new_ranges) - self.distance
```
Calculate the difference of the closest measured scan and where we want the robot to stop. We define this as *error*.
```python
self.twist.linear.x = tanh(error) if (error > 0.05 or error < -0.05) else 0
```
Set the speed according to a tanh function. This method gives a nice smooth mapping from distance to speed, and asymptotes at +/- 1
```python
self.publisher_.publish(self.twist)
```
Publish the Twist message.
```python
def main(args=None):
rclpy.init(args=args)
avoider = Avoider()
rclpy.spin(avoider)
avoider.destroy_node()
rclpy.shutdown()
```
The next line, rclpy.init() method initializes the node. In this case, your node will take on the name 'stretch_avoider'. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/".
Setup Avoider class with `avoider = Avioder()`
Give control to ROS with `rclpy.spin()`. This will allow the callback to be called whenever new messages come in. If we don't put this line in, then the node will not work, and ROS will not process any messages.
**Next Example:** [Example 4](example_4.md)

+ 157
- 0
ros2/example_4.md View File

@ -0,0 +1,157 @@
## Example 4
![image](images/balloon.png)
Let's bringup stretch in RViz by using the following command.
```bash
ros2 launch stretch_core stretch_driver.launch.py
ros2 run rviz2 rviz2 -d `ros2 pkg prefix stretch_calibrtion`/rviz/stretch_simple_test.rviz
```
In a new terminal run the following commands to create a marker.
```bash
ros2 run stretch_ros_tutorials marker
```
The gif below demonstrates how to add a new *Marker* display type, and change the topic name from `visualization_marker` to `balloon`. A red sphere Marker should appear above the Stretch robot.
![image](images/balloon.gif)
### The Code
```python
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from visualization_msgs.msg import Marker
class Balloon(Node):
def __init__(self):
super().__init__('stretch_marker')
self.publisher_ = self.create_publisher(Marker, 'balloon', 10)
self.marker = Marker()
self.marker.header.frame_id = '/base_link'
self.marker.header.stamp = self.get_clock().now().to_msg()
self.marker.type = self.marker.SPHERE
self.marker.id = 0
self.marker.action = self.marker.ADD
self.marker.scale.x = 0.5
self.marker.scale.y = 0.5
self.marker.scale.z = 0.5
self.marker.color.r = 1.0
self.marker.color.g = 0.0
self.marker.color.b = 0.0
self.marker.color.a = 1.0
self.marker.pose.position.x = 0.0
self.marker.pose.position.y = 0.0
self.marker.pose.position.z = 2.0
self.get_logger().info("Publishing the balloon topic. Use RViz to visualize.")
def publish_marker(self):
self.publisher_.publish(self.marker)
def main(args=None):
rclpy.init(args=args)
balloon = Balloon()
while rclpy.ok():
balloon.publish_marker()
balloon.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
```
### The Code Explained
Now let's break the code down.
```python
#!/usr/bin/env python3
```
Every Python ROS [Node](http://wiki.ros.org/Nodes) will have this declaration at the top. The first line makes sure your script is executed as a Python script.
```python
import rclpy
from rclpy.node import Node
from visualization_msgs.msg import Marker
```
You need to import rclpy if you are writing a ROS 2 Node. Import the `Marker` type from the visualization_msgs.msg package. This import is required to publish a Marker, which will be visualized in RViz.
```python
self.publisher_ = self.create_publisher(Marker, 'balloon', 10)
```
This declares that your node is publishing to the */ballon* topic using the message type *Twist*.
```python
self.marker = Marker()
self.marker.header.frame_id = '/base_link'
self.marker.header.stamp = self.get_clock().now().to_msg()
self.marker.type = self.marker.SPHERE
```
<!-- TODO: Update links -->
Create a maker. Markers of all shapes share a common type. Set the frame ID and type. The frame ID is the frame in which the position of the marker is specified. The type is the shape of the marker. Further details on marker shapes can be found here: [RViz Markers](http://wiki.ros.org/rviz/DisplayTypes/Marker)
```python
self.marker.id = 0
```
Each marker has a unique ID number. If you have more than one marker that you want displayed at a given time, then each needs to have a unique ID number. If you publish a new marker with the same ID number of an existing marker, it will replace the existing marker with that ID number.
```python
self.marker.action = self.marker.ADD
```
This line of code sets the action. You can add, delete, or modify markers.
```python
self.marker.scale.x = 0.5
self.marker.scale.y = 0.5
self.marker.scale.z = 0.5
```
These are the size parameters for the marker. These will vary by marker type.
```python
self.marker.color.r = 1.0
self.marker.color.g = 0.0
self.marker.color.b = 0.0
```
Color of the object, specified as r/g/b/a, with values in the range of [0, 1].
```python
self.marker.color.a = 1.0
```
The alpha value is from 0 (invisible) to 1 (opaque). If you don't set this then it will automatically default to zero, making your marker invisible.
```python
self.marker.pose.position.x = 0.0
self.marker.pose.position.y = 0.0
self.marker.pose.position.z = 2.0
```
Specify the pose of the marker. Since spheres are rotationally invariant, we're only going to specify the positional elements. As usual, these are in the coordinate frame named in frame_id. In this case, the position will always be directly 2 meters above the frame_id (*base_link*), and will move with it.
```python
def publish_marker(self):
self.publisher_.publish(self.marker)
```
Publish the Marker data structure to be visualized in RViz.
```python
def main(args=None):
rclpy.init(args=args)
balloon = Balloon()
```
The next line, rospy.init. In this case, your node will take on the name talker. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/".
Setup Balloon class with `balloon = Balloon()`
```python
while rclpy.ok():
balloon.publish_marker()
balloon.destroy_node()
rclpy.shutdown()
```
This loop is a fairly standard rclpy construct: checking the rclpy.ok() flag and then doing work. You have to run this check to see if your program should exit (e.g. if there is a Ctrl-C or otherwise).

+ 283
- 0
ros2/follow_joint_trajectory.md View File

@ -0,0 +1,283 @@
## FollowJointTrajectory Commands
Stretch driver offers a [`FollowJointTrajectory`](http://docs.ros.org/en/api/control_msgs/html/action/FollowJointTrajectory.html) action service for its arm. Within this tutorial we will have a simple FollowJointTrajectory command sent to a Stretch robot to execute.
## Stow Command Example
<p align="center">
<img src="images/stow_command.gif"/>
</p>
Begin by launching stretch_driver in a terminal.
```bash
ros2 launch stretch_core stretch_driver.launch.py
```
In a new terminal type the following commands.
```bash
ros2 run stretch_ros_tutorials stow_command
```
This will send a FollowJointTrajectory command to stow Stretch's arm.
### The Code
```python
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from rclpy.duration import Duration
from rclpy.action import ActionClient
import sys
from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from sensor_msgs.msg import JointState
class StowCommand(Node):
def __init__(self):
super().__init__('stretch_stow_command')
self.joint_state = JointState()
self.trajectory_client = ActionClient(self, FollowJointTrajectory, '/stretch_controller/follow_joint_trajectory')
server_reached = self.trajectory_client.wait_for_server(timeout_sec=60.0)
if not server_reached:
self.get_logger().error('Unable to connect to arm action server. Timeout exceeded.')
sys.exit()
self.subscription = self.create_subscription(JointState, '/stretch/joint_states', self.joint_states_callback, 1)
self.subscription
def joint_states_callback(self, joint_state):
self.joint_state = joint_state
def issue_stow_command(self):
joint_state = self.joint_state
if (joint_state is not None):
self.get_logger().info('stowing...')
stow_point1 = JointTrajectoryPoint()
stow_point2 = JointTrajectoryPoint()
duration1 = Duration(seconds=0.0)
duration2 = Duration(seconds=4.0)
stow_point1.time_from_start = duration1.to_msg()
stow_point2.time_from_start = duration2.to_msg()
joint_value1 = joint_state.position[1] # joint_lift is at index 1
joint_value2 = joint_state.position[0] # wrist_extension is at index 0
joint_value3 = joint_state.position[8] # joint_wrist_yaw is at index 8
stow_point1.positions = [joint_value1, joint_value2, joint_value3]
stow_point2.positions = [0.2, 0.0, 3.14]
trajectory_goal = FollowJointTrajectory.Goal()
trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
trajectory_goal.trajectory.points = [stow_point1, stow_point2]
trajectory_goal.trajectory.header.stamp = self.get_clock().now().to_msg()
trajectory_goal.trajectory.header.frame_id = 'base_link'
self.trajectory_client.send_goal_async(trajectory_goal)
self.get_logger().info('Sent stow goal = {0}'.format(trajectory_goal))
def main(args=None):
rclpy.init(args=args)
stow_command = StowCommand()
rclpy.spin_once(stow_command)
stow_command.issue_stow_command()
rclpy.spin(stow_command)
stow_command.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
```
### The Code Explained
Now let's break the code down.
```python
#!/usr/bin/env python3
```
Every Python ROS [Node](http://wiki.ros.org/Nodes) will have this declaration at the top. The first line makes sure your script is executed as a Python script.
```python
import rclpy
from rclpy.node import Node
from rclpy.duration import Duration
from rclpy.action import ActionClient
import sys
from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from sensor_msgs.msg import JointState
```
<!-- TODO: Update links below -->
You need to import rclpy if you are writing a ROS 2 Node. Import the FollowJointTrajectory from the [control_msgs.msg](http://wiki.ros.org/control_msgs) package to control the Stretch robot. Import JointTrajectoryPoint from the [trajectory_msgs](http://wiki.ros.org/trajectory_msgs) package to define robot trajectories.
```python
class StowCommand(Node):
def __init__(self):
super().__init__('stretch_stow_command')
```
The `StowCommand ` class inherits from the `Node` class from and is initialized.
```python
def issue_stow_command(self):
```
The `issue_stow_command()` method will stow Stretch's arm. Within the function, we set *stow_point* as a `JointTrajectoryPoint`and provide desired positions (in meters). These are the positions of the lift, wrist extension, and yaw of the wrist, respectively. These are defined in the next set of the code.
```python
stow_point1 = JointTrajectoryPoint()
stow_point2 = JointTrajectoryPoint()
duration1 = Duration(seconds=0.0)
duration2 = Duration(seconds=4.0)
stow_point1.time_from_start = duration1.to_msg()
stow_point2.time_from_start = duration2.to_msg()
joint_value1 = joint_state.position[1] # joint_lift is at index 1
joint_value2 = joint_state.position[0] # wrist_extension is at index 0
joint_value3 = joint_state.position[8] # joint_wrist_yaw is at index 8
stow_point1.positions = [joint_value1, joint_value2, joint_value3]
stow_point2.positions = [0.2, 0.0, 3.14]
trajectory_goal = FollowJointTrajectory.Goal()
trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
trajectory_goal.trajectory.points = [stow_point1, stow_point2]
trajectory_goal.trajectory.header.stamp = self.get_clock().now().to_msg()
trajectory_goal.trajectory.header.frame_id = 'base_link'
```
Set *trajectory_goal* as a `FollowJointTrajectory.Goal()` and define the joint names as a list. Then `trajectory_goal.trajectory.points` is defined by the positions set in *stow_point*. Specify the coordinate frame that we want (base_link) and set the time to be now.
```python
self.trajectory_client.send_goal_async(trajectory_goal)
self.get_logger().info('Sent stow goal = {0}'.format(trajectory_goal))
```
Make the action call and send the goal.
```python
def main(args=None):
rclpy.init(args=args)
stow_command = StowCommand()
rclpy.spin_once(stow_command)
stow_command.issue_stow_command()
rclpy.spin(stow_command)
stow_command.destroy_node()
rclpy.shutdown()
```
Create a funcion, `main()`, to do all of the setup in the class and issue the stow command. Initialize the `StowCommand()` class and set it to *node* and run the `main()` function.
```python
if __name__ == '__main__':
main()
```
To make the script executable call the main() function like above.
## Multipoint Command Example
<p align="center">
<img src="images/multipoint.gif"/>
</p>
If you have killed the above instance of stretch_driver relaunch it again through the terminal.
```bash
ros2 launch stretch_core stretch_driver.launch.py
```
In a new terminal type the following commands.
```bash
ros2 run stretch_ros_tutorials multipoint_command
```
This will send a list of JointTrajectoryPoint's to move Stretch's arm.
### The Code
```python
#!/usr/bin/env python3
import sys
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from rclpy.duration import Duration
from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from sensor_msgs.msg import JointState
class MultiPointCommand(Node):
def __init__(self):
super().__init__('stretch_multipoint_command')
self.trajectory_client = ActionClient(self, FollowJointTrajectory, '/stretch_controller/follow_joint_trajectory')
server_reached = self.trajectory_client.wait_for_server(timeout_sec=60.0)
if not server_reached:
self.get_logger().error('Unable to connect to arm action server. Timeout exceeded.')
sys.exit()
self.subscription = self.create_subscription(JointState, '/stretch/joint_states', self.joint_states_callback, 1)
self.subscription
self.get_logger().info('issuing multipoint command...')
def joint_states_callback(self, joint_state):
self.joint_state = joint_state
def issue_multipoint_command(self):
joint_state = self.joint_state
duration0 = Duration(seconds=0.0)
duration1 = Duration(seconds=2.0)
duration2 = Duration(seconds=4.0)
duration3 = Duration(seconds=6.0)
duration4 = Duration(seconds=8.0)
duration5 = Duration(seconds=10.0)
joint_value1 = joint_state.position[1] # joint_lift is at index 1
joint_value2 = joint_state.position[0] # wrist_extension is at index 0
joint_value3 = joint_state.position[8] # joint_wrist_yaw is at index 8
point0 = JointTrajectoryPoint()
point0.positions = [joint_value1, joint_value2, joint_value3]
point0.velocities = [0.2, 0.2, 2.5]
point0.accelerations = [1.0, 1.0, 3.5]
point0.time_from_start = duration0.to_msg()
point1 = JointTrajectoryPoint()
point1.positions = [0.3, 0.1, 2.0]
point1.time_from_start = duration1.to_msg()
point2 = JointTrajectoryPoint()
point2.positions = [0.5, 0.2, -1.0]
point2.time_from_start = duration2.to_msg()
point3 = JointTrajectoryPoint()
point3.positions = [0.6, 0.3, 0.0]
point3.time_from_start = duration3.to_msg()
point4 = JointTrajectoryPoint()
point4.positions = [0.8, 0.2, 1.0]
point4.time_from_start = duration4.to_msg()
point5 = JointTrajectoryPoint()
point5.positions = [0.5, 0.1, 0.0]
point5.time_from_start = duration5.to_msg()
trajectory_goal = FollowJointTrajectory.Goal()
trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
trajectory_goal.trajectory.points = [point0, point1, point2, point3, point4, point5]
trajectory_goal.trajectory.header.stamp = self.get_clock().now().to_msg()
trajectory_goal.trajectory.header.frame_id = 'base_link'
self.trajectory_client.send_goal_async(trajectory_goal)
self.get_logger().info('Sent stow goal = {0}'.format(trajectory_goal))
def main(args=None):
rclpy.init(args=args)
multipoint_command = MultiPointCommand()
rclpy.spin_once(multipoint_command)
multipoint_command.issue_multipoint_command()
rclpy.spin(multipoint_command)
multipoint_command.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
```
### The Code Explained.
Seeing that there are similarities between the multipoint and stow command nodes, we will only breakdown the different components of the multipoint_command node.
```python
point1 = JointTrajectoryPoint()
point1.positions = [0.3, 0.1, 2.0]
point1.time_from_start = duration1.to_msg()
```
Set *point1* as a `JointTrajectoryPoint`and provide desired positions (in meters). These are the positions of the lift, wrist extension, and yaw of the wrist, respectively.
**IMPORTANT NOTE**: The lift and wrist extension can only go up to 0.2 m/s. If you do not provide any velocities or accelerations for the lift or wrist extension, then they go to their default values. However, the Velocity and Acceleration of the wrist yaw will stay the same from the previous value unless updated.
```python
trajectory_goal = FollowJointTrajectory.Goal()
trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
trajectory_goal.trajectory.points = [point0, point1, point2, point3, point4, point5]
trajectory_goal.trajectory.header.stamp = self.get_clock().now().to_msg()
trajectory_goal.trajectory.header.frame_id = 'base_link'
self.trajectory_client.send_goal_async(trajectory_goal)
self.get_logger().info('Sent stow goal = {0}'.format(trajectory_goal))
```
Set *trajectory_goal* as a `FollowJointTrajectory.Goal()` and define the joint names as a list. Then `trajectory_goal.trajectory.points` is defined by a list of the 6 points. Specify the coordinate frame that we want (base_link) and set the time to be now.

+ 33
- 0
ros2/gazebo_basics.md View File

@ -0,0 +1,33 @@
# Spawning Stretch in Simulation (Gazebo)
### NOTE
Simulation support for Stretch in ROS 2 is under active development. Please reach out to us if you want to work with Stretch in a simulated environment like Gazebo/Ignition in ROS 2.
Refer to the instructions below if you want to test this functionality in ROS 1.
### Empty World Simulation
To spawn the Stretch in gazebo's default empty world run the following command in your terminal.
```
roslaunch stretch_gazebo gazebo.launch
```
This will bringup the robot in the gazebo simulation similar to the image shown below.
<!-- <img src="images/stretch_gazebo_empty_world.png" width="500" align="center"> -->
![image](images/stretch_gazebo_empty_world.png)
### Custom World Simulation
In gazebo, you are able to spawn Stretch in various worlds. First, source the gazebo world files by running the following command in a terminal
```
echo "source /usr/share/gazebo/setup.sh"
```
Then using the world argument, you can spawn the stretch in the willowgarage world by running the following
```
roslaunch stretch_gazebo gazebo.launch world:=worlds/willowgarage.world
```
![image](images/stretch_willowgarage_world.png)
**Next Tutorial:** [Teleoperating Stretch](teleoperating_stretch.md)

+ 25
- 0
ros2/getting_started.md View File

@ -0,0 +1,25 @@
# Getting Started
## Installing Ubuntu 20.04 with ROS 2 Galactic on Stretch
Hello Robot utilizes Ubuntu, an open source Linux operating system, for the Stretch RE1 platform. If you are unfamiliar with the operating system, we encourage you to review a [tutorial](https://ubuntu.com/tutorials/command-line-for-beginners#1-overview) provided by Ubuntu. Additionally, the Linux command line, BASH, is used to execute commands and is needed to run ROS on the Stretch robot. Here is a [tutorial](https://ryanstutorials.net/linuxtutorial/) on getting started with BASH.
<!-- TODO: Change the installation instructions link below -->
Instructions on installing Ubuntu 20.04 with ROS Noetic and ROS 2 Galactic can be found in our open source [installation guide](https://github.com/hello-robot/stretch_ros/blob/dev/noetic/install_noetic.md). Following these steps should create a separate Ubuntu 20.04 partition with an ament worskspace created in the home directory.
## ROS 2 Tutorials Setup on Local Computer
Once your system is setup, clone the [stretch_ros_tutorials](https://github.com/hello-sanchez/stretch_ros_tutorials.git) to the src directory of the ament workspace, then build the packages.
```
cd ~/ament_ws/src
<!-- TODO: Change the link below -->
git clone https://github.com/hello-sanchez/stretch_ros_tutorials.git
cd ~/ament_ws
colcon build
```
Then source your workspace with the following command
```
source ~/ament_ws/install/setup.bash"
```
**Next Tutorial:** [Gazebo Basics](gazebo_basics.md)

+ 41
- 0
ros2/internal_state_of_stretch.md View File

@ -0,0 +1,41 @@
## Getting the State of the Robot
Begin by starting up the stretch driver launch file by typing the following in a terminal.
```bash
ros2 launch stretch_core stretch_driver.launch.py
```
Then utilize the ROS command-line tool, ros2 topic, to display Stretch's internal state information. For instance, to view the current state of the robot's joints, simply type the following in a terminal.
```bash
ros2 topic echo /stretch/joint_states
```
Your terminal will then output the information associated with the `/stretch/joint_states` topic. Your `header`, `position`, `velocity`, and `effort` information may vary from what is printed below.
```
header:
seq: 70999
stamp:
secs: 1420
nsecs: 2000000
frame_id: ''
name: [joint_arm_l0, joint_arm_l1, joint_arm_l2, joint_arm_l3, joint_gripper_finger_left,
joint_gripper_finger_right, joint_head_pan, joint_head_tilt, joint_left_wheel, joint_lift,
joint_right_wheel, joint_wrist_yaw]
position: [-1.6137320244357253e-08, -2.9392484829061376e-07, -2.8036125938539207e-07, -2.056847528567165e-07, -2.0518734302754638e-06, -5.98271107676851e-06, 2.9291786329821434e-07, 1.3802900147297237e-06, 0.08154086954434359, 1.4361499260374905e-07, 0.4139061738340768, 9.32603306580404e-07]
velocity: [0.00015598730463972836, -0.00029395074514369584, -0.0002803845454217379, 1.322424459109634e-05, -0.00035084643762840415, 0.0012164337445918797, 0.0002138814988808099, 0.00010419792027496809, 4.0575263146426684e-05, 0.00022487596895736357, -0.0007751929074042957, 0.0002451588607332439]
effort: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---
```
Additionally, if you type `ros2 topic list` in the terminal, you will see the list of active topics being published.
A powerful tool to visualize the ROS communication is through the rqt_graph package. You can see a graph of topics being communicated between nodes by typing the following.
```
ros2 run rqt_graph rqt_graph
```
![image](images/rqt_graph.png)
The graph allows a user to observe and affirm if topics are broadcasted to the correct nodes. This method can also be utilized to debug communication issues.
**Next Tutorial:** [RViz Basics](rviz_basics.md)

+ 54
- 0
ros2/moveit_basics.md View File

@ -0,0 +1,54 @@
# MoveIt! Basics
<!--
## MoveIt! on Stretch
To run MoveIt with the actual hardware, (assuming `stretch_driver` is already running) simply run
```bash
roslaunch stretch_moveit_config move_group.launch
```
This will runs all of the planning capabilities, but without the setup, simulation and interface that the above demo provides. In order to create plans for the robot with the same interface as the offline demo, you can run
```bash
roslaunch stretch_moveit_config moveit_rviz.launch
``` -->
## MoveIt! Without Hardware
To begin running MoveIt! on stretch, checkout to the feature/hybrid_planning branch and run the demo launch file.
```bash
cd ~/ament_ws/src/stretch_ros2
git checkout feature/hybrid_planning
cd ~/ament_ws
colcon build
ros2 launch stretch_moveit_config movegroup_moveit2.launch.py
```
This will brining up an RViz instance where you can move the robot around using [interactive markers](http://wiki.ros.org/rviz/Tutorials/Interactive%20Markers%3A%20Getting%20Started) and create plans between poses. You can reference the bottom gif as a guide to plan and execute motion.
![image](images/moveit.gif)
Additionally, the demo allows a user to select from the five groups, *stretch_arm*, *stretch_gripper*, *stretch_head*, *mobile_base_arm* and *position* to move. The option to change groups in the in *Planning Request* section in the *Displays* tree. A few notes to be kept in mind:
* Pre-defined start and goal states can be specified in Start State and Goal State drop downs in Planning tab of Motion Planning RViz plugin.
* *stretch_gripper* group does not show markers, and is intended to be controlled via the joints tab that is located in the very right of Motion Planning Rviz plugin.
* When planning with *stretch_head* group make sure you select *Approx IK Solutions* in Planning tab of Motion Planning RViz plugin.
![image](images/moveit_groups.gif)
## Running Gazebo with MoveIt! and Stretch
<!-- ```bash
# Terminal 1:
roslaunch stretch_gazebo gazebo.launch
# Terminal 2:
roslaunch stretch_gazebo teleop_keyboard.launch # or use teleop_joy.launch if you have a controller
# Terminal 3
roslaunch stretch_moveit_config demo_gazebo.launch
```
This will launch an Rviz instance that visualizes the joints with markers and an empty world in Gazebo with Stretch and load all the controllers. There are pre-defined positions for each joint group for demonstration purposes. There are three joint groups, namely stretch_arm, stretch_gripper and stretch_head that can be controlled individually via Motion Planning Rviz plugin. Start and goal positions for joints can be selected similar to [this moveit tutorial](https://ros-planning.github.io/moveit_tutorials/doc/quickstart_in_rviz/quickstart_in_rviz_tutorial.html#choosing-specific-start-goal-states).
![image](images/gazebo_moveit.gif) -->
**Next Tutorial:** [Follow Joint Trajectory Commands](follow_joint_trajectory.md)

+ 56
- 0
ros2/navigation_stack.md View File

@ -0,0 +1,56 @@
## Navigation Stack with Actual robot
### 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.
Refer to the instructions below if you want to test this functionality in ROS 1.
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
```
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.
<p align="center">
<img height=600 src="images/mapping.gif"/>
</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/`.
```bash
mkdir -p ~/stretch_user/maps
rosrun map_server map_saver -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`.
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
```
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.
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.
<!-- ## 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
```
```
roslaunch stretch_navigation teleop_keyboard.launch
```
-->
**Next Tutorial:** [MoveIt! Basics](moveit_basics.md)

+ 38
- 0
ros2/rviz_basics.md View File

@ -0,0 +1,38 @@
## Visualizing with RViz
You can utilize RViz to visualize Stretch's sensor information. To begin, run the stretch driver launch file.
```bash
ros2 launch stretch_core stretch_driver.launch.py
```
<!-- TODO: Make this rviz config file available to users in the main branch -->
Then run the following command to bring up a simple RViz configuration of the Stretch robot.
```bash
ros2 run rviz2 rviz2 -d `ros2 pkg prefix stretch_calibration`/rviz/stretch_simple_test.rviz
```
An RViz window should open, allowing you to see the various DisplayTypes in the display tree on the left side of the window.
![image](images/simple_rviz.png)
If you want to visualize Stretch's [tf transform tree](http://wiki.ros.org/rviz/DisplayTypes/TF), you need to add the display type to the RViz window. First, click on the *Add* button and include the *TF* type to the display. You will then see all of the transform frames of the Stretch robot and the visualization can be toggled off and on by clicking the checkbox next to the tree. Below is a gif for reference.
![image](images/rviz_adding_tf.gif)
TODO: Add the correct link for working with rviz2 in ROS 2
There are further tutorials for RViz that can be found [here](http://wiki.ros.org/rviz/Tutorials).
## Running RViz and Gazebo (Simulation)
Let's bringup stretch in the willowgarage world from the [gazebo basics tutorial](gazebo_basics.md) and RViz by using the following command.
```
roslaunch stretch_gazebo gazebo.launch world:=worlds/willowgarage.world rviz:=true
```
the `rviz` flag will open an RViz window to visualize a variety of ROS topics.
![image](images/willowgarage_with_rviz.png)
Bringup the [keyboard teleop](teleoperating_stretch.md) to drive Stretch and observe its sensor input.
**Next Tutorial:** [Navigation Stack](navigation_stack.md)

+ 103
- 0
ros2/teleoperating_stretch.md View File

@ -0,0 +1,103 @@
## Teleoperating Stretch
### NOTE
Teleoperation support for Stretch in ROS 2 is under active development. Please reach out to us if you want to teleoperate Stretch in ROS 2.
Refer to the instructions below if you want to test this functionality in ROS 1.
### Xbox Controller Teleoperating
![image](images/xbox_controller_commands.png)
Stretch comes ready to run out of the box. The Xbox Teleoperation demo will let you quickly test out the robot capabilities by teleoperating it with an Xbox Controller.
Note: Make sure the USB Dongle is plugged into the the USB port of the base trunk.
To start the demo:
* Remove the 'trunk' cover and power on the robot
Wait for about 45 seconds. You will hear the Ubuntu startup sound, followed by two beeps (indicating the demo is running).
* Hit the Connect button on the controller. The upper two LEDs of the ring will illuminate.
* Hit the Home Robot button. Stretch will go through its homing calibration routine.
* **Note**: make sure the space around the robot is clear before running the Home function
You're ready to go! A few things to try:
* Hit the Stow Robot button. The robot will assume the stow pose.
* Practice driving the robot around.
* Pull the Fast Base trigger while driving. When stowed, it will make Stretch drive faster
* Manually stop the arm or lift from moving to make it stop upon contact.
* Try picking up your cellphone from the floor
* Try grasping cup from a counter top
* Try delivering an object to a person
If you're done, hold down the Shutdown PC button for 2 seconds. This will cause the PC to turn off. You can then power down the robot.
### Keyboard Teleoperating
To teleoperate a Stretch's mobile base with the keyboard, you first need to set the ros parameter to *navigation* mode for the robot to receive *Twist* messages. Begin by running `roscore` in a terminal. Then in a new terminal, type the following commands
```
rosparam set /stretch_driver/mode "navigation"
roslaunch stretch_core stretch_driver.launch
```
The first line will set the stretch driver mode to *navigation* before running the `stretch_driver.launch` file.
Then in a new terminal launch the teleop_twist_keyboard node with the argument remapping the *cmd_vel* topic name to *stretch/cmd_vel*.
```
rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=stretch/cmd_vel
```
Below are the keyboard commands that allow a user to move Stretch.
```
Reading from the keyboard and Publishing to Twist!
---------------------------
Moving around:
u i o
j k l
m , .
For Holonomic mode (strafing), hold down the shift key:
---------------------------
U I O
J K L
M < >
t : up (+z)
b : down (-z)
anything else : stop
q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
CTRL-C to quit
currently: speed 0.5 turn 1.0
```
To stop the node from sending twist messages, type **Ctrl** + **c**.
### Create a node for Teleoperating
To move Stretch's mobile base using a python script, please look at [example 1](example_1.md) for reference.
## Teleoperating in Gazebo
### Keyboard Teleoperating
For keyboard teleoperation, first [startup Stretch in simulation](gazebo_basics.md). Then run the following command in a new terminal.
```bash
roslaunch stretch_gazebo gazebo.launch
```
In a new terminal, type the following
```
roslaunch stretch_gazebo teleop_keyboard.launch
```
The same keyboard commands will be presented to a user to move the robot.
### Xbox Controller Teleoperating
An alternative for robot base teleoperation is to use an Xbox controller. Stop the keyboard teleoperation node by typing **Ctrl** + **c** in the terminal where the command was executed. Then connect the Xbox controller device to your local machine and run the following command.
```
roslaunch stretch_gazebo teleop_joy.launch
```
Note that the teleop_twist_joy package has a deadman switch by default which disables the drive commands to be published unless pressed. For a Logitech F310 joystick, this button is A.
**Next Tutorial:** [Internal State of Stretch](internal_state_of_stretch.md)

Write Preview
Loading…
Cancel
Save