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 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