Update follow joint trajectory readme instructions

follow_joint_trajectory.md

@@ -10,7 +10,7 @@ Stretch driver offers a [`FollowJointTrajectory`](http://docs.ros.org/en/api/con
 Begin by launching stretch_driver in a terminal.
 
 ```bash
-ros2 launch stretch_core stretch_driver.launch.py
+ros2 launch stretch_core stretch_driver.launch.py mode:=trajectory
 ```
 
 In a new terminal type the following commands.
@@ -26,73 +26,63 @@ This will send a FollowJointTrajectory command to stow Stretch's arm.
 #!/usr/bin/env python3
 
 import rclpy
-from rclpy.node import Node
 from rclpy.duration import Duration
-from rclpy.action import ActionClient
-import sys
 from control_msgs.action import FollowJointTrajectory
 from trajectory_msgs.msg import JointTrajectoryPoint
-from sensor_msgs.msg import JointState
+from hello_helpers.hello_misc import HelloNode
+import time
 
-class StowCommand(Node):
+class StowCommand(HelloNode):
     def __init__(self):
-        super().__init__('stretch_stow_command')
-        self.joint_state = JointState()
-
-        self.trajectory_client = ActionClient(self, FollowJointTrajectory, '/stretch_controller/follow_joint_trajectory')
-        server_reached = self.trajectory_client.wait_for_server(timeout_sec=60.0)
-        if not server_reached:
-            self.get_logger().error('Unable to connect to arm action server. Timeout exceeded.')
-            sys.exit()
-
-        self.subscription = self.create_subscription(JointState, '/stretch/joint_states', self.joint_states_callback, 1)
-        self.subscription
-
-    def joint_states_callback(self, joint_state):
-        self.joint_state = joint_state
+        HelloNode.__init__(self)
+        HelloNode.main(self, 'stow_command', 'stow_command', wait_for_first_pointcloud=False)
 
     def issue_stow_command(self):
-        joint_state = self.joint_state
-        if (joint_state is not None):
-            self.get_logger().info('stowing...')
+        while not self.joint_state.position:
+            self.get_logger().info("Waiting for joint states message to arrive")
+            time.sleep(0.1)
+            continue
 
-            stow_point1 = JointTrajectoryPoint()
-            stow_point2 = JointTrajectoryPoint()
-            duration1 = Duration(seconds=0.0)
-            duration2 = Duration(seconds=4.0)
-            stow_point1.time_from_start = duration1.to_msg()
-            stow_point2.time_from_start = duration2.to_msg()
-
-            joint_value1 = joint_state.position[1] # joint_lift is at index 1
-            joint_value2 = joint_state.position[0] # wrist_extension is at index 0
-            joint_value3 = joint_state.position[8] # joint_wrist_yaw is at index 8
-            stow_point1.positions = [joint_value1, joint_value2, joint_value3]
-            stow_point2.positions = [0.2, 0.0, 3.14]
-
-            trajectory_goal = FollowJointTrajectory.Goal()
-            trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
-
-            trajectory_goal.trajectory.points = [stow_point1, stow_point2]
+        self.get_logger().info('Stowing...')
+        joint_state = self.joint_state
 
-            trajectory_goal.trajectory.header.stamp = self.get_clock().now().to_msg()
-            trajectory_goal.trajectory.header.frame_id = 'base_link'
+        stow_point1 = JointTrajectoryPoint()
+        stow_point2 = JointTrajectoryPoint()
+        duration1 = Duration(seconds=0.0)
+        duration2 = Duration(seconds=4.0)
+        stow_point1.time_from_start = duration1.to_msg()
+        stow_point2.time_from_start = duration2.to_msg()
+
+        lift_index = joint_state.name.index('joint_lift')
+        arm_index = joint_state.name.index('wrist_extension')
+        wrist_yaw_index = joint_state.name.index('joint_wrist_yaw')
+        joint_value1 = joint_state.position[lift_index]
+        joint_value2 = joint_state.position[arm_index]
+        joint_value3 = joint_state.position[wrist_yaw_index]
+        
+        stow_point1.positions = [joint_value1, joint_value2, joint_value3]
+        stow_point2.positions = [0.2, 0.0, 3.14]
 
-            self.trajectory_client.send_goal_async(trajectory_goal)
-            self.get_logger().info('Sent stow goal = {0}'.format(trajectory_goal))
+        trajectory_goal = FollowJointTrajectory.Goal()
+        trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
 
+        trajectory_goal.trajectory.points = [stow_point1, stow_point2]
 
-def main(args=None):
-    rclpy.init(args=args)
-     
-    stow_command = StowCommand()
-    
-    rclpy.spin_once(stow_command)
-    stow_command.issue_stow_command()
-    rclpy.spin(stow_command)
+        self.trajectory_client.send_goal_async(trajectory_goal)
+        self.get_logger().info("Goal sent")
 
-    stow_command.destroy_node()
-    rclpy.shutdown()
+    def main(self):
+        self.issue_stow_command()
 
+def main():
+    try:
+        node = StowCommand()
+        node.main()
+        node.new_thread.join()
+    except:
+        node.get_logger().info("Exiting")
+        node.destroy_node()
+        rclpy.shutdown()
 
 if __name__ == '__main__':
     main()
@@ -110,23 +100,21 @@ Every Python ROS [Node](http://wiki.ros.org/Nodes) will have this declaration at
 
 ```python
 import rclpy
-from rclpy.node import Node
 from rclpy.duration import Duration
-from rclpy.action import ActionClient
-import sys
 from control_msgs.action import FollowJointTrajectory
 from trajectory_msgs.msg import JointTrajectoryPoint
-from sensor_msgs.msg import JointState
+from hello_helpers.hello_misc import HelloNode
+import time
 ```
-<!-- TODO: Update links below -->
-You need to import rclpy if you are writing a ROS 2 Node. Import the FollowJointTrajectory from the [control_msgs.msg](http://wiki.ros.org/control_msgs) package to control the Stretch robot. Import JointTrajectoryPoint from the [trajectory_msgs](http://wiki.ros.org/trajectory_msgs) package to define robot trajectories.
+You need to import rclpy if you are writing a ROS 2 Node. Import the FollowJointTrajectory from the [control_msgs.action](https://github.com/ros-controls/control_msgs/tree/master/control_msgs) package to control the Stretch robot. Import JointTrajectoryPoint from the [trajectory_msgs](https://github.com/ros2/common_interfaces/tree/rolling/trajectory_msgs/msg) package to define robot trajectories.
 
 ```python
-class StowCommand(Node):
+class StowCommand(HelloNode):
     def __init__(self):
-        super().__init__('stretch_stow_command')
+        HelloNode.__init__(self)
+        HelloNode.main(self, 'stow_command', 'stow_command', wait_for_first_pointcloud=False)
 ```
-The `StowCommand ` class inherits from the `Node` class from and is initialized.
+The `StowCommand ` class inherits from the `HelloNode` class and is initialized with the main method in HelloNode by passing the arguments node_name as 'stow_command', node_namespace as 'stow_command' and wait_for_first_pointcloud as False. Refer to the [Introduction to HelloNode]() tutorial if you haven't already to understand how this works.
 
 ```python
 def issue_stow_command(self):
@@ -134,54 +122,60 @@ def issue_stow_command(self):
 The `issue_stow_command()` method will stow Stretch's arm. Within the function, we set *stow_point* as a `JointTrajectoryPoint`and provide desired positions (in meters). These are the positions of the lift, wrist extension, and yaw of the wrist, respectively. These are defined in the next set of the code.
 
 ```python
-    stow_point1 = JointTrajectoryPoint()
-    stow_point2 = JointTrajectoryPoint()
-    duration1 = Duration(seconds=0.0)
-    duration2 = Duration(seconds=4.0)
-    stow_point1.time_from_start = duration1.to_msg()
-    stow_point2.time_from_start = duration2.to_msg()
-
-    joint_value1 = joint_state.position[1] # joint_lift is at index 1
-    joint_value2 = joint_state.position[0] # wrist_extension is at index 0
-    joint_value3 = joint_state.position[8] # joint_wrist_yaw is at index 8
-    stow_point1.positions = [joint_value1, joint_value2, joint_value3]
-    stow_point2.positions = [0.2, 0.0, 3.14]
-
-    trajectory_goal = FollowJointTrajectory.Goal()
-    trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
-
-    trajectory_goal.trajectory.points = [stow_point1, stow_point2]
-
-    trajectory_goal.trajectory.header.stamp = self.get_clock().now().to_msg()
-    trajectory_goal.trajectory.header.frame_id = 'base_link'
+        while not self.joint_state.position:
+            self.get_logger().info("Waiting for joint states message to arrive")
+            time.sleep(0.1)
+            continue
+
+        self.get_logger().info('Stowing...')
+        joint_state = self.joint_state
+
+        stow_point1 = JointTrajectoryPoint()
+        stow_point2 = JointTrajectoryPoint()
+        duration1 = Duration(seconds=0.0)
+        duration2 = Duration(seconds=4.0)
+        stow_point1.time_from_start = duration1.to_msg()
+        stow_point2.time_from_start = duration2.to_msg()
+
+        lift_index = joint_state.name.index('joint_lift')
+        arm_index = joint_state.name.index('wrist_extension')
+        wrist_yaw_index = joint_state.name.index('joint_wrist_yaw')
+        joint_value1 = joint_state.position[lift_index]
+        joint_value2 = joint_state.position[arm_index]
+        joint_value3 = joint_state.position[wrist_yaw_index]
+        
+        stow_point1.positions = [joint_value1, joint_value2, joint_value3]
+        stow_point2.positions = [0.2, 0.0, 3.14]
+
+        trajectory_goal = FollowJointTrajectory.Goal()
+        trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
+
+        trajectory_goal.trajectory.points = [stow_point1, stow_point2]
 ```
-Set *trajectory_goal* as a `FollowJointTrajectory.Goal()` and define the joint names as a list. Then `trajectory_goal.trajectory.points` is defined by the positions set in *stow_point*. Specify the coordinate frame that we want (base_link) and set the time to be now.
+Set *trajectory_goal* as a `FollowJointTrajectory.Goal()` and define the joint names as a list. Then `trajectory_goal.trajectory.points` is defined by the positions set in *stow_point1* and *stow_point2*.
 
 ```python
-    self.trajectory_client.send_goal_async(trajectory_goal)
-    self.get_logger().info('Sent stow goal = {0}'.format(trajectory_goal))
+        self.trajectory_client.send_goal_async(trajectory_goal)
+        self.get_logger().info("Goal sent")
 ```
 Make the action call and send the goal.
 
 ```python
 def main(args=None):
-    rclpy.init(args=args)
-     
-    stow_command = StowCommand()
-    
-    rclpy.spin_once(stow_command)
-    stow_command.issue_stow_command()
-    rclpy.spin(stow_command)
-
-    stow_command.destroy_node()
-    rclpy.shutdown()
+    try:
+        node = StowCommand()
+        node.main()
+        node.new_thread.join()
+    except:
+        node.get_logger().info("Exiting")
+        node.destroy_node()
+        rclpy.shutdown()
 ```
 Create a funcion, `main()`, to do all of the setup in the class and issue the stow command. Initialize the `StowCommand()` class and set it to *node* and run the `main()` function.
 
 ```python
 if __name__ == '__main__':
     main()
-
 ```
 To make the script executable call the main() function like above.
 
@@ -195,7 +189,7 @@ To make the script executable call the main() function like above.
 If you have killed the above instance of stretch_driver relaunch it again through the terminal.
 
 ```bash
-ros2 launch stretch_core stretch_driver.launch.py
+ros2 launch stretch_core stretch_driver.launch.py mode:=trajectory
 ```
 
 In a new terminal type the following commands.
@@ -207,115 +201,94 @@ ros2 run stretch_ros_tutorials multipoint_command
 This will send a list of JointTrajectoryPoint's to move Stretch's arm.
 
 ### The Code
+
 ```python
 #!/usr/bin/env python3
 
-import sys
 import rclpy
-from rclpy.node import Node
-from rclpy.action import ActionClient
 from rclpy.duration import Duration
 from control_msgs.action import FollowJointTrajectory
 from trajectory_msgs.msg import JointTrajectoryPoint
-from sensor_msgs.msg import JointState
+from hello_helpers.hello_misc import HelloNode
+import time
 
-class MultiPointCommand(Node):
+class MultiPointCommand(HelloNode):
     def __init__(self):
-        super().__init__('stretch_multipoint_command')
-
-        self.trajectory_client = ActionClient(self, FollowJointTrajectory, '/stretch_controller/follow_joint_trajectory')
-        server_reached = self.trajectory_client.wait_for_server(timeout_sec=60.0)
-        if not server_reached:
-            self.get_logger().error('Unable to connect to arm action server. Timeout exceeded.')
-            sys.exit()
-
-        self.subscription = self.create_subscription(JointState, '/stretch/joint_states', self.joint_states_callback, 1)
-        self.subscription
-
-        self.get_logger().info('issuing multipoint command...')
-
-    def joint_states_callback(self, joint_state):
-        self.joint_state = joint_state
+        HelloNode.__init__(self)
+        HelloNode.main(self, 'multipoint_command', 'multipoint_command', wait_for_first_pointcloud=False)
 
     def issue_multipoint_command(self):
+        while not self.joint_state.position:
+            self.get_logger().info("Waiting for joint states message to arrive")
+            time.sleep(0.1)
+            continue
+        
+        self.get_logger().info('Issuing multipoint command...')
         joint_state = self.joint_state
         duration0 = Duration(seconds=0.0)
-        duration1 = Duration(seconds=2.0)
-        duration2 = Duration(seconds=4.0)
-        duration3 = Duration(seconds=6.0)
-        duration4 = Duration(seconds=8.0)
-        duration5 = Duration(seconds=10.0)
-        
-        joint_value1 = joint_state.position[1] # joint_lift is at index 1
-        joint_value2 = joint_state.position[0] # wrist_extension is at index 0
-        joint_value3 = joint_state.position[8] # joint_wrist_yaw is at index 8
-
+        duration1 = Duration(seconds=6.0)
+        duration2 = Duration(seconds=9.0)
+        duration3 = Duration(seconds=12.0)
+        duration4 = Duration(seconds=16.0)
+        duration5 = Duration(seconds=20.0)
+
+        lift_index = joint_state.name.index('joint_lift')
+        arm_index = joint_state.name.index('wrist_extension')
+        wrist_yaw_index = joint_state.name.index('joint_wrist_yaw')
+        gripper_index = joint_state.name.index('joint_gripper_finger_left')
+        joint_value1 = joint_state.position[lift_index]
+        joint_value2 = joint_state.position[arm_index]
+        joint_value3 = joint_state.position[wrist_yaw_index]
+        joint_value4 = joint_state.position[gripper_index]
         point0 = JointTrajectoryPoint()
-
-        point0.positions = [joint_value1, joint_value2, joint_value3]
-
-        point0.velocities = [0.2, 0.2, 2.5]
-
-        point0.accelerations = [1.0, 1.0, 3.5]
-
+        point0.positions = [joint_value1, joint_value2, joint_value3, joint_value4]
+        point0.velocities = [0.0, 0.0, 0.0, 0.0]
         point0.time_from_start = duration0.to_msg()
-
         point1 = JointTrajectoryPoint()
-        point1.positions = [0.3, 0.1, 2.0]
+        point1.positions = [0.9, 0.0, 0.0, 0.0] 
         point1.time_from_start = duration1.to_msg()
-
         point2 = JointTrajectoryPoint()
-        point2.positions = [0.5, 0.2, -1.0]
+        point2.positions = [0.9, 0.2, 0.0, -0.3]
         point2.time_from_start = duration2.to_msg()
-
         point3 = JointTrajectoryPoint()
-        point3.positions = [0.6, 0.3, 0.0]
+        point3.positions = [0.9, 0.4, 0.0, -0.3]
         point3.time_from_start = duration3.to_msg()
-
         point4 = JointTrajectoryPoint()
-        point4.positions = [0.8, 0.2, 1.0]
+        point4.positions = [0.9, 0.4, 0.0, 0.0]
         point4.time_from_start = duration4.to_msg()
-
         point5 = JointTrajectoryPoint()
-        point5.positions = [0.5, 0.1, 0.0]
+        point5.positions = [0.4, 0.0, 1.54, 0.0]
         point5.time_from_start = duration5.to_msg()
-
         trajectory_goal = FollowJointTrajectory.Goal()
-        trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
-
+        trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw', 'joint_gripper_finger_left']
         trajectory_goal.trajectory.points = [point0, point1, point2, point3, point4, point5]
-
-        trajectory_goal.trajectory.header.stamp = self.get_clock().now().to_msg()
         trajectory_goal.trajectory.header.frame_id = 'base_link'
-
         self.trajectory_client.send_goal_async(trajectory_goal)
-        self.get_logger().info('Sent stow goal = {0}'.format(trajectory_goal))
-
-def main(args=None):
-    rclpy.init(args=args)
-
-    multipoint_command = MultiPointCommand()
-
-    rclpy.spin_once(multipoint_command)
-    multipoint_command.issue_multipoint_command()
-
-    rclpy.spin(multipoint_command)
-
-    multipoint_command.destroy_node()
-    rclpy.shutdown()
-
+        self.get_logger().info("Goal sent")
+        
+    def main(self):
+        self.issue_multipoint_command()
+
+def main():
+    try:
+        node = MultiPointCommand()
+        node.main()
+        node.new_thread.join()
+    except KeyboardInterrupt:
+        node.get_logger().info("Exiting")
+        node.destroy_node()
+        rclpy.shutdown()
 
 if __name__ == '__main__':
     main()
-
 ```
 
 ### The Code Explained.
-Seeing that there are similarities between the multipoint and stow command nodes, we will only breakdown the different components of the multipoint_command node.
+Seeing that there are similarities between the multipoint and stow command nodes, we will only breakdown the distinct components of the multipoint_command node.
 
 ```python
         point1 = JointTrajectoryPoint()
-        point1.positions = [0.3, 0.1, 2.0]
+        point1.positions = [0.9, 0.0, 0.0, 0.0] 
         point1.time_from_start = duration1.to_msg()
 ```
 Set *point1* as a `JointTrajectoryPoint`and provide desired positions (in meters). These are the positions of the lift, wrist extension, and yaw of the wrist, respectively.
@@ -325,14 +298,10 @@ Set *point1* as a `JointTrajectoryPoint`and provide desired positions (in meters
 
 ```python
         trajectory_goal = FollowJointTrajectory.Goal()
-        trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
-
+        trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw', 'joint_gripper_finger_left']
         trajectory_goal.trajectory.points = [point0, point1, point2, point3, point4, point5]
-
-        trajectory_goal.trajectory.header.stamp = self.get_clock().now().to_msg()
         trajectory_goal.trajectory.header.frame_id = 'base_link'
-
         self.trajectory_client.send_goal_async(trajectory_goal)
-        self.get_logger().info(9;Sent stow goal = {0}'.format(trajectory_goal))
+        self.get_logger().info(4;Goal sent")
 ```
-Set *trajectory_goal* as a `FollowJointTrajectory.Goal()` and define the joint names as a list. Then `trajectory_goal.trajectory.points` is defined by a list of the 6 points. Specify the coordinate frame that we want (base_link) and set the time to be now.
+Set *trajectory_goal* as a `FollowJointTrajectory.Goal()` and define the joint names as a list. Then `trajectory_goal.trajectory.points` is defined by a list of the 6 points.