Add new introductory tutorials

1 year ago · ca22b4403d
--- a/ros2/intro_to_hellonode.md
+++ b/ros2/intro_to_hellonode.md
@ -0,0 +1,250 @@
 # Introduction to HelloNode

 HelloNode is a convenience class for creating a ROS 2 node for Stretch. The most common way to use this class is to extend it. In your extending class, the main funcion would call `HelloNode`'s main function. This would look like:

 ```python
 import hello_helpers.hello_misc as hm

 class MyNode(hm.HelloNode):
    def __init__(self):
        hm.HelloNode.__init__(self)

    def main(self):
        hm.HelloNode.main(self, 'my_node', 'my_node', wait_for_first_pointcloud=False)
        # my_node's main logic goes here

 node = MyNode()
 node.main()
 ```

 There is also a one-liner class method for instantiating a `HelloNode` for easy prototyping. One example where this is handy in sending pose commands from iPython:

 ```python
 # roslaunch the stretch launch file beforehand

 import hello_helpers.hello_misc as hm
 temp = hm.HelloNode.quick_create('temp')
 temp.move_to_pose({'joint_lift': 0.4})
 ```

 #### Attributes

 ##### `dryrun`

 This attribute allows you to control whether the robot actually moves when calling `move_to_pose()`, `home_the_robot()`, `stow_the_robot()`, or other motion methods in this class. When `dryrun` is set to True, these motion methods return immediately. This attribute is helpful when you want to run just the perception/planning part of your node without actually moving the robot. For example, you could replace the following verbose snippet:

 ```python
 # launch the stretch driver launch file beforehand
 import hello_helpers.hello_misc as hm
 temp = hm.HelloNode.quick_create('temp')
 actually_move = False
 [...]
 if actually_move:
    temp.move_to_pose({'translate_mobile_base': 1.0})
 ```

 to be more consise:

 ```python
 # launch the stretch driver launch file beforehand
 import hello_helpers.hello_misc as hm
 temp = hm.HelloNode.quick_create('temp')
 [...]
 temp.dryrun = True
 temp.move_to_pose({'translate_mobile_base': 1.0})
 ```

 #### Methods

 ##### `move_to_pose(pose, blocking=False, custom_contact_thresholds=False, duration=2.0)`

 This method takes in a dictionary that describes a desired pose for the robot and communicates with [stretch_driver](../stretch_core/README.md#stretchdrivernodesstretchdriver) to execute it. The basic format of this dictionary is string/number key/value pairs, where the keys are joint names and the values are desired position goals. For example, `{'joint_lift': 0.5}` would put the lift at 0.5m in its joint range. A full list of command-able joints is published to the `/stretch/joint_states` topic. Used within a node extending `HelloNode`, calling this method would look like:

 ```python
 self.move_to_pose({'joint_lift': 0.5})
 ```

 Internally, this dictionary is converted into a [JointTrajectory](https://docs.ros2.org/latest/api/trajectory_msgs/msg/JointTrajectory.html) message that is sent to a [FollowJointTrajectory action](http://docs.ros.org/en/noetic/api/control_msgs/html/action/FollowJointTrajectory.html) server in stretch_driver. This method waits by default for the server to report that the goal has completed executing. However, you can return before the goal has completed by setting the `blocking` argument to False. This can be useful for preempting goals.

 When the robot is in `position` mode, if you set `custom_contact_thresholds` to True, this method expects a different format dictionary: string/tuple key/value pairs, where the keys are still joint names, but the values are `(position_goal, effort_threshold)`. The addition of a effort threshold enables you to detect when a joint has made contact with something in the environment, which is useful for manipulation or safe movements. For example, `{'joint_arm': (0.5, 20)}` commands the telescoping arm fully out (the arm is nearly fully extended at 0.5 meters) but with a low enough effort threshold (20% of the arm motor's max effort) that the motor will stop when the end of arm has made contact with something. Again, in a node, this would look like:

 ```python
 self.move_to_pose({'joint_arm': (0.5, 40)}, custom_contact_thresholds=True)
 ```

 When the robot is in `trajectory` mode, if you set argument `duration` as `ts`, this method will ensure that the target joint positions are achieved over `ts` seconds. For example, the below would put the lift at 0.5m from its current position in `5.0` seconds:

 ```python
 self.move_to_pose({'joint_lift': 0.5}, duration=5.0)
 ```

 ##### `home_the_robot()`

 This is a convenience method to interact with the driver's [`/home_the_robot` service](../stretch_core/README.md#home_the_robot-std_srvstrigger).

 ##### `stow_the_robot()`

 This is a convenience method to interact with the driver's [`/stow_the_robot` service](../stretch_core/README.md#stow_the_robot-std_srvstrigger).

 ##### `stop_the_robot()`

 This is a convenience method to interact with the driver's [`/stop_the_robot` service](../stretch_core/README.md#stop_the_robot-std_srvstrigger).

 ##### `get_tf(from_frame, to_frame)`

 Use this method to get the transform ([geometry_msgs/TransformStamped](https://docs.ros2.org/latest/api/geometry_msgs/msg/TransformStamped.html)) between two frames. This method is blocking. For example, this method can do forward kinematics from the base_link to the link between the gripper fingers, link_grasp_center, using:

 ```python
 # launch the stretch driver launch file beforehand

 import hello_helpers.hello_misc as hm
 temp = hm.HelloNode.quick_create('temp')
 t = temp.get_tf('base_link', 'link_grasp_center')
 print(t.transform.translation)
 ```

 ##### `get_robot_floor_pose_xya(floor_frame='odom')`

 Returns the current estimated x, y position and angle of the robot on the floor. This is typically called with respect to the odom frame or the map frame. x and y are in meters and the angle is in radians.

 ```python
 # launch the stretch driver launch file beforehand

 import hello_helpers.hello_misc as hm
 temp = hm.HelloNode.quick_create('temp')
 t = temp.get_robot_floor_pose_xya(floor_frame='odom')
 print(t)
 ```

 ##### `main(node_name, node_topic_namespace, wait_for_first_pointcloud=True)`

 When extending the `HelloNode` class, call this method at the very beginning of your `main()` method. This method handles setting up a few ROS components, including registering the node with the ROS server, creating a TF listener, creating a [FollowJointTrajectory](http://docs.ros.org/en/noetic/api/control_msgs/html/action/FollowJointTrajectory.html) client for the [`move_to_pose()`](#movetoposepose-returnbeforedonefalse-customcontactthresholdsfalse-customfullgoalfalse) method, subscribing to depth camera point cloud topic, and connecting to the quick-stop service.

 Since it takes up to 30 seconds for the head camera to start streaming data, the `wait_for_first_pointcloud` argument will get the node to wait until it has seen camera data, which is helpful if your node is processing camera data.

 ##### `quick_create(name, wait_for_first_pointcloud=False)`

 A class level method for quick testing. This allows you to avoid having to extend `HelloNode` to use it.

 ```python
 # launch the stretch driver launch file beforehand

 import hello_helpers.hello_misc as hm
 temp = hm.HelloNode.quick_create('temp')
 temp.move_to_pose({'joint_lift': 0.4})
 ```

 #### Subscribed Topics

 ##### /camera/depth/color/points ([sensor_msgs/PointCloud2](https://docs.ros2.org/latest/api/sensor_msgs/msg/PointCloud2.html))

 Provides a point cloud as currently seen by the Realsense depth camera in Stretch's head. Accessible from the `self.point_cloud` attribute.

 ```python
 # launch the stretch driver launch file beforehand

 import hello_helpers.hello_misc as hm
 temp = hm.HelloNode.quick_create('temp', wait_for_first_pointcloud=True)
 print(temp.point_cloud)
 ```

 ##### /stretch/joint_states ([sensor_msgs/JointState](https://docs.ros2.org/latest/api/sensor_msgs/msg/JointState.html))

 Provides the current state of robot joints that includes joint names, positions, velocities, efforts. Accessible from the `self.joint_state` attribute.
 ```python
 print(temp.joint_state)
 ```

 ##### /mode ([std_msgs/String](https://docs.ros2.org/latest/api/std_msgs/msg/String.html))

 Provides the mode the stretch driver is currently in. Possible values include `position`, `trajectory`, `navigation`, `homing`, `stowing`.
 ```python
 print(temp.mode)
 ```

 ##### /tool ([std_msgs/String](https://docs.ros2.org/latest/api/std_msgs/msg/String.html))

 Provides the end of arm tool attached to the robot.
 ```python
 print(temp.tool)
 ```

 #### Subscribed Services

 ##### /stop_the_robot ([std_srvs/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html))

 Provides a service to quickly stop any motion currently executing on the robot.

 ```python
 # launch the stretch driver launch file beforehand

 from std_srvs.srv import TriggerRequest
 import hello_helpers.hello_misc as hm
 temp = hm.HelloNode.quick_create('temp')
 temp.stop_the_robot_service(TriggerRequest())
 ```

 ##### /stow_the_robot ([std_srvs/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html))

 Provides a service to stow the robot arm.

 ```python
 # launch the stretch driver launch file beforehand

 from std_srvs.srv import TriggerRequest
 import hello_helpers.hello_misc as hm
 temp = hm.HelloNode.quick_create('temp')
 temp.stow_the_robot_service(TriggerRequest())
 ```

 ##### /home_the_robot ([std_srvs/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html))

 Provides a service to home the robot joints.

 ```python
 # launch the stretch driver launch file beforehand

 from std_srvs.srv import TriggerRequest
 import hello_helpers.hello_misc as hm
 temp = hm.HelloNode.quick_create('temp')
 temp.home_the_robot_service(TriggerRequest())
 ```

 ##### /switch_to_trajectory_mode ([std_srvs/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html))

 Provides a service to quickly stop any motion currently executing on the robot.

 ```python
 # launch the stretch driver launch file beforehand

 from std_srvs.srv import TriggerRequest
 import hello_helpers.hello_misc as hm
 temp = hm.HelloNode.quick_create('temp')
 temp.switch_to_trajectory_mode_service(TriggerRequest())
 ```

 ##### /switch_to_position_mode ([std_srvs/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html))

 Provides a service to quickly stop any motion currently executing on the robot.

 ```python
 # launch the stretch driver launch file beforehand

 from std_srvs.srv import TriggerRequest
 import hello_helpers.hello_misc as hm
 temp = hm.HelloNode.quick_create('temp')
 temp.switch_to_position_mode_service(TriggerRequest())
 ```

 ##### /switch_to_navigation_mode ([std_srvs/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html))

 Provides a service to quickly stop any motion currently executing on the robot.

 ```python
 # launch the stretch driver launch file beforehand

 from std_srvs.srv import TriggerRequest
 import hello_helpers.hello_misc as hm
 temp = hm.HelloNode.quick_create('temp')
 temp.switch_to_navigation_mode_service(TriggerRequest())
 ```
--- a/ros2/intro_to_ros2.md
+++ b/ros2/intro_to_ros2.md
@ -0,0 +1,163 @@
 # Introduction to ROS 2
 In this tutorial we will explore rclpy, the client library for interacting with ROS 2 using the Python API. The rclpy library forms the base of ROS 2 and you will notice that all tutorials in the following sections will use it. In this section we will focus on a few common constructs of rclpy and then follow some examples using the IPython interpreter to get familiar with them.

 ## IPython
 It is not always necessary to write a functional Python script while prototyping or exploring a new library. It's instructive and helpful to use what’s called an REPL (Read-Eval-Print Loop), which quickly allows us to execute Python instructions and see their output immediately. This allows better understanding of what each instruction does and is often a great way to debug unexpected behavior. IPython is a command line based interactive interpreter for Python that uses REPL and can be used to run Python snippets for quick prototyping.

 To run IPython in a terminal, simply execute:
 ```{.bash .shell-prompt}
 python3 -m IPython
 ```

 Try out the following snippets for a ROS 2 quickstart:

 ## Initializationa and Shutdown
 ### rclpy.init()
 All rclpy functionality can be exposed after initialization:
 ```{.bash .shell-prompt}
 import rclpy

 rclpy.init()
 ```

 ### rclpy.create_node()
 To create a new ROS 2 node, one can use the create_node method with the node name as the argument:
 ```{.bash .shell-prompt}
 node = rclpy.create_node('temp')
 ```

 ### rclpy.logging.get_logger()
 The rclpy library also provides a logger to print messages with different severity levels to stdout. Here’s how you can use it:
 ```{.bash .shell-prompt}
 import rclpy.logging
 logger = rclpy.logging.get_logger('temp')
 logger.info("Hello")
 logger.warn("Robot")
 logger.error("Stretch")
 ```

 ### rclpy.ok()
 If you want to check whether rclpy has been initialized, you can run the following snippet. This is especially useful to simulate an infinite loop based on whether rclpy has been shutdown.
 ```{.bash .shell-prompt}
 import time

 while rclpy.ok():
 	print("Hello")
 	time.sleep(1.0)
 ```

 Press ctrl+c to get out of the infinite loop.

 ### rclpy.shutdown()
 Finally, to destroy a node safely and shutdown the instance of rclpy you can run:

 ```{.bash .shell-prompt}
 node.destroy_node()
 rclpy.shutdown()
 ```

 ## Publishing and subscribing
 ### create_publisher()
 ROS 2 is a distributed communication system and one way to send data is through a publisher. It takes the following arguments: msg_type, msg_topic and a history depth (formerly queue_size):
 ```{.bash .shell-prompt}
 from std_msgs.msg import String
 import rclpy

 rclpy.init()
 node = rclpy.create_node('temp')
 pub = node.create_publisher(String, 'hello', 10)
 ```

 ### create_subscription()
 To receive a message, we need to create a subscriber with a callback function that listens to the arriving messages. Let's create a subscriber and define a callback called hello_callback() that logs the a message as soon as one is received:
 ```{.bash .shell-prompt}
 def hello_callback(msg):
 	print("Received message: {}".format(msg.data))

 sub = node.create_subscription(String, 'hello', hello_callback, 10)
 ```

 ### publish()
 Now that you have defined a publisher and a subscriber, let’s send a message and see if it gets printed to the console:
 ```{.bash .shell-prompt}
 msg = String()
 msg.data = "Hello"
 pub.publish(msg)
 ```

 ### rclpy.spin_once()
 That didn’t do it! Although the message was sent, it didn't get printed to the console. Why? Because the hello_callback() method was never called to print the message. In ROS, we don’t call this method manually, but rather leave it to what’s called the executor. The executor can be invoked by calling the spin_once() method. We pass the node object and a timeout of 2 seconds as the arguments. The timeout is important because the spin_once() method is blocking and it will wait for a message to arrive indefinitely if a timeout is not defined. It returns immediately once a message is received.
 ```{.bash .shell-prompt}
 rclpy.spin_once(node, timeout_sec=2.0)
 ```

 ### rclpy.spin()
 The spin_once() method only does work equivalent to a single message callback. What if you want the executor to process callbacks continuously? This can be achieved using the spin() method. While retaining the current interpreter instance, let’s open a new terminal window with a new instance of IPython and execute the following:

 Terminal 2:
 ```{.bash .shell-prompt}
 import rclpy
 from std_msgs.msg import String
 rclpy.init()
 node = rclpy.create_node('temp2')
 def hello_callback(msg):
 	print("I heard: {}".format(msg.data))
 sub = node.create_subscription(String, 'hello', hello_callback, 10)
 rclpy.spin(node)
 ```

 Now, from the first IPython instance, send a series of messages and see what happens:

 Terminal 1:
 ```{.bash .shell-prompt}
 for i in range(10):
 	msg.data = "Hello {}".format(i)
 	pub.publish(msg)
 ```

 Voila! Finally, close both the terminals to end the session.

 ## Service Server and Client
 ### create_service()
 Let’s explore another common way of using ROS 2. Imagine a case where you need to request some information from a node and you expect to receive a response. This can be achieved using the service client paradigm in ROS 2. Let’s fire up IPython again and create a quick service:
 ```{.bash .shell-prompt}
 import rclpy
 from example_interfaces.srv import AddTwoInt
 rclpy.init()

 def add_ints(req, res):
 	print("Received request")
 	res.sum = req.a + req.b
 	return res

 node = rclpy.create_node('temp')
 srv = node.create_service(AddTwoInts, 'add_ints', add_ints)

 # you need to spin to receive the request
 rclpy.spin_once(node, timeout_sec=2.0)
 ```

 The add_ints() method is the callback method for the service server. Once a service request is received, this method will act on it to generate the response. Since a service request is a ROS message, we need to invoke the executor with a spin method to receive the message.

 ### create_client()
 Now, while retaining the current IPython session, open another session of the IPython interpreter in another terminal to write the service client:
 ```{.bash .shell-prompt}
 import rclpy
 from example_interfaces.srv import AddTwoInt
 rclpy.init()
 node = rclpy.create_node('temp2')

 cli = node.create_client(AddTwoInts, 'add_ints')

 req = AddTwoInts.Request()
 req.a = 10
 req.b = 20

 req_future = cli.call_async(req)
 rclpy.spin_until_future_complete(node, req_future, timeout_sec=2.0)

 print("Received response: {}".format(req_future.result().sum)
 ```

 ### rclpy.spin_until_future_complete()
 Notice that the spin method manifests itself as the spin_until_future_complete() method which takes the node, future and timeout_sec as the arguments. The future is an object in ROS 2 that’s returned immediately after an async service call has been made. We can then wait on the result of this future. This way the call to the service is not blocking and the code execution can continue as soon as the service call is issued.
--- a/tutorial_template.md
+++ b/tutorial_template.md
@ -38,12 +38,12 @@ Running this tutorial on Stretch might result in *harm to robot, humans or the s
 ## See It In Action
 Go ahead and execute the following commands to run the demo and visualize the result in RViz:
 Terminal 1:
 ```bash
 ```{.bash .shell-prompt}
 Enter first command here
 ```

 Terminal 2:
 ```bash
 ```{.bash .shell-prompt}
 Enter second command here
 ```