MoveIt中collision_detection::CollisionEnv::distanceRobot方法调用及相关问题

我希望通过MoveIt和ROS获取Franka Panda机器人与障碍物之间的最小距离和法向量，已知MoveIt的collision_detection::CollisionEnv::distanceRobot方法可提供所需信息，但该方法未在Python API中实现，因此寻求C++调用帮助。

我的尝试代码报错：cannot declare variable ‘collision_env’ to be of abstract type ‘collision_detection::CollisionEnv’，具体疑问如下：

• 如何正确声明collision_env并调用distanceRobot()？
• CollisionEnv有两种构造函数（一种参数为model、padding、scale，另一种增加world参数），二者区别是什么？应选择哪一种？
• MoveIt文档推荐使用PlanningSceneMonitor而非直接实例化PlanningScene类，但无相关示例，若当前方式不推荐，哪里能找到PlanningSceneMonitor的使用示例？

更新：根据andersonjwan的提示，我添加了头文件#include <moveit/collision_detection_fcl/collision_env_fcl.h>，并将声明改为collision_detection::CollisionEnvFCL collision_env(kinematic_model_cnstPtr, 0.0, 1.0);

1. 正确声明collision_env并调用distanceRobot()的方式 #

collision_detection::CollisionEnv是抽象类，无法直接实例化，必须使用它的具体实现类。MoveIt默认采用FCL作为碰撞检测后端，因此应使用collision_detection_fcl::CollisionEnvFCL：

• 先引入对应头文件：

  #include <moveit/collision_detection_fcl/collision_env_fcl.h>
  

• 实例化对象（需传入已获取的机器人运动学模型指针）：

  collision_detection::CollisionEnvFCL collision_env(kinematic_model_cnstPtr, 0.0, 1.0);
  

• 调用distanceRobot()时，需要准备DistanceRequest和DistanceResult对象，传入机器人状态等参数：

  collision_detection::DistanceRequest dist_req;
  collision_detection::DistanceResult dist_res;
  // 配置请求：开启最近点和带符号距离计算（用于推导法向量）
  dist_req.enable_nearest_points = true;
  dist_req.enable_signed_distance = true;
  
  // 假设robot_state是已获取的当前机器人状态对象
  collision_env.distanceRobot(dist_res, dist_req, robot_state);
  
  // 提取结果
  if (dist_res.collision) {
      // 处理机器人与障碍物碰撞的情况
  } else {
      double min_distance = dist_res.distance;
      // 获取最近点并计算法向量
      const Eigen::Vector3d& robot_point = dist_res.nearest_points.first;
      const Eigen::Vector3d& obstacle_point = dist_res.nearest_points.second;
      Eigen::Vector3d normal = obstacle_point - robot_point;
      normal.normalize();
  }
  

2. CollisionEnv两种构造函数的区别与选择 #

• 不带world参数的构造函数：仅基于机器人运动学模型初始化碰撞环境，不会加载任何外部障碍物数据，仅适用于检测机器人自碰撞的场景。
• 带world参数的构造函数：除机器人模型外，还会传入collision_detection::World对象（该对象存储场景中的所有障碍物，可通过ROS话题或代码添加），适合需要检测机器人与外部障碍物距离的场景。
• 你的需求是获取机器人与障碍物的距离，因此必须选择带world参数的构造函数，否则碰撞环境中无障碍物数据，无法计算对应距离。实例化示例：

  collision_detection::WorldPtr world = std::make_shared<collision_detection::World>();
  // 可通过world->addCollisionObject()添加自定义障碍物，或从PlanningScene中获取已有的world
  collision_detection::CollisionEnvFCL collision_env(kinematic_model_cnstPtr, world, 0.0, 1.0);
  

3. PlanningSceneMonitor的使用示例 #

MoveIt推荐用PlanningSceneMonitor管理规划场景，它能自动监听ROS话题（如/collision_object、/attached_collision_object）更新场景数据，保证实时性。以下是基础使用框架：

#include <moveit/planning_scene_monitor/planning_scene_monitor.h>

// 初始化PlanningSceneMonitor，传入机器人描述参数
const std::string PLANNING_GROUP = "panda_arm"; // Franka Panda的规划组名称
planning_scene_monitor::PlanningSceneMonitorPtr psm =
    std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");

// 启动各类监听器
psm->startSceneMonitor();
psm->startWorldGeometryMonitor();
psm->startStateMonitor();

// 等待场景初始化完成
if (!psm->waitForCurrentRobotState(ros::Duration(5.0))) {
    ROS_ERROR("Failed to get current robot state");
    return -1;
}

// 获取当前规划场景
planning_scene::PlanningSceneConstPtr planning_scene = psm->getPlanningScene();

// 从规划场景中获取已包含机器人模型和障碍物的CollisionEnv
const collision_detection::CollisionEnvConstPtr& collision_env = planning_scene->getCollisionEnv();

// 调用distanceRobot方法（需准备DistanceRequest、DistanceResult和当前机器人状态）
collision_detection::DistanceRequest dist_req;
collision_detection::DistanceResult dist_res;
dist_req.enable_nearest_points = true;
dist_req.enable_signed_distance = true;

robot_state::RobotStateConstPtr robot_state = planning_scene->getCurrentState();
collision_env->distanceRobot(dist_res, dist_req, *robot_state);

// 处理结果...

你也可以参考MoveIt官方示例包moveit_tutorials中的planning_scene相关节点，核心逻辑均基于PlanningSceneMonitor实现。

内容的提问来源于stack exchange，提问作者eln