godel produce robot-collison trajectory with env

[dbdxnuliba]

godel produce robot-collison trajectory with env .

I add moveit_msgs::CollisionObject at surface_blending_service.cpp
and roslaunch godel_irb2400_support irb2400_blending.launch real_pcd:=true

pcd_location:=/home/ws_catkin/src/meshes/raw_mesh_modify.pcd

void loadCollisonEnv(moveit::planning_interface::PlanningSceneInterface& scene)
{
// First, we will define the collision object message.
moveit_msgs::CollisionObject collision_object;
//collision_object.header.frame_id = group.getPlanningFrame();
collision_object.header.frame_id = "/world_frame";

/* The id of the object is used to identify it. /
collision_object.id = "box1";
/ Define a box to add to the world. */
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.4;
primitive.dimensions[1] = 0.1;
primitive.dimensions[2] = 0.2;

/* A pose for the box (specified relative to frame_id) */
geometry_msgs::Pose box_pose;
// 姿态(四元素)(欧拉角转四元素)
tf::Quaternion quat;
quat.setRPY(deg2rad(0), deg2rad(0), deg2rad(0));
box_pose.orientation.x = quat.x();
box_pose.orientation.y = quat.y();
box_pose.orientation.z = quat.z();
box_pose.orientation.w = quat.w();

box_pose.position.x = 1;
box_pose.position.y = -0.2;
box_pose.position.z = 1;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;

std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);

// Now, let's add the collision object into the world
ROS_INFO("Add an object into the world");
scene.addCollisionObjects(collision_objects);
ROS_INFO_STREAM("Task '"<<FUNCTION<<"' completed");

}

int main(int argc, char** argv)
{
ros::init(argc, argv, "surface_blending_service");
ros::AsyncSpinner spinner(4);
spinner.start();

SurfaceBlendingService service;
moveit::planning_interface::PlanningSceneInterface scene;

if (service.init())
{
ROS_INFO("Godel Surface Blending Service successfully initialized and now running");

loadCollisonEnv(scene);
  //debug add by ygx
ROS_WARN_STREAM("debug :finish godel_surface_detection loadEnv() ");
service.run();

}

please take a time to check it out ,thanks