Browse Source

Include code explanation in example.

pull/1/head
Alan Sanchez 3 years ago
parent
commit
d4cc5012b8
2 changed files with 108 additions and 6 deletions
  1. +106
    -3
      example_4.md
  2. +2
    -3
      src/marker.py

+ 106
- 3
example_4.md View File

@ -2,18 +2,27 @@
![image](images/balloon.png)
Let's bringup stretch in the willowgarage world from the [gazebo basics tutorial](gazebo_basics.md) and RViz by using the following command.
```bash
roslaunch stretch_gazebo gazebo.launch world:=worlds/willowgarage.world rviz:=true
```
python marker.py
the `rviz` flag will open an RViz window to visualize a variety of ROS topics. In a new terminal run the following commands to create a marker.
```bash
cd catkin_ws/src/stretch_ros_turotials/src/
python3 marker.py
```
The gif below demonstrates how to add a new *Marker* display type, and change the topic name from `visualization_marker` to `balloon`. A red sphere Marker should appear above the Stretch robot.
![image](images/balloon.gif)
### The Code
```python
#!/usr/bin/env python
import rospy
import sys
from visualization_msgs.msg import Marker
class Balloon():
@ -47,5 +56,99 @@ if __name__ == '__main__':
while not rospy.is_shutdown():
ballon.publish_marker()
rate.sleep() rate.sleep()
rate.sleep()
```
### The Code Explained
Now let's break the code down.
```python
#!/usr/bin/env python
```
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
from visualization_msgs.msg import Marker
```
You need to import rospy if you are writing a ROS Node. Import the `Marker` type from the visualization_msgs.msg package. This import is required to publish a Marker, which will be visualized in RViz.
```python
self.pub = rospy.Publisher('balloon', Marker, queue_size=10)
```
This section of code defines the talker's interface to the rest of ROS. pub = rospy.Publisher("balloon", Twist, queue_size=1) declares that your node is publishing to the */ballon* topic using the message type *Twist*.
```python
self.marker = Marker()
self.marker.header.frame_id = '/base_link'
self.marker.header.stamp = rospy.Time()
self.marker.type = self.marker.SPHERE
```
Create a maker. Markers of all shapes share a common type. Set the frame ID and type. The frame ID is the frame in which the position of the marker is specified. The type is the shape of the marker. Further details on marker shapes can be found here: [RViz Markers](http://wiki.ros.org/rviz/DisplayTypes/Marker)
```python
self.marker.id = 0
```
Each marker has a unique ID number. If you have more than one marker that you want displayed at a given time, then each needs to have a unique ID number. If you publish a new marker with the same ID number of an existing marker, it will replace the existing marker with that ID number.
```python
self.marker.action = self.marker.ADD
```
This line of code sets the action. You can add, delete, or modify markers.
```python
self.marker.scale.x = 0.5
self.marker.scale.y = 0.5
self.marker.scale.z = 0.5
```
These are the size parameters for the marker. These will vary by marker type.
```python
self.marker.color.r = 1.0
self.marker.color.g = 0.0
self.marker.color.b = 0.0
```
Color of the object, specified as r/g/b/a, with values in the range of [0, 1].
```python
self.marker.color.a = 1.0
```
The alpha value is from 0 (invisible) to 1 (opaque). If you don't set this then it will automatically default to zero, making your marker invisible.
```python
self.marker.pose.position.x = 0.0
self.marker.pose.position.y = 0.0
self.marker.pose.position.z = 2.0
```
Specify the pose of the marker. Since spheres are rotationally invariant, we're only going to specify the positional elements. As usual, these are in the coordinate frame named in frame_id. In this case, the position will always be directly 2 meters above the frame_id (*base_link*), and will move with it.
```python
def publish_marker(self):
self.publisher.publish(self.marker)
```
Publish the Marker data structure to be visualized in RViz.
```python
rospy.init_node('marker', argv=sys.argv)
ballon = Balloon()
rate = rospy.Rate(10)
```
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 `rospy.Rate()` function creates a Rate object rate. With the help of its method sleep(), it offers a convenient way for looping at the desired rate. With its argument of 10, we should expect to go through the loop 10 times per second (as long as our processing time does not exceed 1/10th of a second!)
```python
while not rospy.is_shutdown():
ballon.publish_marker()
rate.sleep()
```
This loop is a fairly standard rospy construct: checking the rospy.is_shutdown() flag and then doing work. You have to check is_shutdown() to check if your program should exit (e.g. if there is a Ctrl-C or otherwise). The loop calls rate.sleep(), which sleeps just long enough to maintain the desired rate through the loop.

+ 2
- 3
src/marker.py View File

@ -3,9 +3,8 @@
# This node will publish a spherical marker above the current robot position, as
# it drives around the world.
# Import rospy and sys, as usual
# Import rospy and sys
import rospy
import sys
# Import the Marker message type from the visualization_msgs package.
from visualization_msgs.msg import Marker
@ -62,7 +61,7 @@ class Balloon():
if __name__ == '__main__':
# Initialize the node, as usual
rospy.init_node('marker', argv=sys.argv)
rospy.init_node('marker')
ballon = Balloon()

Loading…
Cancel
Save