moveit_msgs::CollisionObject createCollisionBox(
moveit::planning_interface::MoveGroup& group, std::string object_id,
double size_x, double size_y, double size_z, double pos_x, double pos_y,
double pos_z, double orientation_x, double orientation_y,
double orientation_z, double orientation_w) {
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = group.getPlanningFrame();
/* The id of the object is used to identify it. */
collision_object.id = object_id;
/* Define a box to add to the world. */
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = size_x;
primitive.dimensions[1] = size_y;
primitive.dimensions[2] = size_z;
/* A pose for the box (specified relative to frame_id) */
geometry_msgs::Pose box_pose;
box_pose.orientation.x = orientation_x;
box_pose.orientation.y = orientation_y;
box_pose.orientation.z = orientation_z;
box_pose.orientation.w = orientation_w;
box_pose.position.x = pos_x;
box_pose.position.y = pos_y;
box_pose.position.z = pos_z;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
return collision_object;
}
bool HapticsPSM::check_collison(){
coll_res.clear();
psm_planning_scene->checkCollisionUnpadded(coll_req,coll_res,*group->getCurrentState(),
psm_planning_scene->getAllowedCollisionMatrix());
if (coll_res.collision)
{
//ROS_INFO("COLLIDING contact_point_count=%d",(int)coll_res.contact_count);
if (coll_res.contact_count > 0)
{
std_msgs::ColorRGBA color;
color.r = 1.0;
color.g = 0.0;
color.b = 0.2;
color.a = 0.8;
visualization_msgs::MarkerArray markers;
collision_detection::getCollisionMarkersFromContacts(markers,
group->getPlanningFrame().c_str(),
coll_res.contacts,
color,
ros::Duration(), // remain until deleted
0.002);
publishMarkers(markers);
}
return true;
}
else
{
//ROS_INFO("Not colliding");
// delete the old collision point markers
visualization_msgs::MarkerArray empty_marker_array;
publishMarkers(empty_marker_array);
return false;
}
}
//引用传参比较好，不改变值的情况下生明为const安全。
void add_object(const moveit::planning_interface::MoveGroup &group)
{
ros::NodeHandle node_handle;
//添加物体
// Advertise the required topic
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Note that this topic may need to be remapped in the launch file
ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
while(planning_scene_diff_publisher.getNumSubscribers() < 1)
{
ros::WallDuration sleep_t(0.5);
sleep_t.sleep();
}
// Define the attached object message
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// We will use this message to add or
// subtract the object from the world
// and to attach the object to the robot
moveit_msgs::AttachedCollisionObject attached_object;
attached_object.link_name = "lLink7";
/* The header must contain a valid TF frame*/
attached_object.object.header.frame_id = group.getPlanningFrame();
/* The id of the object */
attached_object.object.id = "box1";
/* A default left pose */
geometry_msgs::Pose pose1;
pose1.orientation.w = 1.0;
pose1.position.x=0.4;
pose1.position.y=0.64;
pose1.position.z=0.6;
/* Define a left box to be attached */
shape_msgs::SolidPrimitive primitive1;
primitive1.type = primitive1.BOX;
primitive1.dimensions.resize(3);
primitive1.dimensions[0] = 0.03;
primitive1.dimensions[1] = 0.03;
primitive1.dimensions[2] = 0.2;
/* A default right pose */
geometry_msgs::Pose pose2;
pose2.orientation.w = 1.0;
pose2.position.x=-0.4;
pose2.position.y=0.7;
pose2.position.z=0.4;
/* Define a right box to be attached */
shape_msgs::SolidPrimitive primitive2;
primitive2.type = primitive2.BOX;
primitive2.dimensions.resize(3);
primitive2.dimensions[0] = 0.3;
primitive2.dimensions[1] = 0.3;
primitive2.dimensions[2] = 0.4;
//容器使用push_back进行添加元素
attached_object.object.primitives.push_back(primitive1);
attached_object.object.primitive_poses.push_back(pose1);
attached_object.object.primitives.push_back(primitive2);
attached_object.object.primitive_poses.push_back(pose2);
// Note that attaching an object to the robot requires
// the corresponding operation to be specified as an ADD operation
attached_object.object.operation = attached_object.object.ADD;
// Add an object into the environment
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Add the object into the environment by adding it to
// the set of collision objects in the "world" part of the
// planning scene. Note that we are using only the "object"
// field of the attached_object message here.
ROS_INFO("Adding the object into the world ");
moveit_msgs::PlanningScene planning_scene;
planning_scene.world.collision_objects.push_back(attached_object.object);
planning_scene.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene);
sleep(2);
}