Merge pull request #1 from hello-chintan/galactic

Ported tutorials to ROS 2 Galactic - first batch

README.md

@@ -1,6 +1,6 @@
 # Introduction
 
-This repo provides instructions on installing and using code on the Stretch RE1 robot. The goal is to provide a user familiar with ROS with the tools to operate a Stretch robot.
+This branch provides instructions on the installation and use of code on the Stretch RE1 robot. The goal is to provide a user familiar with ROS 2 with the tools to operate a Stretch robot.
 
 ## Stretch ROS Tutorials
 1. [Getting Started](getting_started.md)
@@ -11,16 +11,16 @@ This repo provides instructions on installing and using code on the Stretch RE1
 6. [Navigation Stack](navigation_stack.md)
 7. [MoveIt! Basics](moveit_basics.md)
 8. [Follow Joint Trajectory Commands](follow_joint_trajectory.md)
-9. [FUNMAP](https://github.com/hello-robot/stretch_ros/tree/master/stretch_funmap)
+<!-- 9. 
[FUNMAP](https://github.com/hello-robot/stretch_ros/tree/master/stretch_funmap)
 10. Microphone Array
 11. ROS testing
 12. Other Nav Stack Features
 13. Perception
-14. 4 Modes of Stretch
+14. 4 Modes of Stretch -->
 
 
-## Other ROS Examples
-To help get you get started on your software development, here are examples of nodes to have the stretch perform simple tasks.
+## Other ROS 2 Examples
+To help you get started on your software development, here are examples of nodes to have the stretch perform simple tasks.
 
 1. [Teleoperate Stretch with a Node](example_1.md) - Use a python script that sends velocity commands.  
 2. [Filter Laser Scans](example_2.md) - Publish new scan ranges that are directly in front of Stretch.

example_1.md

@@ -1,56 +1,66 @@
 ## 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.
 
-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
 ```
 
-To drive the robot forward with the move node, type the following in a new terminal.
+To drive the robot in circles with the move node, type the following in a new terminal.
 
 ```bash
-cd catkin_ws/src/stretch_ros_turotials/src/
-python3 move.py
+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 forward.
+Below is the code which will send *Twist* messages to drive the robot in circles.
 
 
 ```python
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
-import rospy
+import rclpy
+from rclpy.node import Node
 from geometry_msgs.msg import Twist
 
-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
+		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_forward(self):
+	def move_around(self):
 		command = Twist()
-		command.linear.x = 0.1
+		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.0
-		self.pub.publish(command)
+		command.angular.z = 0.5
 
-if __name__ == '__main__':
-	rospy.init_node('move')
+		self.publisher_.publish(command)
+
+	
+def main(args=None):
+	rclpy.init(args=args)
 	base_motion = Move()
-	rate = rospy.Rate(10)
-	while not rospy.is_shutdown():
-		base_motion.move_forward()
-		rate.sleep()
+	rclpy.spin(base_motion)
+	base_motion.destroy_node()	
+	rclpy.shutdown()
+
+
+if __name__ == '__main__':
+	main()
 ```
 
 ### The Code Explained
@@ -58,25 +68,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 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
+		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. 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.
 
+```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()
@@ -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
 <p align="center">
   <img src="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**.
 
 

example_2.md

@@ -2,6 +2,7 @@
 
 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.
 ```bash
-rosrun rviz rviz -d `rospack find stretch_core`/rviz/stretch_simple_test.rviz
+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*.
 
@@ -71,31 +71,47 @@ Change the topic name from the LaserScan display from */scan* to */filter_scan*.
 ### 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
 from math import sin
 from sensor_msgs.msg import LaserScan
 
-class Scanfilter:
+class ScanFilter(Node):
 	def __init__(self):
-		self.width = 1.0
+		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.sub = rospy.Subscriber('/scan', LaserScan, self.callback)
-		self.pub = rospy.Publisher('filtered_scan', LaserScan, queue_size=10)
 
-	def callback(self,msg):
+		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__':
-	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.
 
 ```python
 self.width = 1
@@ -123,14 +140,14 @@ 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 = rospy.Subscriber('/scan', LaserScan, self.callback)
+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.
+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 = rospy.Publisher('filtered_scan', LaserScan, queue_size=10)
+self.pub = self.create_publisher(LaserScan, '/filtered_scan', 10)
 ```
-`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.
 
 ```python
 angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
@@ -155,15 +172,17 @@ self.pub.publish(msg)
 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,

example_3.md

@@ -2,20 +2,16 @@
 
 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
-        self.sub = rospy.Subscriber('/scan', LaserScan, self.set_speed)
 
+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()
@@ -51,19 +48,32 @@ class Avoider:
         self.twist.angular.y = 0.0
         self.twist.angular.z = 0.0
 
-    def set_speed(self,msg):
+        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.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
+self.publisher_ = self.create_publisher(Twist, '/stretch/cmd_vel', 1)
 ```
-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.
 
 
 ```python
-self.sub = rospy.Subscriber('/scan', LaserScan, self.set_speed)
+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.
+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.
 
 **Next Example:** [Example 4](example_4.md)

example_4.md

@@ -2,16 +2,16 @@
 
 ![image](images/balloon.png)
 
-Let's bringup stretch in the willowgarage world from the [gazebo basics tutorial](gazebo_basics.md) and RViz by using the following command.
+Let's bringup stretch in RViz by using the following command.
 
 ```bash
-roslaunch stretch_gazebo gazebo.launch world:=worlds/willowgarage.world rviz:=true
+ros2 launch stretch_core stretch_driver.launch.py
+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
 
 ### The Code
 ```python
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
-import rospy
+import rclpy
+from rclpy.node import Node
 from visualization_msgs.msg import Marker
 
-class Balloon():
+class Balloon(Node):
 	def __init__(self):
-		self.publisher = rospy.Publisher('balloon', Marker, queue_size=10)
+		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 = rospy.Time()
+		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)
+		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.
 
 ```python
-self.pub = rospy.Publisher('balloon', Marker, queue_size=10)
+self.publisher_ = self.create_publisher(Marker, 'balloon', 10)	
 ```
-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*.
 
 
 ```python
 self.marker = Marker()
 self.marker.header.frame_id = '/base_link'
-self.marker.header.stamp = rospy.Time()
+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
@@ -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).

follow_joint_trajectory.md

@@ -1,24 +1,22 @@
 ## FollowJointTrajectory Commands
 
-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
 <p align="center">
   <img src="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
-import hello_helpers.hello_misc as hm
-import time
-
-class StowCommand(hm.HelloNode):
+from sensor_msgs.msg import JointState
 
+class StowCommand(Node):
     def __init__(self):
-        hm.HelloNode.__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):
-        stow_point = JointTrajectoryPoint()
-        stow_point.time_from_start = rospy.Duration(0.000)
-        stow_point.positions = [0.2, 0.0, 3.4]
+        joint_state = self.joint_state
+        if (joint_state is not None):
+            self.get_logger().info('stowing...')
 
-        trajectory_goal = FollowJointTrajectoryGoal()
-        trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
-        trajectory_goal.trajectory.points = [stow_point]
-        trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
-        trajectory_goal.trajectory.header.frame_id = 'base_link'
+            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))
 
-        self.trajectory_client.send_goal(trajectory_goal)
-        rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal))
-        self.trajectory_client.wait_for_result()
 
-    def main(self):
-        hm.HelloNode.main(self, 'stow_command', 'stow_command', wait_for_first_pointcloud=False)
-        rospy.loginfo('stowing...')
-        self.issue_stow_command()
-        time.sleep(2)
+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__':
-    try:
-        node = StowCommand()
-        node.main()
-    except KeyboardInterrupt:
-        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.
 
 ```python
 def issue_stow_command(self):
-    stow_point = JointTrajectoryPoint()
-    stow_point.time_from_start = rospy.Duration(0.000)
-    stow_point.positions = [0.2, 0.0, 3.4]
 ```
-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
+    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_point]
-    trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
+
+    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 `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.
 
 ```python
-self.trajectory_client.send_goal(trajectory_goal)
-rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal))
-self.trajectory_client.wait_for_result()
+    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. The last line of code waits for the result before it exits the python script.
+Make the action call and send the goal.
 
 ```python
-def main(self):
-    hm.HelloNode.main(self, 'stow_command', 'stow_command', wait_for_first_pointcloud=False)
-    rospy.loginfo('stowing...')
-    self.issue_stow_command()
-    time.sleep(2)
+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 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
   <img src="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
-import hello_helpers.hello_misc as hm
-
-class MultiPointCommand(hm.HelloNode):
+from sensor_msgs.msg import JointState
 
+class MultiPointCommand(Node):
     def __init__(self):
-        hm.HelloNode.__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):
-      point0 = JointTrajectoryPoint()
-      point0.positions = [0.2, 0.0, 3.4]
-      point0.velocities = [0.2, 0.2, 2.5]
-      point0.accelerations = [1.0, 1.0, 3.5]
+        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()
 
