diff --git a/example_11.md b/example_11.md new file mode 100644 index 0000000..f1cb0e9 --- /dev/null +++ b/example_11.md @@ -0,0 +1,235 @@ +## Example 11 + +This tutorial highlights how to create a [PointCloud](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointCloud.html) message from the data of a [PointCloud2](http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/PointCloud2.html) message type, then transform the PointCloud's reference link to a different frame. The data published by the RealSense is referencing its *camera_color_optical_frame* link, and we will be changing its reference to the *base_link*. + +Begin by starting up the stretch driver launch file. + +```bash +# Terminal 1 +roslaunch stretch_core stretch_driver.launch +``` + +To activate the RealSense camera and publish topics to be visualized, run the following launch file in a new terminal. + + +```bash +# Terminal 2 +roslaunch stretch_core d435i_low_resolution.launch +``` + +Then run the `PointCloud` transformer node. + +```bash +# Terminal 3 +cd catkin_ws/src/stretch_tutorials/src/ +python pointcloud_transformer.py +``` + +Within this tutorial package, there is an RViz config file with the `PointCloud` in the Display tree. You can visualize this topic and the robot model by running the command below in a new terminal. + +```bash +# Terminal 4 +rosrun rviz rviz -d /home/hello-robot/catkin_ws/src/stretch_tutorials/rviz/PointCloud_transformer_example.rviz +``` + +The gif below visualizes what happens when running the previous node. + +

+ +

