ROS相关笔记

获得当前机械臂六轴弧度，并修改：

std::vector<double> group_variable_values;
group->getCurrentState()->copyJointGroupPositions(group->getCurrentState()->getRobotModel()->getJointModelGroup(group->getName()), group_variable_values);

group_variable_values[5] = 3.14/180*(0-group_variable_values[5]/3.14*180);
group->setJointValueTarget(group_variable_values);
success = group->plan(plan);//第二次规划

添加障碍物，要ASCii格式的STL文件：

         moveit_msgs::CollisionObject toilet;
         shapes::Mesh* m = shapes::createMeshFromResource("package://rospc/src/chilun.STL");
         shape_msgs::Mesh toilet_mesh;
         shapes::ShapeMsg toilet_mesh_msg;
         shapes::constructMsgFromShape(m,toilet_mesh_msg);
         toilet_mesh = boost::get<shape_msgs::Mesh>(toilet_mesh_msg);
         toilet.meshes.resize(1);
         toilet.id = "chilun.stl";
         toilet.meshes[0] = toilet_mesh;
         toilet.mesh_poses.resize(1);
         toilet.mesh_poses[0].position.x = 0;
         toilet.mesh_poses[0].position.y = 0;
         toilet.mesh_poses[0].position.z = 0;
         toilet.mesh_poses[0].orientation.w= 1;
         toilet.mesh_poses[0].orientation.x= 0;
         toilet.mesh_poses[0].orientation.y= 0;
         toilet.mesh_poses[0].orientation.z= 0;
         //pub_co.publish(toilet);

         toilet.meshes.push_back(toilet_mesh);
         toilet.mesh_poses.push_back(toilet.mesh_poses[0]);
         toilet.operation = toilet.ADD;

         collision_objects.push_back(toilet);
         ROS_INFO("Add obstacles.");
         current_scene.addCollisionObjects(collision_objects);

添加关节约束：

         group->al