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;
}
示例#2
0
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;
      }

}
示例#3
0
//引用传参比较好,不改变值的情况下生明为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);
}