refactor dirs

2 years ago · ffe6c8df64
--- a/README.md
+++ b/README.md
@ -12,10 +12,11 @@ This repo provides instructions on installing and using code on the Stretch RE1
 7. [MoveIt! Basics](moveit_basics.md)
 8. [Follow Joint Trajectory Commands](follow_joint_trajectory.md)
 9. [Perception](perception.md)
 10. [FUNMAP](https://github.com/hello-robot/stretch_ros/tree/master/stretch_funmap)
 11. Microphone Array
 11. ROS testing
 12. Other Nav Stack Features
 10. [ArUco Marker Detection](aruco_marker_detection.md)
 11. [ReSpeaker Microphone Array](respeaker_microphone_array.md)
 12. [FUNMAP](https://github.com/hello-robot/stretch_ros/tree/master/stretch_funmap)
 13. ROS testing
 14. Other Nav Stack Features


 ## Other ROS Examples
@ -27,3 +28,9 @@ To help get you get started on your software development, here are examples of n
 4. [Give Stretch a Balloon](example_4.md) - Create a "balloon" marker that goes where ever Stretch goes.
 5. [Print Joint States](example_5.md) - Print the joint states of Stretch.
 6. [Store Effort Values](example_6.md) - Print, store, and plot the effort values of the Stretch robot.
 7. [Capture Image](example_7.md) - Capture images from the RealSense camera data.
 8. [Voice to Text](example_8.md) - Interpret speech and save transcript to a text file.
 9. [Voice Teleoperation of Base](example_9.md) - Use speech to teleoperate the mobile base.
 10. [Tf2 Broadcaster and Listener](example_10.md) - Create a tf2 broadcaster and listener.
 11. [PointCloud Transformation](example_11.md) - Convert PointCloud2 data to a PointCloud and transform to a different frame.
 12. [ArUco Tag Locator](example_12.md) - Actuate the head to locate a requested ArUco marker tag and return a transform.
--- a/docs/images/hello_robot_favicon.png
+++ b/docs/images/hello_robot_favicon.png
--- a/docs/images/hello_robot_logo_light.png
+++ b/docs/images/hello_robot_logo_light.png
--- a/example_1.md
+++ b/example_1.md
@ -1,160 +0,0 @@
 ## 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 the following command in a new terminal.

 ```bash
 # Terminal 1
 roslaunch stretch_core stretch_driver.launch
 ```

 Switch the mode to *navigation* mode using a rosservice call. Then drive the robot forward with the move node.

 ```bash
 # Terminal 2
 rosservice call /switch_to_navigation_mode
 cd catkin_ws/src/stretch_ros_tutorials/src/
 python move.py
 ```
 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.

 ```python
 #!/usr/bin/env python

 import rospy
 from geometry_msgs.msg import Twist

 class Move:
 	"""
 	A class that sends Twist messages to move the Stretch robot foward.
 	"""
 	def __init__(self):
 		"""
 		Function that initializes the subscriber.
 		:param self: The self reference.
 		"""
 		self.pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=1) #/stretch_diff_drive_controller/cmd_vel for gazebo

 	def move_forward(self):
 		"""
 		Function that publishes Twist messages
 		:param self: The self reference.

 		:publishes command: Twist message.
 		"""
 		command = Twist()
 		command.linear.x = 0.1
 		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)

 if __name__ == '__main__':
 	rospy.init_node('move')
 	base_motion = Move()
 	rate = rospy.Rate(10)
 	while not rospy.is_shutdown():
 		base_motion.move_forward()
 		rate.sleep()
 ```

 ### The Code Explained

 Now let's break the code down.

 ```python
 #!/usr/bin/env python
 ```
 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
 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.


 ```python
 class Move:
 	def __init__(self):
 		self.pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=1)#/stretch_diff_drive_controller/cmd_vel for gazebo
 ```
 This section of code defines the talker's interface to the rest of ROS. pub = rospy.Publisher("/stretch/cmd_vel", Twist, queue_size=1) declares that your node is publishing to the /stretch/cmd_vel topic using the message type Twist. The queue_size argument limits the amount of queued messages if any subscriber is not receiving them fast enough.


 ```Python
 command = Twist()
 ```
 Make a Twist message.  We're going to set all of the elements, since we
 can't depend on them defaulting to safe values.

 ```python
 command.linear.x = 0.1
 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.


 ```python
 command.angular.x = 0.0
 command.angular.y = 0.0
 command.angular.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.


 ```python
 self.pub.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()
 ```
 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.


 ## Move Stretch in Simulation
 <p align="center">
  <img src="images/move.gif"/>
 </p>

 Using your preferred text editor, modify the topic name of the published *Twist* messages. Please review the edit in the **move.py** script below.

 ```python
 self.pub = rospy.Publisher('/stretch_diff_drive_controller/cmd_vel', Twist, queue_size=1)
 ```

 After saving the edited node, bringup [Stretch in the empty world simulation](gazebo_basics.md). To drive the robot with the node, type the follwing in a new terminal

 ```bash
 cd catkin_ws/src/stretch_ros_tutorials/src/
 python move.py
 ```
 To stop the node from sending twist messages, type **Ctrl** + **c**.


 **Next Example:** [Example 2](example_2.md)
--- a/example_2.md
+++ b/example_2.md
@ -1,186 +0,0 @@
 ## Example 2

 The aim of this example is to provide instruction on how to filter scan messages.

 For robots with laser scanners, ROS provides a special Message type in the [sensor_msgs](http://wiki.ros.org/sensor_msgs) package called [LaserScan](http://docs.ros.org/en/api/sensor_msgs/html/msg/LaserScan.html) to hold information about a given scan. Let's take a look at the message specification itself:

 ```
 # Laser scans angles are measured counter clockwise, with Stretch's LiDAR having
 # both angle_min and angle_max facing forward (very closely along the x-axis)

 Header header
 float32 angle_min        # start angle of the scan [rad]
 float32 angle_max        # end angle of the scan [rad]
 float32 angle_increment  # angular distance between measurements [rad]
 float32 time_increment   # time between measurements [seconds]
 float32 scan_time        # time between scans [seconds]
 float32 range_min        # minimum range value [m]
 float32 range_max        # maximum range value [m]
 float32[] ranges         # range data [m] (Note: values < range_min or > range_max should be discarded)
 float32[] intensities    # intensity data [device-specific units]
 ```
 The above message tells you everything you need to know about a scan. Most importantly, you have the angle of each hit and its distance (range) from the scanner. If you want to work with raw range data, then the above message is all you need. There is also an image below that illustrates the components of the message type.

 <p align="center">
  <img src="images/lidar.png"/>
 </p>

 For a Stretch robot the start angle of the scan, `angle_min`, and
 end angle, `angle_max`, are closely located along the x-axis of Stretch's frame. `angle_min` and `angle_max` are set at **-3.1416** and **3.1416**, respectively. This is illustrated by the images below.

 <p align="center">
  <img height=500 src="images/stretch_axes.png"/>
  <img height=500 src="images/scan_angles.png"/>

 </p>


 Knowing the orientation of the LiDAR allows us to filter the scan values for a desired range. In this case, we are only considering the scan ranges in front of the stretch robot.

 First, open a terminal and run the stretch driver launch file.

 ```bash
 # Terminal 1
 roslaunch stretch_core stretch_driver.launch
 ```

 Then in a new terminal run the rplidar launch file from `stretch_core`.
 ```bash
 # Terminal 2
 roslaunch stretch_core rplidar.launch
 ```

 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
 # Terminal 3
 cd catkin_ws/src/stretch_ros_tutorials/src/
 python scan_filter.py
 ```

 Then run the following command to bring up a simple RViz configuration of the Stretch robot.
 ```bash
 # Terminal 4
 rosrun rviz rviz -d `rospack find stretch_core`/rviz/stretch_simple_test.rviz
 ```
 Change the topic name from the LaserScan display from */scan* to */filter_scan*.

 <p align="center">
  <img height=600 src="images/scanfilter.gif"/>
 </p>

 ### The Code

 ```python
 #!/usr/bin/env python

 import rospy
 from numpy import linspace, inf
 from math import sin
 from sensor_msgs.msg import LaserScan

 class ScanFilter:
  """
  A class that implements a LaserScan filter that removes all of the points
  that are not infront of the robot.
  """
 	def __init__(self):
 		self.width = 1.0
 		self.extent = self.width / 2.0
 		self.sub = rospy.Subscriber('/scan', LaserScan, self.callback)
 		self.pub = rospy.Publisher('filtered_scan', LaserScan, queue_size=10)
    rospy.loginfo("Publishing the filtered_scan topic. Use RViz to visualize.")

 	def callback(self,msg):
    """
 		Callback function to deal with incoming laserscan messages.
 		:param self: The self reference.
 		:param msg: The subscribed laserscan message.

 		:publishes msg: updated laserscan message.
 		"""
 		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)

 if __name__ == '__main__':
 	rospy.init_node('scan_filter')
 	ScanFilter()
 	rospy.spin()
 ```

 ### The Code Explained

 Now let's break the code down.

 ```python
 #!/usr/bin/env python
 ```
 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
 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.

 ```python
 self.width = 1
 self.extent = self.width / 2.0
 ```
 We're going to assume that the robot is pointing up the x-axis, so that any points with y coordinates further than half of the defined width (1 meter) from the axis are not considered.

 ```python
 self.sub = rospy.Subscriber('/scan', LaserScan, self.callback)
 ```
 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)
 ```
 `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.

 ```python
 angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
 ```
 This line of code utilizes linspace to compute each angle of the subscribed scan.

 ```python
 points = [r * sin(theta) if (theta < -2.5 or theta > 2.5) else inf for r,theta in zip(msg.ranges, angles)]
 ```
 Here we are computing the y coordinates of the ranges that are **below -2.5** and **above 2.5** radians of the scan angles. These limits are sufficient for considering scan ranges in front of Stretch, but these values can be altered to your preference.

 ```python
 new_ranges = [r if abs(y) < self.extent else inf for r,y in zip(msg.ranges, points)]
 ```
 If the absolute value of a point's y-coordinate is under *self.extent* then keep the range, otherwise use inf, which means "no return".


 ```python
 msg.ranges = new_ranges
 self.pub.publish(msg)
 ```
 Substitute in the new ranges in the original message, and republish it.

 ```python
 rospy.init_node('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 "/".

 Instantiate the class with `ScanFilter()`

 ```python
 rospy.spin()
 ```
 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.

 **Previous Example:** [Example 1](example_1.md)
 **Next Example:** [Example 3](example_3.md)
--- a/example_3.md
+++ b/example_3.md
@ -1,177 +0,0 @@
 ## Example 3

 The aim of example 3 is to combine the two previous examples and have Stretch utilize its laser scan data to avoid collision with objects as it drives forward.

 Begin by running the following commands in a new terminal.

 ```bash
 # Terminal 1
 roslaunch stretch_core stretch_driver.launch
 ```
 Then in a new terminal type the following to activate the LiDAR sensor.
 ```bash
 # Terminal 2
 roslaunch stretch_core rplidar.launch
 ```

 To set *navigation* mode and to activate the avoider node, type the following in a new terminal.

 ```bash
 # Terminal 3
 rosservice call /switch_to_navigation_mode
 cd catkin_ws/src/stretch_ros_tutorials/src/
 python avoider.py
 ```
 To stop the node from sending twist messages, type **Ctrl** + **c** in the terminal running the avoider node.

 <p align="center">
  <img height=600 src="images/avoider.gif"/>
 </p>

 ### The Code

 ```python
 #!/usr/bin/env python

 import rospy
 from numpy import linspace, inf, tanh
 from math import sin
 from geometry_msgs.msg import Twist
 from sensor_msgs.msg import LaserScan

 class Avoider:
    """
    A class that implements both a LaserScan filter and base velocity control
    for collision avoidance.
    """
    def __init__(self):
        """
        Function that initializes the subscriber, publisher, and marker features.
        :param self: The self reference.
        """
        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)

        self.width = 1
        self.extent = self.width / 2.0
        self.distance = 0.5

        self.twist = Twist()
        self.twist.linear.x = 0.0
        self.twist.linear.y = 0.0
        self.twist.linear.z = 0.0
        self.twist.angular.x = 0.0
        self.twist.angular.y = 0.0
        self.twist.angular.z = 0.0

    def set_speed(self,msg):
        """
        Callback function to deal with incoming laserscan messages.
        :param self: The self reference.
        :param msg: The subscribed laserscan message.

        :publishes self.twist: Twist message.
        """
        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)		

 if __name__ == '__main__':
    rospy.init_node('avoider')
    Avoider()
    rospy.spin()
 ```

 ### The Code Explained

 Now let's break the code down.

 ```python
 #!/usr/bin/env python
 ```
 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
 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.


 ```python
 self.pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=1)#/stretch_diff_drive_controller/cmd_vel for gazebo
 ```
 This section of code defines the talker's interface to the rest of ROS. pub = rospy.Publisher("/stretch/cmd_vel", Twist, queue_size=1) declares that your node is publishing to the /stretch/cmd_vel topic using the message type Twist.


 ```python
 self.sub = rospy.Subscriber('/scan', LaserScan, self.set_speed)
 ```
 Set up a subscriber.  We're going to subscribe to the topic "scan", looking for LaserScan messages.  When a message comes in, ROS is going to pass it to the function "set_speed" automatically.


 ```python
 self.width = 1
 self.extent = self.width / 2.0
 self.distance = 0.5
 ```
 *self.width* is the width of the laser scan we want in front of Stretch. Since Stretch's front is pointing in the x-axis, any points with y coordinates further than half of the defined width (*self.extent*) from the x-axis are not considered. *self.distance* defines the stopping distance from an object.


 ```python
 self.twist = Twist()
 self.twist.linear.x = 0.0
 self.twist.linear.y = 0.0
 self.twist.linear.z = 0.0
 self.twist.angular.x = 0.0
 self.twist.angular.y = 0.0
 self.twist.angular.z = 0.0
 ```
 Allocate a Twist to use, and set everything to zero.  We're going to do this when the class is initiating. Redefining this within the callback function, `set_speed()` can be more computationally taxing.

 ```python
 angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
 points = [r * sin(theta) if (theta < -2.5 or theta > 2.5) else inf for r,theta in zip(msg.ranges, angles)]
 new_ranges = [r if abs(y) < self.extent else inf for r,y in zip(msg.ranges, points)]
 ```
 This line of code utilizes linspace to compute each angle of the subscribed scan. Here we  compute the y coordinates of the ranges that are below -2.5 and above 2.5 radians of the scan angles. These limits are sufficient for considering scan ranges in front of Stretch, but these values can be altered to your preference. If the absolute value of a point's y-coordinate is under self.extent then keep the range, otherwise use inf, which means "no return".


 ```python
 error = min(new_ranges) - self.distance
 ```
 Calculate the difference of the closest measured scan and where we want the robot to stop. We define this as *error*.


 ```python
 self.twist.linear.x = tanh(error) if (error > 0.05 or error < -0.05) else 0
 ```
 Set the speed according to a tanh function. This method gives a nice smooth mapping from distance to speed, and asymptotes at +/- 1


 ```python
 self.pub.publish(self.twist)
 ```
 Publish the Twist message.

 ```python
 rospy.init_node('avoider')
 Avoider()
 rospy.spin()
 ```
 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 "/".

 Instantiate class with `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.

 **Previous Example:** [Example 2](example_2.md)
 **Next Example:** [Example 4](example_4.md)
--- a/example_4.md
+++ b/example_4.md
@ -1,175 +0,0 @@
 ## Example 4

 ![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.

 ```bash
 # Terminal 1
 roslaunch stretch_gazebo gazebo.launch world:=worlds/willowgarage.world rviz:=true
 ```
 the `rviz` flag will open an RViz window  to visualize a variety of ROS topics. In a new terminal run the following commands to create a marker.

 ```bash
 # Terminal 2
 cd catkin_ws/src/stretch_ros_tutorials/src/
 python marker.py
 ```
 The gif below demonstrates how to add a new *Marker* display type, and change the topic name from `visualization_marker` to `balloon`. A red sphere Marker should appear above the Stretch robot.

 ![image](images/balloon.gif)


 ### The Code
 ```python
 #!/usr/bin/env python

 import rospy
 from visualization_msgs.msg import Marker

 class Balloon():
 	"""
 	A class that attaches a Sphere marker directly above the Stretch robot.
 	"""
 	def __init__(self):
 		"""
 		Function that initializes the markers features.
 		:param self: The self reference.
 		"""
 		self.publisher = rospy.Publisher('balloon', Marker, queue_size=10)
 		self.marker = Marker()
 		self.marker.header.frame_id = '/base_link'
 		self.marker.header.stamp = rospy.Time()
 		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
 		rospy.loginfo("Publishing the balloon topic. Use RViz to visualize.")

 	def publish_marker(self):
 		"""
 		Function that publishes the sphere marker.
 		:param self: The self reference.

 		:publishes self.marker: Marker message.
 		"""
 		self.publisher.publish(self.marker)


 if __name__ == '__main__':
 	rospy.init_node('marker', argv=sys.argv)
 	balloon = Balloon()
 	rate = rospy.Rate(10)

 	while not rospy.is_shutdown():
 		balloon.publish_marker()
 		rate.sleep()		
 ```


 ### The Code Explained
 Now let's break the code down.

 ```python
 #!/usr/bin/env python
 ```
 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
 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.

 ```python
 self.pub = rospy.Publisher('balloon', Marker, queue_size=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*.


 ```python
 self.marker = Marker()
 self.marker.header.frame_id = '/base_link'
 self.marker.header.stamp = rospy.Time()
 self.marker.type = self.marker.SPHERE
 ```

 Create a maker. Markers of all shapes share a common type. Set the frame ID and type. The frame ID is the frame in which the position of the marker is specified. The type is the shape of the marker. Further details on marker shapes can be found here: [RViz Markers](http://wiki.ros.org/rviz/DisplayTypes/Marker)

 ```python
 self.marker.id = 0
 ```
 Each marker has a unique ID number. If you have more than one marker that you want displayed at a given time, then each needs to have a unique ID number. If you publish a new marker with the same ID number of an existing marker, it will replace the existing marker with that ID number.

 ```python
 self.marker.action = self.marker.ADD
 ```
 This line of code sets the action. You can add, delete, or modify markers.

 ```python
 self.marker.scale.x = 0.5
 self.marker.scale.y = 0.5
 self.marker.scale.z = 0.5
 ```
 These are the size parameters for the marker. These will vary by marker type.

 ```python
 self.marker.color.r = 1.0
 self.marker.color.g = 0.0
 self.marker.color.b = 0.0
 ```
 Color of the object, specified as r/g/b/a, with values in the range of [0, 1].

 ```python
 self.marker.color.a = 1.0
 ```
 The alpha value is from 0 (invisible) to 1 (opaque). If you don't set this then it will automatically default to zero, making your marker invisible.

 ```python
 self.marker.pose.position.x = 0.0
 self.marker.pose.position.y = 0.0
 self.marker.pose.position.z = 2.0
 ```

 Specify the pose of the marker. Since spheres are rotationally invariant, we're only going to specify the positional elements. As usual, these are in the coordinate frame named in frame_id. In this case, the position will always be directly 2 meters above the frame_id (*base_link*), and will move with it.


 ```python
 def publish_marker(self):
 		self.publisher.publish(self.marker)
 ```
 Publish the Marker data structure to be visualized in RViz.

 ```python
 rospy.init_node('marker', argv=sys.argv)
 balloon = Balloon()
 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 "/".

 Instantiate class with `Balloon()`

 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.


 ```python
 while not rospy.is_shutdown():
 	balloon.publish_marker()
 	rate.sleep()
 ```

 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.

 **Previous Example:** [Example 3](example_3.md)
 **Next Example:** [Example 5](example_5.md)
--- a/example_5.md
+++ b/example_5.md
@ -1,157 +0,0 @@
 ## Example 5

 In this example, we will review a Python script that prints out the positions of a selected group of Stretch's joints. This script is helpful if you need the joint positions after you teleoperated Stretch with the Xbox controller or physically moved the robot to the desired configuration after hitting the run stop button.

 If you are looking for a continuous print of the joint states while Stretch is in action, then you can use the [rostopic command-line tool](http://wiki.ros.org/rostopic) shown in the [Internal State of Stretch Tutorial](internal_state_of_stretch.md).

 Begin by starting up the stretch driver launch file.

 ```bash
 # Terminal 1
 roslaunch stretch_core stretch_driver.launch
 ```

 You can then hit the run-stop button (you should hear a beep and the LED light in the button blink) and move the robot's joints to a desired configuration. Once you are satisfied with the configuration, hold the run-stop button until you hear a beep. Then run the following command to print the joint positions of the lift, arm, and wrist.

 ```bash
 cd catkin_ws/src/stretch_ros_tutorials/src/
 python joint_state_printer.py
 ```
 Your terminal will output the `position` information of the previously mentioned joints shown below.
 ```
 name: ['joint_lift', 'joint_arm_l0', 'joint_arm_l1', 'joint_arm_l2', 'joint_arm_l3', 'joint_wrist_yaw']
 position: [1.004518435897309, 0.12361581673760723, 0.023224914142933994, 0.07758496706423101, 0.12309362763409384, 1.8771004095879587]

 ```

 It's important to note that the arm has 4 prismatic joints and the sum of these positions gives the wrist extension distance. The wrist extension is needed when sending [joint trajectory commands](follow_joint_trajectory.md) to the robot. Here is an image of the arm joints for reference:

 ![image](images/joints.png)


 ### The Code
 ```python
 #!/usr/bin/env python

 import rospy
 import sys
 from sensor_msgs.msg import JointState

 class JointStatePublisher():
 	"""
 	A class that prints the positions of desired joints in Stretch.
 	"""
 	def __init__(self):
 		"""
 		Function that initializes the subsriber.
 		:param self: The self reference.
 		"""
 		self.sub = rospy.Subscriber('joint_states', JointState, self.callback)

 	def callback(self, msg):
 		"""
 		Callback function to deal with the incoming JointState messages.
 		:param self: The self reference.
 		:param msg: The JointState message.
 		"""
 		self.joint_states = msg

 	def print_states(self, joints):
 		"""
 		print_states function to deal with the incoming JointState messages.
 		:param self: The self reference.
 		:param joints: A list of joint names.
 		"""
 		joint_positions = []
 		for joint in joints:
 			index = self.joint_states.name.index(joint)
 			joint_positions.append(self.joint_states.position[index])
 		print("name: " + str(joints))
 		print("position: " + str(joint_positions))
 		rospy.signal_shutdown("done")
 		sys.exit(0)

 if __name__ == '__main__':
 	rospy.init_node('joint_state_printer', anonymous=True)
 	JSP = JointStatePublisher()
 	rospy.sleep(.1)
 	joints = ["joint_lift", "joint_arm_l0", "joint_arm_l1", "joint_arm_l2", "joint_arm_13", "joint_wrist_yaw"]
 	#joints = ["joint_head_pan","joint_head_tilt", joint_gripper_finger_left", "joint_gripper_finger_right"]
 	JSP.print_states(joints)
 	rospy.spin()
 ```


 ### The Code Explained
 Now let's break the code down.

 ```python
 #!/usr/bin/env python
 ```
 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 sys
 from sensor_msgs.msg import JointState
 ```
 You need to import rospy if you are writing a ROS Node. Import sensor_msgs.msg so that we can subscribe to JointState messages.

 ```python
 self.sub = rospy.Subscriber('joint_states', JointState, self.callback)
 ```
 Set up a subscriber.  We're going to subscribe to the topic "joint_states", looking for JointState messages.  When a message comes in, ROS is going to pass it to the function "callback" automatically

 ```python
 def callback(self, msg):
 	self.joint_states = msg
 ```
 This is the callback function where he JointState messages are stored as *self.joint_states*. Further information about the this message type can be found here: [JointState Message](http://docs.ros.org/en/lunar/api/sensor_msgs/html/msg/JointState.html)

 ```python
 def print_states(self, joints):
 	joint_positions = []

 ```
 This is the *print_states()* function which takes in a list of joints of interest as its argument. the is also an empty list set as *joint_positions* and this is where the positions of the requested joints will be appended.

 ```python
 for joint in joints:
 	index = self.joint_states.name.index(joint)
 	joint_positions.append(self.joint_states.position[index])
 print(joint_positions)
 ```
 In this section of the code, a forloop is used to parse the names of the requested joints from the *self.joint_states* list. The index() function returns the index a of the name of the requested joint and appends the respective position to our *joint_positions* list.

 ```python
 rospy.signal_shutdown("done")
 sys.exit(0)
 ```
 The first line of code initias a clean shutodwn of ROS. The second line of code exits the Python interpreter.

 ```python
 rospy.init_node('joint_state_printer', anonymous=True)
 JSP = JointStatePublisher()
 rospy.sleep(.1)
 ```
 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 "/".

 Declare object, *JSP*, from the `JointStatePublisher` class.

 The use of the `rospy.sleep()` function is to allow the *JSP* class to initialize all of its features before requesting to publish joint positions of desired joints (running the `print_states()` function).

 ```python
 joints = ["joint_lift", "joint_arm_l0", "joint_arm_l1", "joint_arm_l2", "joint_arm_13", "joint_wrist_yaw"]
 #joints = ["joint_head_pan","joint_head_tilt", joint_gripper_finger_left", "joint_gripper_finger_right"]
 JSP.print_states(joints)
 ```
 Create a list of the desired joints that you want positions to be printed. Then use that list as an argument for the `print_states()` function.

 ```python
 rospy.spin()
 ```
 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.

 **Previous Example** [Example 4](example_4.md)
 **Next Example** [Example 6](example_6.md)
--- a/example_6.md
+++ b/example_6.md
@ -1,307 +0,0 @@
 ## Example 6

 In this example, we will review a Python script that prints out and stores the effort values from a specified joint. If you are looking for a continuous print of the joint state efforts while Stretch is in action, then you can use the [rostopic command-line tool](http://wiki.ros.org/rostopic) shown in the [Internal State of Stretch Tutorial](internal_state_of_stretch.md).


 ![image](images/effort_sensing.gif)


 Begin by running the following command in the terminal in a terminal.

 ```bash
 # Terminal 1
 roslaunch stretch_core stretch_driver.launch
 ```

 Switch the mode to *manipulation* mode using a rosservice call. Then run the single effort sensing node.

 ```bash
 # Terminal 2
 rosservice call /switch_to_manipulation_mode
 cd catkin_ws/src/stretch_ros_tutorials/src/
 python effort_sensing.py
 ```

 This will send a `FollowJointTrajectory` command to move Stretch's arm or head while also printing the effort of the lift.

 ### The Code
 ```python
 #!/usr/bin/env python
 import rospy
 import time
 import actionlib
 import os
 import csv
 from datetime import datetime

 from control_msgs.msg import FollowJointTrajectoryGoal
 from trajectory_msgs.msg import JointTrajectoryPoint
 from sensor_msgs.msg import JointState
 import hello_helpers.hello_misc as hm

 class JointActuatorEffortSensor(hm.HelloNode):
    """
    A class that sends multiple joint trajectory goals to a single joint.
    """
    def __init__(self):
        """
        Function that initializes the subscriber,and other features.
        :param self: The self reference.
        """
        hm.HelloNode.__init__(self)
        self.sub = rospy.Subscriber('joint_states', JointState, self.callback)
        self.joints = ['joint_lift']
        self.joint_effort = []
        self.save_path = '/home/hello-robot/catkin_ws/src/stretch_ros_tutorials/stored_data'
        self.export_data = False

    def callback(self, msg):
        """
        Callback function to update and store JointState messages.
        :param self: The self reference.
        :param msg: The JointState message.
        """
        self.joint_states = msg

    def issue_command(self):
        """
        Function that makes an action call and sends joint trajectory goals
        to a single joint.
        :param self: The self reference.
        """
        trajectory_goal = FollowJointTrajectoryGoal()
        trajectory_goal.trajectory.joint_names = self.joints

        point0 = JointTrajectoryPoint()
        point0.positions = [0.9]

        trajectory_goal.trajectory.points = [point0]
        trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
        trajectory_goal.trajectory.header.frame_id = 'base_link'
        self.trajectory_client.send_goal(trajectory_goal, feedback_cb=self.feedback_callback, done_cb=self.done_callback)
        rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal))
        self.trajectory_client.wait_for_result()

    def feedback_callback(self,feedback):
        """
        The feedback_callback function deals with the incoming feedback messages
        from the trajectory_client. Although, in this function, we do not use the
        feedback information.
        :param self: The self reference.
        :param feedback: FollowJointTrajectoryActionFeedback message.
        """
        if 'wrist_extension' in self.joints:
            self.joints.remove('wrist_extension')
            self.joints.append('joint_arm_l0')

        current_effort = []
        for joint in self.joints:
            index = self.joint_states.name.index(joint)
            current_effort.append(self.joint_states.effort[index])

        if not self.export_data:
            print("name: " + str(self.joints))
            print("effort: " + str(current_effort))
        else:
            self.joint_effort.append(current_effort)


    def done_callback(self, status, result):
        """
        The done_callback function will be called when the joint action is complete.
        Within this function we export the data to a .txt file in  the /stored_data directory.
        :param self: The self reference.
        :param status: status attribute from FollowJointTrajectoryActionResult message.
        :param result: result attribute from FollowJointTrajectoryActionResult message.
        """
        if status == actionlib.GoalStatus.SUCCEEDED:
            rospy.loginfo('Suceeded')
        else:
            rospy.loginfo('Failed')

        if self.export_data:
            file_name = datetime.now().strftime("%Y-%m-%d_%I:%M:%S-%p")
            completeName = os.path.join(self.save_path, file_name)
            with open(completeName, "w") as f:
                writer = csv.writer(f)
                writer.writerow(self.joints)
                writer.writerows(self.joint_effort)

    def main(self):
        """
        Function that initiates the issue_command function.
        :param self: The self reference.
        """
        hm.HelloNode.main(self, 'issue_command', 'issue_command', wait_for_first_pointcloud=False)
        rospy.loginfo('issuing command...')
        self.issue_command()
        time.sleep(2)

 if __name__ == '__main__':
 	try:
 		node = JointActuatorEffortSensor()
 		node.main()
 	except KeyboardInterrupt:
 		rospy.loginfo('interrupt received, so shutting down')

 ```


 ### The Code Explained
 This code is similar to that of the [multipoint_command](https://github.com/hello-sanchez/stretch_ros_tutorials/blob/main/src/multipoint_command.py) and [joint_state_printer](https://github.com/hello-sanchez/stretch_ros_tutorials/blob/main/src/joint_state_printer.py) node. Therefore, this example will highlight sections that are different from that tutorial. Now let's break the code down.

 ```python
 #!/usr/bin/env python
 ```
 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 time
 import actionlib
 import os
 import csv
 from datetime import datetime

 from control_msgs.msg import FollowJointTrajectoryGoal
 from trajectory_msgs.msg import JointTrajectoryPoint
 from sensor_msgs.msg import JointState
 import hello_helpers.hello_misc as hm
 ```

 You need to import rospy if you are writing a ROS Node. Import the `FollowJointTrajectoryGoal` from the `control_msgs.msg` package to control the Stretch robot. Import `JointTrajectoryPoint` from the `trajectory_msgs` package to define robot trajectories. The `hello_helpers` package consists of a module that provides various Python scripts used across stretch_ros. In this instance, we are importing the `hello_misc` script.


 ```Python
 class JointActuatorEffortSensor(hm.HelloNode):
    """
    A class that sends multiple joint trajectory goals to a single joint.
    """
    def __init__(self):
        """
        Function that initializes the subscriber,and other features.
        :param self: The self reference.
        """
        hm.HelloNode.__init__(self)
 ```
 The `JointActuatorEffortSensor ` class inherits the `HelloNode` class from `hm` and is initialized.

 ```python
 self.sub = rospy.Subscriber('joint_states', JointState, self.callback)
 self.joints = ['joint_lift']

 ```
 Set up a subscriber.  We're going to subscribe to the topic "joint_states", looking for `JointState` messages.  When a message comes in, ROS is going to pass it to the function "callback" automatically. Create a list of the desired joints you want to print.
 ```Python
 self.joint_effort = []
 self.save_path = '/home/hello-robot/catkin_ws/src/stretch_ros_tutorials/stored_data'
 self.export_data = False
 ```

 Create an empty list to store the joint effort values. The *self.save_path* is the directory path where the .txt file of the effort values will be stored. You can change this path to a preferred directory. The *self.export_data* is a boolean and its default value is set to False. If set to True, then the joint values will be stored in a .txt file, otherwise, the values will be printed in the terminal where you ran the effort sensing node.
 ```python
 self.trajectory_client.send_goal(trajectory_goal, feedback_cb=self.feedback_callback, done_cb=self.done_callback)
 ```

 Include the feedback and done call back functions in the send goal function.

 ```python
 def feedback_callback(self,feedback):
    """
    The feedback_callback function deals with the incoming feedback messages
    from the trajectory_client. Although, in this function, we do not use the
    feedback information.
    :param self: The self reference.
    :param feedback: FollowJointTrajectoryActionFeedback message.
    """
 ```

 The feedback callback function takes in the `FollowJointTrajectoryActionFeedback` message as its argument.

 ```python
 if 'wrist_extension' in self.joints:
    self.joints.remove('wrist_extension')
    self.joints.append('joint_arm_l0')
 ```

 Use a conditional statement to replace `wrist_extenstion` to `joint_arm_l0`. This is because `joint_arm_l0` has the effort values that the `wrist_extension` is experiencing.

 ```python
 current_effort = []
 for joint in self.joints:
    index = self.joint_states.name.index(joint)
    current_effort.append(self.joint_states.effort[index])
 ```

 Create an empty list to store the current effort values. Then use a for loop to parse the joint names and effort values.

 ```python
 if not self.export_data:
    print("name: " + str(self.joints))
    print("effort: " + str(current_effort))
 else:
    self.joint_effort.append(current_effort)
 ```
 Use a conditional statement to print effort values in the terminal or store values into a list that will be used for exporting the data in a .txt file.

 ```Python
 def done_callback(self, status, result):
      """
      The done_callback function will be called when the joint action is complete.
      Within this function we export the data to a .txt file in  the /stored_data directory.
      :param self: The self reference.
      :param status: status attribute from FollowJointTrajectoryActionResult message.
      :param result: result attribute from FollowJointTrajectoryActionResult message.
      """
 ```

 The done callback function takes in the FollowJointTrajectoryActionResult messages as its arguments.

 ```python
 if status == actionlib.GoalStatus.SUCCEEDED:
    rospy.loginfo('Suceeded')
 else:
    rospy.loginfo('Failed')

 ```

 Conditional statement to print whether the goal status in the `FollowJointTrajectoryActionResult` succeeded or failed.

 ```python
 if self.export_data:
    file_name = datetime.now().strftime("%Y-%m-%d_%I:%M:%S-%p")
    completeName = os.path.join(self.save_path, file_name)

    with open(completeName, "w") as f:
        writer = csv.writer(f)
        writer.writerow(self.joints)
        writer.writerows(self.joint_effort)

 ```

 A conditional statement is used to export the data to a .txt file. The file's name is set to the date and time the node was executed. That way, no previous files are overwritten.


 ### Plotting/Animating Effort Data

 ![image](stored_data/2022-06-30_11:26:20-AM.png)

 We added a simple python script, [stored_data_plotter.py](https://github.com/hello-sanchez/stretch_ros_tutorials/blob/main/src/stored_data_plotter.py), to this package for plotting the stored data. Note you have to change the name of the file you wish to see in the python script. This is shown below:

 ```Python
 ####################### Copy the file name here! #######################
 file_name = '2022-06-30_11:26:20-AM'
 ```

 Once you have changed the file name, then run the following in a new command.

 ```bash
 cd catkin_ws/src/stretch_ros_tutorials/src/
 python stored_data_plotter.py

 ```

 Because this is not a node, you don't need roscore to run this script. Please review the comments in the python script for additional guidance.

 **Previous Example** [Example 5](example_4.md)
--- a/docs/extra.css
+++ b/docs/extra.css
--- a/follow_joint_trajectory.md
+++ b/follow_joint_trajectory.md
@ -1,415 +0,0 @@
 ## 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.

 ## Stow Command Example
 <p align="center">
  <img src="images/stow_command.gif"/>
 </p>



 Begin by running the following command in the terminal in a terminal.

 ```bash
 # Terminal 1
 roslaunch stretch_core stretch_driver.launch
 ```

 Switch the mode to *manipulation* mode using a rosservice call. Then run the stow command node.

 ```bash
 # Terminal 2
 rosservice call /switch_to_manipulation_mode
 cd catkin_ws/src/stretch_ros_tutorials/src/
 python stow_command.py
 ```


 This will send a `FollowJointTrajectory` command to stow Stretch's arm.
 ### The Code

 ```python
 #!/usr/bin/env python

 import rospy
 from control_msgs.msg import FollowJointTrajectoryGoal
 from trajectory_msgs.msg import JointTrajectoryPoint
 import hello_helpers.hello_misc as hm
 import time

 class StowCommand(hm.HelloNode):
  '''
  A class that sends a joint trajectory goal to stow the Stretch's arm.
  '''
  def __init__(self):
    hm.HelloNode.__init__(self)

  def issue_stow_command(self):
    '''
    Function that makes an action call and sends stow postion goal.
    :param self: The self reference.
    '''
    stow_point = JointTrajectoryPoint()
    stow_point.time_from_start = rospy.Duration(0.000)
    stow_point.positions = [0.2, 0.0, 3.4]

    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'

    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):
    '''
    Function that initiates stow_command function.
    :param self: The self reference.
    '''
    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:
    node = StowCommand()
    node.main()
  except KeyboardInterrupt:
    rospy.loginfo('interrupt received, so shutting down')
 ```

 ### The Code Explained

 Now let's break the code down.

 ```python
 #!/usr/bin/env python
 ```
 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
 from control_msgs.msg import FollowJointTrajectoryGoal
 from trajectory_msgs.msg import JointTrajectoryPoint
 import hello_helpers.hello_misc as hm
 import time
 ```
 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.

 ```python
 class StowCommand(hm.HelloNode):

    def __init__(self):
        hm.HelloNode.__init__(self)
 ```
 The `StowCommand ` class inherits the `HelloNode` class from `hm` 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.

 ```python
    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'
 ```
 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.

 ```python
 self.trajectory_client.send_goal(trajectory_goal)
 rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal))
 self.trajectory_client.wait_for_result()
 ```
 Make the action call and send the goal. The last line of code waits for the result before it exits the python script.

 ```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)
 ```
 Create a funcion, `main()`, to do all of the setup the `hm.HelloNode` class and issue the stow command.

 ```python
 if __name__ == '__main__':
    try:
        node = StowCommand()
        node.main()
    except KeyboardInterrupt:
        rospy.loginfo('interrupt received, so shutting down')

 ```
 Declare object, *node*, from the `StowCommand()` class. Then run the `main()` function.


 ## Multipoint Command Example

 <p align="center">
  <img src="images/multipoint.gif"/>
 </p>

 Begin by running the following command in the terminal in a terminal.

 ```bash
 # Terminal 1
 roslaunch stretch_core stretch_driver.launch
 ```

 Switch the mode to *manipulation* mode using a rosservice call. Then run the multipoint command node.

 ```bash
 # Terminal 2
 rosservice call /switch_to_manipulation_mode
 cd catkin_ws/src/stretch_ros_tutorials/src/
 python multipoint_command.py
 ```

 This will send a list of `JointTrajectoryPoint` message types to move Stretch's arm.

 ### The Code
 ```python
 #!/usr/bin/env python

 import rospy
 import time
 from control_msgs.msg import FollowJointTrajectoryGoal
 from trajectory_msgs.msg import JointTrajectoryPoint
 import hello_helpers.hello_misc as hm

 class MultiPointCommand(hm.HelloNode):
  """
  A class that sends multiple joint trajectory goals to the stretch robot.
  """
  def __init__(self):
    hm.HelloNode.__init__(self)

  def issue_multipoint_command(self):
    """
    Function that makes an action call and sends multiple joint trajectory goals
    to the joint_lift, wrist_extension, and joint_wrist_yaw.
    :param self: The self reference.
    """
    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]

    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]

    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(trajectory_goal)
    rospy.loginfo('Sent list of goals = {0}'.format(trajectory_goal))
    self.trajectory_client.wait_for_result()

  def main(self):
    """
    Function that initiates the multipoint_command function.
    :param self: The self reference.
    """
    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:
    node = MultiPointCommand()
    node.main()
  except KeyboardInterrupt:
    rospy.loginfo('interrupt received, so shutting down')

 ```

 ### The Code Explained
 Seeing that there are similarities between the multipoint and stow command nodes, we will only breakdown the different components of the multipoint_command node.

 ```python
 point0 = JointTrajectoryPoint()
 point0.positions = [0.2, 0.0, 3.4]
 ```
 Set *point0* as a `JointTrajectoryPoint`and provide desired positions. These are the positions of the lift, wrist extension, and yaw of the wrist, respectively. The lift and wrist extension positions are expressed in meters, where as the wrist yaw is in radians.


 ```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 accelerations of the lift (m/s^2), wrist extension (m/s^2), and wrist yaw (rad/s^2).

 **IMPORTANT NOTE**: The lift and wrist extension can only go up to 0.2 m/s. If you do not provide any velocities or accelerations for the lift or wrist extension, then they go to their default values. However, the Velocity and Acceleration of the wrist yaw will stay the same from the previous value unless updated.


 ```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'
 ```
 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.

 ## Single Joint Actuator


 <p align="center">
  <img src="images/single_joint_actuator.gif"/>
 </p>

 You can also actuate a single joint for the Stretch. Below are the list of joints and their position limit that can be moved in the *position* mode.

 ```
 ############################# Joint limits #############################
 joint_lift:      lower_limit =  0.15,  upper_limit =  1.10  # in meters
 wrist_extension: lower_limit =  0.00,  upper_limit =  0.50  # in meters
 joint_wrist_yaw: lower_limit = -1.75,  upper_limit =  4.00  # in radians
 joint_head_pan:  lower_limit = -2.80,  upper_limit =  2.90  # in radians
 joint_head_tilt: lower_limit = -1.60,  upper_limit =  0.40  # in radians
 joint_gripper_finger_left:  lower_limit = -0.35,  upper_limit =  0.165  # in radians
 ########################################################################
 ```

 Begin by running the following command in the terminal in a terminal.

 ```bash
 # Terminal 1
 roslaunch stretch_core stretch_driver.launch
 ```

 Switch the mode to *manipulation* mode using a rosservice call. Then run the single joint actuator node.

 ```bash
 # Terminal 2
 rosservice call /switch_to_manipulation_mode
 cd catkin_ws/src/stretch_ros_tutorials/src/
 python single_joint_actuator.py
 ```

 This will send a list of `JointTrajectoryPoint` message types to move Stretch's arm.


 The joint, *joint_gripper_finger_left*, is only needed when actuating the gripper.


 ### The Code
 ```python
 #!/usr/bin/env python

 import rospy
 import time
 from control_msgs.msg import FollowJointTrajectoryGoal
 from trajectory_msgs.msg import JointTrajectoryPoint
 import hello_helpers.hello_misc as hm

 class SingleJointActuator(hm.HelloNode):
 	"""
 	A class that sends multiple joint trajectory goals to a single joint.
 	"""
 	def __init__(self):
 		hm.HelloNode.__init__(self)

 	def issue_command(self):
 		"""
 		Function that makes an action call and sends joint trajectory goals
 		to a single joint
 		:param self: The self reference.
 		"""
 		trajectory_goal = FollowJointTrajectoryGoal()
 		trajectory_goal.trajectory.joint_names = ['joint_head_pan']

 		point0 = JointTrajectoryPoint()
 		point0.positions = [0.65]

 		# point1 = JointTrajectoryPoint()
 		# point1.positions = [0.5]

 		trajectory_goal.trajectory.points = [point0]#, point1]
 		trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
 		trajectory_goal.trajectory.header.frame_id = 'base_link'
 		self.trajectory_client.send_goal(trajectory_goal)
 		rospy.loginfo('Sent goal = {0}'.format(trajectory_goal))
 		self.trajectory_client.wait_for_result()

 	def main(self):
 		"""
 		Function that initiates the issue_command function.
 		:param self: The self reference.
 		"""
 		hm.HelloNode.main(self, 'issue_command', 'issue_command', wait_for_first_pointcloud=False)
 		rospy.loginfo('issuing command...')
 		self.issue_command()
 		time.sleep(2)


 if __name__ == '__main__':
 	try:
 		node = SingleJointActuator()
 		node.main()
 	except KeyboardInterrupt:
 		rospy.loginfo('interrupt received, so shutting down')
 ```
 ### The Code Explained

 Since the code is quite similar to the multipoint_command code, we will only review the parts that differ.

 Now let's break the code down.

 ```python
 trajectory_goal = FollowJointTrajectoryGoal()
 trajectory_goal.trajectory.joint_names = ['joint_head_pan']
 ```
 Here we only input joint name that we want to actuate. In this instance, we will actuate the *joint_head_pan*.

 ```python
 point0 = JointTrajectoryPoint()
 point0.positions = [0.65]

 # point1 = JointTrajectoryPoint()
 # point1.positions = [0.5]
 ```
 Set *point0* as a `JointTrajectoryPoint`and provide desired position. You also have the option to send multiple point positions rather than one.

 ```python
 trajectory_goal.trajectory.points = [point0]#, point1]
 trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
 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` set by your list of points. Specify the coordinate frame that we want (base_link) and set the time to be now.


 **Next Tutorial:** [Perception](perception.md)
--- a/gazebo_basics.md
+++ b/gazebo_basics.md
@ -1,28 +0,0 @@
 # Spawning Stretch in Simulation (Gazebo)

 ### Empty World Simulation
 To spawn the Stretch in gazebo's default empty world run the following command in your terminal.
 ```bash
 roslaunch stretch_gazebo gazebo.launch
 ```
 This will bringup the robot in the gazebo simulation similar to the image shown below.

 <!-- <img src="images/stretch_gazebo_empty_world.png" width="500" align="center"> -->
 ![image](images/stretch_gazebo_empty_world.png)

 ### Custom World Simulation
 In gazebo, you are able to spawn Stretch in various worlds. First, source the gazebo world files by running the following command in a terminal
 ```bash
 echo "source /usr/share/gazebo/setup.sh"
 ```


 Then using the world argument, you can spawn the stretch in the willowgarage world by running the following

 ```bash
 roslaunch stretch_gazebo gazebo.launch world:=worlds/willowgarage.world
 ```

 ![image](images/stretch_willowgarage_world.png)

 **Next Tutorial:** [Teleoperating Stretch](teleoperating_stretch.md)
--- a/getting_started.md
+++ b/getting_started.md
@ -1,57 +0,0 @@
 # Getting Started

 ## Ubuntu

 Hello Robot utilizes Ubuntu, an open-source Linux operating system, for the Stretch RE1 platform. If you are unfamiliar with the operating system, we encourage you to review a [tutorial](https://ubuntu.com/tutorials/command-line-for-beginners#1-overview) provided by Ubuntu. Additionally, the Linux command line, BASH, is used to execute commands and is needed to run ROS on the Stretch robot. Here is a [tutorial](https://ryanstutorials.net/linuxtutorial/) on getting started with BASH.

 ## Installing Noetic on Stretch
 Instructions on installing Noetic can be found in our open-source [installation guide](https://github.com/hello-robot/stretch_ros/blob/dev/noetic/install_noetic.md). 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.

 ```bash
 cd ~/catkin_ws/src
 git clone https://github.com/hello-sanchez/stretch_ros_tutorials.git
 cd ~/catkin_ws
 catkin_make
 ```

 ## 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

 ```bash
 sudo apt-get install ros-noetic-realsense2-camera
 ```

 After your system is setup, clone the [stretch_ros](https://github.com/hello-robot/stretch_ros.git), [stretch_ros_tutorials](https://github.com/hello-sanchez/stretch_ros_tutorials.git), and [realsense_gazebo_plugin packages]( https://github.com/pal-robotics/realsense_gazebo_plugin) to your **src** folder in your preferred workspace.
 ```bash
 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
 ```


 Change the directory to that of your catkin workspace and install system dependencies of the ROS packages. Then build your workspace.
 ```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
 ```bash
 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)
--- a/docs/getting_started/quick_start_guide.md
+++ b/docs/getting_started/quick_start_guide.md
--- a/docs/getting_started/robot_safety_guide.md
+++ b/docs/getting_started/robot_safety_guide.md
--- a/docs/getting_started/troubleshooting_guide.md
+++ b/docs/getting_started/troubleshooting_guide.md
--- a/docs/getting_started/untethered_operation.md
+++ b/docs/getting_started/untethered_operation.md
--- a/docs/getting_started/updating_software.md
+++ b/docs/getting_started/updating_software.md
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/internal_state_of_stretch.md
+++ b/internal_state_of_stretch.md
@ -1,61 +0,0 @@
 ## Getting the State of the Robot

 Begin by starting up the stretch driver launch file.
 ```bash
 # Terminal 1
 roslaunch stretch_core stretch_driver.launch
 ```

 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.
 ```bash
 # Terminal 2
 rostopic echo /joint_states -n1
 ```
 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.
 ```
 header:
  seq: 70999
  stamp:
    secs: 1420
    nsecs:   2000000
  frame_id: ''
 name: [joint_arm_l0, joint_arm_l1, joint_arm_l2, joint_arm_l3, joint_gripper_finger_left,
  joint_gripper_finger_right, joint_head_pan, joint_head_tilt, joint_left_wheel, joint_lift,
  joint_right_wheel, joint_wrist_yaw]
 position: [-1.6137320244357253e-08, -2.9392484829061376e-07, -2.8036125938539207e-07, -2.056847528567165e-07, -2.0518734302754638e-06, -5.98271107676851e-06, 2.9291786329821434e-07, 1.3802900147297237e-06, 0.08154086954434359, 1.4361499260374905e-07, 0.4139061738340768, 9.32603306580404e-07]
 velocity: [0.00015598730463972836, -0.00029395074514369584, -0.0002803845454217379, 1.322424459109634e-05, -0.00035084643762840415, 0.0012164337445918797, 0.0002138814988808099, 0.00010419792027496809, 4.0575263146426684e-05, 0.00022487596895736357, -0.0007751929074042957, 0.0002451588607332439]
 effort: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
 ```

 Let's say you are interested in only seeing the `header` component of the `/joint_states` topic, you can output this within the rostopic command-line tool by typing the following command.
 ```bash
 # Terminal 2
 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.

 ```bash
 # Terminal 3
 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)
--- a/moveit_basics.md
+++ b/moveit_basics.md
@ -1,57 +0,0 @@
 # MoveIt! Basics
 <!--
 ## MoveIt! on Stretch

 To run MoveIt with the actual hardware, (assuming `stretch_driver` is already running) simply run

 ```bash
 roslaunch stretch_moveit_config move_group.launch
 ```

 This will runs all of the planning capabilities, but without the setup, simulation and interface that the above demo provides. In order to create plans for the robot with the same interface as the offline demo, you can run
 ```bash
 roslaunch stretch_moveit_config moveit_rviz.launch
 ``` -->

 ## MoveIt! Without Hardware
 To begin running MoveIt! on stretch, run the demo launch file. This doesn't require any simulator or robot to run.

 ```bash
 roslaunch stretch_moveit_config demo.launch
 ```
 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:

 * Pre-defined start and goal states can be specified in Start State and Goal State drop downs in Planning tab of Motion Planning RViz plugin.

 * *stretch_gripper* group does not show markers, and is intended to be controlled via the joints tab that is located in the very right of Motion Planning Rviz plugin.

 * When planning with *stretch_head* group make sure you select *Approx IK Solutions* in Planning tab of Motion Planning RViz plugin.


 ![image](images/moveit_groups.gif)


 ## Running Gazebo with MoveIt! and Stretch


 ```bash
 # Terminal 1:
 roslaunch stretch_gazebo gazebo.launch
 # Terminal 2:
 roslaunch stretch_core teleop_twist.launch twist_topic:=/stretch_diff_drive_controller/cmd_vel linear:=1.0 angular:=2.0 teleop_type:=keyboard # or use teleop_type:=joystick if you have a controller
 # Terminal 3
 roslaunch stretch_moveit_config demo_gazebo.launch
 ```


 This will launch an Rviz instance that visualizes the joints with markers and an empty world in Gazebo with Stretch and load all the controllers. There are pre-defined positions for each joint group for demonstration purposes. There are three joint groups, namely stretch_arm, stretch_gripper and stretch_head that can be controlled individually via Motion Planning Rviz plugin. Start and goal positions for joints can be selected similar to [this moveit tutorial](https://ros-planning.github.io/moveit_tutorials/doc/quickstart_in_rviz/quickstart_in_rviz_tutorial.html#choosing-specific-start-goal-states).


 ![image](images/gazebo_moveit.gif)

 **Next Tutorial:** [Follow Joint Trajectory Commands](follow_joint_trajectory.md)
--- a/navigation_stack.md
+++ b/navigation_stack.md
@ -1,74 +0,0 @@
 ## Navigation Stack with Actual robot

 stretch_navigation provides the standard ROS navigation stack as two launch files. This package utilizes gmapping, move_base, and AMCL to drive the stretch RE1 around a mapped space. Running this code will require the robot to be untethered.


 Then run the following commands to map the space that the robot will navigate in.
 ```bash
 roslaunch stretch_navigation mapping.launch
 ```
 Rviz will show the robot and the map that is being constructed. With the terminal open, use the instructions printed by the teleop package to teleoperate the robot around the room. Avoid sharp turns and revisit previously visited spots to form loop closures.

 <p align="center">
  <img height=600 src="images/mapping.gif"/>
 </p>

 In Rviz, once you see a map that has reconstructed the space well enough, you can run the following commands to save the map to `stretch_user/`.

 ```bash
 mkdir -p ~/stretch_user/maps
 rosrun map_server map_saver -f ${HELLO_FLEET_PATH}/maps/<map_name>
 ```

 The `<map_name>` does not include an extension. Map_saver will save two files as `<map_name>.pgm` and `<map_name>.yaml`.

 Next, with `<map_name>.yaml`, we can navigate the robot around the mapped space. Run:

 ```bash
 roslaunch stretch_navigation navigation.launch map_yaml:=${HELLO_FLEET_PATH}/maps/<map_name>.yaml
 ```

 Rviz will show the robot in the previously mapped space; however, the robot's location on the map does not match the robot's location in the real space. In the top bar of Rviz, use 2D Pose Estimate to lay an arrow down roughly where the robot is located in the real space. AMCL, the localization package, will better localize our pose once we give the robot a 2D Nav Goal. In the top bar of Rviz, use 2D Nav Goal to lay down an arrow where you'd like the robot to go. In the terminal, you'll see move_base go through the planning phases and then navigate the robot to the goal. If planning fails, the robot will begin a recovery behavior: spinning around 360 degrees in place.

 It is also possible to send 2D Pose Estimates and Nav Goals programmatically. In your launch file, you may include `navigation.launch` to bring up the navigation stack. Then, you can send *move_base_msgs::MoveBaseGoal* messages in order to navigate the robot programmatically.



 ## Running in Simulation

 To perform mapping and navigation in the Gazebo simulation of Stretch, substitute the `mapping_gazebo.launch` and `navigation_gazebo.launch` launch files into the commands above. The default Gazebo environment is the Willow Garage HQ. Use the "world" ROS argument to specify the Gazebo world within which to spawn Stretch.

 ```bash
 # Terminal 1
 roslaunch stretch_navigation mapping_gazebo.launch gazebo_world:=worlds/willowgarage.world
 ```

 ### Teleop using a Joystick Controller

 The mapping launch files, `mapping.launch` and `mapping_gazebo.launch` expose the ROS argument, "teleop_type". By default, this ROS arg is set to "keyboard", which launches keyboard teleop in the terminal. If the xbox controller that ships with Stretch RE1 is plugged into your computer, the following command will launch mapping with joystick teleop:

 ```bash
 # Terminal 2
 roslaunch stretch_navigation mapping.launch teleop_type:=joystick
 ```

 <p align="center">
  <img height=600 src="images/gazebo_mapping.gif"/>
 </p>

 ### Using ROS Remote Master

 If you have set up [ROS Remote Master](https://docs.hello-robot.com/untethered_operation/#ros-remote-master) for [untethered operation](https://docs.hello-robot.com/untethered_operation/), you can use Rviz and teleop locally with the following commands:

 ```bash
 # On Robot
 roslaunch stretch_navigation mapping.launch rviz:=false teleop_type:=none

 # On your machine, Terminal 1:
 rviz -d `rospack find stretch_navigation`/rviz/mapping.launch
 # On your machine, Terminal 2:
 roslaunch stretch_core teleop_twist.launch teleop_type:=keyboard # or use teleop_type:=joystick if you have a controller
 ```


 **Next Tutorial:** [MoveIt! Basics](moveit_basics.md)
--- a/perception.md
+++ b/perception.md
@ -1,59 +0,0 @@
 # Perception Introduction

 The Stretch robot is equipped with the Intel RealSense D435i camera, an essential component that allows the robot to measure and analyze the world around it. In this tutorial, we are going to showcase how to visualize the various topics published from the camera.

 Begin by checking out the [feature/upright_camera_view](https://github.com/hello-robot/stretch_ros/tree/feature/upright_camera_view) branch in the stretch_ros repository. The configuration of the camera results in the images being displayed sideways. Thus, this branch publishes a new topic that rotates the raw image upright.

 ```bash
 cd ~/catkin_ws/src/stretch_ros/stretch_core
 git checkout feature/upright_camera_view
 ```
 Then run the stretch driver launch file.

 ```bash
 # Terminal 1
 roslaunch stretch_core stretch_driver.launch
 ```

 To activate the RealSense camera and publish topics to be visualized, run the following launch file in a new terminal.

 ```bash
 # Terminal 2
 roslaunch stretch_core d435i_low_resolution.launch
 ```

 Within this tutorial package, there is an RViz config file with the topics for perception already in the Display tree. You can visualize these topics and the robot model by running the command below in a new terminal.

 ```bash
 # Terminal 3
 rosrun rviz rviz -d /home/hello-robot/catkin_ws/src/stretch_ros_tutorials/rviz/perception_example.rviz
 ```

 ## PointCloud2 Display

 A list of displays on the left side of the interface can visualize the camera data. Each display has its properties and status that notify a user if topic messages are received.

 For the `PointCloud2` display, a [sensor_msgs/pointCloud2](http://docs.ros.org/en/lunar/api/sensor_msgs/html/msg/PointCloud2.html) message named */camera/depth/color/points*, is received and the gif below demonstrates the various display properties when visualizing the data.


 <p align="center">
  <img src="images/perception_rviz.gif"/>
 </p>

 ## Image Display
 The `Image` display when toggled creates a new rendering window that visualizes a [sensor_msgs/Image](http://docs.ros.org/en/lunar/api/sensor_msgs/html/msg/Image.html) messaged, */camera/color/image/raw*. This feature shows the image data from the camera; however, the image comes out sideways. Thus, you can select the */camera/color/image/raw_upright_view* from the **Image Topic** options to get an upright view of the image.
 <p align="center">
  <img src="images/perception_image.gif"/>
 </p>

 ## Camera Display
 The `Camera` display is similar to that of the `Image` display. In this setting, the rendering window also visualizes other displays, such as the PointCloud2, the RobotModel, and Grid Displays. The **visibility** property can toggle what displays your are interested in visualizing.
 <p align="center">
  <img src="images/perception_camera.gif"/>
 </p>

 ## DepthCloud Display
 The `DepthCloud` display is visualized in the main RViz window. This display takes in the depth image and RGB image, provided by the RealSense,  to visualize and register a point cloud. 
 <p align="center">
  <img src="images/perception_depth.gif"/>
 </p>
--- a/rviz/perception_example.rviz
+++ b/rviz/perception_example.rviz
@ -1,409 +0,0 @@
 Panels:
  - Class: rviz/Displays
    Help Height: 78
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /Status1
        - /RobotModel1/Links1/camera_bottom_screw_frame1
      Splitter Ratio: 0.5
    Tree Height: 1079
  - Class: rviz/Selection
    Name: Selection
  - Class: rviz/Tool Properties
    Expanded:
      - /2D Pose Estimate1
      - /2D Nav Goal1
      - /Publish Point1
    Name: Tool Properties
    Splitter Ratio: 0.5886790156364441
  - Class: rviz/Views
    Expanded:
      - /Current View1
    Name: Views
    Splitter Ratio: 0.5
  - Class: rviz/Time
    Experimental: false
    Name: Time
    SyncMode: 0
    SyncSource: PointCloud2
 Preferences:
  PromptSaveOnExit: true
 Toolbars:
  toolButtonStyle: 2
 Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 1
      Class: rviz/Grid
      Color: 160; 160; 164
      Enabled: true
      Line Style:
        Line Width: 0.029999999329447746
        Value: Lines
      Name: Grid
      Normal Cell Count: 0
      Offset:
        X: 0
        Y: 0
        Z: 0
      Plane: XY
      Plane Cell Count: 10
      Reference Frame: <Fixed Frame>
      Value: true
    - Alpha: 1
      Class: rviz/RobotModel
      Collision Enabled: false
      Enabled: true
      Links:
        All Links Enabled: true
        Expand Joint Details: false
        Expand Link Details: false
        Expand Tree: false
        Link Tree Style: Links in Alphabetic Order
        base_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        camera_bottom_screw_frame:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        camera_color_frame:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        camera_color_optical_frame:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        camera_depth_frame:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        camera_depth_optical_frame:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        camera_infra1_frame:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        camera_infra1_optical_frame:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        camera_infra2_frame:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        camera_infra2_optical_frame:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        camera_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        laser:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_arm_l0:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_arm_l1:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_arm_l2:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_arm_l3:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_arm_l4:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_aruco_inner_wrist:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_aruco_left_base:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_aruco_right_base:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_aruco_shoulder:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_aruco_top_wrist:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_grasp_center:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        link_gripper:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_gripper_finger_left:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_gripper_finger_right:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_gripper_fingertip_left:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_gripper_fingertip_right:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_head:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_head_pan:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_head_tilt:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_left_wheel:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_lift:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_mast:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_right_wheel:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_wrist_yaw:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        respeaker_base:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
      Name: RobotModel
      Robot Description: robot_description
      TF Prefix: ""
      Update Interval: 0
      Value: true
      Visual Enabled: true
    - Alpha: 1
      Autocompute Intensity Bounds: true
      Autocompute Value Bounds:
        Max Value: 10
        Min Value: -10
        Value: true
      Axis: Z
      Channel Name: intensity
      Class: rviz/PointCloud2
      Color: 255; 255; 255
      Color Transformer: RGB8
      Decay Time: 0
      Enabled: true
      Invert Rainbow: false
      Max Color: 255; 255; 255
      Min Color: 0; 0; 0
      Name: PointCloud2
      Position Transformer: XYZ
      Queue Size: 10
      Selectable: true
      Size (Pixels): 3
      Size (m): 0.009999999776482582
      Style: Flat Squares
      Topic: /camera/depth/color/points
      Unreliable: false
      Use Fixed Frame: true
      Use rainbow: true
      Value: true
    - Class: rviz/Image
      Enabled: false
      Image Topic: /camera/color/image_raw_upright_view
      Max Value: 1
      Median window: 5
      Min Value: 0
      Name: Image
      Normalize Range: true
      Queue Size: 2
      Transport Hint: raw
      Unreliable: false
      Value: false
    - Class: rviz/Camera
      Enabled: false
      Image Rendering: background and overlay
      Image Topic: ""
      Name: Camera
      Overlay Alpha: 0.5
      Queue Size: 2
      Transport Hint: raw
      Unreliable: false
      Value: false
      Visibility:
        DepthCloud: true
        Grid: true
        Image: true
        PointCloud2: true
        RobotModel: true
        Value: true
      Zoom Factor: 1
    - Alpha: 1
      Auto Size:
        Auto Size Factor: 1
        Value: true
      Autocompute Intensity Bounds: true
      Autocompute Value Bounds:
        Max Value: 10
        Min Value: -10
        Value: true
      Axis: Z
      Channel Name: intensity
      Class: rviz/DepthCloud
      Color: 255; 255; 255
      Color Image Topic: ""
      Color Transformer: ""
      Color Transport Hint: raw
      Decay Time: 0
      Depth Map Topic: ""
      Depth Map Transport Hint: raw
      Enabled: false
      Invert Rainbow: false
      Max Color: 255; 255; 255
      Min Color: 0; 0; 0
      Name: DepthCloud
      Occlusion Compensation:
        Occlusion Time-Out: 30
        Value: false
      Position Transformer: ""
      Queue Size: 5
      Selectable: true
      Size (Pixels): 3
      Style: Flat Squares
      Topic Filter: true
      Use Fixed Frame: true
      Use rainbow: true
      Value: false
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
    Default Light: true
    Fixed Frame: base_link
    Frame Rate: 30
  Name: root
  Tools:
    - Class: rviz/Interact
      Hide Inactive Objects: true
    - Class: rviz/MoveCamera
    - Class: rviz/Select
    - Class: rviz/FocusCamera
    - Class: rviz/Measure
    - Class: rviz/SetInitialPose
      Theta std deviation: 0.2617993950843811
      Topic: /initialpose
      X std deviation: 0.5
      Y std deviation: 0.5
    - Class: rviz/SetGoal
      Topic: /move_base_simple/goal
    - Class: rviz/PublishPoint
      Single click: true
      Topic: /clicked_point
  Value: true
  Views:
    Current:
      Class: rviz/Orbit
      Distance: 3.6434335708618164
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Focal Point:
        X: 0.1298813819885254
        Y: -0.5344312787055969
        Z: 1.5416947603225708
      Focal Shape Fixed Size: true
      Focal Shape Size: 0.05000000074505806
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
      Pitch: 0.23479780554771423
      Target Frame: <Fixed Frame>
      Value: Orbit (rviz)
      Yaw: 3.4154603481292725
    Saved: ~
 Window Geometry:
  Camera:
    collapsed: false
  Displays:
    collapsed: false
  Height: 1376
  Hide Left Dock: false
  Hide Right Dock: false
  Image:
    collapsed: false
  QMainWindow State: 000000ff00000000fd000000040000000000000253000004c2fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061000000022f000000c70000000000000000fb0000000a0049006d00610067006500000002920000026d0000001600fffffffb0000000c00430061006d0065007200610000000429000000d60000001600ffffff000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006550000003efc0100000002fb0000000800540069006d0065010000000000000655000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003fc000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Time:
    collapsed: false
  Tool Properties:
    collapsed: false
  Views:
    collapsed: false
  Width: 1621
  X: 864
  Y: 38
--- a/rviz_basics.md
+++ b/rviz_basics.md
@ -1,38 +0,0 @@
 ## Visualizing with RViz

 You can utilize RViz to visualize Stretch's sensor information. To begin, run the stretch driver launch file.

 ```bash
 # Terminal 1
 roslaunch stretch_core stretch_driver.roslaunch
 ```

 Then run the following command to bring up a simple RViz configuration of the Stretch robot.
 ```bash
 # Terminal 2
 rosrun rviz rviz -d `rospack find stretch_core`/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.

 ![image](images/rviz_adding_tf.gif)

 There are further tutorials for RViz and can be found [here](http://wiki.ros.org/rviz/Tutorials).


 ## Running RViz and Gazebo (Simulation)
 Let's bringup stretch in the willowgarage world from the [gazebo basics tutorial](gazebo_basics.md) and RViz by using the following command.

 ```bash
 roslaunch stretch_gazebo gazebo.launch world:=worlds/willowgarage.world rviz:=true
 ```
 the `rviz` flag will open an RViz window  to visualize a variety of ROS topics.

 ![image](images/willowgarage_with_rviz.png)

 Bringup the [keyboard teleop](teleoperating_stretch.md) to drive Stretch and observe its sensor input.

 **Next Tutorial:** [Navigation Stack](navigation_stack.md)
--- a/src/avoider.py
+++ b/src/avoider.py
@ -1,90 +0,0 @@
 #!/usr/bin/env python

 # Every python controller needs these lines
 import rospy
 from numpy import linspace, inf, tanh
 from math import sin

 # 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
 from sensor_msgs.msg import LaserScan

 class Avoider:
    """
    A class that implements both a LaserScan filter and base velocity control
    for collision avoidance.
    """
    def __init__(self):
        """
        Function that initializes the subscriber, publisher, and marker features.
        :param self: The self reference.
        """
        # 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)

        # 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.
        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
        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()
        self.twist = Twist()
        self.twist.linear.x = 0.0
        self.twist.linear.y = 0.0
        self.twist.linear.z = 0.0
        self.twist.angular.x = 0.0
        self.twist.angular.y = 0.0
        self.twist.angular.z = 0.0


    # Called every time we get a LaserScan message from ROS
    def set_speed(self,msg):
        """
        Callback function to deal with incoming laserscan messages.
        :param self: The self reference.
        :param msg: The subscribed laserscan message.

        :publishes self.twist: Twist message.
        """
        # 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
        angles = linspace(msg.angle_min, msg.angle_max, len(msg.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"
        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
        error = min(new_ranges) - self.distance

        # Using hyperbolic tanget for speed regulation, with a threshold to stop
        # and driving when it is close to the desired distance
        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)

 if __name__ == '__main__':
    # Initialize the node, and call it "avoider"
    rospy.init_node('avoider')

 	# Instantiate he Avoider class
    Avoider()

    # 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()
--- a/src/effort_sensing.py
+++ b/src/effort_sensing.py
@ -1,179 +0,0 @@
 #!/usr/bin/env python

 # Every python controller needs these lines
 import rospy
 import time
 import actionlib
 import os
 import csv

 # 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

 # We're going to subscribe to 64-bit integers, so we need to import the definition
 # for them.
 from sensor_msgs.msg import JointState

 # Import hello_misc script for handling trajecotry goals with an action client.
 import hello_helpers.hello_misc as hm

 # Import datetime for naming the exported data files
 from datetime import datetime

 class JointActuatorEffortSensor(hm.HelloNode):
    """
    A class that sends multiple joint trajectory goals to a single joint.
    """
    # Initialize the inhereted hm.Hellonode class.
    def __init__(self):
        """
        Function that initializes the subscriber,and other features.
        :param self: The self reference.
        """
        hm.HelloNode.__init__(self)

        # Set up a subscriber. We're going to subscribe to the topic "joint_states"
        self.sub = rospy.Subscriber('joint_states', JointState, self.callback)

        # Create a list of joints
        self.joints = ['joint_lift']

        # Create an empty list for later storage.
        self.joint_effort = []

        # Create path to save effort and position values
        self.save_path = '/home/hello-robot/catkin_ws/src/stretch_ros_tutorials/stored_data'

        # Create boolean data type for conditional statements if you want to export effort data.
        self.export_data = False


    def callback(self, msg):
        """
        Callback function to update and store JointState messages.
        :param self: The self reference.
        :param msg: The JointState message.
        """
        # Store the joint messages for later use
        self.joint_states = msg


    def issue_command(self):
        """
        Function that makes an action call and sends joint trajectory goals
        to a single joint
        :param self: The self reference.
        """

        # Set trajectory_goal as a FollowJointTrajectoryGoal and define
        # the joint name.
        trajectory_goal = FollowJointTrajectoryGoal()
        trajectory_goal.trajectory.joint_names = self.joints

        # Provide desired positions for joint name.
        point0 = JointTrajectoryPoint()
        point0.positions = [0.9]

        # Set a list to the trajectory_goal.trajectory.points
        trajectory_goal.trajectory.points = [point0]

        # 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. Also define the feedback and
        # done callback function.
        self.trajectory_client.send_goal(trajectory_goal, feedback_cb=self.feedback_callback, done_cb=self.done_callback)
        rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal))
        self.trajectory_client.wait_for_result()


    def feedback_callback(self,feedback):
        """
        The feedback_callback function deals with the incoming feedback messages
        from the trajectory_client. Although, in this function, we do not use the
        feedback information.
        :param self: The self reference.
        :param feedback: FollowJointTrajectoryActionFeedback message.
        """
        # Conditional statement for replacement of joint names if wrist_extension
        # is in the self.joint_names list.
        if 'wrist_extension' in self.joints:
            self.joints.remove('wrist_extension')
            self.joints.append('joint_arm_l0')

        # create an empty list of current effort values that will be appended
        # to the overall joint_effort list.
        current_effort = []

        # Use of forloop to parse the names of the requested joints list.
        # The index() function returns the index at the first occurrence of
        # the name of the requested joint in the self.joint_states.name list.
        for joint in self.joints:
            index = self.joint_states.name.index(joint)
            current_effort.append(self.joint_states.effort[index])

        # If the self.export_data boolean is false, then print the names and efforts
        # of the joints in the terminal.
        if not self.export_data:
            # Print the joint position values to the terminal
            print("name: " + str(self.joints))
            print("effort: " + str(current_effort))

        # Else, append the current effort to the joint_effort list.
        else:
            self.joint_effort.append(current_effort)


    def done_callback(self, status, result):
        """
        The done_callback function will be called when the joint action is complete.
        Within this function we export the data to a .txt file in  the /stored_data directory.
        :param self: The self reference.
        :param status: status attribute from FollowJointTrajectoryActionResult message.
        :param result: result attribute from FollowJointTrajectoryActionResult message.
        """
        # Conditional statemets to notify whether the action succeeded or failed.
        if status == actionlib.GoalStatus.SUCCEEDED:
            rospy.loginfo('Suceeded')
        else:
            rospy.loginfo('Failed')

        # Conditional statement for exporting data.
        if self.export_data:
            # File name is the date and time the action was complete
            file_name = datetime.now().strftime("%Y-%m-%d_%I:%M:%S-%p")
            completeName = os.path.join(self.save_path, file_name)

            # Write the joint names and efforts to a .txt file.
            with open(completeName, "w") as f:
                writer = csv.writer(f)
                writer.writerow(self.joints)
                writer.writerows(self.joint_effort)


    def main(self):
        """
        Function that initiates the issue_command function.
        :param self: The self reference.
        """
        # 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, 'issue_command', 'issue_command', wait_for_first_pointcloud=False)
        rospy.loginfo('issuing command...')
        self.issue_command()
        time.sleep(2)


 if __name__ == '__main__':
 	try:
    	# Declare object, node, from JointActuatorEffortSensor class
 		node = JointActuatorEffortSensor()
 		node.main()
 	except KeyboardInterrupt:
 		rospy.loginfo('interrupt received, so shutting down')
--- a/src/joint_state_printer.py
+++ b/src/joint_state_printer.py
@ -1,84 +0,0 @@
 #!/usr/bin/env python

 # Import ROS Python basic API and sys
 import rospy
 import sys

 # We're going to subscribe to 64-bit integers, so we need to import the definition
 # for them
 from sensor_msgs.msg import JointState

 class JointStatePublisher():
 	"""
 	A class that prints the positions of desired joints in Stretch.
 	"""

 	def __init__(self):
 		"""
 		Function that initializes the subsriber.
 		:param self: The self reference
 		"""
 		# Set up a subscriber. We're going to subscribe to the topic "joint_states"
 		self.sub = rospy.Subscriber('joint_states', JointState, self.callback)


 	def callback(self, msg):
 		"""
 		Callback function to deal with the incoming JointState messages.
 		:param self: The self reference.
 		:param msg: The JointState message.
 		"""
 		# Store the joint messages for later use
 		self.joint_states = msg


 	def print_states(self, joints):
 		"""
 		print_states function to deal with the incoming JointState messages.
 		:param self: The self reference.
 		:param joints: A list of joint names.
 		"""
 		# Create an empty list that will store the positions of the requested joints
 		joint_positions = []

 		# Use of forloop to parse the names of the requested joints list.
 		# The index() function returns the index at the first occurrence of
 		# the name of the requested joint in the self.joint_states.name list
 		for joint in joints:
 			index = self.joint_states.name.index(joint)
 			joint_positions.append(self.joint_states.position[index])

 		# Print the joint position values to the terminal
 		print("name: " + str(joints))
 		print("position: " + str(joint_positions))

 		# Sends a signal to rospy to shutdown the ROS interfaces
 		rospy.signal_shutdown("done")

 		# Exit the Python interpreter
 		sys.exit(0)



 if __name__ == '__main__':
 	# Initialize the node.
 	rospy.init_node('joint_state_printer', anonymous=True)

 	# Declare object from JointStatePublisher class
 	JSP = JointStatePublisher()

 	# Use the rospy.sleep() function to allow the class to initialize before
 	# requesting to publish joint_positions of desired joints (running the
 	# print_states() function)
 	rospy.sleep(.1)

 	# Create a list of the joints and name them joints. These will be an argument
 	# for the print_states() function
 	joints = ["joint_lift", "joint_arm_l0", "joint_arm_l1", "joint_arm_l2", "joint_arm_l3", "joint_wrist_yaw"]
 	# joints = ["joint_head_pan","joint_head_tilt", "joint_gripper_finger_left", "joint_gripper_finger_right"]
 	JSP.print_states(joints)

 	# 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()
--- a/src/marker.py
+++ b/src/marker.py
@ -1,89 +0,0 @@
 #!/usr/bin/env python

 # Import rospy and sys
 import rospy

 # Import the Marker message type from the visualization_msgs package.
 from visualization_msgs.msg import Marker

 class Balloon():
 	"""
 	A class that attaches a Sphere marker directly above the Stretch robot.
 	"""
 	def __init__(self):
 		"""
 		Function that initializes the marker's features.
 		:param self: The self reference.
 		"""
 		# Set up a publisher.  We're going to publish on a topic called balloon
 		self.pub = 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

 		# Create log message
 		rospy.loginfo("Publishing the balloon topic. Use RViz to visualize.")


 	def publish_marker(self):
 		"""
 		Function that publishes the sphere marker
 		:param self: The self reference.

 		:publishes self.marker: Marker message.
 		"""
 		# publisher the marker
 		self.pub.publish(self.marker)


 if __name__ == '__main__':
 	# Initialize the node, as usual
 	rospy.init_node('marker')

 	# Declare object from the Balloon class
 	balloon = Balloon()

 	# Set a rate
 	rate = rospy.Rate(10)

 	# Publish the marker at 10Hz
 	while not rospy.is_shutdown():
 		balloon.publish_marker()
 		rate.sleep()
--- a/src/move.py
+++ b/src/move.py
@ -1,71 +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:
 	"""
 	A class that sends Twist messages to move the Stretch robot foward.
 	"""
 	def __init__(self):
 		"""
 		Function that initializes the publisher.
 		:param self: The self reference
 		"""
 		# 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):
 		"""
 		Function that publishes Twist messages
 		:param self: The self reference.

 		:publishes command: Twist message.
 		"""
 		# 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')

 	# Declare object from Move class
 	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()
--- a/src/multipoint_command.py
+++ b/src/multipoint_command.py
@ -1,109 +0,0 @@
 #!/usr/bin/env python

 # 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):
 	"""
 	A class that sends multiple joint trajectory goals to the stretch robot.
 	"""

 	# Initialize the inhereted hm.Hellonode class
 	def __init__(self):
 		hm.HelloNode.__init__(self)

 	def issue_multipoint_command(self):
 		"""
 		Function that makes an action call and sends multiple joint trajectory goals
 		to the joint_lift, wrist_extension, and joint_wrist_yaw.
 		:param self: The self reference.
 		"""
 		# 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 accelerations 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 list of goals = {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):
 		"""
 		Function that initiates the multipoint_command function.
 		:param self: The self reference.
 		"""
 		# 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:
        # Instantiate the MultiPointCommand class and execute the main() method
        node = MultiPointCommand()
        node.main()
    except KeyboardInterrupt:
        rospy.loginfo('interrupt received, so shutting down')
--- a/src/scan_filter.py
+++ b/src/scan_filter.py
@ -1,67 +0,0 @@
 #!/usr/bin/env python

 # Every python controller needs these lines
 import rospy
 from numpy import linspace, inf
 from math import sin
 # We're going to subscribe to a LaserScan message
 from sensor_msgs.msg import LaserScan

 class ScanFilter:
 	"""
 	A class that implements a LaserScan filter that removes all of the points.
 	that are not infront of the robot.
 	"""
 	def __init__(self):
 		# 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
 		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)

 		# 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)

 		# Create log message
 		rospy.loginfo("Publishing the filtered_scan topic. Use RViz to visualize.")


 	def callback(self,msg):
 		"""
 		Callback function to deal with incoming laserscan messages.
 		:param self: The self reference.
 		:param msg: The subscribed laserscan message.

 		:publishes msg: updated laserscan message.
 		"""
 		# 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
 		angles = linspace(msg.angle_min, msg.angle_max, len(msg.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"
 		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
 		msg.ranges = new_ranges
 		self.pub.publish(msg)

 if __name__ == '__main__':
 	# Initialize the node, and call it "scan_filter"
 	rospy.init_node('scan_filter')

 	# Instantiate the ScanFilter class
 	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()
--- a/src/single_joint_actuator.py
+++ b/src/single_joint_actuator.py
@ -1,89 +0,0 @@
 #!/usr/bin/env python

 # 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 SingleJointActuator(hm.HelloNode):
 	"""
 	A class that sends multiple joint trajectory goals to a single joint.
 	"""
 	# Initialize the inhereted hm.Hellonode class
 	def __init__(self):
 		hm.HelloNode.__init__(self)

 	def issue_command(self):
 		"""
 		Function that makes an action call and sends joint trajectory goals
 		to a single joint
 		:param self: The self reference.
 		"""
 		# Here is a list of joints and their recommended position limits:
 		############################# Joint limits #############################
 		# joint_lift:      lower_limit =  0.15,  upper_limit =  1.10  # in meters
 		# wrist_extension: lower_limit =  0.00,  upper_limit =  0.50  # in meters
 		# joint_wrist_yaw: lower_limit = -1.75,  upper_limit =  4.00  # in radians
 		# joint_head_pan:  lower_limit = -2.80,  upper_limit =  2.90  # in radians
 		# joint_head_tilt: lower_limit = -1.60,  upper_limit =  0.40  # in radians
 		# joint_gripper_finger_left:  lower_limit = -0.35,  upper_limit =  0.165  # in radians
 		########################################################################

 		# Set trajectory_goal as a FollowJointTrajectoryGoal and define
 		# the joint name
 		trajectory_goal = FollowJointTrajectoryGoal()
 		trajectory_goal.trajectory.joint_names = ['joint_head_pan']

 		# Provide desired positions for joint name.
 		# Set positions for the following 5 trajectory points
 		point0 = JointTrajectoryPoint()
 		point0.positions = [0.65]

 		# point1 = JointTrajectoryPoint()
 		# point1.positions = [-1.50]

 		# Then trajectory_goal.trajectory.points is set as a list of the joint
 		# trajectory points
 		trajectory_goal.trajectory.points = [point0]#, point1]

 		# 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 goal = {0}'.format(trajectory_goal))
 		self.trajectory_client.wait_for_result()

 	def main(self):
 		"""
 		Function that initiates the issue_command function.
 		:param self: The self reference.
 		"""
 		# 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, 'issue_command', 'issue_command', wait_for_first_pointcloud=False)
 		rospy.loginfo('issuing command...')
 		self.issue_command()
 		time.sleep(2)


 if __name__ == '__main__':
 	try:
 		# Declare object from the SingleJointActuator class. Then execute the
 		# main() method/function
 		node = SingleJointActuator()
 		node.main()
 	except KeyboardInterrupt:
 		rospy.loginfo('interrupt received, so shutting down')
--- a/src/stored_data_plotter.py
+++ b/src/stored_data_plotter.py
@ -1,90 +0,0 @@
 #!/usr/bin/env python

 # Import needed python packages
 import numpy as np
 import os
 import csv
 import pandas as pd
 import matplotlib.pyplot as plt
 from matplotlib import animation

 class Plotter():
    """
    Class the plots stored data from the effort sensing node.
    """
    def __init__(self,animate=False):
        """
        Function that stores the dataframe.
        :param self: The self reference.
        :param animate: Boolean to determine animation plotting.
        """
        # Create path to save effort and position values
        dir_path = '/home/hello-robot/catkin_ws/src/stretch_ros_tutorials/stored_data'

        ####################### Copy the file name here! #######################
        file_name = '2022-06-30_11:26:20-AM'

        # Complete name of directory to pull from
        self.completeName = os.path.join(dir_path, file_name)

        # Store dataframe
        self.data = pd.read_csv(self.completeName)

        # Create empty list for animation
        self.y_anim = []

        # Set self.animate to boolean argument
        self.animate = animate

    def plot_data(self):
        """
        Function that plots dataframe
        :param self: The self reference.
        """
        # Utililze a forloop to print each joint's effort
        for joint in self.data.columns:

            # Create figure, labels, and title
            fig = plt.figure()
            plt.title(joint + ' Effort Sensing')
            plt.ylabel('Effort')
            plt.xlabel('Data Points')

            # Conditional statement for animation plotting
            if self.animate:
                self.effort = self.data[joint]
                frames = len(self.effort)-1
                anim = animation.FuncAnimation(fig=fig,func=self.plot_animate, repeat=False,blit=False,frames=frames, interval =75)
                plt.show()

                ## If you want to save a video, make sure to comment out plt.show(),
                ## right before this comment.
                # save_path = str(self.completeName + '.mp4')
                # anim.save(save_path, writer=animation.FFMpegWriter(fps=10))

                # Reset y_anim for the next joint effort animation
                del self.y_anim[:]


            # Conditional statement for regular plotting (No animation)
            else:
                self.data[joint].plot(kind='line')
                # save_path = str(self.completeName + '.png')
                # plt.savefig(save_path, bbox_inches='tight')
                plt.show()


    def plot_animate(self,i):
        """
        Function that plots every increment of the dataframe.
        :param self: The self reference.
        :param i: index value.
        """
        # Append self.effort values for given joint.
        self.y_anim.append(self.effort.values[i])
        plt.plot(self.y_anim, color='blue')


 if __name__ == '__main__':
    viz = Plotter(animate=True)
    viz.plot_data()
--- a/src/stow_command.py
+++ b/src/stow_command.py
@ -1,81 +0,0 @@
 #!/usr/bin/env python

 # 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):
    '''
    A class that sends a joint trajectory goal to stow the Stretch's arm.
    '''

    # Initialize the inhereted hm.Hellonode class
    def __init__(self):
        hm.HelloNode.__init__(self)


    def issue_stow_command(self):
        '''
        Function that makes an action call and sends stow postion goal.
        :param self: The self reference.
        '''
        # 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):
        '''
        Function that initiates stow_command function.
        :param self: The self reference.
        '''
        # 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:
        # Declare object from StowCommand class then execute the main() method
        node = StowCommand()
        node.main()
    except KeyboardInterrupt:
        rospy.loginfo('interrupt received, so shutting down')
--- a/stored_data/2022-06-30_11:26:20-AM
+++ b/stored_data/2022-06-30_11:26:20-AM
@ -1,54 +0,0 @@
 joint_lift
 30.90921974182129
 30.90921974182129
 30.90921974182129
 35.687774658203125
 46.455074310302734
 53.12174606323242
 74.74340057373047
 74.74340057373047
 78.7086181640625
 81.2074203491211
 83.81580352783203
 84.66924285888672
 86.44051361083984
 86.29670715332031
 84.92870330810547
 85.26908111572266
 83.41191101074219
 83.6566162109375
 82.04027557373047
 82.04027557373047
 81.03909301757812
 81.51132202148438
 81.67819213867188
 82.48448944091797
 82.48580169677734
 83.62725067138672
 84.09523010253906
 84.09523010253906
 85.30419158935547
 84.31338500976562
 85.0532455444336
 84.40817260742188
 82.8752212524414
 82.47306823730469
 83.45247650146484
 83.28417205810547
 84.3813247680664
 84.37670135498047
 85.32344818115234
 84.87259674072266
 86.09488677978516
 85.2378921508789
 85.44424438476562
 85.44424438476562
 84.78129577636719
 84.24555206298828
 84.24555206298828
 83.79412078857422
 84.11444091796875
 83.61358642578125
 83.63019561767578
 83.30967712402344
 83.30967712402344
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/stretch_body/tool_change_tutorial.md
+++ b/stretch_body/tool_change_tutorial.md
--- a/teleoperating_stretch.md
+++ b/teleoperating_stretch.md
@ -1,164 +0,0 @@
 ## Teleoperating Stretch

 ### Xbox Controller Teleoperating
 ![image](images/xbox_controller_commands.png)

 Stretch comes ready to run out of the box. The Xbox Teleoperation demo will let you quickly test out the robot capabilities by teleoperating it with an Xbox Controller.


 Note: Make sure the USB Dongle is plugged into the the USB port of the base trunk.

 To start the demo:

 * Remove the 'trunk' cover and power on the robot
 Wait for about 45 seconds. You will hear the Ubuntu startup sound, followed by two beeps (indicating the demo is running).
 * Hit the Connect button on the controller. The upper two LEDs of the ring will illuminate.
 * Hit the Home Robot button. Stretch will go through its homing calibration routine.
 * **Note**: make sure the space around the robot is clear before running the Home function

 You're ready to go! A few things to try:

 * Hit the Stow Robot button. The robot will assume the stow pose.
 * Practice driving the robot around.
 * Pull the Fast Base trigger while driving. When stowed, it will make Stretch drive faster
 * Manually stop the arm or lift from moving to make it stop upon contact.
 * Try picking up your cellphone from the floor
 * Try grasping cup from a counter top
 * Try delivering an object to a person

 If you're done, hold down the Shutdown PC button for 2 seconds. This will cause the PC to turn off. You can then power down the robot.

 ### Keyboard Teleoperating: Mobile Base

 Begin by running the following command in your terminal:

 ```bash
 # Terminal 1
 roslaunch stretch_core stretch_driver.launch
 ```

 To teleoperate a Stretch's mobile base with the keyboard, you first need to switch the mode to *nagivation* for the robot to receive *Twist* messages. This is done using a rosservice call in a new terminal. In the same terminal run the teleop_twist_keyboard node with the argument remapping the *cmd_vel* topic name to *stretch/cmd_vel*.

 ```bash
 # Terminal 2
 rosservice call /switch_to_navigation_mode
 rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=stretch/cmd_vel
 ```

 Below are the keyboard commands that allow a user to move Stretch's base.  
 ```
 Reading from the keyboard  and Publishing to Twist!
 Moving around:
   u    i    o
   j    k    l
   m    ,    .

 For Holonomic mode (strafing), hold down the shift key:
   U    I    O
   J    K    L
   M    <    >

 t : up (+z)
 b : down (-z)

 anything else : stop

 q/z : increase/decrease max speeds by 10%
 w/x : increase/decrease only linear speed by 10%
 e/c : increase/decrease only angular speed by 10%

 CTRL-C to quit

 currently:	speed 0.5	turn 1.0

 ```
 To stop the node from sending twist messages, type **Ctrl** + **c**.

 ### Create a node for Mobile Base Teleoperating
 To move Stretch's mobile base using a python script, please look at [example 1](example_1.md) for reference.

 ### Keyboard Teleoperating: Full Body

 For full body teleoperation with the keyboard, you first need to run the `stretch_driver.launch` in a terminal.

 ```bash
 # Terminal 1
 roslaunch stretch_core stretch_driver.launch
 ```
 Then in a new terminal, type the following command

 ```bash
 # Terminal 2
 rosrun stretch_core keyboard_teleop
 ```

 Below are the keyboard commands that allow a user to control all of Stretch's joints.  
 ```

              i HEAD UP                    
 j HEAD LEFT            l HEAD RIGHT       
              , HEAD DOWN                  


 7 BASE ROTATE LEFT     9 BASE ROTATE RIGHT
 home                   page-up            


              8 LIFT UP                    
              up-arrow                     
 4 BASE FORWARD         6 BASE BACK        
 left-arrow             right-arrow        
              2 LIFT DOWN                  
              down-arrow                   


              w ARM OUT                    
 a WRIST FORWARD        d WRIST BACK       
              x ARM IN                     


              5 GRIPPER CLOSE              
              0 GRIPPER OPEN               

  step size:  b BIG, m MEDIUM, s SMALL     

              q QUIT                       


 ```
 To stop the node from sending twist messages, type **Ctrl** + **c**.


 ## Teleoperating in Gazebo


 ### Keyboard Teleoperating: Mobile Base
 For keyboard teleoperation of the Stretch's mobile base, first [startup Stretch in simulation](gazebo_basics.md). Then run the following command in a new terminal.

 ```bash
 # Terminal 1
 roslaunch stretch_gazebo gazebo.launch
 ```

 In a new terminal, type the following

 ```bash
 # Terminal 2
 roslaunch stretch_gazebo teleop_keyboard.launch
 ```
 The same keyboard commands will be presented to a user to move the robot.

 ### Xbox Controller Teleoperating
 An alternative for robot base teleoperation is to use an Xbox controller. Stop the keyboard teleoperation node by typing **Ctrl** + **c** in the terminal where the command was executed. Then connect the Xbox controller device to your local machine and run the following command.

 ```bash
 # Terminal 2
 roslaunch stretch_gazebo teleop_joy.launch
 ```
 Note that the teleop_twist_joy package has a deadman switch by default which disables the drive commands to be published unless pressed. For a Logitech F310 joystick, this button is A.

 **Next Tutorial:** [Internal State of Stretch](internal_state_of_stretch.md)