Collision box position not updated

edit

I use Moveit pkg to control a robot arm. In a node I detect a chess-board (CB) atteched to the gripper of the arm and publish its TF relative to the position of the camera. I want to place a collision object (box) on which is the CB.

The problem appears when the camera looses the chessboard and regains vision over it. In this case the collision box is not placed at the current coordinates of the CB, but to the previously known position and orientation (before the CB was lost from sight). It seams to be that the planning_scene_interface does not get the new TF coordinates of the CB.

The collision box is created relative to the /calibration_grid frame which is actually the chess-board. How can I "force" the 'planning_scene_interface' to re-check the current frame of the CB and to place there the collision box?

The code:

std::vector<moveit_msgs::CollisionObject> ArmControl::creatCollisionObjChessboard(
    tf::StampedTransform cameraGrid) {
if (!collisionCB_id.empty())
    collisionCB_id.erase(collisionCB_id.begin());
this->collisionBoxesPresent = true;
moveit_msgs::CollisionObject chessBoard_obj;
std::vector<moveit_msgs::CollisionObject> collision_objects;
chessBoard_obj.header.frame_id = "calibration_grid"; 
// The id of the object is used to identify it.
chessBoard_obj.id = "ChessBoard";
// Define a box to add to the world.
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
geometry_msgs::Pose box_pose;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.06; 
primitive.dimensions[1] = 0.075;
primitive.dimensions[2] = 0.005;
box_pose.orientation.w = 1; 
box_pose.orientation.x = 0; 
box_pose.orientation.y = 0; 
box_pose.orientation.z = 0; 
box_pose.position.x =  0.015;
box_pose.position.y =  0.0225;
box_pose.position.z =  0.00;
// collision object parameter setup
chessBoard_obj.primitives.push_back(primitive);
chessBoard_obj.primitive_poses.push_back(box_pose);
chessBoard_obj.operation = chessBoard_obj.ADD;
// add collision object
collision_objects.push_back(chessBoard_obj);
collisionCB_id.push_back(chessBoard_obj.id);
return collision_objects;
}
bool ArmControl::findCB(arm_action_server::FindChessBoard::Request& req,
    arm_action_server::FindChessBoard::Response& res) {
int rotation_counter = 0;
    bool CBfound = false;
double degrees = 15;
this->current_pos = this->moveToInitialPos();
while (!CBfound)
{
    if (this->chessBoard_listener.waitForTransform("/calibration_grid", eef ,ros::Time(0), ros::Duration(1.0))) {
                CBfound = true;
            } else {                    
                std::cout << "CB not found \n";
                rotation_counter++;
                this->current_pos = rotateBase(degrees); 
            }
    if (rotation_counter*degrees > 360)
    {
        this->planning_scene_interface.removeCollisionObjects(this->collisionCB_id);
        this->collisionBoxesPresent = false;
        sleep(2.0);
        return false;
    }
}
    this->planning_scene_interface.addCollisionObjects(
        creatCollisionObjChessboard(transform_cameraToGrid));
sleep(3.0);

res.distance = target_pos.position.y;
res.yaw = target_pos.orientation.z;
res.moveAccomplished = 1.0;
return true;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "arm_action_server");
ros::NodeHandle nh;
ros::AsyncSpinner spinner(0);
spinner.start();

ArmControl controller;  
controller.moveToInitialPos();

ros::ServiceServer findCB_service = nh.advertiseService("find_chess_board",
                &ArmControl::findCB, &controller);

ros::ServiceServer flippSW_service = nh.advertiseService("flipp_switch",
                    &ArmControl::flippSW, &controller);

cout << "Services ON \n \n ";
ros::waitForShutdown();
 }