+ + + +### The Code + +```python +#!/usr/bin/env python +import rospy +import tf +import sensor_msgs.point_cloud2 as pc2 +from sensor_msgs.msg import PointCloud2, PointCloud +from geometry_msgs.msg import Point32 +from std_msgs.msg import Header + +class PointCloudTransformer: + """ + A class that takes in a PointCloud2 message and stores its points into a + PointCloud message. Then that PointCloud is transformed to reference the + 'base_link' frame. + """ + def __init__(self): + """ + Function that initializes the subscriber, publisher, and other variables. + :param self: The self reference. + """ + self.pointcloud2_sub = rospy.Subscriber("/camera/depth/color/points", PointCloud2, self.callback_pcl2, queue_size=1) + self.pointcloud_pub = rospy.Publisher("/camera_cloud", PointCloud, queue_size=1) + self.pcl2_cloud = None + self.listener = tf.TransformROS(True, rospy.Duration(10.0)) + rospy.loginfo('Publishing transformed PointCloud. Use RViz to visualize') + + def callback_pcl2(self,msg): + """ + Callback function that stores the PointCloud2 message. + :param self: The self reference. + :param msg: The PointCloud2 message type. + """ + self.pcl2_cloud = msg + + def pcl_transformer(self): + """ + A function that extracts the points from the stored PointCloud2 message + and appends those points to a PointCloud message. Then the function transforms + the PointCloud from its the header frame id, 'camera_color_optical_frame' + to the 'base_link' frame. + :param self: The self reference. + """ + temp_cloud = PointCloud() + temp_cloud.header = self.pcl2_cloud.header + for data in pc2.read_points(self.pcl2_cloud, skip_nans=True): + temp_cloud.points.append(Point32(data[0],data[1],data[2])) + + transformed_cloud = self.transform_pointcloud(temp_cloud) + self.pointcloud_pub.publish(transformed_cloud) + + def transform_pointcloud(self,msg): + """ + Function that stores the PointCloud2 message. + :param self: The self reference. + :param msg: The PointCloud message. + + :returns new_cloud: PointCloud message. + """ + while not rospy.is_shutdown(): + try: + new_cloud = self.listener.transformPointCloud("/base_link" ,msg) + return new_cloud + if new_cloud: + break + except (tf.LookupException, tf.ConnectivityException,tf.ExtrapolationException): + pass + +if __name__=="__main__": + rospy.init_node('pointcloud_transformer',anonymous=True) + PCT = PointCloudTransformer() + rate = rospy.Rate(1) + rospy.sleep(1) + + while not rospy.is_shutdown(): + PCT.pcl_transformer() + 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 +import tf +import sensor_msgs.point_cloud2 as pc2 +from sensor_msgs.msg import PointCloud2, PointCloud +from geometry_msgs.msg import Point32 +from std_msgs.msg import Header +``` + +You need to import `rospy` if you are writing a ROS Node. Import `tf` to utilize the `transformPointCloud` function. Import various the message types from `sensor_msgs`. + +```python +self.pointcloud2_sub = rospy.Subscriber("/camera/depth/color/points", PointCloud2, self.callback_pcl2, queue_size=1) +``` +Set up a subscriber. We're going to subscribe to the topic */camera/depth/color/points*, looking for `PointCloud2` message. When a message comes in, ROS is going to pass it to the function `callback_pcl2()` automatically. + +```python +self.pointcloud_pub = rospy.Publisher("/camera_cloud", PointCloud, queue_size=1) +``` + +This section of code defines the talker's interface to the rest of ROS. `self.pointcloud_pub = rospy.Publisher("/camera_cloud", PointCloud, queue_size=1)` declares that your node is publishing to the */camera_cloud* topic using the message type `PointCloud`. + +```python +self.pcl2_cloud = None +self.listener = tf.TransformListener(True, rospy.Duration(10.0)) +``` + +The first line of code initializes *self.pcl2_cloud* to store the `PointCloud2` message. The second line creates a `tf.TransformListener` object. Once the listener is created, it starts receiving tf transformations over the wire, and buffers them for up to 10 seconds. + +```python +def callback_pcl2(self,msg): + """ + Callback function that stores the PointCloud2 message. + :param self: The self reference. + :param msg: The PointCloud2 message type. + """ + self.pcl2_cloud = msg +``` +The callback function that stores the the `PointCloud2` message. + +```python +temp_cloud = PointCloud() +temp_cloud.header = self.pcl2_cloud.header +``` + +Create a `PointCloud` for temporary use. Set the temporary PointCloud's header to the stored PointCloud2 header. + +```python +for data in pc2.read_points(self.pcl2_cloud, skip_nans=True): + temp_cloud.points.append(Point32(data[0],data[1],data[2])) +``` + +Use a for loop to extract `PointCloud2` data into a list of x, y, z points and append those values to the `PointCloud` message, *temp_cloud*. + +```python +transformed_cloud = self.transform_pointcloud(temp_cloud) +``` + +Utilize the `transform_pointcloud` function to transform the points in the `PointCloud` message to reference the *base_link* + +```python +while not rospy.is_shutdown(): + try: + new_cloud = self.listener.transformPointCloud("/base_link" ,msg) + return new_cloud + if new_cloud: + break + except (tf.LookupException, tf.ConnectivityException,tf.ExtrapolationException): + pass +``` + +Try to look up and transform the `PointCloud` input. Use a try-except block, since it may fail on any single call, due to internal timing issues in the transform publishers. Transform the point cloud data from *camera_color_optical_frame* to *base_link* with the `transformPointCloud()` function. + + +```python +self.pointcloud_pub.publish(transformed_cloud) +``` + +Publish the new transformed `PointCloud`. + +```python +rospy.init_node('pointcloud_transformer',anonymous=True) +PCT = PointCloudTransformer() + +``` +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 "/". + +Declare object, *PCT*, from the `PointCloudTransformer` class. + +```python +rate = rospy.Rate(1) +rospy.sleep(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 +# Run while loop until the node is shutdown + while not rospy.is_shutdown(): + + # Run the pcl_transformer method + PCT.pcl_transformer() + rate.sleep() +``` +Run a while loop until the node is shutdown. Within the while loop run the `pcl_transformer()` method. diff --git a/images/PointCloud_transformer.gif b/images/PointCloud_transformer.gif new file mode 100644 index 0000000..d9096b6 Binary files /dev/null and b/images/PointCloud_transformer.gif differ diff --git a/rviz/PointCloud_transformer_example.rviz b/rviz/PointCloud_transformer_example.rviz new file mode 100644 index 0000000..736eac7 --- /dev/null +++ b/rviz/PointCloud_transformer_example.rviz @@ -0,0 +1,368 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Status1 + - /RobotModel1/Links1/camera_bottom_screw_frame1 + - /PointCloud1 + Splitter Ratio: 0.6395061612129211 + Tree Height: 915 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra1_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra2_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_arm_l0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_arm_l1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_arm_l2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_arm_l3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_arm_l4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_aruco_inner_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_aruco_left_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_aruco_right_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_aruco_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_aruco_top_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_grasp_center: + Alpha: 1 + Show Axes: false + Show Trail: false + link_gripper: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_gripper_finger_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_gripper_finger_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_gripper_fingertip_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_gripper_fingertip_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_head: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_head_pan: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_head_tilt: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_lift: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_mast: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_wrist_yaw: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + respeaker_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /camera/depth/color/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /camera_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 3.6434335708618164 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.23329290747642517 + Y: -0.838670551776886 + Z: 1.411441683769226 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.14979790151119232 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.530461549758911 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1212 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000015c0000041efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000041e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061000000022f000000c70000000000000000fb0000000a0049006d00610067006500000002920000026d0000000000000000fb0000000c00430061006d0065007200610000000429000000d60000000000000000000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000056f0000003efc0100000002fb0000000800540069006d006501000000000000056f000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000040d0000041e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1391 + X: 980 + Y: 201