Lab 28: Recording and publishing point clouds

You might want to work on developing perceptual algorithms for the robot even when you do not have direct access to the robot's sensor, as you are sharing the robot with other teams or you would like to work remotely. To enable such development a common workflow in robotics is to record representative sensory data and replay it later while testing your algorithms, before you test with the real sensor in the loop. This lab will help you get set up with that workflow to enable later labs for developing perceptual capabilities for Fetch. This workflow will allow us to spend most of our time working in simulation but with real sensor data.

Recording perception data

Set up a representative scene in front of the robot (the warehouse shelf with objects) and record a point cloud. Use RViz to make sure you can see most of the shelf, then run:

rosrun perception save_cloud shelf
rosbag info shelf.bag
mkdir ~/data
mv shelf.bag ~/data

Hallucinate the data

Now that we have saved a point cloud, let's hallucinate it in the scene.

Write the hallucinator

We will write a class in perception/src/perception/mock_camera.py that allows us to publish a saved point cloud.

camera = perception.MockCamera()
cloud = camera.read_cloud('~/data/shelf.bag')

Look at the rosbag code API and figure out how to read the point cloud from the bag file you saved:

class MockCamera(object): 
    """A MockCamera reads saved point clouds.
    """
    def __init__(self):
        pass

    def read_cloud(self, path):
        """Returns the sensor_msgs/PointCloud2 in the given bag file.
    
        Args:
            path: string, the path to a bag file with a single
            sensor_msgs/PointCloud2 in it.

        Returns: A sensor_msgs/PointCloud2 message, or None if there were no
            PointCloud2 messages in the bag file.
        """
        return None

Now let's set up the perception Python module by creating perception/src/perception/__init__.py:

from .mock_camera import MockCamera

Create a setup.py for your module in ~/catkin_ws/src/fetch-picker/perception/setup.py

## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD

from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup

# fetch values from package.xml
setup_args = generate_distutils_setup(
    packages=['perception'],
    package_dir={'': 'src'})

setup(**setup_args)

Finally, in perception/CMakeLists.txt, uncomment catkin_python_setup().

Now, run catkin build.

Add a demo file publish_saved_cloud.py to applications that just reads a point cloud and republishes it:

#!/usr/bin/env python 

from sensor_msgs.msg import PointCloud2
import perception
import rospy


def wait_for_time(): 
    """Wait for simulated time to begin.
    """
    while rospy.Time().now().to_sec() == 0:
        pass

    
def main():                                                                             
    rospy.init_node('publish_saved_cloud')
    wait_for_time()                                                                     
    argv = rospy.myargv()
    if len(argv) < 2:
        print 'Publishes a saved point cloud to a latched topic.'
        print 'Usage: rosrun applications publish_saved_cloud.py ~/cloud.bag'
        return
    path = argv[1]
    camera = perception.MockCamera()
    cloud = camera.read_cloud(path)

    if cloud is None:
        rospy.logerr('Could not load point cloud from {}'.format(path))
        return

    pub = rospy.Publisher('mock_point_cloud', PointCloud2, queue_size=1)       
    rate = rospy.Rate(2)
    while not rospy.is_shutdown():
        cloud.header.stamp = rospy.Time.now()
        pub.publish(cloud)
        rate.sleep()                                          
    
    
if __name__ == '__main__':
    main()

Run your code and check RViz to see your point cloud.

Note that this node just reads the saved data and continually publishes it. But you can do more. For example, suppose you have code like:

Go to Shelf
Look for soda can
Pick up soda can
Go to Table
Place soda can on Table

You can save two scenes: one where the robot sees the shelf, and one at the Table. Then, you can load the mock point cloud after arriving at each location:

if is_sim:
  cloud = camera.read_cloud('shelf.bag')
else:
  cloud = rospy.wait_for_message('head_camera/depth_registered/points', PointCloud2)

You can additionally transform the pre-recorded cloud to be at a particular location relative to the map or to the robot. You can visualize how the robot moves relative to the hallucinated data to get an idea of whether your code is correct. Once you have built some confidence in your system, you can test on the real robot.