-      point1 = JointTrajectoryPoint()
-      point1.positions = [0.3, 0.1, 2.0]
+        point0.positions = [joint_value1, joint_value2, joint_value3]
 
-      point2 = JointTrajectoryPoint()
-      point2.positions = [0.5, 0.2, -1.0]
+        point0.velocities = [0.2, 0.2, 2.5]
 
-      point3 = JointTrajectoryPoint()
-      point3.positions = [0.6, 0.3, 0.0]
+        point0.accelerations = [1.0, 1.0, 3.5]
 
-      point4 = JointTrajectoryPoint()
-      point4.positions = [0.8, 0.2, 1.0]
+        point0.time_from_start = duration0.to_msg()
 
-      point5 = JointTrajectoryPoint()
-      point5.positions = [0.5, 0.1, 0.0]
+        point1 = JointTrajectoryPoint()
+        point1.positions = [0.3, 0.1, 2.0]
+        point1.time_from_start = duration1.to_msg()
 
-      trajectory_goal = FollowJointTrajectoryGoal()
-      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 = rospy.Time(0.0)
-      trajectory_goal.trajectory.header.frame_id = 'base_link'
+        point2 = JointTrajectoryPoint()
+        point2.positions = [0.5, 0.2, -1.0]
+        point2.time_from_start = duration2.to_msg()
 
-      self.trajectory_client.send_goal(trajectory_goal)
-      rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal))
-      self.trajectory_client.wait_for_result()
+        point3 = JointTrajectoryPoint()
+        point3.positions = [0.6, 0.3, 0.0]
+        point3.time_from_start = duration3.to_msg()
 
-    def main(self):
-        hm.HelloNode.main(self, 'multipoint_command', 'multipoint_command', wait_for_first_pointcloud=False)
-        rospy.loginfo('issuing multipoint command...')
-        self.issue_multipoint_command()
-        time.sleep(2)
+        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__':
-    try:
-        node = MultiPointCommand()
-        node.main()
-    except KeyboardInterrupt:
-        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).
+        trajectory_goal = FollowJointTrajectory.Goal()
+        trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
 
-**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.
+        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'
 
-```python
-trajectory_goal = FollowJointTrajectoryGoal()
-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 = rospy.Time(0.0)
-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 `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.

gazebo_basics.md

@@ -1,5 +1,10 @@
 # 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.
 ```

getting_started.md

@@ -1,57 +1,25 @@
 # Getting Started
 
-## Ubuntu
+## 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.
 
-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.
 
-## 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). Then create a catkin workspace for your ROS packages. Here is an [installation guide for creating a workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace). Once your system is set up, clone the [stretch_ros_tutorials](https://github.com/hello-sanchez/stretch_ros_tutorials.git) to your workspace and build the package in your workspace. This can be done by copying the commands below and pasting them into 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.
 
 ```
-cd ~/catkin_ws/src
+cd ~/ament_ws/src
+<!-- TODO: Change the link below -->
 git clone https://github.com/hello-sanchez/stretch_ros_tutorials.git
-cd ~/catkin_ws
-catkin_make
+cd ~/ament_ws
+colcon build
 ```
 
-## ROS Setup on Local Computer
-
-Hello Robot is currently running Stretch on Ubuntu 20.04 and ROS Noetic. To begin the setup, start with [installing Ubuntu 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.
-
-Currently, the Realsense2_description package isn't installed by rosdep and requires a user to install the package manually. Run the following command in your terminal
-
-```
-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 **src** folder in your preferred workspace.
-```
-cd ~/catkin_ws/src
-git clone https://github.com/hello-robot/stretch_ros
-git clone https://github.com/pal-robotics/realsense_gazebo_plugin
-git clone https://github.com/hello-sanchez/stretch_ros_tutorials.git
+Then source your workspace with the following command
 ```
-
-
-Change the directory to that of your catkin workspace and install system dependencies of the ROS packages. Then build your workspace.
+source ~/ament_ws/install/setup.bash"
 ```
-cd ~/catkin_ws
-rosdep install --from-paths src --ignore-src -r -y
-catkin_make
-```
-
-
-Once `caktin_make` has finished compiling,source your workspace and **.bashrc** file
-```
-echo "source /home/USER_NAME/catkin_ws/devel/setup.bash"
-source ~/.bashrc
-```
-
-## RoboMaker
-
-![image](images/aws-robomaker.png)
-
-
-If you cannot 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 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.
 
 **Next Tutorial:** [Gazebo Basics](gazebo_basics.md)

