How to remove collision objects from the planning scene RViz

edit

Hi,

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_CollisionObject.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.resize(3);
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.primitives.push_back(primitive);
m_CollisionObject.primitive_poses.push_back(a_pose);
m_CollisionObject.operation = m_CollisionObject.ADD;
collision_objects.push_back(m_CollisionObject);
m_PlanningSceneInterface.applyCollisionObjects(collision_objects);

}

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;
  remove_object.id = obs;
  remove_object.operation = remove_object.REMOVE;
  m_PlanningSceneInterface.applyCollisionObject(remove_object);
}

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);
    RemoveObstacle(prev_obs_loc);
    ros::Duration(0.5).sleep();
    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.

thanks,

Zahid