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 rospy
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
```
You need to import rospy if you are writing a ROS Node. The geometry_msgs.msg import is so that we can send velocity commands to the robot.
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:
class Move(Node):
def __init__(self):
self.pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=1)#/stretch_diff_drive_controller/cmd_vel for gazebo
This section of code defines the talker's interface to the rest of ROS. pub = rospy.Publisher("/stretch/cmd_vel", Twist, queue_size=1) 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.
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.
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()
@ -85,45 +102,39 @@ 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.1
command.linear.y = 0.0
command.linear.z = 0.0
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.
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.0
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.
The Stretch will only respond to rotations around the z (vertical) axis. We set this to a non-zero value.
```python
self.pub.publish(command)
self.publisher_.publish(command)
```
Publish the Twist commands in the previously defined topic name */stretch/cmd_vel*.
```Python
rospy.init_node('move')
base_motion = Move()
rate = rospy.Rate(10)
```
The next line, rospy.init_node(NAME, ...), is very important as it tells rospy the name of your node -- until rospy has this information, it cannot start communicating with the ROS Master. 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 "/".
The `rospy.Rate()` function creates a Rate object rate. With the help of its method sleep(), it offers a convenient way for looping at the desired rate. With its argument of 10, we should expect to go through the loop 10 times per second (as long as our processing time does not exceed 1/10th of a second!)
```python
while not rospy.is_shutdown():
base_motion.move_forward()
rate.sleep()
def main(args=None):
rclpy.init(args=args)
base_motion = Move()
rclpy.spin(base_motion)
base_motion.destroy_node()
rclpy.shutdown()
```
This loop is a fairly standard rospy construct: checking the rospy.is_shutdown() flag and then doing work. You have to check is_shutdown() to check if your program should exit (e.g. if there is a Ctrl-C or otherwise). The loop calls rate.sleep(), which sleeps just long enough to maintain the desired rate through the loop.
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
<!-- ## Move Stretch in Simulation
<palign="center">
<imgsrc="images/move.gif"/>
</p>
@ -139,7 +150,7 @@ After saving the edited node, bringup [Stretch in the empty world simulation](ga
```
cd catkin_ws/src/stretch_ros_turotials/src/
python3 move.py
```
``` -->
To stop the node from sending twist messages, type **Ctrl** + **c**.
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:
```
@ -43,24 +44,23 @@ Knowing the orientation of the LiDAR allows us to filter the scan values for a d
First, open a terminal and run the stretch driver launch file.
```bash
roslaunch stretch_core stretch_driver.launch
ros2 launch stretch_core stretch_driver.launch.py
```
Then in a new terminal run the rplidar launch file from `stretch_core`.
```bash
roslaunch stretch_core rplidar.launch
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
cd catkin_ws/src/stretch_ros_turotials/src/
python3 scan_filter.py
ros2 run stretch_ros_tutorials scan_filter
```
Then run the following command to bring up a simple RViz configuration of the Stretch robot.
points = [r * sin(theta) if (theta <-2.5ortheta> 2.5) else inf for r,theta in zip(msg.ranges, angles)]
new_ranges = [r if abs(y) <self.extentelseinfforr,yinzip(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__':
rospy.init_node('scan_filter')
Scanfilter()
rospy.spin()
main()
```
### The Code Explained
@ -103,18 +119,19 @@ if __name__ == '__main__':
Now let's break the code down.
```python
#!/usr/bin/env 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 rospy
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 rospy if you are writing a ROS Node. There are functions from numpy and math that are required within this code, thus why linspace, inf, and sin are imported. The sensor_msgs.msg import is so that we can subscribe and publish LaserScan messages.
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.
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.
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.
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.
`pub = rospy.Publisher("filtered_scan", LaserScan, queue_size=10)` declares that your node is publishing to the *filtered_scan* topic using the message type LaserScan. This lets the master tell any nodes listening on *filtered_scan* that we are going to publish data on that topic.
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.
Substitute in the new ranges in the original message, and republish it.
```python
rospy.init_node('scan_filter')
Scanfilter()
def main(args=None):
rclpy.init(args=args)
scan_filter = ScanFilter()
```
The next line, rospy.init_node(NAME, ...), is very important as it tells rospy the name of your node -- until rospy has this information, it cannot start communicating with the ROS Master. 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 "/".
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 `Scanfilter()`
Setup Scanfilter class with `scan_filter = Scanfilter()`
```python
rospy.spin()
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,
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.
Begin by running `roscore` in a terminal. Then set the ros parameter to *navigation* mode by running the following commands in a new terminal.
```bash
rosparam set /stretch_driver/mode "navigation"
roslaunch stretch_core stretch_driver.launch
ros2 launch stretch_core stretch_driver.launch.py
```
Then in a new terminal type the following to activate the LiDAR sensor.
```bash
roslaunch stretch_core rplidar.launch
ros2 launch stretch_core rplidar.launch.py
```
To activate the avoider node, type the following in a new terminal.
```bash
cd catkin_ws/src/stretch_ros_turotials/src/
python3 avoider.py
ros2 run stretch_ros_tutorials avoider
```
To stop the node from sending twist messages, type **Ctrl** + **c** in the terminal running the avoider node.
@ -26,21 +22,22 @@ To stop the node from sending twist messages, type **Ctrl** + **c** in the termi
### The Code
```python
#!/usr/bin/env python
#!/usr/bin/env python3
import rospy
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:
def __init__(self):
self.pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=1) #/stretch_diff_drive_controller/cmd_vel for gazebo
points = [r * sin(theta) if (theta <-2.5ortheta> 2.5) else inf for r,theta in zip(msg.ranges, angles)]
new_ranges = [r if abs(y) <self.extentelseinfforr,yinzip(msg.ranges,points)]
error = min(new_ranges) - self.distance
self.twist.linear.x = tanh(error) if (error > 0.05 or error <-0.05)else0
self.pub.publish(self.twist)
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__':
rospy.init_node('avoider')
Avoider()
rospy.spin()
main()
```
### The Code Explained
@ -71,31 +81,32 @@ if __name__ == '__main__':
Now let's break the code down.
```python
#!/usr/bin/env 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 rospy
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 rospy 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.
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.pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=1)#/stretch_diff_drive_controller/cmd_vel for gazebo
This section of code defines the talker's interface to the rest of ROS. pub = rospy.Publisher("/stretch/cmd_vel", Twist, queue_size=1) declares that your node is publishing to the /stretch/cmd_vel topic using the message type Twist.
This declares that your node is publishing to the /stretch/cmd_vel topic using the message type Twist.
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.
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
@ -138,19 +149,22 @@ Set the speed according to a tanh function. This method gives a nice smooth mapp
```python
self.pub.publish(self.twist)
self.publisher_.publish(self.twist)
```
Publish the Twist message.
```python
rospy.init_node('avoider')
Avoider()
rospy.spin()
def main(args=None):
rclpy.init(args=args)
avoider = Avoider()
rclpy.spin(avoider)
avoider.destroy_node()
rclpy.shutdown()
```
The next line, rospy.init_node(NAME, ...), is very important as it tells rospy the name of your node -- until rospy has this information, it cannot start communicating with the ROS Master. 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 "/".
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 `Avioder()`
Setup Avoider class with `avoider = Avioder()`
Give control to ROS with `rospy.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.
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.
ros2 run rviz2 rviz2 -d `ros2 pkg prefix stretch_calibrtion`/rviz/stretch_simple_test.rviz
```
the `rviz` flag will open an RViz window to visualize a variety of ROS topics. In a new terminal run the following commands to create a marker.
In a new terminal run the following commands to create a marker.
```bash
cd catkin_ws/src/stretch_ros_turotials/src/
python3 marker.py
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.
@ -20,43 +20,57 @@ The gif below demonstrates how to add a new *Marker* display type, and change th
self.get_logger().info("Publishing the balloon topic. Use RViz to visualize.")
def publish_marker(self):
self.publisher.publish(self.marker)
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__':
rospy.init_node('marker', argv=sys.argv)
ballon = Balloon()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
ballon.publish_marker()
rate.sleep()
main()
```
@ -64,30 +78,33 @@ if __name__ == '__main__':
Now let's break the code down.
```python
#!/usr/bin/env 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 rospy
import rclpy
from rclpy.node import Node
from visualization_msgs.msg import Marker
```
You need to import rospy if you are writing a ROS 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.
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.
This section of code defines the talker's interface to the rest of ROS. pub = rospy.Publisher("balloon", Twist, queue_size=1) declares that your node is publishing to the */ballon* topic using the message type *Twist*.
This declares that your node is publishing to the */ballon* topic using the message type *Twist*.
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
@ -130,27 +147,26 @@ Specify the pose of the marker. Since spheres are rotationally invariant, we're
```python
def publish_marker(self):
self.publisher.publish(self.marker)
self.publisher_.publish(self.marker)
```
Publish the Marker data structure to be visualized in RViz.
```python
rospy.init_node('marker', argv=sys.argv)
ballon = Balloon()
rate = rospy.Rate(10)
def main(args=None):
rclpy.init(args=args)
balloon = Balloon()
```
The next line, rospy.init_node(NAME, ...), is very important as it tells rospy the name of your node -- until rospy has this information, it cannot start communicating with the ROS Master. 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()`
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 "/".
Give control to ROS with `rospy.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.
Setup Balloon class with `balloon = Balloon()`
```python
while not rospy.is_shutdown():
ballon.publish_marker()
rate.sleep()
while rclpy.ok():
balloon.publish_marker()
balloon.destroy_node()
rclpy.shutdown()
```
This loop is a fairly standard rospy construct: checking the rospy.is_shutdown() flag and then doing work. You have to check is_shutdown() to check if your program should exit (e.g. if there is a Ctrl-C or otherwise). The loop calls rate.sleep(), which sleeps just long enough to maintain the desired rate through the loop.
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).
Stretch ROS 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.
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
<palign="center">
<imgsrc="images/stow_command.gif"/>
</p>
Begin by running `roscore` in a terminal. Then set the ros parameter to *position* mode by running the following commands in a new terminal.
Begin by launching stretch_driver in a terminal.
```bash
rosparam set /stretch_driver/mode "position"
roslaunch stretch_core stretch_driver.launch
ros2 launch stretch_core stretch_driver.launch.py
```
In a new terminal type the following commands.
```bash
cd catkin_ws/src/stretch_ros_turotials/src/
python3 stow_command.py
ros2 run stretch_ros_tutorials stow_command
```
This will send a FollowJointTrajectory command to stow Stretch's arm.
@ -27,45 +25,77 @@ This will send a FollowJointTrajectory command to stow Stretch's arm.
```python
#!/usr/bin/env python3
import rospy
from control_msgs.msg import FollowJointTrajectoryGoal
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
rospy.loginfo('interrupt received, so shutting down')
main()
```
### The Code Explained
@ -79,65 +109,81 @@ Every Python ROS [Node](http://wiki.ros.org/Nodes) will have this declaration at
```python
import rospy
from control_msgs.msg import FollowJointTrajectoryGoal
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
import hello_helpers.hello_misc as hm
import time
from sensor_msgs.msg import JointState
```
You need to import rospy if you are writing a ROS Node. Import the FollowJointTrajectoryGoal 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. The [hello_helpers](https://github.com/hello-robot/stretch_ros/tree/master/hello_helpers) package consists of a module the provides various Python scripts used across [stretch_ros](https://github.com/hello-robot/stretch_ros). In this instance we are importing the hello_misc script.
<!-- 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(hm.HelloNode):
class StowCommand(Node):
def __init__(self):
hm.HelloNode.__init__(self)
super().__init__('stretch_stow_command')
```
The `StowCommand ` class inherits the `HelloNode` class from`hm` and is initialized.
The `StowCommand ` class inherits from the `Node` class from and is initialized.
The `issue_stow_command()`is the name of the function that 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.
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
trajectory_goal = FollowJointTrajectoryGoal()
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
Set *trajectory_goal* as a `FollowJointTrajectoryGoal` 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.
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.
Create a funcion, `main()`, to do all of the setup the `hm.HelloNode` class and issue the stow command.
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__':
try:
node = StowCommand()
node.main()
except KeyboardInterrupt:
rospy.loginfo('interrupt received, so shutting down')
main()
```
Initialize the `StowCommand()` class and set it to *node* and run the `main()` function.
To make the script executable call the main() function like above.
## Multipoint Command Example
@ -146,18 +192,16 @@ Initialize the `StowCommand()` class and set it to *node* and run the `main()` f
<imgsrc="images/multipoint.gif"/>
</p>
Begin by running `roscore` in a terminal. Then set the ros parameter to *position* mode by running the following commands in a new terminal.
If you have killed the above instance of stretch_driver relaunch it again through the terminal.
```bash
rosparam set /stretch_driver/mode "position"
roslaunch stretch_core stretch_driver.launch
ros2 launch stretch_core stretch_driver.launch.py
```
In a new terminal type the following commands.
```bash
cd catkin_ws/src/stretch_ros_turotials/src/
python3 multipoint_command.py
ros2 run stretch_ros_tutorials multipoint_command
```
This will send a list of JointTrajectoryPoint's to move Stretch's arm.
@ -166,61 +210,103 @@ This will send a list of JointTrajectoryPoint's to move Stretch's arm.
```python
#!/usr/bin/env python3
import rospy
import time
from control_msgs.msg import FollowJointTrajectoryGoal
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
rospy.loginfo('interrupt received, so shutting down')
main()
```
@ -228,31 +314,25 @@ if __name__ == '__main__':
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
point0 = JointTrajectoryPoint()
point0.positions = [0.2, 0.0, 3.4]
point1 = JointTrajectoryPoint()
point1.positions = [0.3, 0.1, 2.0]
point1.time_from_start = duration1.to_msg()
```
Sett *point0* as a `JointTrajectoryPoint`and provide desired positions (in meters). These are the positions of the lift, wrist extension, and yaw of the wrist, respectively.
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
point0.velocities = [0.2, 0.2, 2.5]
```
Provide desired velocity of the lift (m/s), wrist extension (m/s), and wrist yaw (rad/s) for *point0*.
```python
point0.accelerations = [1.0, 1.0, 3.5]
```
Provide desired velocity of the lift (m/s^2), wrist extension (m/s^2), and wrist yaw (rad/s^2).
**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.
Set *trajectory_goal* as a `FollowJointTrajectoryGoal` 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.
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.
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.
## 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.
## Installing Noetic on Stretch
Instructions on installing Noetic can be found in our open source [installation guide](https://github.com/hello-robot/stretch_ros/blob/dev/noetic/install_noetic.md). Once your system is setup, clone the [stretch_ros_tutorials](https://github.com/hello-sanchez/stretch_ros_tutorials.git) to your workspace. Then build the packages in your workspace.
Hello Robot is currently running Stretch on Ubuntu 20.04 and on ROS Noetic. To begin the setup, begin with [installing Ubnutu desktop](https://ubuntu.com/tutorials/install-ubuntu-desktop#1-overview) on your local machine. Then follow the [installation guide for ROS Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) on your system.
<!-- 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.
Currently, the Realsense2_description package isn't installed by rosdep and requires a user to manually install the package. Run the following command in your terminal
## 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.
```bash
sudo apt-get install ros-noetic-realsense2-camera
```
After your system is setup, clone the [stretch_ros](https://github.com/hello-robot/stretch_ros.git), [stretch_ros_tutorials](https://github.com/hello-sanchez/stretch_ros_tutorials.git), and [realsense_gazebo_plugin packages]( https://github.com/pal-robotics/realsense_gazebo_plugin) to your preferred workspace. Then install dependencies and build the packages.
If a you are unable to dual boot and install ubuntu on your local machine, an alternative is to use [AWS RoboMaker](https://aws.amazon.com/robomaker/). AWS RoboMaker extends the ROS framework with cloud services. The service provides a robotics simulation service, allowing for testing of the Stretch RE1 platform. If you are a first-time user of AWS RoboMaker, follow the [guide here](https://github.com/aws-robotics/aws-robomaker-robotics-curriculum/blob/main/getting-started-with-aws-robomaker/_modules/mod-2a-aws.md) to get up and running with the service.
Begin by starting up the stretch driver launch file by typing the following in a terminal.
```bash
roslaunch stretch_core stretch_driver.launch
ros2 launch stretch_core stretch_driver.launch.py
```
Then utilize the ROS command-line tool, [rostopic](http://wiki.ros.org/rostopic), 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.
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
rostopic echo /joint_states -n1
ros2 topic echo /stretch/joint_states
```
Note that the flag, `-n1`, at the end of the command, defines the count of how many times you wish to publish the current topic information. If you prefer to continuously print the topic for debugging purposes, then remove the flag.
Your terminal will then output the information associated with the `/joint_states` topic. Your `header`, `position`, `velocity`, and `effort` information may vary from what is printed below.
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.
Let's say you are interested in only seeing the `header` component of the `/joint_states` topic, you can output this within the rostopic command-line tool by typing the following command.
```bash
rostopic echo /joint_states/header -n1
```
Your terminal will then output something similar to this:
```
seq: 97277
stamp:
secs: 1945
nsecs: 562000000
frame_id: ''
```
Additionally, if you were to type `rostopic echo /` in the terminal, then press your *Tab* key on your keyboard, you will see the list of active topics being published.
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 the ROS [rqt_graph package](http://wiki.ros.org/rqt_graph). You can see a graph of topics being communicated between nodes by typing the following.
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.
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.
Additionally, the demo allows a user to select from the three groups, *stretch_arm*, *stretch_gripper*, *stretch_head* 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:
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.
@ -39,7 +43,7 @@ Additionally, the demo allows a user to select from the three groups, *stretch_a
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).
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.
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.
If you want the 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.
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.
There are further tutorials for RViz and can be found [here](http://wiki.ros.org/rviz/Tutorials).
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).
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
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.
hello-chintan
2 years ago