internal_state_of_stretch.md

@@ -1,17 +1,17 @@
 ## Getting the State of the Robot
 
-Begin by starting up the stretch driver launch file.
-```
-roslaunch stretch_core stretch_driver.launch
+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, [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.
-```
-rostopic echo /joint_states -n1
+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
 ```
-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. Remove the flag if you prefer to continuously print the topic for debugging purposes.
 
-Your terminal will 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.
+
 ```
 header:
   seq: 70999
@@ -27,31 +27,15 @@ velocity: [0.00015598730463972836, -0.00029395074514369584, -0.00028038454542173
 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.
 
-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.
-```
-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 published active topics.
-
-A powerful tool to visualize the ROS communication is the ROS [rqt_graph package](http://wiki.ros.org/rqt_graph). By typing the following, you can see a graph of topics being communicated between nodes.
+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.
 
 ```
-rqt_graph
+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)
+**Next Tutorial:** [RViz Basics](rviz_basics.md)

moveit_basics.md

@@ -14,17 +14,21 @@ roslaunch stretch_moveit_config moveit_rviz.launch
 ``` -->
 
 ## MoveIt! Without Hardware
-To begin running MoveIt! on stretch, run the demo launch file. This doesn't require any simulator or robot to run.
+To begin running MoveIt! on stretch, checkout to the feature/hybrid_planning branch and run the demo launch file.
 
 ```bash
-roslaunch stretch_moveit_config demo.launch
+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 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
 ## Running Gazebo with MoveIt! and Stretch
 
 
-```bash
+<!-- ```bash
 # Terminal 1:
 roslaunch stretch_gazebo gazebo.launch
 # Terminal 2:
@@ -52,6 +56,6 @@ 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)
+![image](images/gazebo_moveit.gif) -->
 
 **Next Tutorial:** [Follow Joint Trajectory Commands](follow_joint_trajectory.md)

navigation_stack.md

@@ -1,5 +1,10 @@
 ## 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.
 
 

package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>stretch_ros_tutorials</name>
+  <version>0.0.0</version>
+  <description>This package contains a set of ROS tutorials for Stretch beginners</description>
+  <maintainer email="cdesai@hello-robot.com">Chintan Desai</maintainer>
+  <license>Apache License 2.0</license>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>control_msgs</exec_depend>
+  <exec_depend>trajectory_msgs</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

rviz_basics.md

@@ -2,23 +2,25 @@
 
 You can utilize RViz to visualize Stretch's sensor information. To begin, run the stretch driver launch file.
 
-```
-roslaunch stretch_core stretch_driver.roslaunch
+```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.
-```
-rosrun rviz rviz -d `rospack find stretch_core`/rviz/stretch_simple_test.rviz
+```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 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 the *Add* button and include the *TF*  type in 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.
 
 ![image](images/rviz_adding_tf.gif)
 
-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).
 
 
 ## Running RViz and Gazebo (Simulation)

setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/stretch_ros_tutorials
+[install]
+install_scripts=$base/lib/stretch_ros_tutorials

setup.py

@@ -0,0 +1,33 @@
+from setuptools import setup
+
+package_name = 'stretch_ros_tutorials'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Chintan Desai',
+    maintainer_email='cdesai@hello-robot.com',
+    description='This package contains a set of ROS tutorials for Stretch beginners',
+    license='Apache License 2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'stow_command = stretch_ros_tutorials.stow_command:main',
+            'multipoint_command = stretch_ros_tutorials.multipoint_command:main',
+            'marker = stretch_ros_tutorials.marker:main',
+            'tf_broadcaster = stretch_ros_tutorials.tf_broadcaster:main',
+            'tf_listener = stretch_ros_tutorials.tf_listener:main',
+            'scan_filter = stretch_ros_tutorials.scan_filter:main',
+            'avoider = stretch_ros_tutorials.avoider:main',
+            'move = stretch_ros_tutorials.move:main',
+        ],
+    },
+)

src/marker.py

@@ -1,74 +0,0 @@
-#!/usr/bin/env python
-
-# This node will publish a spherical marker above the current robot position, as
-# it drives around the world.
-
-# Import rospy and sys
-import rospy
-
-# Import the Marker message type from the visualization_msgs package.
-from visualization_msgs.msg import Marker
-
-class Balloon():
-	def __init__(self):
-		# Set up a publisher.  We're going to publish on a topic called balloon.
-		self.publisher = rospy.Publisher('balloon', Marker, queue_size=10)
-
-		# Create a marker.  Markers of all shapes share a common type.
-		self.marker = Marker()
-
-		# 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, detailed on the wiki page.
-		self.marker.header.frame_id = '/base_link'
-		self.marker.header.stamp = rospy.Time()
-		self.marker.type = self.marker.SPHERE
-
-		# 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 and an existing
-		# marker, it will replace the existing marker with that ID number.
-		self.marker.id = 0
-
-		# Set the action.  We can add, delete, or modify markers.
-		self.marker.action = self.marker.ADD
-
-		# These are the size parameters for the marker. These will vary by marker type
-		self.marker.scale.x = 0.5
-		self.marker.scale.y = 0.5
-		self.marker.scale.z = 0.5
-
-		# Color, as an RGB triple, from 0 to 1.
-		self.marker.color.r = 1.0
-		self.marker.color.g = 0.0
-		self.marker.color.b = 0.0
-
-		# Alpha value, from 0 (invisible) to 1 (opaque). If you don't set this and
-		# it defaults to zero, then your marker will be invisible.
-		self.marker.color.a = 1.0
-
-		# Specify the pose of the marker. Since spheres are rotationally invarient,
-		# 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 above the robot, and will move with it.
-		self.marker.pose.position.x = 0.0
-		self.marker.pose.position.y = 0.0
-		self.marker.pose.position.z = 2.0
-
-	def publish_marker(self):
-		# publisher the marker
-		self.publisher.publish(self.marker)
-
-
-if __name__ == '__main__':
-	# Initialize the node, as usual
-	rospy.init_node('marker')
-
-	ballon = Balloon()
-
-	# Set a rate.
-	rate = rospy.Rate(10)
-
-	# Publish the marker at 10Hz.
-	while not rospy.is_shutdown():
-		ballon.publish_marker()
-		rate.sleep()

src/move.py

