Then run the tf2 broadcaster node to visualize three static frames.
```bash
@ -40,7 +39,6 @@ cd catkin_ws/src/stretch_tutorials/src/
python3 stow_command.py
```
<palign="center">
<imgsrc="images/tf2_broadcaster_with_stow.gif"/>
</p>
@ -134,8 +132,7 @@ import tf.transformations
from geometry_msgs.msg import TransformStamped
from tf2_ros import StaticTransformBroadcaster
```
You need to import rospy if you are writing a ROS Node. Import `tf.transformations` to get quaternion values from Euler angles. Import the `TransformStamped` from the `geometry_msgs.msg` package because we will be publishing static frames and it requires this message type. The `tf2_ros` package provides an implementation of a `tf2_ros.StaticTransformBroadcaster` to help make the task of publishing transforms easier.
You need to import rospy if you are writing a ROS [Node](http://wiki.ros.org/Nodes). Import `tf.transformations` to get quaternion values from Euler angles. Import the `TransformStamped` from the `geometry_msgs.msg` package because we will be publishing static frames and it requires this message type. The `tf2_ros` package provides an implementation of a `tf2_ros.StaticTransformBroadcaster` to help make the task of publishing transforms easier.
```python
def __init__(self):
@ -146,7 +143,6 @@ def __init__(self):
"""
self.br = StaticTransformBroadcaster()
```
Here we create a `TransformStamped` object which will be the message we will send over once populated.
We need to give the transform being published a timestamp, we'll just stamp it with the current time, `rospy.Time.now()`. Then, we need to set the name of the parent frame of the link we're creating, in this case *link_mast*. Finally, we need to set the name of the child frame of the link we're creating. In this instance, the child frame is *fk_link_mast*.
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 "/".
Instantiate the class with `FixedFrameBroadcaster()`
Instantiate the `FixedFrameBroadcaster()` class.
```python
rospy.spin()
@ -198,7 +193,6 @@ messages come in. If we don't put this line in, then the node will not work,
and ROS will not process any messages.
## tf2 Static Listener
In the previous section of the tutorial, we created a tf2 broadcaster to publish three static transform frames. In this section we will create a tf2 listener that will find the transform between *fk_link_lift* and *link_grasp_center*.
@ -208,7 +202,6 @@ Begin by starting up the stretch driver launch file.
# Terminal 1
roslaunch stretch_core stretch_driver.launch
```
Then run the tf2 broadcaster node to create the three static frames.
```bash
@ -216,7 +209,6 @@ Then run the tf2 broadcaster node to create the three static frames.
cd catkin_ws/src/stretch_tutorials/src/
python3 tf2_broadcaster.py
```
Finally, run the tf2 listener node to print the transform between two links.
```bash
@ -224,7 +216,6 @@ Finally, run the tf2 listener node to print the transform between two links.
cd catkin_ws/src/stretch_tutorials/src/
python3 tf2_listener.py
```
Within the terminal the transform will be printed every 1 second. Below is an example of what will be printed in the terminal. There is also an image for reference of the two frames.
```bash
@ -261,7 +252,7 @@ class FrameListener():
"""
def __init__(self):
"""
A function that initializes the variables and looks up a tranformation
A function that initializes the variables and looks up a transformation
between a target and source frame.
:param self: The self reference.
"""
@ -306,7 +297,7 @@ from geometry_msgs.msg import TransformStamped
import tf2_ros
```
You need to import rospy if you are writing a ROS Node. Import the `TransformStamped` from the `geometry_msgs.msg` package because we will be publishing static frames and it requires this message type. The `tf2_ros` package provides an implementation of a `tf2_ros.TransformListener` to help make the task of receiving transforms easier.
You need to import rospy if you are writing a ROS [Node](http://wiki.ros.org/Nodes). Import the `TransformStamped` from the `geometry_msgs.msg` package because we will be publishing static frames and it requires this message type. The `tf2_ros` package provides an implementation of a `tf2_ros.TransformListener` to help make the task of receiving transforms easier.
```python
tf_buffer = tf2_ros.Buffer()
@ -324,7 +315,6 @@ Store frame names in variables that will be used to compute transformations.
rospy.sleep(1.0)
rate = rospy.Rate(1)
```
The first line gives the listener some time to accumulate transforms. The second line is the rate the node is going to publish information (1 Hz).
rospy.logwarn(' Could not transform %s from %s ', to_frame_rel, from_frame_rel)
```
Try to look up the transform we want. Use a try-except block, since it may fail on any single call, due to internal timing issues in the transform publishers. Look up transform between *from_frame_rel* and *to_frame_rel* frames with the `lookup_transform()` function.
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 "/".
@ -8,15 +8,13 @@ Begin by running the following command in a new terminal.
# Terminal 1
roslaunch stretch_core stretch_driver.launch
```
Switch the mode to *position* mode using a rosservice call. The run the `sample_respeaker.launch`.
Switch the mode to *position* mode using a rosservice call. Then run the `respeaker.launch`.
```bash
# Terminal 2
rosservice call /switch_to_position_mode
roslaunch respeaker_ros sample_respeaker.launch
roslaunch stretch_core respeaker.launch
```
Then run the voice teleoperation base node in a new terminal.
```bash
@ -24,8 +22,8 @@ Then run the voice teleoperation base node in a new terminal.
cd catkin_ws/src/stretch_tutorials/src/
python3 voice_teleoperation_base.py
```
In terminal 3, a menu of voice commands is printed. You can reference this menu layout below.
```
------------ VOICE TELEOP MENU ------------
@ -251,15 +249,12 @@ class VoiceTeleopNode(hm.HelloNode):
self.send_command(command)
rate.sleep()
if __name__ == '__main__':
try:
node = VoiceTeleopNode()
node.main()
except KeyboardInterrupt:
rospy.loginfo('interrupt received, so shutting down')
```
### The Code Explained
@ -268,8 +263,7 @@ This code is similar to that of the [multipoint_command](https://github.com/hell
```python
#!/usr/bin/env python3
```
Every Python ROS [Node](http://wiki.ros.org/Nodes) will have this declaration at the top. The first line makes sure your script is executed as a Python3 script.
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 math
@ -283,10 +277,8 @@ from trajectory_msgs.msg import JointTrajectoryPoint
import hello_helpers.hello_misc as hm
from speech_recognition_msgs.msg import SpeechRecognitionCandidates
```
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. Import `sensor_msgs.msg` so that we can subscribe to JointState messages.
```python
class GetVoiceCommands:
```
@ -296,8 +288,7 @@ Create a class that subscribes to the speech to text recognition messages, print
self.step_size = 'medium'
self.rad_per_deg = math.pi/180.0
```
Set the default step size as medium and create a float value, self.rad_per_deg, to convert degrees to radians.
Set the default step size as medium and create a float value, *self.rad_per_deg*, to convert degrees to radians.
```python
self.small_deg = 5.0
@ -312,7 +303,6 @@ self.big_deg = 20.0
self.big_rad = self.rad_per_deg * self.big_deg
self.big_translate = 0.1
```
Define the three rotation and translation step sizes.
Initialize the voice command and sound direction to values that will not result in moving the base.
Set up two subscribers. The first one subscribes to the topic */speech_to_text*, looking for `SpeechRecognitionCandidates` messages. When a message comes in, ROS is going to pass it to the function `callback_speech` automatically. The second subscribes to */sound_direction* message and passes it to the `callback_direction` function.
The `callback_speech` stores the increment size for translational and rotational base motion to *inc*. The increment size is contingent on the *self.step_size* string value.
```python
@ -358,7 +344,6 @@ if self.voice_command == 'right':
In the `get_command()` function, the *command* is initialized as None, or set as a dictionary where the *joint* and *inc* values are stored. The *command* message type is dependent on the *self.voice_command* string value.
```python
@ -366,7 +351,6 @@ if (self.voice_command == "small") or (self.voice_command == "medium") or (self.
The `send_command` function stores the joint state message and uses a conditional statement to send joint trajectory goals. Also, assign *point* as a `JointTrajectoryPoint` message type.
Extract the joint name from the command dictionary.
```python
@ -431,27 +409,23 @@ inc = command['inc']
rospy.loginfo('inc = {0}'.format(inc))
new_value = inc
```
Extract the increment type from the command dictionary.
```python
point.positions = [new_value]
trajectory_goal.trajectory.points = [point]
```
Assign the new value position to the trajectory goal message type.
```python
self.trajectory_client.send_goal(trajectory_goal)
rospy.loginfo('Done sending command.')
```
Make the action call and send goal of the new joint position.
```python
self.speech.print_commands()
```
Reprint the voice command menu after the trajectory goal is sent.
```python
@ -466,9 +440,7 @@ def main(self):
rate = rospy.Rate(self.rate)
self.speech.print_commands()
```
The main function instantiates the HelloNode class, initializes the subscriber,
and call other methods in both the VoiceTeleopNode and GetVoiceCommands classes.
The main function instantiates the `HelloNode` class, initializes the subscriber, and call other methods in both the `VoiceTeleopNode` and `GetVoiceCommands` classes.
```python
while not rospy.is_shutdown():
@ -476,10 +448,8 @@ while not rospy.is_shutdown():
self.send_command(command)
rate.sleep()
```
Run a while loop to continuously check speech commands and send those commands to execute an action.
```python
try:
node = VoiceTeleopNode()
@ -487,7 +457,7 @@ try:
except KeyboardInterrupt:
rospy.loginfo('interrupt received, so shutting down')
```
Declare object from the VoiceTeleopNode class. Then execute the main() method/function.
Declare a `VoiceTeleopNode` object. Then execute the `main()` method.
**Previous Example** [Voice to Text](example_8.md)
**Next Example** [Tf2 Broadcaster and Listener](example_10.md)
Alan G. Sanchez
2 years ago