Revision history [back]

Here is the code that is working

void chatterCallback(const darknet_ros_3d_msgs::BoundingBoxes3d& boxes)
{ 

 int counter_id = 0;
 const std::string PLANNING_GROUP = "crane_control";    
 moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
 std::vector<std::string> object_ids;
 moveit_msgs::CollisionObject collision_object;
 collision_object.header.frame_id = "world";
 collision_object.id = "BOX_";
 std::vector<moveit_msgs::CollisionObject> collision_objects;

          for(auto bb : boxes.bounding_boxes)
                    {

                                  shape_msgs::SolidPrimitive primitive;
                                  primitive.type = primitive.BOX;
                                  primitive.dimensions.resize(3);
                                                                 string str= to_string(counter_id++);

                                  //Pose
                                  geometry_msgs::Pose box_pose;
                                                                  box_pose.position.x = abs ((bb.xmax + bb.xmin)/2);
                                  box_pose.position.y = abs ((bb.ymax + bb.ymin)/2);
                                  box_pose.position.z = abs ((bb.zmax + bb.zmin)/2);
                                  box_pose.orientation.x = 0; 
                                  box_pose.orientation.y = 0;   
                                  box_pose.orientation.z = 0;
                                  box_pose.orientation.w = 1.0;

                                  //Dimension
                                  primitive.dimensions[0] = abs (bb.xmax - bb.xmin);
                                  primitive.dimensions[1] = abs (bb.ymax - bb.ymin);
                                  primitive.dimensions[2] = abs (bb.zmax - bb.zmin); 

                                  //Collision object
                                  collision_object.primitives.push_back(primitive);
                                  collision_object.primitive_poses.push_back(box_pose);



                            }
                                 collision_object.operation = collision_object.REMOVE; 
                                 object_ids.push_back(collision_object.id);
                                                                 planning_scene_interface.removeCollisionObjects(object_ids); 
                                     sleep(0.5); 
                                 collision_object.operation = collision_object.ADD;
                                 planning_scene_interface.applyCollisionObjects(collision_objects);
                                                                 sleep(0.5); 


 }

int main(int argc,  char** argv)

{
  ros::init(argc, argv, "cpp_subscriber");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("/darknet_ros_3d/bounding_boxes", 1, chatterCallback);
  ros::spin();  


}