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();
// 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");
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");
}
please take a time to check it out ,thanks