编码构建场景

场景对象创建流程

1. 障碍物创建

moveit_msgs::CollisionObject obj;
obj.operation = obj.ADD;// 设置操作方式
obj.header.frame_id = group.getPlanningFrame();//设置坐标系
obj.id = "desk";//设置id

//设置原始形状数据
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;//矩形，长宽高
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.5;
primitive.dimensions[1] = 0.2;
primitive.dimensions[2] = 0.3;
obj.primitives.push_back(primitive);

//设置位姿
geometry_msgs::Pose pose;
pose.position.x = 0.25;
pose.position.y = 0.4;
pose.position.z = 0.15;
pose.orientation.w = 1;
obj.primitive_poses.push_back(pose);

Tip

障碍物类型为moveit_msgs::CollisionObject。

必须设置的数据为:

1. 操作方式
2. 以哪个坐标系为参考
3. 障碍物的id，唯一标识
4. 障碍物的形状
5. 障碍物的位姿

2. 障碍物添加

string groupName = "manipulator";
moveit::planning_interface::MoveGroupInterface group(groupName);

moveit::planning_interface::PlanningSceneInterface scene;

/////////////////// 障碍物 ////////////////////////////////
vector<moveit_msgs::CollisionObject> objects;

// 障碍物创建
obj = ...;

objects.push_back(obj);
/////////////////////////////////////////////////////////

// 添加障碍物和对应的颜色
scene.applyCollisionObjects(objects);

3. 颜色设置

......
//////////////// 颜色 ///////////////////////////////////
vector<moveit_msgs::ObjectColor> colors;
moveit_msgs::ObjectColor color0;
color0.color.a = 1;
color0.color.r = 0;
color0.color.g = 0;
color0.color.b = 1;
color0.id = "desk";
colors.push_back(color0);

// 添加障碍物和对应的颜色
scene.applyCollisionObjects(objects, colors);

Tip

moveit_msgs::ObjectColor用来描述颜色的。

a,r,g,b分别代表透明度，rgb色。

id必须指定，和障碍物的id相同。

几何形状类型

矩形

shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;//矩形，长宽高
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.5;//长
primitive.dimensions[1] = 0.2;//宽
primitive.dimensions[2] = 0.3;//高

球形

shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.SPHERE;//球体，半径
primitive.dimensions.resize(1);
primitive.dimensions[0] = 0.5;//半径

圆柱体

shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.CYLINDER;//圆柱体，长，半径
primitive.dimensions.resize(2);
primitive.dimensions[0] = 0.5;//长
primitive.dimensions[1] = 0.1;//半径

椎体

shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.CONE;//椎体，长，半径
primitive.dimensions.resize(2);
primitive.dimensions[0] = 0.5;//长
primitive.dimensions[1] = 0.1;//半径

Mesh类型数据

#include "geometric_shapes/shapes.h"
#include "geometric_shapes/shape_operations.h"
#include "geometric_shapes/mesh_operations.h"

......

//设置mesh
shapes::Mesh *mesh = shapes::createMeshFromResource("package://demo_moveit/meshes/bowl.dae");

shapes::ShapeMsg shape_msg;
shapes::constructMsgFromShape(mesh, shape_msg);

shape_msgs::Mesh &mesh_msg = boost::get<shape_msgs::Mesh>(shape_msg);