MoveIt允许使用Octomap无缝集成3D传感器。一旦正确配置，应该看到这样的东西在rviz中。
一.Configuration
在本节中，将使用MoveIt配置机器人上的3D传感器。MoveIt的主要组件处理3D感知的是Occupancy Map Updater。更新程序使用插件架构来处理不同类型的输入。MoveIt中当前可用的插件是：
- The PointCloud Occupancy Map Updater：可以作为输入点云[sensor_msgs/PointCloud2]
- The Depth Image Occupancy Map Updater：可以作为输入深度图像[sensor_msgs/Image]
1.YAML Configuration file [Point Cloud]
必须生成一个YAML配置文件来配置3D传感器。请查看这个处理点云的示例文件，它位于panda_moveit_config repository for Kinetic。将这个文件保存在机器人的moveit_config包中的config文件夹中，文件名为sensors_kinect_pointcloud.yaml：
sensors:
- sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
point_cloud_topic: /camera/depth_registered/points
max_range: 5.0
point_subsample: 1
padding_offset: 0.1
padding_scale: 1.0
max_update_rate: 1.0
filtered_cloud_topic: filtered_cloud
一般参数为：
- sensor_plugin：使用的插件的名称。
- max_update_rate：octomap表示将以小于或等于此值的速率更新。
特定于点云更新器的参数是：
- point_cloud_topic：这指定了侦听点云的主题。
- max_range：[in m]超过此值的点将不被使用。
- point_subsample：从每个点point_subsample点中选择一个。
- padding_offset：填充的大小[in cm]。
- padding_scale：填充物的比例。
- filtered_cloud_topic：发布过滤后的云的主题[主要用于调试]。滤波云是自滤波后的合成云。
2.YAML Configuration file [Depth Map]
必须生成一个YAML配置文件来配置3D传感器。在panda_moveit_config存储库中也可以找到处理深度图像的示例文件。将这个文件保存在机器人moveit_config包中的config文件夹中，文件名为sensors_kinect_depthmap.yaml：
sensors:
- sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
image_topic: /camera/depth_registered/image_raw
queue_size: 5
near_clipping_plane_distance: 0.3
far_clipping_plane_distance: 5.0
shadow_threshold: 0.2
padding_scale: 4.0
padding_offset: 0.03
max_update_rate: 1.0
filtered_cloud_topic: filtered_cloud
一般参数为：
- sensor_plugin：使用的插件的名称。
- max_update_rate：octomap表示将以小于或等于此值的速率更新。
特定于深度映射更新器的参数是：
- image_topic：这指定了要监听深度图像的主题。
- queue_size：要排队的图像数量。
- near_clipping_plane_distance：能见度不足前的最小距离。
- far_clipping_plane_distance：最大距离前缺乏能见度。
- shadow_threshold：一个实体下面阴影地图的最小亮度，使其动态阴影可见。
- padding_offset：填充的大小[in cm]。
- padding_scale：填充物的比例。
- filtered_cloud_topic：发布过滤后的云的主题[主要用于调试]。滤波云是自滤波后的合成云。
3.Update the launch file
[1]Add the YAML file to the launch script
现在需要更新sensor_manager.launch文件，在带有传感器信息的panda_moveit_config目录的launch目录中[此文件由安装助手自动生成，但为空]。将需要添加以下行到该文件中，以配置MoveIt的传感器源集使用：
<rosparam command="load" file="$(find panda_moveit_config)/config/sensors_kinect_pointcloud.yaml" />
如果正在使用depthmap，请将yaml文件的名称更改为sensors_kinect_depthmap.yaml。注意，需要输入上面创建的正确文件的路径。
[2]Octomap Configuration
还需要通过在sensor_manager.launch中添加以下代码来配置Octomap：
<param name="octomap_frame" type="string" value="odom_combined" />
<param name="octomap_resolution" type="double" value="0.05" />
<param name="max_range" type="double" value="5.0" />
MoveIt使用一个基于octreet的框架来表示它周围的世界。上面的Octomap参数是这个表示的配置参数：
- octomap_frame：指定此表示将存储在其中的坐标系。如果使用的是移动机器人，这个框架应该是世界上固定的框架。
- octomap_resolution：指定保持此表示的分辨率[以米为单位]。
- max_range：指定应用于此节点的任何传感器输入的最大范围值。
二.Obstacle Avoidance
如果将机器人的初始位置和最终位置设置成它们之间没有直线的路径，那么规划器将自动避开octomap并围绕它进行规划。
1.Running the Interface
Roslaunch启动文件，直接从moveit_tutorials运行代码：
roslaunch moveit_tutorials obstacle_avoidance_demo.launch
应该看到类似于本教程开头所示的图像。通过手动设置目标状态，然后计划和执行，可以自己测试障碍规避。要学习如何做，看MoveIt在RViz快速入门。
三.Detecting and Adding Object as Collision Object
在本节中，将展示一个从点云中提取圆柱体、计算相关值并将其作为冲突对象添加到规划场景的示例。将使用点云，但它也可以用深度图来实现。运行代码之后，应该能够在rviz中看到类似的内容。
1.Running the Code
Roslaunch启动文件，直接从moveit_tutorials运行代码：
roslaunch moveit_tutorials detect_and_add_cylinder_collision_object_demo.launch
已知问题-可能会看到下面的错误运行演示：
ros.moveit_ros_planning.planning_scene_monitor: Transform error: Lookup would require extrapolation into the future. Requested time 1527473962.793050157 but the latest data is at time 1527473962.776993978, when looking up transform from frame [panda_link2] to frame [camera_rgb_optical_frame]
ros.moveit_ros_perception: Transform cache was not updated. Self-filtering may fail.
我们正在修复它，它不应该中断工作的演示。可以在问题跟踪器中跟踪它的状态。
2.Relevant Code
完整的代码可以在moveit_tutorials GitHub项目中看到。关于每个感知管道函数实现的细节在本教程中被省略，因为它们在这里有很好的文档记录。
[1]Perception Related
首先，将sensor_msgs转换为pcl::PointXYZRGB，这是大多数处理都需要的。
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromROSMsg(*input, *cloud);
使用passthrough过滤器获得感兴趣的区域。passthrough过滤器只会消除不位于用户指定范围内的点云值。
passThroughFilter(cloud);
声明法线并调用函数来计算点法线。
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
computeNormals(cloud, cloud_normals);
inliers_plane将保持点云的指数对应于一个平面。
pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices);
检测并消除圆柱体所在平面，以简化寻找圆柱体的过程。
removePlaneSurface(cloud, inliers_plane);
在之前的computeNormals调用中已经计算了点法线，现在将提取与圆柱所在平面相对应的法线。它将被用来提取圆柱体：
extractNormals(cloud_normals, inliers_plane);
模型系数将包含用来定义无限长圆柱的参数。它有一个std::vector类型的公共属性values。Values[0-2]保持点在圆柱体的中心线上；Values[3-5]保持z轴方向向量；Values[6]表示圆柱的半径。
pcl::ModelCoefficients::Ptr coefficients_cylinder(new pcl::ModelCoefficients);
/* Extract the cylinder using SACSegmentation. */
extractCylinder(cloud, coefficients_cylinder, cloud_normals);
[2]Storing Relevant Cylinder Values
在coefficients_cylinder中所拥有的信息不足以定义cylinder。它没有圆柱体的实际位置和实际高度。定义了一个结构体来保存实际需要的参数来完整地定义一个冲突对象。有4个字段和7个参数用于定义它。
struct AddCylinderParams
{
/* Radius of the cylinder. */
double radius;
/* Direction vector towards the z-axis of the cylinder. */
double direction_vec[3];
/* Center point of the cylinder. */
double center_pt[3];
/* Height of the cylinder. */
double height;
};
声明AddCylinderParams类型的变量，并从ModelCoefficients中存储相关值。
AddCylinderParams* cylinder_params;
/* Store the radius of the cylinder. */
cylinder_params->radius = coefficients_cylinder->values[6];
/* Store direction vector of z-axis of cylinder. */
cylinder_params->direction_vec[0] = coefficients_cylinder->values[3];
cylinder_params->direction_vec[1] = coefficients_cylinder->values[4];
cylinder_params->direction_vec[2] = coefficients_cylinder->values[5];
[3]Extracting Location and Height
使用标准几何计算圆柱体的中心点。
extractLocationHeight(cloud);
考虑点云中的一个点，想象这个点形成在XY平面上，从平面到摄像机的垂直距离是Z。从摄像机到平面的垂线落在XY平面的中心。有在XY平面上形成的点的x和y坐标。X是横轴，Y是纵轴。C是平面的中心，距离摄像机中心Z米，A是平面上的任意一点。现在知道Z是点到摄像机的垂直距离。如果需要计算从点到摄像机的实际距离d，应该计算斜边-hypot(point.z,point.x);使点水平成角度-atan2(point.z,point.x);：使点垂直成角-atan2(point.z,point.y);遍历整个点云。
for (auto const point : cloud->points)
{
/* Find the coordinates of the highest point */
if (atan2(point.z, point.y) < min_angle_y)
{
min_angle_y = atan2(point.z, point.y);
lowest_point[0] = point.x;
lowest_point[1] = point.y;
lowest_point[2] = point.z;
}
/* Find the coordinates of the lowest point */
else if (atan2(point.z, point.y) > max_angle_y)
{
max_angle_y = atan2(point.z, point.y);
highest_point[0] = point.x;
highest_point[1] = point.y;
highest_point[2] = point.z;
}
}
/* Store the center point of cylinder */
cylinder_params->center_pt[0] = (highest_point[0] + lowest_point[0]) / 2;
cylinder_params->center_pt[1] = (highest_point[1] + lowest_point[1]) / 2;
cylinder_params->center_pt[2] = (highest_point[2] + lowest_point[2]) / 2;
/* Store the height of cylinder */
cylinder_params->height =
sqrt(pow((lowest_point[0] - highest_point[0]), 2) + pow((lowest_point[1] - highest_point[1]), 2) +
pow((lowest_point[2] - highest_point[2]), 2));
使用提取的参数将圆柱作为碰撞对象添加到规划场景中。
addCylinder();
[4]Adding Cylinder to Planning Scene
定义一个冲突对象ROS消息。
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = "camera_rgb_optical_frame";
collision_object.id = "cylinder";
定义一个将被添加到世界中的圆柱体。
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.CYLINDER;
primitive.dimensions.resize(2);
/* Setting height of cylinder. */
primitive.dimensions[0] = cylinder_params->height;
/* Setting radius of cylinder. */
primitive.dimensions[1] = cylinder_params->radius;
为柱体定义一个位姿[相对于frame_id指定]。
geometry_msgs::Pose cylinder_pose;
/* Computing and setting quaternion from axis angle representation. */
Eigen::Vector3d cylinder_z_direction(cylinder_params->direction_vec[0], cylinder_params->direction_vec[1],
cylinder_params->direction_vec[2]);
Eigen::Vector3d origin_z_direction(0., 0., 1.);
Eigen::Vector3d axis;
axis = origin_z_direction.cross(cylinder_z_direction);
axis.normalize();
double angle = acos(cylinder_z_direction.dot(origin_z_direction));
cylinder_pose.orientation.x = axis.x() * sin(angle / 2);
cylinder_pose.orientation.y = axis.y() * sin(angle / 2);
cylinder_pose.orientation.z = axis.z() * sin(angle / 2);
cylinder_pose.orientation.w = cos(angle / 2);
设定圆柱体位置。
cylinder_pose.position.x = cylinder_params->center_pt[0];
cylinder_pose.position.y = cylinder_params->center_pt[1];
cylinder_pose.position.z = cylinder_params->center_pt[2];
添加圆柱作为冲突对象。
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(cylinder_pose);
collision_object.operation = collision_object.ADD;
planning_scene_interface.applyCollisionObject(collision_object);
参考文献：
[1]OctoMap：https://octomap.github.io/
[2]Perception Pipeline Tutorial：http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/perception_pipeline/perception_pipeline_tutorial.html
