如何基于MoveIt当前活动环境初始化CollisionEnvFCL以调用distanceRobot()？

我希望使用MoveIt的collision_detection::CollisionEnvFCL::distanceRobot()方法，计算机器人与物体之间的最小距离及该方法提供的其他相关信息。但我不知道如何基于包含机器人和物体的当前活动环境，正确初始化collision_detection::CollisionEnvFCL。以下是我的尝试代码：

#include <ros/ros.h>

// MoveIt
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/collision_detection_fcl/collision_env_fcl.h>
#include <moveit_msgs/GetPlanningScene.h>


int main(int argc, char** argv)
{
  ros::init(argc, argv, "panda_coll_detection");
  ros::AsyncSpinner spinner(8);
  spinner.start();

  // ---------------------------------------------------------------------------
  // Define interfaces
  // ---------------------------------------------------------------------------

  // Define move_group_interface & planning_scene_interface
  static const std::string PLANNING_GROUP = "panda_arm";
  moveit::planning_interface::MoveGroupInterface move_group_interface(PLANNING_GROUP);
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface; 

  // Define robot model
  robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
  const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel();

  // ---------------------------------------------------------------------------
  // Add a collision object
  // ---------------------------------------------------------------------------

  // Define a collision object ROS message for the robot to avoid
  moveit_msgs::CollisionObject collision_object;
  collision_object.header.frame_id = move_group_interface.getPlanningFrame();
  collision_object.id = "box1";

  // Define a box to add to the world
  shape_msgs::SolidPrimitive primitive;
  primitive.type = primitive.BOX;
  primitive.dimensions.resize(3);
  primitive.dimensions[primitive.BOX_X] = 0.4;
  primitive.dimensions[primitive.BOX_Y] = 0.02;
  primitive.dimensions[primitive.BOX_Z] = 0.02;

  // Define a pose for the box (specified relative to frame_id)
  geometry_msgs::Pose box_pose;
  box_pose.orientation.w = 1.0;
  box_pose.position.x = 1.0;//0.5
  box_pose.position.y = 0.0;
  box_pose.position.z = 0.95; //0.35

  // Add box to collision_object
  collision_object.primitives.push_back(primitive);
  collision_object.primitive_poses.push_back(box_pose);
  collision_object.operation = collision_object.ADD;

  // Define a vector of collision objects that could contain additional objects
  std::vector<moveit_msgs::CollisionObject> collision_objects;
  collision_objects.push_back(collision_object);

  // Add the collision object into the world  
  planning_scene_interface.applyCollisionObjects(collision_objects);
    
  // ---------------------------------------------------------------------------
  // Get (updated) planning scene
  // ---------------------------------------------------------------------------
  auto planning_scene = std::make_shared<planning_scene::PlanningScene>(kinematic_model);

  ros::NodeHandle h;
  ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");

  ros::Duration timeout(1.0);
  if (client.waitForExistence(timeout)) {
    moveit_msgs::GetPlanningScene::Request req;
    moveit_msgs::GetPlanningScene::Response res;

    if (client.call(req, res))
        planning_scene->setPlanningSceneMsg(res.scene); // apply result to actual PlanningScene
  }

    
  // ---------------------------------------------------------------------------
  // Collision Checking with CollisionEnvFCL.distanceRobot()
  // ---------------------------------------------------------------------------
  
  // Define distance request & result
  auto distance_request = collision_detection::DistanceRequest();
  auto distance_result = collision_detection::DistanceResult();
  distance_request.enable_nearest_points =true;

  // Get robot state 
  moveit::core::RobotState copied_state2 = planning_scene->getCurrentState();
  
  // Get the active collision environment as a CollisionEnvFCL type
  // -------- This is where I need help ---------
  const collision_detection::CollisionEnvConstPtr c_env = planning_scene->getCollisionEnv();
  collision_detection::CollisionEnvFCL collision_env = ???
  // --------------------------------------------

  
  // Compute the shortest distance between the robot and the world
  collision_env.distanceRobot(distance_request, distance_result, copied_state2);

  // Show result
  ROS_INFO_STREAM("Current state is " << (distance_result.collision ? "in" : "not in") << " collision");
  ROS_INFO_STREAM("Min distance between two bodies=" << distance_result.minimum_distance.distance << "\n");

  ros::shutdown();
  return 0;
}

请问我该如何基于当前活动环境初始化collision_detection::CollisionEnvFCL？是否需要将c_env转换为CollisionEnvFCL类型？如果需要，具体该如何操作？

核心思路 #

planning_scene->getCollisionEnv()返回的是基类CollisionEnv的const智能指针，当你的MoveIt配置使用FCL作为碰撞检测后端时，底层实际实例就是CollisionEnvFCL，因此你需要通过动态类型转换来获取对应的子类指针，而非直接初始化新的CollisionEnvFCL实例（这样会丢失当前场景中的碰撞物体信息）。

具体修改步骤 #

1. 使用std::dynamic_pointer_cast将CollisionEnvConstPtr转换为CollisionEnvFCLConstPtr（注意保持const属性，因为getCollisionEnv()返回的是不可修改的环境指针）。
2. 检查转换后的指针是否有效，避免空指针访问。
3. 调用distanceRobot()方法（该方法是const成员函数，可通过const指针调用）。

修改后的关键代码段 #

替换原代码中需要帮助的部分：

// Get the active collision environment as a CollisionEnvFCL type
const collision_detection::CollisionEnvConstPtr c_env = planning_scene->getCollisionEnv();
// 动态转换为CollisionEnvFCL的const智能指针
const collision_detection::CollisionEnvFCLConstPtr collision_env_fcl = 
    std::dynamic_pointer_cast<const collision_detection::CollisionEnvFCL>(c_env);

// 检查转换是否成功
if (!collision_env_fcl) {
    ROS_ERROR("Failed to cast CollisionEnv to CollisionEnvFCL! Is FCL enabled in MoveIt?");
    ros::shutdown();
    return 1;
}

// Compute the shortest distance between the robot and the world
collision_env_fcl->distanceRobot(distance_request, distance_result, copied_state2);

额外注意事项 #

• 确保你的MoveIt配置启用了FCL碰撞检测后端（默认情况下大部分MoveIt配置都是启用的）。
• distance_request需要根据需求设置参数，比如你已经开启了enable_nearest_points，这样可以在distance_result中获取最近点的坐标信息。
• distanceRobot()方法会修改传入的distance_result对象，确保它被正确初始化。

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