You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

167 lines
6.2 KiB

2 years ago
  1. ## Example 4
  2. <p align="center">
  3. <img src="https://raw.githubusercontent.com/hello-robot/stretch_tutorials/main/images/balloon.png"/>
  4. </p>
  5. Let's bringup stretch in the willowgarage world from the [gazebo basics tutorial](gazebo_basics.md) and RViz by using the following command.
  6. ```bash
  7. # Terminal 1
  8. roslaunch stretch_gazebo gazebo.launch world:=worlds/willowgarage.world rviz:=true
  9. ```
  10. the `rviz` flag will open an RViz window to visualize a variety of ROS topics. In a new terminal run the following commands to execute the [marker.py](https://github.com/hello-robot/stretch_tutorials/blob/main/src/marker.py) node.
  11. ```bash
  12. # Terminal 2
  13. cd catkin_ws/src/stretch_tutorials/src/
  14. python marker.py
  15. ```
  16. 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.
  17. <p align="center">
  18. <img src="https://raw.githubusercontent.com/hello-robot/stretch_tutorials/main/images/balloon.gif"/>
  19. </p>
  20. ### The Code
  21. ```python
  22. #!/usr/bin/env python
  23. import rospy
  24. from visualization_msgs.msg import Marker
  25. class Balloon():
  26. """
  27. A class that attaches a Sphere marker directly above the Stretch robot.
  28. """
  29. def __init__(self):
  30. """
  31. Function that initializes the marker's features.
  32. :param self: The self reference.
  33. """
  34. self.publisher = rospy.Publisher('balloon', Marker, queue_size=10)
  35. self.marker = Marker()
  36. self.marker.header.frame_id = 'base_link'
  37. self.marker.header.stamp = rospy.Time()
  38. self.marker.type = self.marker.SPHERE
  39. self.marker.id = 0
  40. self.marker.action = self.marker.ADD
  41. self.marker.scale.x = 0.5
  42. self.marker.scale.y = 0.5
  43. self.marker.scale.z = 0.5
  44. self.marker.color.r = 1.0
  45. self.marker.color.g = 0.0
  46. self.marker.color.b = 0.0
  47. self.marker.color.a = 1.0
  48. self.marker.pose.position.x = 0.0
  49. self.marker.pose.position.y = 0.0
  50. self.marker.pose.position.z = 2.0
  51. rospy.loginfo("Publishing the balloon topic. Use RViz to visualize.")
  52. def publish_marker(self):
  53. """
  54. Function that publishes the sphere marker.
  55. :param self: The self reference.
  56. :publishes self.marker: Marker message.
  57. """
  58. self.publisher.publish(self.marker)
  59. if __name__ == '__main__':
  60. rospy.init_node('marker')
  61. balloon = Balloon()
  62. rate = rospy.Rate(10)
  63. while not rospy.is_shutdown():
  64. balloon.publish_marker()
  65. rate.sleep()
  66. ```
  67. ### The Code Explained
  68. Now let's break the code down.
  69. ```python
  70. #!/usr/bin/env python
  71. ```
  72. 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.
  73. ```python
  74. import rospy
  75. from visualization_msgs.msg import Marker
  76. ```
  77. You need to import `rospy` if you are writing a ROS [Node](http://wiki.ros.org/Nodes). Import the `Marker` type from the `visualization_msgs.msg` package. This import is required to publish a `Marker`, which will be visualized in RViz.
  78. ```python
  79. self.pub = rospy.Publisher('balloon', Marker, queue_size=10)
  80. ```
  81. 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`.
  82. ```python
  83. self.marker = Marker()
  84. self.marker.header.frame_id = 'base_link'
  85. self.marker.header.stamp = rospy.Time()
  86. self.marker.type = self.marker.SPHERE
  87. ```
  88. Create a `Marker()` message type. 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)
  89. ```python
  90. self.marker.id = 0
  91. ```
  92. 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.
  93. ```python
  94. self.marker.action = self.marker.ADD
  95. ```
  96. This line of code sets the action. You can add, delete, or modify markers.
  97. ```python
  98. self.marker.scale.x = 0.5
  99. self.marker.scale.y = 0.5
  100. self.marker.scale.z = 0.5
  101. ```
  102. These are the size parameters for the marker. These will vary by marker type.
  103. ```python
  104. self.marker.color.r = 1.0
  105. self.marker.color.g = 0.0
  106. self.marker.color.b = 0.0
  107. ```
  108. Color of the object, specified as r/g/b/a, with values in the range of [0, 1].
  109. ```python
  110. self.marker.color.a = 1.0
  111. ```
  112. 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.
  113. ```python
  114. self.marker.pose.position.x = 0.0
  115. self.marker.pose.position.y = 0.0
  116. self.marker.pose.position.z = 2.0
  117. ```
  118. 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.
  119. ```python
  120. def publish_marker(self):
  121. self.publisher.publish(self.marker)
  122. ```
  123. Publish the Marker data structure to be visualized in RViz.
  124. ```python
  125. rospy.init_node('marker', argv=sys.argv)
  126. balloon = Balloon()
  127. rate = rospy.Rate(10)
  128. ```
  129. 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 "/".
  130. Instantiate class with `Balloon()`
  131. Give control to ROS with `rospy.spin()`. This will allow the callback to be called whenever new messages come in. If we don't put this line in, then the node will not work, and ROS will not process any messages.
  132. ```python
  133. while not rospy.is_shutdown():
  134. balloon.publish_marker()
  135. rate.sleep()
  136. ```
  137. 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.