@@ -1,57 +0,0 @@
-#!/usr/bin/env python
-
-# Every python controller needs these lines
-import rospy
-
-# The Twist message is used to send velocities to the robot.
-from geometry_msgs.msg import Twist
-
-class Move:
-	def __init__(self):
-		# Setup a publisher that will send the velocity commands for the Stretch
-		# This will publish on a topic called "/stretch/cmd_vel" with a message type Twist.
-		self.pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=1) #/stretch_diff_drive_controller/cmd_vel for gazebo
-
-	def move_forward(self):
-		# Make a Twist message.  We're going to set all of the elements, since we
-		# can't depend on them defaulting to safe values.
-		command = Twist()
-
-		# 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.
-		command.linear.x = 0.1
-		command.linear.y = 0.0
-		command.linear.z = 0.0
-
-		# A Twist also has three rotational velocities (in radians per second).
-		# The Stretch will only respond to rotations around the z (vertical) axis.
-		command.angular.x = 0.0
-		command.angular.y = 0.0
-		command.angular.z = 0.0
-
-		# Publish the Twist commands
-		self.pub.publish(command)
-
-if __name__ == '__main__':
-	# Initialize the node, and call it "move".
-	rospy.init_node('move')
-
-	# Setup Move class to base_motion
-	base_motion = Move()
-
-	# Rate allows us to control the (approximate) rate at which we publish things.
-	# For this example, we want to publish at 10Hz.
-	rate = rospy.Rate(10)
-
-	# This will loop until ROS shuts down the node.  This can be done on the
-	# command line with a ctrl-C, or automatically from roslaunch.
-	while not rospy.is_shutdown():
-		# Run the move_foward function in the Move class
-		base_motion.move_forward()
-
-		# Do an idle wait to control the publish rate.  If we don't control the
-		# rate, the node will publish as fast as it can, and consume all of the
-		# available CPU resources.  This will also add a lot of network traffic,
-		# possibly slowing down other things.
-		rate.sleep()

src/multipoint_command.py

@@ -1,98 +0,0 @@
-#!/usr/bin/env python3
-
-# Every python controller needs these lines
-import rospy
-import time
-
-# Import the FollowJointTrajectoryGoal from the control_msgs.msg package to
-# control the Stretch robot.
-from control_msgs.msg import FollowJointTrajectoryGoal
-
-# Import JointTrajectoryPoint from the trajectory_msgs package to define
-# robot trajectories.
-from trajectory_msgs.msg import JointTrajectoryPoint
-
-# Import hello_misc script for handling trajecotry goals with an action client.
-import hello_helpers.hello_misc as hm
-
-class MultiPointCommand(hm.HelloNode):
-
-    # Initialize the inhereted hm.Hellonode class.
-    def __init__(self):
-        hm.HelloNode.__init__(self)
-
-    def issue_multipoint_command(self):
-      # Set point0 as a JointTrajectoryPoint().
-      point0 = JointTrajectoryPoint()
-
-      # Provide desired positions of lift, wrist extension, and yaw of
-      # the wrist (in meters).
-      point0.positions = [0.2, 0.0, 3.4]
-
-      # Provide desired velocity of the lift (m/s), wrist extension (m/s),
-      # and wrist yaw (rad/s).
-      # IMPORTANT NOTE: The lift and wrist extension can only go up to 0.2 m/s!
-      point0.velocities = [0.2, 0.2, 2.5]
-
-      # Provide desired velocity of the lift (m/s^2), wrist extension (m/s^2),
-      # and wrist yaw (rad/s^2).
-      point0.accelerations = [1.0, 1.0, 3.5]
-
-      # Set positions for the following 5 trajectory points.
-      # IMPORTANT NOTE: 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.
-      point1 = JointTrajectoryPoint()
-      point1.positions = [0.3, 0.1, 2.0]
-
-      point2 = JointTrajectoryPoint()
-      point2.positions = [0.5, 0.2, -1.0]
-
-      point3 = JointTrajectoryPoint()
-      point3.positions = [0.6, 0.3, 0.0]
-
-      point4 = JointTrajectoryPoint()
-      point4.positions = [0.8, 0.2, 1.0]
-
-      point5 = JointTrajectoryPoint()
-      point5.positions = [0.5, 0.1, 0.0]
-
-      # Set trajectory_goal as a FollowJointTrajectoryGoal and define
-      # the joint names as a list.
-      trajectory_goal = FollowJointTrajectoryGoal()
-      trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
-
-      # Then trajectory_goal.trajectory.points is defined by a list of the joint
-      # trajectory points
-      trajectory_goal.trajectory.points = [point0, point1, point2, point3, point4, point5]
-
-    # Specify the coordinate frame that we want (base_link) and set the time to be now.
-      trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
-      trajectory_goal.trajectory.header.frame_id = 'base_link'
-
-      # Make the action call and send the goal. The last line of code waits
-      # for the result before it exits the python script.
-      self.trajectory_client.send_goal(trajectory_goal)
-      rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal))
-      self.trajectory_client.wait_for_result()
-
-    # Create a funcion, main(), to do all of the setup the hm.HelloNode class
-    # and issue the stow command.
-    def main(self):
-        # The arguments of the main function of the hm.HelloNode class are the
-        # node_name, node topic namespace, and boolean (default value is true).
-        hm.HelloNode.main(self, 'multipoint_command', 'multipoint_command', wait_for_first_pointcloud=False)
-        rospy.loginfo('issuing multipoint command...')
-        self.issue_multipoint_command()
-        time.sleep(2)
-
-
-if __name__ == '__main__':
-    try:
-        # Initialize the MultiPointCommand() class and set it to node and run the
-        # main() function.
-        node = MultiPointCommand()
-        node.main()
-    except KeyboardInterrupt:
-        rospy.loginfo('interrupt received, so shutting down')

src/stow_command.py

