@ -10,7 +10,6 @@ Begin by starting up the stretch driver launch file.
# 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.
**IMPORTANT NOTE:** Stretch's 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. Further, you **can not** actuate an individual arm joint. Here is an image of the arm joints for reference:
print_states function to deal with the incoming JointState messages.
:param self: The self reference.
:param joints: A list of joint names.
:param joints: A list of string values of joint names.
"""
joint_positions = []
for joint in joints:
@ -95,13 +93,12 @@ Now let's break the code down.
```
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.
You need to import `rospy` if you are writing a ROS [Node](http://wiki.ros.org/Nodes). Import `sensor_msgs.msg` so that we can subscribe to `JointState` messages.
@ -112,12 +109,11 @@ Set up a subscriber. We're going to subscribe to the topic "*joint_states*", lo
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)
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.
@ -136,7 +132,6 @@ In this section of the code, a for loop is used to parse the names of the reques
rospy.signal_shutdown("done")
sys.exit(0)
```
The first line of code initiates a clean shutdown of ROS. The second line of code exits the Python interpreter.
The next line, `rospy.init_node(NAME, ...)`, is very important as it tells rospy the name of your node -- until rospy has this information, it cannot start communicating with the ROS Master. In this case, your node will take on the name talker. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/".
The next line, `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. **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).
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()`method).