Ask Your Question
0

Collision box position not updated

asked 2015-09-28 05:42:05 -0600

updated 2015-09-28 07:19:55 -0600

gvdhoorn gravatar image

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();
 }
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2015-10-21 04:53:30 -0600

What I found out is that I should use ros::Time::now() rather than ros::Time(0), because the second one refers to the last published information which can be in the past a while ago. So, the solution is to change the waitForTransform parameter:

if (this->chessBoard_listener.waitForTransform("/calibration_grid", eef ,ros::Time(0), ros::Duration(1.0))
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2015-09-28 05:42:05 -0600

Seen: 98 times

Last updated: Oct 21 '15