Hi everyone
I use this code for adding collisionObject
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = planner.move_group1->getPlanningFrame();
collision_object.id = "box";
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.050;
primitive.dimensions[1] = 0.50;
primitive.dimensions[2] = 2;
geometry_msgs::Pose stlPose;
stlPose.orientation.w = 1.0;
stlPose.position.x = 0.3; //left
stlPose.position.y = -0.2; //back
stlPose.position.z = 1.5;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(stlPose);
collision_object.operation = collision_object.ADD;
std::vector collision_objects;
collision_objects.push_back(collision_object);
ROS_INFO("Add an object into the world");
planner.planning_scene_interface.addCollisionObjects(collision_objects);
Planning is good, robot avoids obstacles. But function checkCollision works wrong. It does't show collisions between robot and environment. Why? Need I change CollisionMatrix()?
bool isRobotStateCollision(robot_state::RobotState& robSt)
{
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
collision_detection::CollisionRobotConstPtr colRob = planning_scene->getCollisionRobot();
collision_detection::AllowedCollisionMatrix acm = planning_scene->getAllowedCollisionMatrix();
planning_scene->checkCollision(req, res, robSt, acm);
return res.collision;
}
↧