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

asked 2020-10-18 22:43:14 -0600

Astronaut
302 ●177 ●206 ●214

updated 2020-11-10 00:50:01 -0600

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

edit retag flag offensive close merge delete

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2020-10-18 23:56:08 -0600

fvd
2180 ●15 ●98 ●56

updated 2020-10-19 00:06:22 -0600

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.

edit flag offensive delete link

Comments

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

Astronaut ( 2020-10-19 03:31:44 -0600 )edit

ok I edit the code. But still same problem. Any help?

Astronaut ( 2020-10-19 03:56:10 -0600 )edit

I don't understand which version to look at. What did you edit, which code still has which problem?

fvd ( 2020-10-22 00:38:52 -0600 )edit

this latest one

Astronaut ( 2020-10-28 02:45:26 -0600 )edit

Ok. Solve it. Works now. I will edit the correct version

Astronaut ( 2020-11-10 00:28:10 -0600 )edit

The correct version is edit

Astronaut ( 2020-11-10 00:32:26 -0600 )edit

I am not sure I understand which code works now and which does not. Could you please post your solution in a separate answer?

fvd ( 2020-11-10 00:35:11 -0600 )edit

add a comment

answered 2020-11-10 00:41:55 -0600

Astronaut
302 ●177 ●206 ●214

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();  


}

edit flag offensive delete link

Comments

What part did you change to make it work?

fvd ( 2020-11-10 01:03:48 -0600 )edit

you can see from the code.

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);

these are out of the loop for(auto bb : boxes.bounding_boxes)

Astronaut ( 2020-11-10 22:45:20 -0600 )edit

add a comment

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

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

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

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

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