在您提供的代码中，有几个关键问题和潜在的误解可能导致 DistanceMap 为空。以下是一些可能的原因和修改建议：
未正确设置或请求距离结果：
在您的代码中，您创建了一个 DistanceRequest 对象 distance_request，但没有在调用 distanceToCollision 方法时使用它。distanceToCollision 方法需要一个 DistanceRequest 对象来定义计算距离时的参数。由于您没有提供这个请求，MoveIt 可能没有按预期计算距离信息。
distanceToCollision 方法的使用：
您的代码中调用了 planning_scene.distanceToCollision(current_state);，但这个方法实际上需要一个 DistanceRequest 对象作为参数。正确的方法是传递一个已配置的 DistanceRequest 对象。
检查 DistanceResult 的时机：
您需要在调用 distanceToCollision 方法后检查 DistanceResult。由于您没有正确调用 distanceToCollision，所以 DistanceResult 可能没有被正确填充。
代码中的逻辑错误：
您在代码中提前定义了 distance_result 和 distance_result_map，这些变量在 distanceToCollision 调用之前就被初始化了，但此时它们还未被实际填充数据。
下面是修改后的代码片段，以正确设置和使用 DistanceRequest 并检查 DistanceResult：
#include <moveit/collision_detection/collision_detection_fcl.h> #include <moveit/planning_scene/planning_scene.h> // ... 其他必要的头文件和命名空间 int main(int argc, char **argv) { // ... 初始化节点、加载机器人模型等代码 // 设置碰撞检测请求 collision_detection::CollisionRequest collision_request; collision_request.distance = true; collision_request.detailed_distance = true; collision_detection::CollisionResult collision_result; // 设置距离请求 collision_detection::DistanceRequest distance_request; distance_request.enabled = true; // 确保距离计算是启用的 // 加载关节位置和设置当前状态（您的代码已包含这部分） // 检查自碰撞（可选） planning_scene.checkSelfCollision(collision_request, collision_result); // 计算距离到碰撞 planning_scene.distanceToCollision(current_state, distance_request, collision_result); // 输出结果 RCLCPP_INFO_STREAM(LOGGER, "Test 5: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); if (!collision_result.contacts.empty()) { for (const auto& contact : collision_result.contacts) { RCLCPP_INFO(LOGGER, "Contact between: %s and %s", contact.first.first.c_str(), contact.first.second.c_str()); } } RCLCPP_INFO(LOGGER, "Closest distance: %lf", collision_result.distance); const collision_detection::DistanceResult& distance_result = collision_result.distance_result; const collision_detection::DistanceMap& distance_result_map = distance_result.distances; if (!distance_result_map.empty()) { RCLCPP_INFO(LOGGER, "Distance map is not empty"); // 可以进一步处理 distance_result_map } else { RCLCPP_INFO(LOGGER, "Distance map is empty"); } // ... 其他代码和节点关闭 }确保 DistanceRequest 中的 enabled 设置为 true，并且正确地将 DistanceRequest 传递给 distanceToCollision 方法。这样，MoveIt 才能正确地计算和填充 DistanceResult。
以上为生成回答，仅供参考～
