Motion planning with point clouds is much simpler than it would appear to be. The main thing to take into account is that we need to provide a point cloud feed as well as tell MoveIt! to take this into account when performing planning. The Gazebo simulation we have set up for this chapter already contains an RGBD sensor, which publishes a point cloud for us. To start with this example, let's launch the following commands:

    $ roslaunch rosbook_arm_gazebo rosbook_arm_grasping_world.launch
    $ roslaunch rosbook_arm_moveit_config moveit_rviz.launch config:=true  

The user might have noticed that the Gazebo simulation now appears to include several objects in the world. Those objects are scanned by an RGBD sensor, and the resulting point cloud is published to the /rgbd_camera/depth/points topic. What we need to do in this case is tell MoveIt! where to get the information from and what the format of that information is. The first file we need to modify is the following one:

rosbook_arm_moveit_conifg/config/sensors_rgbd.yaml 

This file will be used to store the information of the RGBD sensor. In this file, we need to tell MoveIt! which plugin it needs to use to manage the point cloud as well as some other parameters specific to the sensor plugin itself. In this particular case, the plugin to use is Octomap Updater, which will generate an octomap with the point cloud provided, downsample it, and publish the resulting cloud. With this first step we have set up a plugin which will provide MoveIt! with enough information to plan, while also taking into account possible collisions with the point cloud:

sensors: 
- sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater 
  point_cloud_topic: /rgbd_camera/depth/points 
  max_range: 10 
  padding_offset: 0.01 
  padding_scale: 1.0 
  point_subsample: 1 
  filtered_cloud_topic: output_cloud 

As you might have suspected, the file itself is nothing more than a configuration file. The next step we need to perform is to load this configuration file into the environment so that MoveIt! is aware of the new sensor we have added. In order to do so, we will need to modify the following XML file:

    $ rosbook_arm_moveit_config/launch/ rosbook_arm_moveit_sensor_manager.launch.xml

In this XML file, we can potentially specify a few parameters that will be used by the sensor plugin, such as the cloud resolution and the frame of reference. It is important to take into account that some of these parameters might be redundant and can be omitted. Finally, we need to add a command to load the configuration file into the environment:

<launch> 
  <rosparam command="load" file="$(find 
   rosbook_arm_moveit_config)/config/sensors_rgbd.yaml" /> 
</launch> 

The result of running the commands specified in the beginning with the new changes we have added can be seen in the following screenshot. In this particular case, we can see both the Gazebo simulation and the RViz visualization. The RViz simulation contains a point cloud, and we have already performed some manual motion planning, which successfully took the point cloud into account to avoid any collisions: