diff --git a/example_10.md b/example_10.md index b8b058a..cea0163 100644 --- a/example_10.md +++ b/example_10.md @@ -18,7 +18,6 @@ Within this tutorial package, there is an RViz config file with the topics for t # Terminal 2 rosrun rviz rviz -d /home/hello-robot/catkin_ws/src/stretch_tutorials/rviz/tf2_broadcaster_example.rviz ``` - 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 ``` -
@@ -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. ```python @@ -155,7 +151,6 @@ self.mast.header.stamp = rospy.Time.now() self.mast.header.frame_id = 'link_mast' self.mast.child_frame_id = 'fk_link_mast' ``` - 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*. ```python @@ -186,9 +181,9 @@ rospy.init_node('tf2_broadcaster') FixedFrameBroadcaster() ``` -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). ```python @@ -337,7 +327,6 @@ try: except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): 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. ```python @@ -345,9 +334,9 @@ rospy.init_node('tf2_listener') FrameListener() ``` -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 `FrameListener()` +Instantiate the `FrameListener()` class. ```python rospy.spin() diff --git a/example_9.md b/example_9.md index 1d30de1..76d930b 100644 --- a/example_9.md +++ b/example_9.md @@ -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. ```python @@ -321,16 +311,13 @@ self.sound_direction = 0 self.speech_to_text_sub = rospy.Subscriber("/speech_to_text", SpeechRecognitionCandidates, self.callback_speech) self.sound_direction_sub = rospy.Subscriber("/sound_direction", Int32, self.callback_direction) ``` - 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. - ```python self.sound_direction = msg.data * -self.rad_per_deg ``` - The `callback_direction` function converts the *sound_direction* topic from degrees to radians. ```python @@ -342,7 +329,6 @@ if self.step_size == 'big': inc = {'rad': self.big_rad, 'translate': self.big_translate} return inc ``` - 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': if self.voice_command == 'stretch': command = {'joint': 'rotate_mobile_base', 'inc': self.sound_direction} ``` - 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. self.step_size = self.voice_command rospy.loginfo('Step size = {0}'.format(self.step_size)) ``` - Based on the *self.voice_command* value, set the step size for the increments. ```python @@ -374,10 +358,8 @@ if self.voice_command == 'quit': rospy.signal_shutdown("done") sys.exit(0) ``` - If the *self.voice_command* is equal to "quit", then initiate a clean shutdown of ROS and exit the Python interpreter. - ```python class VoiceTeleopNode(hm.HelloNode): """ @@ -394,8 +376,7 @@ class VoiceTeleopNode(hm.HelloNode): self.joint_state = None self.speech = GetVoiceCommands() ``` - -A class that inherits the HelloNode class from hm, declares object from the GetVoiceCommands class, and sends joint trajectory commands. +A class that inherits the `HelloNode` class from `hm`, declares object from the `GetVoiceCommands` class, and sends joint trajectory commands. ```python def send_command(self, command): @@ -409,21 +390,18 @@ def send_command(self, command): point = JointTrajectoryPoint() point.time_from_start = rospy.Duration(0.0) ``` - 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. ```python trajectory_goal = FollowJointTrajectoryGoal() trajectory_goal.goal_time_tolerance = rospy.Time(1.0) ``` - Assign *trajectory_goal* as a `FollowJointTrajectoryGoal` message type. ```python joint_name = command['joint'] trajectory_goal.trajectory.joint_names = [joint_name] ``` - 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)