To start adding an object, we need to have a planning scene; this is only possible when MoveIt! is running, so the first step is to start Gazebo, MoveIt!, the controllers, and RViz. Since the planning scene only exists in MoveIt!. RViz is required to be able to visualize objects contained in it. In order to launch all of the required modules, we need to run the following commands:

$ roslaunch rosbook_arm_gazebo rosbook_arm_empty_world.launch
$ roslaunch rosbook_arm_moveit_config moveit_RViz.launch config:=true

The snippet of code then starts by instantiating the planning scene interface object, which can be used to perform actions on the planning scene itself:

moveit::planning_interface::PlanningSceneInterface current_scene;

The next step is to create the collision object message that we want to send through the planning scene interface. The first thing we need to provide for the collision object is a name, which will uniquely identify this object and will allow us to perform actions on it, such as removing it from the scene once we're done with it:

moveit_msgs::CollisionObject box;

box.id = "rosbook_box";

The next step is to provide the properties of the object itself. This is done through a solid primitive message, which specifies the type of object we are creating, and depending on the type of object, it also specifies its properties. In our case, we are simply creating a box, which essentially has three dimensions:

shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.2;
primitive.dimensions[1] = 0.2;
primitive.dimensions[2] = 0.2;

To continue, we need to provide the pose of the box in the planning scene. Since we want to produce a possible collision scenario, we have placed the box close to our robotic arm. The pose itself is specified with a pose message from the standard geometry messages package:

geometry_msgs::Pose pose;
pose.orientation.w = 1.0;
pose.position.x =  0.7;
pose.position.y = -0.5;
pose.position.z =  1.0;

We then add the primitive and the pose to the message and specify that the operation we want to perform is to add it to the planning scene:

box.primitives.push_back(primitive);
box.primitive_poses.push_back(pose);
box.operation = box.ADD;

Finally, we add the collision object to a vector of collision object messages and call the addCollisionObjects method from the planning scene interface. This will take care of sending the required messages through the appropriate topics, in order to ensure that the object is created in the current planning scene:

std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(box);

current_scene.addCollisionObjects(collision_objects);

We can test this snippet by running the following command in a terminal, as said earlier. Since the object is added to the planning scene, it is important to have the RViz visualization running; otherwise, the reader won't be able to see the object:

$ rosrun rosbook_arm_snippets move_group_add_object

The result can be seen in Figure 16 as a simple, green, squared box in the middle of the way between the arm's goal and the current position of the arm:

Figure 16: Scene collision object in RViz

Removing the added object from the planning scene is a very simple process. Following the same initialization as in the previous example, we only need to create a string vector containing the IDs of the objects we want to remove and call the removeCollisionObjects function from the planning scene interface:

std::vector<std::string> object_ids;
object_ids.push_back("rosbook_box");
current_scene.removeCollisionObjects(object_ids);

We can test this snippet by running the following command, which will remove the object created with the previous snippet from the planning scene:

$ rosrun rosbook_arm_snippets move_group_remove_object

Alternatively, we can also use the Scene objects tab in the RViz plugin to remove any objects from the scene.

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 enough information to MoveIt! to plan while 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_conifg/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 on Figure 17. 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:

Figure 17: Gazebo simulation (left), point cloud in RViz (right)