@@ -1,70 +0,0 @@
-#!/usr/bin/env python3
-
-# Every python controller needs these lines
-import rospy
-import time
-
-# Import the FollowJointTrajectoryGoal from the control_msgs.msg package to
-# control the Stretch robot.
-from control_msgs.msg import FollowJointTrajectoryGoal
-
-# Import JointTrajectoryPoint from the trajectory_msgs package to define
-# robot trajectories.
-from trajectory_msgs.msg import JointTrajectoryPoint
-
-# Import hello_misc script for handling trajecotry goals with an action client.
-import hello_helpers.hello_misc as hm
-
-class StowCommand(hm.HelloNode):
-
-    # Initialize the inhereted hm.Hellonode class.
-    def __init__(self):
-        hm.HelloNode.__init__(self)
-
-    def issue_stow_command(self):
-        # Set stow_point as a JointTrajectoryPoint().
-        stow_point = JointTrajectoryPoint()
-        stow_point.time_from_start = rospy.Duration(0.000)
-
-        # Provide desired positions of lift, wrist extension, and yaw of
-        # the wrist (in meters).
-        stow_point.positions = [0.2, 0.0, 3.4]
-
-        # Set trajectory_goal as a FollowJointTrajectoryGoal and define
-        # the joint names as a list.
-        trajectory_goal = FollowJointTrajectoryGoal()
-        trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
-
-        # Then trajectory_goal.trajectory.points is defined by the positions
-        # set in stow_point
-        trajectory_goal.trajectory.points = [stow_point]
-
-        # Specify the coordinate frame that we want (base_link) and set the time to be now.
-        trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
-        trajectory_goal.trajectory.header.frame_id = 'base_link'
-
-        # Make the action call and send the goal. The last line of code waits
-        # for the result before it exits the python script.
-        self.trajectory_client.send_goal(trajectory_goal)
-        rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal))
-        self.trajectory_client.wait_for_result()
-
-    # Create a funcion, main(), to do all of the setup the hm.HelloNode class
-    # and issue the stow command.
-    def main(self):
-        # The arguments of the main function of the hm.HelloNode class are the
-        # node_name, node topic namespace, and boolean (default value is true).
-        hm.HelloNode.main(self, 'stow_command', 'stow_command', wait_for_first_pointcloud=False)
-        rospy.loginfo('stowing...')
-        self.issue_stow_command()
-        time.sleep(2)
-
-
-if __name__ == '__main__':
-    try:
-        # Initialize the StowCommand() class and set it to node and run the
-        # main() function.
-        node = StowCommand()
-        node.main()
-    except KeyboardInterrupt:
-        rospy.loginfo('interrupt received, so shutting down')

src/avoider.py → stretch_ros_tutorials/avoider.py

@@ -1,36 +1,33 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
-# Every python controller needs these lines
-import rospy
+import rclpy
+from rclpy.node import Node
 from numpy import linspace, inf, tanh
 from math import sin
 
-# The Twist message is used to send velocities to the robot.
+# The Twist message is used to send velocities to the robot
 from geometry_msgs.msg import Twist
 
-# We're going to subscribe to a LaserScan message.
+# We're going to subscribe to a LaserScan message
 from sensor_msgs.msg import LaserScan
 
-class Avoider:
-    def __init__(self):
-        # Set up a publisher and a subscriber.  We're going to call the subscriber
-        # "scan", and filter the ranges similar to what we did in example 2.
-        # For the publisher, we're going to use the topic name /stretch/cmd_vel.
-        self.pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=1) #/stretch_diff_drive_controller/cmd_vel for gazebo
-        self.sub = rospy.Subscriber('/scan', LaserScan, self.set_speed)
 
+class Avoider(Node):
+    def __init__(self):
+        super().__init__('stretch_avoider')
+        
         # 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.
+        # width (1 meter) from the axis are not considered
         self.width = 1
         self.extent = self.width / 2.0
 
         # We want the robot to drive foward or backwards until it is 0.5 m from
-        # the closest obstacle measured in front of it.
+        # the closest obstacle measured in front of it
         self.distance = 0.5
 
-        # Alocate a Twist to use, and set everything to zero.  We're going to do
-        # this here, to save some time in the callback function, set_speed().
+        # Allocate a Twist to use, and set everything to zero here, 
+        # to save some time in the callback function set_speed()
         self.twist = Twist()
         self.twist.linear.x = 0.0
         self.twist.linear.y = 0.0
@@ -39,19 +36,25 @@ class Avoider:
         self.twist.angular.y = 0.0
         self.twist.angular.z = 0.0
 
+        # Set up a publisher and a subscriber.  We're going to call the subscriber
+        # "scan", and filter the ranges similar to what we did in example 2
+        # For the publisher, we're going to use the topic name /stretch/cmd_vel
+        self.publisher_ = self.create_publisher(Twist, '/stretch/cmd_vel', 1) #/stretch_diff_drive_controller/cmd_vel for gazebo
+        self.subscriber_ = self.create_subscription(LaserScan, '/scan', self.set_speed, 10)
+
     # Called every time we get a LaserScan message from ROS.
-    def set_speed(self,msg):
+    def set_speed(self, msg):
         # Figure out the angles of the scan.  We're going to do this each time, in case we're subscribing to more than one
-        # laser, with different numbers of beams.
+        # laser, with different numbers of beams
         angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
 
-        # Work out the y coordinates of the ranges.
+        # Work out the y coordinates of the ranges
         points = [r * sin(theta) if (theta < -2.5 or theta > 2.5) else inf for r,theta in zip(msg.ranges, angles)]
 
-        # If we're close to the x axis, keep the range, otherwise use inf, which means "no return".
+        # If we're close to the x axis, keep the range, otherwise use inf, which means "no return"
         new_ranges = [r if abs(y) < self.extent else inf for r,y in zip(msg.ranges, points)]
 
-		# Calculate the difference of the closest measured scan and where we want the robot to stop.
+		# Calculate the difference of the closest measured scan and where we want the robot to stop
         error = min(new_ranges) - self.distance
 
         # Using hyperbolic tanget for speed regulation, with a threshold to stop
@@ -59,16 +62,20 @@ class Avoider:
         self.twist.linear.x = tanh(error) if (error > 0.05 or error < -0.05) else 0
 
         # Publish the command using the publisher
-        self.pub.publish(self.twist)
+        self.publisher_.publish(self.twist)
 
-if __name__ == '__main__':
-    # Initialize the node, and call it "avoider".
-    rospy.init_node('avoider')
 
-	# Setup Avoider class
-    Avoider()
+def main(args=None):
+    rclpy.init(args=args)
+
+    avoider = Avoider()
+
+    rclpy.spin(avoider)
+
+    avoider.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
 
-    # 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.
-    rospy.spin()

stretch_ros_tutorials/forward_kinematics.py

