Detect Box Pick Point

The first step in a pick and place operation is that the pick location must be found. To do that, we will leverage a 3D camera sensor and the Point Cloud Library (PCL).

The coordinate frame of the box’s pick can be requested from a ros service that detects it by processing the sensor data. In this exercise, we will learn to write a ROS service and apply the neccessary filters to locate the pick location.

Overview of the process

The perception node is launched. This registers a new service with roscore and pulls necessary parameters from the ROS parameter server. It then waits until the service is called. When the ROS perception service is called:

  1. Service pulls latest point cloud from the 3D sensor

  2. Point cloud is cropped to exclude areas outside of the work cell

  3. RANSAC plane segmentation is used to remove the work table from the point cloud

  4. Euclidean cluster extraction is used to cluster the remaining points

  5. Outliers are removed

  6. The largest cluster is taken as the pick object (additional logic could be added here to support multiple pick objects in sight)

  7. RANSAC plane segmentations is used to find the top of the box

  8. The centroid of these points is calculated

  9. The service returns this pose

Explore processing_node.launch

Open processing_node.launch in demo3_perception/launch. This file launches the perception node. Note the standalone flag which can be set to true to test the perception without the rest of the system.

Additionally, it adds some associated rosparams to the parameter server. Explore these parameters. While the given values should work in simulation, it is likely that some of the PCL filter parameters will have to be changed when moving to real hardware. Here are some of them associated with the ROS setup:

  • cloud_debug: true means intermediate point clouds will be published for debugging

  • cloud_topic: Topic from which the service pulls the point cloud

  • world_frame: Frame into which the point cloud is placed

  • camera_frame: Frame associated with the camera location

Define ROS Service

We first review the ROS service definition. This file will define the service inputs and outputs.

  • In the pick_and_place_perception/srv directory, open GetTargetPose.srv

  • Ensure that the following code is in this file. Note that the service request is empty. It returns a boolean flag, a ROS message of the geometry_msgs/Pose type for the location of the center of the top of the box, and two geometry_msgs/Pose messages that define a bounding box around the top of the box.

# Request - empty
---
# Response - service returns a bool, a geometry_msgs/Pose, and 2 geometry_msgs/Points
bool succeeded
geometry_msgs/Pose target_pose
geometry_msgs/Point min_pt
geometry_msgs/Point max_pt

Explore perception_node architecture

For a ROS service to be registered on roscore, it must be advertised similarly to a node. This is done with nh.advertiseService(args). Although this is often done in the main() function, for our use case this is done in the constructor of the PerceptionPipeline class. The service function is then a member of that class. Creating a pipeline class has several advantages.

  • It allows to create ROS publishers as class members that are also maintained between service calls. We are using this for debugging in our case by publishing the intermediate point clouds.

  • It allows to read parameters and store them in class members that are maintained between service calls, reducing processing time.

  • It allows to instantiate multiple instances of the same pipeline.

Complete Code

  • A template has been provided with some of the setup complete

  • Find every line that begins with the comment “’’Fill Code: ‘’” and read the description. Then, replace every instance of the comment “’’ENTER CODE HERE’’” with the appropriate line of code:

/* Fill Code:
     .
     .
     .
*/
//ENTER CODE HERE: Brief Description
  • The ‘’’find_pick_client’’’ object in your program can use the ‘’’call()’’’ method to send a request to a ROS service.

  • The ROS service that receives the call will process the sensor data and return the pose for the box pick in the service structure member ‘’’srv.response.target_pose’’’.

Build Code and Run

  • Compile the pick and place node in QT

Build -> Build Project
  • Alternatively, in a terminal cd into your workspace directory and do the following

catkin build
  • Run your node with the launch file which calls the processing_node.launch that you used previously. Although you could call that launch file directly, the environment would not load, and there would be no point cloud to process.

roslaunch pick_and_place pick_and_place.launch
  • test_bed_core_node will call your service automatically. However, you can also test your code from the command line by calling rosservice call /find_pick when a point cloud is being published on the cloud_topic (/cloud by default).

  • Some errors and warnings will pop up when you run the template code. As the correct code is filled, they will disappear until a fully defined Pose message is returned from the service.

Additional exploration (optional)

You may notice that this service is not quick to run. Aside from publishing the clouds for debugging, intermediate copies of the point cloud are made that simplify the demonstration but hinder performance. Additionally, PCL has a wide variety of filters available and the solution given here is just one out of many possibilities. Use the included timing functions to explore changes that can speed up processing.

API References

call()