How to remove collision objects from the planning scene RViz

asked 2022-03-08 05:28:36 -0600

zahid990170 gravatar image


I am adding a collision object to the planning scene. Here is the code that I used for achieving that.

void AddObstacle(int id, double x_offset) 
moveit_msgs::CollisionObject m_CollisionObject;
shape_msgs::SolidPrimitive primitive;
geometry_msgs::Pose a_pose;
std::string m_strObstacleId;

m_strObstacleId = "box_" + std::to_string(id); = m_strObstacleId;
m_CollisionObject.header.frame_id = m_MoveGroup->getPlanningFrame().c_str();
m_CollisionObject.header.stamp = ros::Time::now();

primitive.type = primitive.BOX;
primitive.dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.065;  
primitive.dimensions[shape_msgs::SolidPrimitive::BOX_Y] =  0.24; 
primitive.dimensions[shape_msgs::SolidPrimitive::BOX_Z] =  0.20;  

a_pose.orientation.w = 1.0;
a_pose.position.x = x_offset + 0.350; 
a_pose.position.y = 0.17; 
a_pose.position.z = 0.48328;  

m_CollisionObject.operation = m_CollisionObject.ADD;


This collision object appears in RViz. However, with some delay, I want to add another box shifted by some offset ( as given x_offset). At the same time, I want to remove the previous one. This shows that there is a displacement of the obstacle along the given axis. Here is the code that I use to remove the obstacle.

void RemoveObstacle(std::string obs) 
  moveit_msgs::CollisionObject remove_object; = obs;
  remove_object.operation = remove_object.REMOVE;

Here is the code, where I am invoking the above two methods, one after the other. m_iObsId is a static global variable.

m_iObsId += 1;
    prev_obs_loc = "box_" + std::to_string(m_iObsId - 1);
    x_offset += 0.6;
    this->AddObstacle(m_iObsId, x_offset);

When, I print the known objects in the planning scene, I get the identifier for just ONE object, the latest. That is correct. But, in RViz, either

  1. I do not see the new collision object appearing with a shift, when the x_offset < 1.0, There is a flickr, but the object is always in the same location.
  2. Or, I see a trail of collision objects, but the previous ones are not removed. when x_offset >= 1.0

Could somebody help me with that.



edit retag flag offensive close merge delete