@@ -0,0 +1,56 @@
+import rclpy
+import math
+import numpy as np
+from rclpy.node import Node 
+
+class ForwardKinematics():
+    def __init__(self, lift_pos, arm_pos, wrist_pos):
+        # Get joint values as node parameters that can be set from CLI
+        self.a1 = 0
+        self.alpha1 = 0
+        self.d1 = 0.2
+        self.theta1 = 0
+        self.a2 = 0
+        self.alpha2 = 0
+        self.d2 = lift_pos
+        self.theta2 = 0
+        self.a3 = 0
+        self.alpha3 = -1.5708
+        self.d3 = 0.5 + arm_pos
+        self.theta3 = -wrist_pos
+        self.a4 = 0
+        self.alpha4 = 1.5708
+        self.d4 = 0
+        self.theta4 = 1.5708
+  
+    def print_dh_table(self):
+        # Fancy print of the DH table here
+        return 0    
+
+    def get_hm_matrix(self, a, alpha, d, theta):
+        # Angles are in radians
+        Ax = np.array([(math.cos(theta), -math.sin(theta)*math.cos(alpha), math.sin(theta)*math.sin(alpha), a*math.cos(theta)),
+                       (math.sin(theta), math.cos(theta)*math.cos(alpha), -math.cos(theta)*math.sin(alpha), a*sin(theta)),
+                       (0, math.sin(alpha), math.cos(alpha), d),
+                       (0, 0, 0, 1)])
+        return Ax
+
+    def hm_matrices(self):
+        # Create numpy array to deal with matrix operations
+        A1 = get_hm_matrix(self.a1, self.alpha1, self.d1, self.theta1)
+        A2 = get_hm_matrix(self.a2, self.alpha2, self.d2, self.theta2)
+        A3 = get_hm_matrix(self.a3, self.alpha3, self.d3, self.theta3)
+        A4 = get_hm_matrix(self.a4, self.alpha4, self.d4, self.theta4)
+
+    def compute_fk(self):
+        An = A1*A2*A3*A4
+        return 0
+
+    def main(self):
+        self.print_dh_table()
+        
+        return 0
+
+if __name__ == '__main__':
+    fk = ForwardKinematics(lift_pos=0, arm_pos=0, wrist_pos=0)
+    fk.main()

stretch_ros_tutorials/marker.py

@@ -0,0 +1,78 @@
+#!/usr/bin/env python3
+
+# This node will publish a spherical marker above the current robot position, as
+# it drives around the world
+
+import rclpy
+from rclpy.node import Node
+
+# Import the Marker message type from the visualization_msgs package
+from visualization_msgs.msg import Marker
+
+class Balloon(Node):
+	def __init__(self):
+		# Initialize the node named marker
+		super().__init__('stretch_marker')
+
+		# Set up a publisher that will publish on a topic called balloon
+		self.publisher_ = self.create_publisher(Marker, 'balloon', 10)	
+		
+		# Markers of all shapes share a common type
+		self.marker = Marker()
+
+		# The frame ID is the frame in which the position of the marker is specified
+		# Refer to the wiki page for more marker types
+		self.marker.header.frame_id = '/base_link'
+		self.marker.header.stamp = self.get_clock().now().to_msg()
+		self.marker.type = self.marker.SPHERE
+
+		# If you have more than one marker that you want displayed at a given time,
+		# then each needs to have a unique ID number
+		self.marker.id = 0
+
+		# You can Add, Delete, or Modify markers by defining the action
+		self.marker.action = self.marker.ADD
+
+		# Define the size parameters for the marker
+		self.marker.scale.x = 0.5
+		self.marker.scale.y = 0.5
+		self.marker.scale.z = 0.5
+
+		# Define the color as an RGB triple from 0 to 1
+		self.marker.color.r = 1.0
+		self.marker.color.g = 0.0
+		self.marker.color.b = 0.0
+
+		# Define the Alpha value, from 0 (invisible) to 1 (opaque)
+		self.marker.color.a = 1.0
+
+		# Specify the pose of the marker. Since spheres are rotationally invariant,
+		# we're only going to specify the positional elements. These are
+		# in the coordinate frame named in frame_id. In this case, the position
+		# will always be directly above the robot, and will move with it
+		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):
+		# publisher the marker
+		self.publisher_.publish(self.marker)
+
+def main(args=None):
+	rclpy.init(args=args)
+
+	balloon = Balloon()
+
+	# This will loop until ROS shuts down the node.  This can be done on the
+	# command line with a ctrl-C, or automatically from roslaunch.
+	while rclpy.ok():
+		balloon.publish_marker()
+
+	balloon.destroy_node()	
+	rclpy.shutdown()
+
+
+if __name__ == '__main__':
+	main()

stretch_ros_tutorials/move.py

@@ -0,0 +1,57 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+
+# Imports the built-in Twist message type that the node uses to structure the velocity data
+from geometry_msgs.msg import Twist
+
+
+class Move(Node):
+	def __init__(self):
+		super().__init__('stretch_base_move')
+		
+		# Setup a publisher that will send the velocity commands to the base
+		# This will publish on a topic called "/stretch/cmd_vel" with a message type Twist
+		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):
+		# Define a Twist message
+		command = Twist()
+
+		# A Twist has three linear velocities (in meters per second), along each of the axes
+		# Stretch has a ddifferential drive base and can't move laterally, it will only pay 
+		# attention to velocity along the x-axis
+		command.linear.x = 0.0
+		command.linear.y = 0.0
+		command.linear.z = 0.0
+
+		# A Twist also has three rotational velocities (in radians per second)
+		# Stretch will only respond to rotations along the z-axis
+		command.angular.x = 0.0
+		command.angular.y = 0.0
+		command.angular.z = 0.5
+
+		# Publish the Twist message
+		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()
+	

stretch_ros_tutorials/multipoint_command.py

