After removing collision objects from the world they still remain in RVIZ

Question

Hi all,

I need to remove the collision objects from the world instantaneously. I have the detected objects from the darknet_ros and want to include them in the scene as collision objects. After they are no longer anymore in the scene would like to remove them from the world in RVIZ. I check this ROS Answer ROS answers, but it still not working properly. Sometimes the collision objects still remain in RVIZ although they are not anymore in the scene. Here is the code

void chatterCallback(const darknet_ros_3d_msgs::BoundingBoxes3d& boxes)
{ 
         
          int counter_id = 0;

          for(auto bb : boxes.bounding_boxes)
                    {
                    
                                  //moveit_visual_tools::MoveItVisualTools visual_tools("BOX_");
                                  const std::string PLANNING_GROUP = "crane_control";   
                                  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
                                  //sleep(1);


                                  shape_msgs::SolidPrimitive primitive;
                                  primitive.type = primitive.BOX;
                                  primitive.dimensions.resize(3);

                                  string str= to_string(counter_id++);
                                  moveit_msgs::CollisionObject collision_object;
                                  collision_object.header.frame_id = "world";
                                  collision_object.id = "BOX_" + str;
                                  //collision_object.header.stamp = ros::Time::now();
                                  //collision_object.header.stamp = boxes.header.stamp;

                                  //Pose
                                  geometry_msgs::Pose box_pose;
                                  box_pose.position.x = (bb.xmax + bb.xmin)/2;
                                  box_pose.position.y = (bb.ymax + bb.ymin)/2;
                                  box_pose.position.z = (bb.zmax + bb.zmin)/2;  
                                  box_pose.orientation.w = 1.0;
                                
                                  //Dimension
                                  primitive.dimensions[0] = (bb.xmax - bb.xmin);
                                  primitive.dimensions[1] = (bb.ymax - bb.ymin);
                                  primitive.dimensions[2] = (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.ADD;
                                  std::vector<moveit_msgs::CollisionObject> collision_objects;
                                  collision_objects.push_back(collision_object); 
                                  planning_scene_interface.applyCollisionObjects(collision_objects);
                                  sleep(0.4);*/
                                  

                                  //Collision object
                                  collision_object.primitives.push_back(primitive);
                                  collision_object.primitive_poses.push_back(box_pose);
                                 
                                  
                                  //Remove
                                  //collision_object.operation = collision_object.Remove;
                                  bool applyCollisionObject(const moveit_msgs::CollisionObject& collision_objects); 
                                  collision_object.operation = collision_object.REMOVE; 
                                  //std::vector<std::string> object_ids;
                                  //object_ids.push_back("BOX_" + str);
                                  //planning_scene_interface.removeCollisionObjects(object_ids);
                                  //sleep(0.5);    
                                    
                                  //Collision object
                                  //collision_object.primitives.push_back(primitive);
                                  //collision_object.primitive_poses.push_back(box_pose);
                                 
                                  collision_object.operation = collision_object.ADD;
                                  std::vector<moveit_msgs::CollisionObject> collision_objects;
                                  collision_objects.push_back(collision_object); 
                                  planning_scene_interface.applyCollisionObjects(collision_objects);
                                  sleep(0.4);                     
                                  



                            }
                                  

                                  


 }

int main(int argc,  char** argv)

{
  ros::init(argc, argv, "cpp_subscriber");
  ros::NodeHandle n;
  //moveit_visual_tools::MoveItVisualTools visual_tools("BOX_");
  ros::Subscriber sub = n.subscribe("/darknet_ros_3d/bounding_boxes", 10, chatterCallback);// bese 5
  //markers_pub_ = n.advertise<visualization_msgs::MarkerArray> ("msg_marker", 1);
  ros::spin();  
  

}

Any help? Thanks

---

Originally posted by Astronaut on ROS Answers with karma: 330 on 2020-10-18

Post score: 0

Answer 1

In your code, the only CollisionObject message you send to the planning scene contains the ADD operation. You can remove the object by sending the message with the REMOVE flag instead.

[...]
moveit_msgs::CollisionObject collision_object;
collision_object.id = "BOX_" + str;
collision_object.operation = collision_object.REMOVE; 
planning_scene_interface.applyCollisionObject(collision_object);

On another note, it seems like you are sending a vector with a single element to the planning scene inside your for-loop. It would make more sense to construct the vector of collision objects inside the loop and the apply them to the planning scene once the vector is complete.

Also, the line in your code that says bool applyCollisionObject(const moveit_msgs::CollisionObject& collision_objects); does not really make sense (that's a function declaration, but you are not defining the function anywhere). The question where you copied this from was referring to this applyCollisionObject method, which you can use as described in the example code above.

---

Originally posted by fvd with karma: 2180 on 2020-10-18

This answer was ACCEPTED on the original site

Post score: 0

---

Original comments

Comment by Astronaut on 2020-10-19:
Yes. I have a version of the code with

/

/Remove
collision_object.operation = collision_object.REMOVE; 
std::vector<std::string> object_ids;
object_ids.push_back(collision_object.id);
planning_scene_interface.removeCollisionObjects(object_ids);
      
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object); 

But still the same problem remains

Comment by Astronaut on 2020-10-19:
ok I edit the code. But still same problem. Any help?

Comment by fvd on 2020-10-22:
I don't understand which version to look at. What did you edit, which code still has which problem?

Comment by Astronaut on 2020-10-28:
this latest one

Comment by Astronaut on 2020-11-10:
Ok. Solve it. Works now. I will edit the correct version

Comment by Astronaut on 2020-11-10:
The correct version is edit

Comment by fvd on 2020-11-10:
I am not sure I understand which code works now and which does not. Could you please post your solution in a separate answer?