Hi to all,
I try to move a box in my planning scene using a tf-frame. But it's not working at the moment.
I insert the box as shown in the moveit-tutorial and giving it a tf-frame additionally like this:
/* The id of the object is used to identify it. */
co_box.id = "box1";
co_box.header.frame_id = "//box_frame";
If I now insert the box, it is correctly inserted at the position and orientation I publish in my tf-broadcaster. But when I move the frame in the broadcaster, I can see the frame moving in Rviz but not the box.
What can I do here. Is the box not correctly linked to the tf-frame or do I constantly need to update the planning scene.
I'm really stuck here. I would really appreciate if somebody could help me out.
Edit (the code for inserting the box and trying to move):
// insert BOX
// ^^^^^^^^^^
moveit_msgs::CollisionObject co_box;
co_box.header.frame_id = "//box_frame";
/* The id of the object is used to identify it. */
co_box.id = "box1";
// co_box.header.frame_id = "box_frame";
/* Define a box to add to the world. */
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 1.0;
primitive.dimensions[1] = 0.2;
primitive.dimensions[2] = 0.4;
/* A pose for the box (specified relative to frame_id) */
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.0;
box_pose.position.y = 0.0;
box_pose.position.z = 0.0;
co_box.primitives.push_back(primitive);
co_box.primitive_poses.push_back(box_pose);
co_box.operation = co_box.ADD;
std::vector collision_objects;
collision_objects.push_back(co_box);
ROS_INFO("Add collision objects into the world");
planning_scene_interface.addCollisionObjects(collision_objects);
/* Sleep so we have time to see the object in RViz */
sleep(10.0);
while(ros::ok()){
co_box.operation = co_box.MOVE;
sleeprate.sleep();
}
↧