@@ -0,0 +1,133 @@
+#!/usr/bin/env python3
+
+# Every python controller needs these lines
+import sys
+import rclpy
+from rclpy.node import Node
+from rclpy.action import ActionClient
+from rclpy.duration import Duration
+
+# Import the FollowJointTrajectoryGoal from the control_msgs.action package to
+# control the Stretch robot.
+from control_msgs.action import FollowJointTrajectory
+
+# Import JointTrajectoryPoint from the trajectory_msgs package to define
+# robot trajectories.
+from trajectory_msgs.msg import JointTrajectoryPoint
+from sensor_msgs.msg import JointState
+
+
+class MultiPointCommand(Node):
+    # Initialize the inhereted hm.Hellonode class.
+    def __init__(self):
+        super().__init__('stretch_multipoint_command')
+
+        # Create an action client that can communicate with the /stretch_controller/follow_joint_trajectory action server
+        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()
+
+        # Create a subscriber for the /stretch/joint_states topic
+        self.subscription = self.create_subscription(JointState, '/stretch/joint_states', self.joint_states_callback, 1)
+        self.subscription
+
+        self.get_logger().info('issuing multipoint command...')
+
+    # Callback function for the /stretch/joint_states topic
+    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)
+        
+        # Provide desired positions of lift, wrist extension, and yaw of
+        # the wrist
+        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 point0 as a JointTrajectoryPoint().
+        point0 = JointTrajectoryPoint()
+
+        # Provide desired positions of lift, wrist extension, and yaw of
+        # the wrist (in meters).
+        point0.positions = [joint_value1, joint_value2, joint_value3]
+
+        # Provide desired velocity of the lift (m/s), wrist extension (m/s),
+        # and wrist yaw (rad/s).
+        # IMPORTANT NOTE: The lift and wrist extension can only go up to 0.2 m/s!
+        point0.velocities = [0.2, 0.2, 2.5]
+
+        # Provide desired velocity of the lift (m/s^2), wrist extension (m/s^2),
+        # and wrist yaw (rad/s^2).
+        point0.accelerations = [1.0, 1.0, 3.5]
+
+        point0.time_from_start = duration0.to_msg()
+
+        # Set positions for the following 5 trajectory points.
+        # IMPORTANT NOTE: 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.
+        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()
+
+        # Set trajectory_goal as a FollowJointTrajectoryGoal and define
+        # the joint names as a list.
+        trajectory_goal = FollowJointTrajectory.Goal()
+        trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
+
+        # Then trajectory_goal.trajectory.points is defined by a list of the joint
+        # trajectory points
+        trajectory_goal.trajectory.points = [point0, point1, point2, point3, point4, point5]
+
+        # Specify the coordinate frame that we want (base_link) and set the time to be now.
+        trajectory_goal.trajectory.header.stamp = self.get_clock().now().to_msg()
+        trajectory_goal.trajectory.header.frame_id = 'base_link'
+
+        # Make the action call and send the goal. The last line of code waits
+        # for the result before it exits the python script.
+        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()

src/scan_filter.py → stretch_ros_tutorials/scan_filter.py

@@ -1,52 +1,68 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 # Every python controller needs these lines
-import rospy
+import rclpy
+from rclpy.node import Node
 from numpy import linspace, inf
 from math import sin
-# We're going to subscribe to a LaserScan message.
+
+# We're going to subscribe to a LaserScan message
 from sensor_msgs.msg import LaserScan
 
-class Scanfilter:
+
+class ScanFilter(Node):
 	def __init__(self):
+		# Initialize a ROS node named scan_filter
+		super().__init__('stretch_scan_filter')
+
+		# Set up a publisher that will publish on a topic called "filtered_scan",
+		# with a LaserScan message type
+		self.pub = self.create_publisher(LaserScan, '/filtered_scan', 10) #/stretch_diff_drive_controller/cmd_vel for gazebo
+
+		# 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
+		self.sub = self.create_subscription(LaserScan, '/scan', self.scan_filter_callback, 10)
+		
 		# 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.
+		# width (1 meter) from the axis are not considered
 		self.width = 1
 		self.extent = self.width / 2.0
 
-		# 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.
-		self.sub = rospy.Subscriber('/scan', LaserScan, self.callback)
+		self.get_logger().info("Publishing the filtered_scan topic. Use RViz to visualize.")
 
-		# Set up a publisher.  This will publish on a topic called "filtered_scan",
-		# with a LaserScan message type.
-		self.pub = rospy.Publisher('filtered_scan', LaserScan, queue_size=10)
-
-	def callback(self,msg):
+	def scan_filter_callback(self,msg):
 		# Figure out the angles of the scan.  We're going to do this each time, in case we're subscribing to more than one
-		# laser, with different numbers of beams.
+		# laser, with different numbers of beams
 		angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
 
-		# Work out the y coordinates of the ranges.
+		# Work out the y coordinates of the ranges
 		points = [r * sin(theta) if (theta < -2.5 or theta > 2.5) else inf for r,theta in zip(msg.ranges, angles)]
 
-		# If we're close to the x axis, keep the range, otherwise use inf, which means "no return".
+		# If we're close to the x axis, keep the range, otherwise use inf, which means "no return"
 		new_ranges = [r if abs(y) < self.extent else inf for r,y in zip(msg.ranges, points)]
 
-		# Substitute in the new ranges in the original message, and republish it.
+		# Substitute in the new ranges in the original message, and republish it
 		msg.ranges = new_ranges
 		self.pub.publish(msg)
 
-if __name__ == '__main__':
-	# Initialize the node, and call it "scan_filter".
-	rospy.init_node('scan_filter')
 
-	# Setup Scanfilter class
-	Scanfilter()
+def main(args=None):
+	# First the rclpy library is initialized
+	rclpy.init(args=args)
+
+	# Create object of the ScanFilter class
+	scan_filter = ScanFilter()
 
 	# 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.
-	rospy.spin()
+	# and ROS will not process any messages
+	rclpy.spin(scan_filter)
+
+	scan_filter.destroy_node()
+	rclpy.shutdown()
+
+
+if __name__ == '__main__':
+	main()

stretch_ros_tutorials/stow_command.py

@@ -0,0 +1,105 @@
+#!/usr/bin/env python3
+
+# Every python controller needs these lines
+import rclpy
+from rclpy.node import Node
+from rclpy.duration import Duration
+from rclpy.action import ActionClient
+
+import sys
+
+# Import the FollowJointTrajectoryGoal from the control_msgs.action package to
+# control the Stretch robot.
+from control_msgs.action import FollowJointTrajectory
+
+# Import JointTrajectoryPoint from the trajectory_msgs package to define
+# robot trajectories and JointState from sensor_msgs to store joint states
+from trajectory_msgs.msg import JointTrajectoryPoint
+from sensor_msgs.msg import JointState
+
+
+class StowCommand(Node):
+    def __init__(self):
+        # Initialize a ROS node named stow_command
+        super().__init__('stretch_stow_command')
+        self.joint_state = JointState()
+
+        # Create an action client that can communicate with the /stretch_controller/follow_joint_trajectory action server
+        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()
+
+        # Create a subscriber for the /stretch/joint_states topic
+        self.subscription = self.create_subscription(JointState, '/stretch/joint_states', self.joint_states_callback, 1)
+        self.subscription
+
+    # Callback function for the /stretch/joint_states topic
+    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...')
+
+            # Set two points as JointTrajectoryPoint(): stow_point1 is the current state, 
+            # stow_point2 is the goal state
+            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()
+
+            # Provide desired positions of lift, wrist extension, and yaw of
+            # the wrist
+            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]
+
+            # Set trajectory_goal as a FollowJointTrajectoryGoal and define
+            # the joint names as a list.
+            trajectory_goal = FollowJointTrajectory.Goal()
+            trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
+
+            # Then trajectory_goal.trajectory.points is defined by the positions
+            # set in stow_point1 and stow_point2
+            trajectory_goal.trajectory.points = [stow_point1, stow_point2]
+
+            # Specify the coordinate frame that we want (base_link) and set the time to be now
+            trajectory_goal.trajectory.header.stamp = self.get_clock().now().to_msg()
+            trajectory_goal.trajectory.header.frame_id = 'base_link'
+
+            # Make the action call and send the goal
+            self.trajectory_client.send_goal_async(trajectory_goal)
+            self.get_logger().info('Sent stow goal = {0}'.format(trajectory_goal))
+
+
+# Create a funcion, main(), to do all of the setup the hm.HelloNode class
+# and issue the stow command.
+def main(args=None):
+    rclpy.init(args=args)
+     
+    # Create object of the StowCommand class
+    stow_command = StowCommand()
+    
+    # spin_once allows the joint_states_callback functions to be executed once 
+    # for joint states to be received to this node
+    rclpy.spin_once(stow_command)
+    stow_command.issue_stow_command()
+
+    rclpy.spin(stow_command)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    stow_command.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

