Perception Pipeline Tutorial¶

Add the YAML file to the launch script¶

You will now need to update the sensor_manager.launch file in the “launch” directory of your panda_moveit_config directory with this sensor information (this file is auto-generated by the Setup Assistant but is empty). You will need to add the following line into that file to configure the set of sensor sources for MoveIt to use:

<rosparam command="load" file="$(find panda_moveit_config)/config/sensors_kinect_pointcloud.yaml" />

If you are using depthmap change the name of the yaml file to sensors_kinect_depthmap.yaml. Note that you will need to input the path to the right file you have created above.

Octomap Configuration¶

You will also need to configure the Octomap by adding the following lines into the sensor_manager.launch:

<param name="octomap_frame" type="string" value="odom_combined" />
<param name="octomap_resolution" type="double" value="0.05" />
<param name="max_range" type="double" value="5.0" />

MoveIt uses an octree-based framework to represent the world around it. The Octomap parameters above are configuration parameters for this representation:

• octomap_frame: specifies the coordinate frame in which this representation will be stored. If you are working with a mobile robot, this frame should be a fixed frame in the world.
• octomap_resolution: specifies the resolution at which this representation is maintained (in meters).
• max_range: specifies the maximum range value to be applied for any sensor input to this node.

Perception Related¶

First, convert from sensor_msgs to pcl::PointXYZRGB which is needed for most of the processing.

pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromROSMsg(*input, *cloud);

Using passthough filter to get region of interest. A passthrough filter just eliminates the point cloud values which do not lie in the user specified range.

passThroughFilter(cloud);

Declare normals and call function to compute point normals.

pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
computeNormals(cloud, cloud_normals);

inliers_plane will hold the indices of the point cloud that correspond to a plane.

pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices);

Detect and eliminate the plane on which the cylinder is kept to ease the process of finding the cylinder.

removePlaneSurface(cloud, inliers_plane);

We had calculated the point normals in a previous call to computeNormals, now we will be extracting the normals that correspond to the plane on which cylinder lies. It will be used to extract the cylinder.

extractNormals(cloud_normals, inliers_plane);

ModelCoefficients will hold the parameters using which we can define a cylinder of infinite length. It has a public attribute values of type std::vector< float >.
Values[0-2] hold a point on the center line of the cylinder.
Values[3-5] hold direction vector of the z-axis.
Values[6] is the radius of the cylinder.

pcl::ModelCoefficients::Ptr coefficients_cylinder(new pcl::ModelCoefficients);
/* Extract the cylinder using SACSegmentation. */
extractCylinder(cloud, coefficients_cylinder, cloud_normals);

Storing Relevant Cylinder Values¶

The information that we have in coefficients_cylinder is not enough to define our cylinder. It does not have the actual location of the cylinder nor the actual height.
We define a struct to hold the parameters that are actually needed for defining a collision object completely.
There are 4 fields and a total of 7 parameters used to define this.

struct AddCylinderParams
{
  /* Radius of the cylinder. */
  double radius;
  /* Direction vector towards the z-axis of the cylinder. */
  double direction_vec[3];
  /* Center point of the cylinder. */
  double center_pt[3];
  /* Height of the cylinder. */
  double height;
};

Declare a variable of type AddCylinderParams and store relevant values from ModelCoefficients.

AddCylinderParams* cylinder_params;
    /* Store the radius of the cylinder. */
    cylinder_params->radius = coefficients_cylinder->values[6];
    /* Store direction vector of z-axis of cylinder. */
    cylinder_params->direction_vec[0] = coefficients_cylinder->values[3];
    cylinder_params->direction_vec[1] = coefficients_cylinder->values[4];
    cylinder_params->direction_vec[2] = coefficients_cylinder->values[5];

Extracting Location and Height¶

Compute the center point of the cylinder using standard geometry

extractLocationHeight(cloud);

Consider a point inside the point cloud and imagine that point is formed on a XY plane where the perpendicular distance from the plane to the camera is Z.
The perpendicular drawn from the camera to the plane hits at center of the XY plane.
We have the x and y coordinate of the point which is formed on the XY plane.
X is the horizontal axis and Y is the vertical axis.
C is the center of the plane which is Z meter away from the center of camera and A is any point on the plane.
Now we know Z is the perpendicular distance from the point to the camera.
If you need to find the actual distance d from the point to the camera, you should calculate the hypotenuse- hypot(point.z, point.x);
angle the point made horizontally- atan2(point.z,point.x);
angle the point made Vertically- atan2(point.z, point.y);
Loop over the entire pointcloud.

for (auto const point : cloud->points)
{
  /* Find the coordinates of the highest point */
  if (atan2(point.z, point.y) < min_angle_y)
  {
    min_angle_y = atan2(point.z, point.y);
    lowest_point[0] = point.x;
    lowest_point[1] = point.y;
    lowest_point[2] = point.z;
  }
  /* Find the coordinates of the lowest point */
  else if (atan2(point.z, point.y) > max_angle_y)
  {
    max_angle_y = atan2(point.z, point.y);
    highest_point[0] = point.x;
    highest_point[1] = point.y;
    highest_point[2] = point.z;
  }
}
/* Store the center point of cylinder */
cylinder_params->center_pt[0] = (highest_point[0] + lowest_point[0]) / 2;
cylinder_params->center_pt[1] = (highest_point[1] + lowest_point[1]) / 2;
cylinder_params->center_pt[2] = (highest_point[2] + lowest_point[2]) / 2;
/* Store the height of cylinder */
cylinder_params->height =
    sqrt(pow((lowest_point[0] - highest_point[0]), 2) + pow((lowest_point[1] - highest_point[1]), 2) +
         pow((lowest_point[2] - highest_point[2]), 2));

Use the parameters extracted to add the cylinder to the planning scene as a collision object.

addCylinder();

Adding Cylinder to Planning Scene¶

Define a collision object ROS message.

moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = "camera_rgb_optical_frame";
collision_object.id = "cylinder";

Define a cylinder which will be added to the world.

shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.CYLINDER;
primitive.dimensions.resize(2);
/* Setting height of cylinder. */
primitive.dimensions[0] = cylinder_params->height;
/* Setting radius of cylinder. */
primitive.dimensions[1] = cylinder_params->radius;

Define a pose for the cylinder (specified relative to frame_id).

geometry_msgs::Pose cylinder_pose;
/* Computing and setting quaternion from axis angle representation. */
Eigen::Vector3d cylinder_z_direction(cylinder_params->direction_vec[0], cylinder_params->direction_vec[1],
                                     cylinder_params->direction_vec[2]);
Eigen::Vector3d origin_z_direction(0., 0., 1.);
Eigen::Vector3d axis;
axis = origin_z_direction.cross(cylinder_z_direction);
axis.normalize();
double angle = acos(cylinder_z_direction.dot(origin_z_direction));
cylinder_pose.orientation.x = axis.x() * sin(angle / 2);
cylinder_pose.orientation.y = axis.y() * sin(angle / 2);
cylinder_pose.orientation.z = axis.z() * sin(angle / 2);
cylinder_pose.orientation.w = cos(angle / 2);

Setting the position of cylinder.

cylinder_pose.position.x = cylinder_params->center_pt[0];
cylinder_pose.position.y = cylinder_params->center_pt[1];
cylinder_pose.position.z = cylinder_params->center_pt[2];

Add cylinder as collision object

collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(cylinder_pose);
collision_object.operation = collision_object.ADD;
planning_scene_interface.applyCollisionObject(collision_object);