3. MoveIt与传感器数据的关系：感知场景、碰撞检测、点云处理

好，咱们进入第三章。这一章，我想跟你聊聊MoveIt和传感器数据之间那点事儿。

说实话，很多初学者把MoveIt当成一个纯运动规划器，觉得给它一个起点、一个终点，它就能算出轨迹。嗯，理论上没错。但实际项目中你会发现——没有传感器数据的MoveIt，就像闭着眼睛走路。能走，但撞墙是迟早的事。

我最早做双臂协作项目时，就吃过这个亏。当时觉得模型建得够精确了，结果机械臂一伸过去，直接怼到了旁边临时放的料箱上。从那以后，我养成了一个习惯：任何规划，都必须先问传感器一句——周围环境变了吗？

3.1 感知场景（PlanningScene）——MoveIt的“世界模型”

先搞清楚一个概念：PlanningScene。

你可以把它理解成MoveIt内部维护的一张“环境快照”。这张快照里，包含了机器人自身状态（关节角度、速度）、周围障碍物（物体、点云、网格）、以及一些约束条件。

每次运动规划前，MoveIt都会基于当前的PlanningScene来做碰撞检测和路径搜索。说白了，PlanningScene就是MoveIt感知外部世界的唯一窗口。

核心要点：PlanningScene不是静态的。它需要被持续更新，才能反映真实环境的变化。

我个人习惯把PlanningScene的更新分成两种模式：

被动更新：通过ROS话题订阅传感器数据，自动触发场景更新。
主动更新：在代码中手动调用接口，添加或移除障碍物。

来看一个最基础的例子——如何从代码中获取当前的PlanningScene：

// C++ 示例：获取并打印当前场景信息
moveit::planning_interface::PlanningScenePtr current_scene = 
    std::make_shared<moveit::planning_interface::PlanningScene>(robot_model);

// 或者通过话题监听
moveit::planning_interface::PlanningSceneInterface scene_interface;
std::map<std::string, moveit_msgs::CollisionObject> objects = 
    scene_interface.getObjects();

你想想看，如果场景里有一个突然出现的障碍物，而你的PlanningScene没更新，那规划出来的轨迹大概率会撞上去。这就是为什么我说——传感器数据是PlanningScene的“燃料”。

3.2 碰撞检测——不只是“碰没碰到”

碰撞检测，听起来很简单对吧？就是检查两个物体有没有重叠。但实际做起来，坑不少。

MoveIt默认使用FCL（Flexible Collision Library）来做碰撞检测。它支持两种模式：

自碰撞检测：检查机器人自己的连杆之间有没有碰撞。
环境碰撞检测：检查机器人与周围物体（包括传感器感知到的障碍物）有没有碰撞。

我记得有一次调试，机械臂规划出来的轨迹明明看着没问题，但一执行就报“碰撞警告”。查了半天，发现是自碰撞检测的精度设置太高，导致一些微小的模型间隙被判定为碰撞。嗯，这里要注意：碰撞检测的精度和速度，需要做权衡。

我的经验：在动态环境中，建议把碰撞检测的采样步长设得稍微大一点（比如0.01秒），同时开启continuous_collision_checking，这样既能保证安全，又不会让规划器卡死。

碰撞检测的配置，通常在planning_scene_monitor的参数中设置：

# 在 launch 文件中配置碰撞检测参数
<param name="planning_scene_monitor/publish_planning_scene" value="true"/>
<param name="planning_scene_monitor/publish_geometry_updates" value="true"/>
<param name="planning_scene_monitor/publish_state_updates" value="true"/>
<param name="planning_scene_monitor/publish_transforms_updates" value="true"/>

我曾经遇到过一个很隐蔽的问题：碰撞检测明明没报错，但机械臂就是卡在半路不动。后来发现，是因为点云数据太密，导致碰撞检测的计算量暴增，规划器直接超时了。所以——点云进来之后，一定要做降采样。

3.3 点云处理——从原始数据到可用的障碍物

点云，是传感器给MoveIt最原始、也最丰富的信息。但原始点云不能直接用。为什么？

太密了：一个深度相机每秒能产生几十万个点，全塞进PlanningScene，MoveIt会直接崩溃。
有噪声：传感器本身有误差，直接用来做碰撞检测，会误报。
坐标系不统一：点云在相机坐标系下，需要转换到机器人基坐标系。

所以，点云处理的标准流程，我总结为三步：

滤波与降采样：用PCL的VoxelGrid滤波器，把点云密度降下来。
坐标系变换：通过TF，把点云从相机坐标系变换到机器人基坐标系。
生成碰撞对象：把处理后的点云，封装成MoveIt能识别的CollisionObject。

来看一个实际的处理流程：

// 伪代码：点云处理并发布到PlanningScene
// 1. 接收原始点云（例如从 /camera/depth/points 话题）
void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) {
    // 2. 降采样
    pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;
    voxel_filter.setLeafSize(0.01, 0.01, 0.01); // 1cm 体素
    // ... 执行滤波
    
    // 3. 变换到基坐标系
    // 使用 tf::transformCloud 或 lookupTransform
    
    // 4. 发布为 CollisionObject
    moveit_msgs::CollisionObject co;
    co.header.frame_id = "base_link";
    co.id = "sensor_obstacle";
    // ... 填充点云数据到 co.meshes 或 co.primitives
    planning_scene_interface.applyCollisionObject(co);
}

注意：不要每帧都重新添加CollisionObject。我建议的做法是：先检查场景中是否已存在同名对象，如果存在，就更新它的位姿或形状；如果不存在，才新建。这样可以避免场景频繁重建带来的性能开销。

说到这，我想起一个项目中的教训。当时我们用RealSense D435做感知，点云数据量很大。我一开始直接把原始点云塞进PlanningScene，结果MoveIt的规划频率从10Hz直接掉到了0.5Hz。后来加了VoxelGrid滤波，体素大小设为2cm，规划频率才恢复到8Hz左右。所以——点云降采样不是可选项，是必选项。

3.4 知识体系图：传感器数据与MoveIt的交互流程

下面这张图，是我自己总结的传感器数据在MoveIt中的流转路径。你可以把它当成一个“地图”，以后遇到相关问题，先对照这张图看看卡在哪一步。

从这张图你可以看到，整个流程是闭环的。传感器不断提供新数据，PlanningScene不断更新，规划器基于最新场景做规划，执行过程中如果场景变了，再触发新一轮更新。

嗯，这一章的内容就到这里。核心就三句话：PlanningScene是MoveIt感知世界的窗口，碰撞检测是安全底线，点云处理是让传感器数据变得可用的关键步骤。下一章，我们会深入聊一聊如何用代码把这些流程串起来。