stretch_ros_tutorials/tf_broadcaster.py

@@ -0,0 +1,88 @@
+from geometry_msgs.msg import TransformStamped
+
+import rclpy
+from rclpy.node import Node
+from tf2_ros import TransformBroadcaster
+import tf_transformations
+
+
+# This node publishes three child static frames in reference to their parent frames as below:
+# parent -> link_mast            child -> fk_link_mast
+# parent -> link_lift            child -> fk_link_lift
+# parent -> link_wrist_yaw       child -> fk_link_wrist_yaw
+# Tf frames are not defined according to the DH convention by default 
+# The new frames have been declared to make the computation of 
+# forward and inverse kinematics easier by defining the new frames
+# according to the DH convention
+class FixedFrameBroadcaster(Node):
+    def __init__(self):
+        super().__init__('stretch_tf_broadcaster')
+        self.br = TransformBroadcaster(self)
+        time_period = 0.1 # seconds
+        self.timer = self.create_timer(time_period, self.broadcast_timer_callback)
+
+        self.mast = TransformStamped()
+        self.mast.header.frame_id = 'link_mast'
+        self.mast.child_frame_id = 'fk_link_mast'
+        self.mast.transform.translation.x = 0.0
+        self.mast.transform.translation.y = 0.0
+        self.mast.transform.translation.z = 0.0
+        q = tf_transformations.quaternion_from_euler(1.5707, 0, -1.5707)
+        self.mast.transform.rotation.x = q[0]
+        self.mast.transform.rotation.y = q[1]
+        self.mast.transform.rotation.z = q[2]
+        self.mast.transform.rotation.w = q[3]
+
+        self.lift = TransformStamped()
+        self.lift.header.stamp = self.get_clock().now().to_msg()
+        self.lift.header.frame_id = 'link_lift'
+        self.lift.child_frame_id = 'fk_link_lift'
+        self.lift.transform.translation.x = 0.0
+        self.lift.transform.translation.y = 2.0
+        self.lift.transform.translation.z = 0.0
+        q = tf_transformations.quaternion_from_euler(1.5707, 0, -1.5707)
+        self.lift.transform.rotation.x = q[0]
+        self.lift.transform.rotation.y = q[1]
+        self.lift.transform.rotation.z = q[2]
+        self.lift.transform.rotation.w = q[3]
+        self.br.sendTransform(self.lift)
+
+        self.wrist = TransformStamped()
+        self.wrist.header.stamp = self.get_clock().now().to_msg()
+        self.wrist.header.frame_id = 'link_wrist_yaw'
+        self.wrist.child_frame_id = 'fk_link_wrist_yaw'
+        self.wrist.transform.translation.x = 0.0
+        self.wrist.transform.translation.y = 2.0
+        self.wrist.transform.translation.z = 0.0
+        q = tf_transformations.quaternion_from_euler(1.5707, 0, -1.5707)
+        self.wrist.transform.rotation.x = q[0]
+        self.wrist.transform.rotation.y = q[1]
+        self.wrist.transform.rotation.z = q[2]
+        self.wrist.transform.rotation.w = q[3]
+        self.br.sendTransform(self.wrist)
+
+        self.get_logger().info("Publishing Tf frames. Use RViz to visualize.")
+
+    def broadcast_timer_callback(self):
+        self.mast.header.stamp = self.get_clock().now().to_msg()
+        self.br.sendTransform(self.mast)
+
+        self.lift.header.stamp = self.get_clock().now().to_msg()
+        self.br.sendTransform(self.lift)
+
+        self.wrist.header.stamp = self.get_clock().now().to_msg()
+        self.br.sendTransform(self.wrist)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    tf_broadcaster = FixedFrameBroadcaster()
+
+    rclpy.spin(tf_broadcaster)
+
+    tf_broadcaster.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

stretch_ros_tutorials/tf_listener.py

@@ -0,0 +1,63 @@
+import rclpy
+from rclpy.node import Node
+from rclpy.time import Time
+
+from tf2_ros import TransformException
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
+
+# This node prints the transformation between the fk_link_mast frame and a given target frame
+# The default target frame is the link_grasp_center frame
+# To change the target frame just pass it as an argument to the node, for ex. "target_frame:='fk_link_lift'"
+class FrameListener(Node):
+
+    def __init__(self):
+        super().__init__('stretch_tf_listener')
+
+        # Declare and acquire `target_frame` parameter
+        self.declare_parameter('target_frame', 'link_grasp_center')
+        self.target_frame = self.get_parameter(
+            'target_frame').get_parameter_value().string_value
+
+        # Start a Tf buffer that will store the tf information for a few seconds
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+
+        # Call on_timer function every second
+        time_period = 1.0 # seconds
+        self.timer = self.create_timer(time_period, self.on_timer)
+
+    def on_timer(self):
+        # Store frame names in variables that will be used to
+        # compute transformations
+        from_frame_rel = self.target_frame
+        to_frame_rel = 'fk_link_mast'
+
+        # Look up the transformation between target_frame and link_mast frames
+        try:
+            now = Time()
+            trans = self.tf_buffer.lookup_transform(
+                to_frame_rel,
+                from_frame_rel,
+                now)
+        except TransformException as ex:
+            self.get_logger().info(
+                f'Could not transform {to_frame_rel} to {from_frame_rel}: {ex}')
+            return
+
+        self.get_logger().info(
+                        f'the pose of target frame {from_frame_rel} with reference to {to_frame_rel} is: {trans}')
+
+
+def main():
+    rclpy.init()
+    node = FrameListener()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

teleoperating_stretch.md

@@ -1,11 +1,15 @@
 ## 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:

test/test_copyright.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'