如何使用Moveit2做距离检测

[Jason_Duan]

如题，需要做距离检测，比如，想要知道某状态下距离发生碰撞还有多少距离。我看CollisionResult中是有distance和DistanceResult信息的，CollisionRequest中也将distance属性设为了true，但是CollisionResult中还是拿不到距离数据，结果总是1.79769e+308，即std::numeric_limits<double>::max()的值。
也试了其他的方法，都不行。
Ros2版本为humble。
以下是我的代码(请忽略引用过多的头文件)：

#include <iostream>
#include <ostream>
#include <memory>
#include <vector>

#include <rclcpp/rclcpp.hpp>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit/moveit_cpp/moveit_cpp.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit_msgs/msg/planning_scene.hpp>
#include <moveit/collision_detection_fcl/collision_common.h>
#include <moveit/collision_detection_fcl/collision_env_fcl.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit/moveit_cpp/moveit_cpp.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/collision_detection/collision_detector_allocator.h>
#include <moveit/collision_detection_fcl/collision_common.h>
#include <moveit/collision_detection_fcl/collision_env_fcl.h>
#include <moveit/collision_detection_fcl/collision_detector_allocator_fcl.h>
#include <moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h>
#include <moveit/collision_detection_fcl/fcl_compat.h>
#include <moveit/rdf_loader/rdf_loader.h>

// #include <shape_msgs/msg/solid_primitive.hpp>

#define PI 3.1415926

using namespace std;

static const rclcpp::Logger LOGGER = rclcpp::get_logger("collision_node");

int main(int argc, char **argv)
{
  // ROS2 init
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions node_options;
  node_options.automatically_declare_parameters_from_overrides(true);
  auto collision_node = rclcpp::Node::make_shared("collision_node", node_options);

  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(collision_node);
  std::thread([&executor]()
              { executor.spin(); })
      .detach();
  robot_model_loader::RobotModelLoader robot_model_loader(collision_node, "robot_description");
  const moveit::core::RobotModelPtr &robot_model = robot_model_loader.getModel();
  planning_scene::PlanningScene planning_scene(robot_model);
  moveit::core::RobotState &current_state = planning_scene.getCurrentStateNonConst();

  // ----- check SRDF ----- //
  srdf::ModelConstSharedPtr model_srdf = robot_model->getSRDF();
  RCLCPP_INFO_STREAM(LOGGER, "SRDF model load " << (model_srdf ? "SUCCESSED." : "FAILED!"));

  // ----- check ACM ----- //
  collision_detection::AllowedCollisionMatrix &acm = planning_scene.getAllowedCollisionMatrixNonConst();
  RCLCPP_INFO_STREAM(LOGGER, "acm.Size = " << acm.getSize());
  std::vector<std::string> allEntrynames;
  acm.getAllEntryNames(allEntrynames);
  for (auto name : allEntrynames)
    RCLCPP_INFO_STREAM(LOGGER, "Entryname : " << name);
  // acm.print(std::cout);

  // ----- check disabled and enabled collision pairs ----- //
  std::vector<srdf::Model::CollisionPair> disabled_collision_pairs = model_srdf->getDisabledCollisionPairs();
  std::vector<srdf::Model::CollisionPair> enabled_collision_pairs = model_srdf->getEnabledCollisionPairs();
  RCLCPP_INFO_STREAM(LOGGER, "enabled collision pair size: " << enabled_collision_pairs.size());
  // for (auto it : enabled_collision_pairs) RCLCPP_INFO_STREAM(LOGGER, "enabled collision pair: (" << it.link1_ << "  -  " << it.link2_ << ")");
  RCLCPP_INFO_STREAM(LOGGER, "disabled collision pair size: (" << disabled_collision_pairs.size());
  for (auto it : disabled_collision_pairs)
    RCLCPP_INFO_STREAM(LOGGER, "disabled collision pair: (" << it.link1_ << "  -  " << it.link2_ << ") | reason: " << it.reason_);

  // ----- use CollisionEnvFCL -> distanceRobot() ----- //
  {
    RCLCPP_INFO_STREAM(LOGGER, "----- use CollisionEnvFCL -> distanceRobot() -----");
    RCLCPP_INFO_STREAM(LOGGER, "std::numeric_limits<double>::max = " << std::numeric_limits<double>::max());

    current_state.setToDefaultValues();
    collision_detection::CollisionEnvFCL collision_env(robot_model, 0.0, 1.0);

    collision_detection::DistanceRequest distance_request;
    distance_request.enable_nearest_points = true;
    distance_request.enable_signed_distance = true;
    distance_request.type = collision_detection::DistanceRequestType::GLOBAL;
    distance_request.distance_threshold = 10;

    collision_detection::DistanceResult distance_result;
    collision_detection::DistanceResultsData distance_res_data = distance_result.minimum_distance;
    auto min_dis_link_names = distance_res_data.link_names;
    double distance_res_data_min_distance = distance_res_data.distance;
    collision_detection::DistanceMap distance_map = distance_result.distances;

    distance_result.clear();
    current_state.setToDefaultValues();
    RCLCPP_INFO(LOGGER, "  Default Positions >>>");
    collision_env.distanceRobot(distance_request, distance_result, current_state);
    RCLCPP_INFO_STREAM(LOGGER, "\tMinimum distance between: " << min_dis_link_names[0] << " and " << min_dis_link_names[1]);
    RCLCPP_INFO_STREAM(LOGGER, "\tMinimum distance is : " << distance_result.minimum_distance.distance);
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance_result.minimum_distance.distance : " << distance_res_data_min_distance);
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance_map is " << (distance_map.empty() ? "" : "not") << " empty");

    distance_result.clear();
    current_state.setToRandomPositions();
    RCLCPP_INFO_STREAM(LOGGER, "  Random Positions >>>");
    collision_env.distanceRobot(distance_request, distance_result, current_state);
    RCLCPP_INFO_STREAM(LOGGER, "\tMinimum distance between: " << min_dis_link_names[0] << " and " << min_dis_link_names[1]);
    RCLCPP_INFO_STREAM(LOGGER, "\tMinimum distance is : " << distance_result.minimum_distance.distance);
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance_result.minimum_distance.distance : " << distance_res_data_min_distance);
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance_map is " << (distance_map.empty() ? "" : "not") << " empty");

    RCLCPP_INFO_STREAM(LOGGER, "===== END : use CollisionEnvFCL -> distanceRobot() =====" << std::endl);
  }

  // ----- use PlanningScene -> checkSelfCollision() ----- //
  {
    RCLCPP_INFO_STREAM(LOGGER, "----- use PlanningScene -> checkSelfCollision() -----");
    collision_detection::CollisionRequest collision_request;
    collision_request.distance = true;

    collision_detection::CollisionResult collision_result;
    double distance = collision_result.distance;
    collision_detection::DistanceResult distance_result = collision_result.distance_result;
    collision_detection::DistanceResultsData distance_res_data = distance_result.minimum_distance;
    double distance_res_data_min_distance = distance_res_data.distance;
    collision_detection::DistanceMap distance_map = distance_result.distances;

    collision_result.clear();
    current_state.setToDefaultValues();
    RCLCPP_INFO(LOGGER, "  Default Positions >>>");
    planning_scene.checkSelfCollision(collision_request, collision_result);
    RCLCPP_INFO_STREAM(LOGGER, "\tCurrent state is " << (collision_result.collision ? "in" : "not in") << " self collision");
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance : " << distance);
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance_result.minimum_distance.distance : " << distance_res_data_min_distance);
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance_map is " << (distance_map.empty() ? "" : "not") << " empty");

    bool loopFlag = true;
    while (loopFlag)
    {
      collision_result.clear();
      current_state.setToRandomPositions();
      RCLCPP_INFO_STREAM(LOGGER, "  Random Positions >>>");
      planning_scene.checkSelfCollision(collision_request, collision_result);
      if (collision_result.collision)
        loopFlag = false;
      RCLCPP_INFO_STREAM(LOGGER, "\tCurrent state is " << (collision_result.collision ? "in" : "not in") << " self collision");
      RCLCPP_INFO_STREAM(LOGGER, "\tdistance : " << distance);
      RCLCPP_INFO_STREAM(LOGGER, "\tdistance_result.minimum_distance.distance : " << distance_res_data_min_distance);
      RCLCPP_INFO_STREAM(LOGGER, "\tdistance_map is " << (distance_map.empty() ? "" : "not") << " empty");
    }

    RCLCPP_INFO_STREAM(LOGGER, "===== END : use PlanningScene -> checkSelfCollision() =====" << std::endl);
  }

  // ----- use PlanningScene -> distanceToCollision() ----- //
  {
    RCLCPP_INFO_STREAM(LOGGER, "----- use PlanningScene -> checkSelfCollision() -----");

    current_state.setToDefaultValues();
    RCLCPP_INFO(LOGGER, "  Default Positions >>>");
    double distance_default_pos = planning_scene.distanceToCollision(current_state);
    RCLCPP_INFO_STREAM(LOGGER, "    distance : " << distance_default_pos);

    current_state.setToRandomPositions();
    RCLCPP_INFO(LOGGER, "  Random Positions >>>");
    double distance_random_pos = planning_scene.distanceToCollision(current_state);
    RCLCPP_INFO_STREAM(LOGGER, "    distance : " << distance_random_pos);

    RCLCPP_INFO_STREAM(LOGGER, "===== END : use PlanningScene -> checkSelfCollision() =====" << std::endl);
  }

  // ----- use PlanningScene -> checkCollision() ----- //
  {
    RCLCPP_INFO_STREAM(LOGGER, "----- use PlanningScene -> checkCollision() -----");

    collision_detection::CollisionRequest collision_request;
    collision_detection::CollisionResult collision_result;
    double distance = collision_result.distance;
    collision_detection::DistanceResult distance_result = collision_result.distance_result;
    collision_detection::DistanceResultsData distance_res_data = distance_result.minimum_distance;
    double distance_res_data_min_distance = distance_res_data.distance;
    collision_detection::DistanceMap distance_map = distance_result.distances;

    collision_result.clear();
    current_state.setToDefaultValues();
    RCLCPP_INFO(LOGGER, "  Default Positions >>>");
    planning_scene.checkCollision(collision_request, collision_result);
    RCLCPP_INFO_STREAM(LOGGER, "\tCurrent state is " << (collision_result.collision ? "in" : "not in") << " self collision");
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance : " << distance);
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance_result.minimum_distance.distance : " << distance_res_data_min_distance);
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance_map is " << (distance_map.empty() ? "" : "not") << " empty");

    bool loopFlag = true;
    while (loopFlag)
    {
      collision_result.clear();
      current_state.setToRandomPositions();
      RCLCPP_INFO_STREAM(LOGGER, "  Random Positions >>>");
      planning_scene.checkCollision(collision_request, collision_result);
      if (collision_result.collision)
        loopFlag = false;
      RCLCPP_INFO_STREAM(LOGGER, "\tCurrent state is " << (collision_result.collision ? "in" : "not in") << " self collision");
      RCLCPP_INFO_STREAM(LOGGER, "\tdistance : " << distance);
      RCLCPP_INFO_STREAM(LOGGER, "\tdistance_result.minimum_distance.distance : " << distance_res_data_min_distance);
      RCLCPP_INFO_STREAM(LOGGER, "\tdistance_map is " << (distance_map.empty() ? "" : "not") << " empty");
    }

    RCLCPP_INFO_STREAM(LOGGER, "===== END : use PlanningScene -> checkCollision() =====" << std::endl);
  }

  rclcpp::shutdown();
  return 0;
}

[Jason_Duan]

此回复已被删除！

[Jason_Duan]

@Jason_Duan 在 如何使用Moveit2做距离检测 中说：

PlanningScene -> checkCollision()

补充说明：在PlanningScene -> checkCollision()这个方式中，将collision_request.distance设为true，也不行

[Jason_Duan]

自顶一下，不要沉啊

[Jason_Duan]

更新：新增使用CollisionEnvFCL::distanceSelf，设置方式和上述代码中使用CollisionEnvFCL::distanceRobot的一致，结果一样

[Jason_Duan]

搞定了，有需要的自己看吧

#include <iostream>
#include <ostream>
#include <memory>
#include <vector>

#include <rclcpp/rclcpp.hpp>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit/moveit_cpp/moveit_cpp.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit_msgs/msg/planning_scene.hpp>
#include <moveit/collision_detection_fcl/collision_common.h>
#include <moveit/collision_detection_fcl/collision_env_fcl.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit/moveit_cpp/moveit_cpp.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/collision_detection/collision_detector_allocator.h>
#include <moveit/collision_detection_fcl/collision_common.h>
#include <moveit/collision_detection_fcl/collision_env_fcl.h>
#include <moveit/collision_detection_fcl/collision_detector_allocator_fcl.h>
#include <moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h>
#include <moveit/collision_detection_fcl/fcl_compat.h>
#include <moveit/rdf_loader/rdf_loader.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>

// #include <shape_msgs/msg/solid_primitive.hpp>

#define PI 3.1415926

using namespace std;

static const rclcpp::Logger LOGGER = rclcpp::get_logger("collision_node");

int main(int argc, char **argv)
{
  // ROS2 init
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions node_options;
  node_options.automatically_declare_parameters_from_overrides(true);
  auto collision_node = rclcpp::Node::make_shared("collision_node", node_options);

  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(collision_node);
  std::thread([&executor]()
              { executor.spin(); })
      .detach();
  robot_model_loader::RobotModelLoader robot_model_loader(collision_node, "robot_description");
  const moveit::core::RobotModelPtr &robot_model = robot_model_loader.getModel();
  planning_scene::PlanningScene planning_scene(robot_model);
  moveit::core::RobotState &current_state = planning_scene.getCurrentStateNonConst();

  // ----- check SRDF ----- //
  // srdf::ModelConstSharedPtr model_srdf = robot_model->getSRDF();
  // RCLCPP_INFO_STREAM(LOGGER, "SRDF model load " << (model_srdf ? "SUCCESS." : "FAILED!")); // success

  // ----- check ACM ----- //
  collision_detection::AllowedCollisionMatrix &acm = planning_scene.getAllowedCollisionMatrixNonConst();
  // acm.print(std::cout);
  // acm.setEntry("panda_link5", "panda_hand", true); // in collision,-0.045408
  // acm.setEntry("panda_link5", "panda_link7", true); // in collision,-0.0131087
  // acm.setEntry("panda_link5", "panda_rightfinger", true); // not in collision,0.00929973
  // acm.setEntry("panda_link5", "panda_leftfinger", true); // 0.0284744
  // acm.setEntry("panda_link5", "panda_link2", true); // 0.241446

  // RCLCPP_INFO_STREAM(LOGGER, "acm.Size = " << acm.getSize());
  // std::vector<std::string> allEntrynames;
  // acm.getAllEntryNames(allEntrynames);
  // for (auto name : allEntrynames) RCLCPP_INFO_STREAM(LOGGER, "Entryname : " << name);
  // acm.print(std::cout);

  // ----- check disabled and enabled collision pairs ----- //
  // std::vector<srdf::Model::CollisionPair> disabled_collision_pairs = model_srdf->getDisabledCollisionPairs();
  // std::vector<srdf::Model::CollisionPair> enabled_collision_pairs = model_srdf->getEnabledCollisionPairs();
  // RCLCPP_INFO_STREAM(LOGGER, "enabled collision pair size: " << enabled_collision_pairs.size()); // empty
  // for (auto it : enabled_collision_pairs) RCLCPP_INFO_STREAM(LOGGER, "enabled collision pair: (" << it.link1_ << "  -  " << it.link2_ << ")");
  // RCLCPP_INFO_STREAM(LOGGER, "disabled collision pair size: (" << disabled_collision_pairs.size());
  // for (auto it : disabled_collision_pairs)
  //   RCLCPP_INFO_STREAM(LOGGER, "disabled collision pair: (" << it.link1_ << "  -  " << it.link2_ << ") | reason: " << it.reason_);

  // ----- 01:use CollisionEnvFCL -> distanceRobot() ----- //
  /*
  {
    RCLCPP_INFO_STREAM(LOGGER, "----- 01:use CollisionEnvFCL -> distanceRobot() -----");
    // RCLCPP_INFO_STREAM(LOGGER, "std::numeric_limits<double>::max = " << std::numeric_limits<double>::max());

    current_state.setToDefaultValues();
    collision_detection::CollisionEnvFCL collision_env(robot_model, 0.0, 1.0);

    collision_detection::DistanceRequest distance_request;
    distance_request.enable_nearest_points = true;
    distance_request.enable_signed_distance = true;
    distance_request.type = collision_detection::DistanceRequestType::GLOBAL;
    distance_request.distance_threshold = 10;
    // distance_request.enableGroup();
    if (nullptr == distance_request.active_components_only)
      RCLCPP_INFO(LOGGER, "active_components_only is not NULL");
    else
      RCLCPP_INFO_STREAM(LOGGER, "active_components_only is " << distance_request.active_components_only);

    collision_detection::DistanceResult distance_result;
    collision_detection::DistanceResultsData min_distance = distance_result.minimum_distance;
    auto min_dis_link_names = min_distance.link_names;
    double min_distance_min_distance = min_distance.distance;
    collision_detection::DistanceMap distance_map = distance_result.distances;

    distance_result.clear();
    current_state.setToDefaultValues();
    RCLCPP_INFO(LOGGER, "  Default Positions >>>");
    collision_env.distanceRobot(distance_request, distance_result, current_state);
    RCLCPP_INFO_STREAM(LOGGER, "\tMinimum distance between: " << min_dis_link_names[0] << " and " << min_dis_link_names[1]);
    RCLCPP_INFO_STREAM(LOGGER, "\tMinimum distance is : " << distance_result.minimum_distance.distance);
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance_result.minimum_distance.distance : " << min_distance_min_distance);
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance_map is " << (distance_map.empty() ? "" : "not") << " empty");

    distance_result.clear();
    current_state.setToRandomPositions();
    RCLCPP_INFO_STREAM(LOGGER, "  Random Positions >>>");
    collision_env.distanceRobot(distance_request, distance_result, current_state);
    RCLCPP_INFO_STREAM(LOGGER, "\tMinimum distance between: " << min_dis_link_names[0] << " and " << min_dis_link_names[1]); // both are empty
    RCLCPP_INFO_STREAM(LOGGER, "\tMinimum distance is : " << distance_result.minimum_distance.distance);                     // 1.79769e+308
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance_result.minimum_distance.distance : " << min_distance_min_distance);          // 1.79769e+308
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance_map is " << (distance_map.empty() ? "" : "not") << "empty");                      // empty

    RCLCPP_INFO_STREAM(LOGGER, "===== END : 02:use CollisionEnvFCL -> distanceRobot() =====" << std::endl);
  }
  */

  // ----- 02:use CollisionEnv -> distanceSelf() ----- //
  /*
  {
    RCLCPP_INFO_STREAM(LOGGER, "----- 02:use CollisionEnvFCL -> distanceSelf() -----");

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

    collision_detection::DistanceRequest distance_request;
    distance_request.enable_nearest_points = true;
    distance_request.enable_signed_distance = true;
    distance_request.type = collision_detection::DistanceRequestType::GLOBAL;
    distance_request.distance_threshold = 10;
    if (nullptr == distance_request.active_components_only)
      RCLCPP_INFO(LOGGER, "active_components_only is not NULL");
    else
      RCLCPP_INFO_STREAM(LOGGER, "active_components_only is " << distance_request.active_components_only);

    collision_detection::DistanceResult distance_result;
    collision_detection::DistanceResultsData min_distance = distance_result.minimum_distance;
    auto min_dis_link_names = min_distance.link_names;
    double min_distance_min_distance = min_distance.distance;
    collision_detection::DistanceMap distance_map = distance_result.distances;

    distance_result.clear();
    current_state.setToDefaultValues();
    RCLCPP_INFO(LOGGER, "  Default Positions >>>");
    collision_env.distanceSelf(distance_request, distance_result, current_state);
    RCLCPP_INFO_STREAM(LOGGER, "\tMinimum distance between: " << min_dis_link_names[0] << " and " << min_dis_link_names[1]);
    RCLCPP_INFO_STREAM(LOGGER, "\tMinimum distance is : " << distance_result.minimum_distance.distance);
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance_result.minimum_distance.distance : " << min_distance_min_distance);
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance_map is " << (distance_map.empty() ? "" : "not") << " empty");

    distance_result.clear();
    current_state.setToRandomPositions();
    RCLCPP_INFO_STREAM(LOGGER, "  Random Positions >>>");
    collision_env.distanceSelf(distance_request, distance_result, current_state);
    RCLCPP_INFO_STREAM(LOGGER, "\tMinimum distance between: " << min_dis_link_names[0] << " and " << min_dis_link_names[1]); // both are empty
    RCLCPP_INFO_STREAM(LOGGER, "\tMinimum distance is : " << distance_result.minimum_distance.distance);                     // 1.79769e+308
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance_result.minimum_distance.distance : " << min_distance_min_distance);          // 1.79769e+308
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance_map is " << (distance_map.empty() ? "" : "not") << "empty");                      // empty

    RCLCPP_INFO_STREAM(LOGGER, "===== END : 02:use CollisionEnvFCL -> distanceSelf() =====" << std::endl);
  }
  */

  // ----- 03:use PlanningScene -> checkSelfCollision() ----- //
  /*
  {
    RCLCPP_INFO_STREAM(LOGGER, "----- use PlanningScene -> checkSelfCollision() -----");
    collision_detection::CollisionRequest collision_request;
    collision_request.distance = true;
    collision_request.verbose = true;

    collision_detection::CollisionResult collision_result;
    double minimum_distance_distance;

    collision_result.clear();
    current_state.setToDefaultValues();
    current_state.setToRandomPositions();
    RCLCPP_INFO(LOGGER, "  Default Positions >>>");
    planning_scene.checkSelfCollision(collision_request, collision_result);
    ///////////////////////////////////////////////////////////////////////
    collision_detection::DistanceResult distance_result = collision_result.distance_result; // Result of a distance request
    collision_detection::DistanceResultsData minimum_distance = distance_result.minimum_distance; // ResultsData for the two objects with the minimum distance
    minimum_distance_distance = minimum_distance.distance; // distance for the two objects with the minimum distance
    std::string* link_names = minimum_distance.link_names; // link names for the two objects with the minimum distance
    collision_detection::DistanceMap distances_map = distance_result.distances; // A map of distance data for each link in the req.Notice : active_components_only !!!

    RCLCPP_INFO_STREAM(LOGGER, "\tCurrent state is " << (collision_result.collision ? "in" : "not in") << " self collision"); // in self collision
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance : " << collision_result.distance);

    RCLCPP_INFO_STREAM(LOGGER, "\tdistance_result.minimum_distance.distance : " << minimum_distance_distance);           // 1.79769e+308????
    RCLCPP_INFO_STREAM(LOGGER, "\tBetween " << link_names[0] << " and " << link_names[1]); // empty!!!
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance_map.size = " << distances_map.size()); // empty!!!

    bool loopFlag = true;
    while (loopFlag)
    {
      collision_result.clear();
      current_state.setToRandomPositions();
      RCLCPP_INFO_STREAM(LOGGER, "  Random Positions >>>");
      planning_scene.checkSelfCollision(collision_request, collision_result);
      ///////////////////////////////////////////////////////////////////////
      if (collision_result.collision) loopFlag = false;
      collision_detection::DistanceResult distance_result = collision_result.distance_result; // Result of a distance request
      collision_detection::DistanceResultsData minimum_distance = distance_result.minimum_distance; // ResultsData for the two objects with the minimum distance
      minimum_distance_distance = minimum_distance.distance; // distance for the two objects with the minimum distance
      std::string* link_names = minimum_distance.link_names; // link names for the two objects with the minimum distance
      collision_detection::DistanceMap distances_map = distance_result.distances; // A map of distance data for each link in the req.Notice : active_components_only !!!

      RCLCPP_INFO_STREAM(LOGGER, "\tCurrent state is " << (collision_result.collision ? "in" : "not in") << " self collision"); // in self collision
      RCLCPP_INFO_STREAM(LOGGER, "\tdistance : " << collision_result.distance);


      RCLCPP_INFO_STREAM(LOGGER, "\tdistance_result.minimum_distance.distance : " << minimum_distance_distance);           // 1.79769e+308????
      RCLCPP_INFO_STREAM(LOGGER, "\tBetween " << link_names[0] << " and " << link_names[1]); // empty!!!
      RCLCPP_INFO_STREAM(LOGGER, "\tdistance_map.size = " << distances_map.size()); // empty!!!
    }

    RCLCPP_INFO_STREAM(LOGGER, "===== END : use PlanningScene -> checkSelfCollision() =====" << std::endl);
  }
  */

  // ----- 04:use PlanningScene -> distanceToCollision() ----- //
  /*
  {
    RCLCPP_INFO_STREAM(LOGGER, "----- use PlanningScene -> checkSelfCollision() -----");

    current_state.setToDefaultValues();
    RCLCPP_INFO(LOGGER, "  Default Positions >>>");
    double distance_default_pos = planning_scene.distanceToCollision(current_state);
    RCLCPP_INFO_STREAM(LOGGER, "    distance : " << distance_default_pos); // 1.79769e+308

    current_state.setToRandomPositions();
    RCLCPP_INFO(LOGGER, "  Random Positions >>>");
    double distance_random_pos = planning_scene.distanceToCollision(current_state);
    RCLCPP_INFO_STREAM(LOGGER, "    distance : " << distance_random_pos); // 1.79769e+308

    RCLCPP_INFO_STREAM(LOGGER, "===== END : use PlanningScene -> checkSelfCollision() =====" << std::endl);
  }
  */

  // ----- 05:use PlanningScene -> checkCollision() ----- //
  /*
  {
    RCLCPP_INFO_STREAM(LOGGER, "----- use PlanningScene -> checkCollision() -----");

    collision_detection::CollisionRequest collision_request;
    collision_request.distance = true;

    collision_detection::CollisionResult collision_result;
    double distance = collision_result.distance;
    collision_detection::DistanceResult distance_result = collision_result.distance_result;
    collision_detection::DistanceResultsData min_distance = distance_result.minimum_distance;
    double min_distance_min_distance = min_distance.distance;
    collision_detection::DistanceMap distance_map = distance_result.distances;

    collision_result.clear();
    current_state.setToDefaultValues();
    RCLCPP_INFO(LOGGER, "  Default Positions >>>");
    planning_scene.checkCollision(collision_request, collision_result);
    RCLCPP_INFO_STREAM(LOGGER, "\tCurrent state is " << (collision_result.collision ? "in" : "not in") << " self collision");
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance : " << distance);                                                        // 1.79769e+308
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance_result.minimum_distance.distance : " << min_distance_min_distance); // 1.79769e+308
    RCLCPP_INFO_STREAM(LOGGER, "\tdistance_map is " << (distance_map.empty() ? "" : "not") << " empty");            // empty

    bool loopFlag = true;
    while (loopFlag)
    {
      collision_result.clear();
      current_state.setToRandomPositions();
      RCLCPP_INFO_STREAM(LOGGER, "  Random Positions >>>");
      planning_scene.checkCollision(collision_request, collision_result);
      // if (collision_result.collision)
        loopFlag = false;
      RCLCPP_INFO_STREAM(LOGGER, "\tCurrent state is " << (collision_result.collision ? "in" : "not in") << " self collision");
      RCLCPP_INFO_STREAM(LOGGER, "\tdistance : " << distance);                                                        // 1.79769e+308
      RCLCPP_INFO_STREAM(LOGGER, "\tdistance_result.minimum_distance.distance : " << min_distance_min_distance); // 1.79769e+308
      RCLCPP_INFO_STREAM(LOGGER, "\tdistance_map is " << (distance_map.empty() ? "" : "not") << " empty");            // empty
    }

    RCLCPP_INFO_STREAM(LOGGER, "===== END : use PlanningScene -> checkCollision() =====" << std::endl);
  }
  */

  // ----- 06:use collision_env = planning_scene.getCollisionEnv() ----- //
  // /*
  {
    RCLCPP_INFO_STREAM(LOGGER, "----- use collision_env = planning_scene.getCollisionEnv() -----");

    // collision_detection::CollisionEnvFCL collision_env(robot_model, 0.0, 1.0);
    auto collision_env = planning_scene.getCollisionEnv();

    collision_detection::DistanceRequest distance_request;
    distance_request.enable_nearest_points = true;
    distance_request.enable_signed_distance = true;
    // distance_request.type = collision_detection::DistanceRequestType::GLOBAL;
    distance_request.type = collision_detection::DistanceRequestType::ALL;
    distance_request.distance_threshold = 10;
    distance_request.acm = &acm;
    // if (nullptr == distance_request.active_components_only)
    //   RCLCPP_INFO(LOGGER, "active_components_only is not NULL");
    // else
    //   RCLCPP_INFO_STREAM(LOGGER, "active_components_only is " << distance_request.active_components_only); // 0?

    collision_detection::DistanceResult distance_result;
    collision_detection::DistanceResultsData minimum_distance;

    // double min_distance_min_distance = min_distance.distance;
    collision_detection::DistanceMap distances_map;

    std::vector<double> joint_values;
    current_state.setToDefaultValues();
    // current_state.setToRandomPositions();
    current_state.copyJointGroupPositions("panda_arm", joint_values);

    RCLCPP_INFO(LOGGER, "  Default Positions >>>");
    for (auto joint_value : joint_values)
      RCLCPP_INFO_STREAM(LOGGER, "    joint_value: " << joint_value);
    // collision_env->distanceRobot(distance_request, distance_result, current_state);

    distance_result.clear();
    collision_env->distanceSelf(distance_request, distance_result, current_state);
    // collision_env->distanceSelf(current_state, acm);
    //////////////////////////////////////

    // RCLCPP_INFO_STREAM(LOGGER, "  distance with ACM is : " << distance);

    RCLCPP_INFO_STREAM(LOGGER, "  Current state is " << (distance_result.collision ? "in" : "not in") << " self collision"); // in self collision

    minimum_distance = distance_result.minimum_distance;
    // RCLCPP_INFO_STREAM(LOGGER, "  Minimum distance is : " << collision_env->distanceSelf(current_state, acm, true));
    RCLCPP_INFO_STREAM(LOGGER, "  Minimum distance is : " << minimum_distance.distance);

    // auto min_dis_link_names = minimum_distance.link_names;
    std::string *min_dis_link_names = minimum_distance.link_names;
    RCLCPP_INFO_STREAM(LOGGER, "  Minimum distance between: " << min_dis_link_names[0] << " and " << min_dis_link_names[1]);

    distances_map = distance_result.distances;
    RCLCPP_INFO_STREAM(LOGGER, "  distance_map.size = " << distances_map.size());
    if(0 != distances_map.size())
    {
      for(auto distance_pair : distances_map)
      {
        RCLCPP_INFO_STREAM(LOGGER, "  distance between " << distance_pair.first.first << " and " << distance_pair.first.second << " : " << distance_pair.second.at(0).distance);
      }
    }

    // int loop_times = 3;
    // while (loop_times--)
    // {
    //   distance_result.clear();
    //   RCLCPP_INFO_STREAM(LOGGER, "  distance after clear: " << distance_result.minimum_distance.distance); //
    //   current_state.setToRandomPositions();
    //   RCLCPP_INFO_STREAM(LOGGER, "  Random Positions >>>");
    //
    //   current_state.copyJointGroupPositions("panda_arm", joint_values);
    //   for (auto joint_value : joint_values)
    //     RCLCPP_INFO_STREAM(LOGGER, "    joint_value: " << joint_value);
    //   // collision_env->distanceRobot(distance_request, distance_result, current_state);
    //   collision_env->distanceSelf(distance_request, distance_result, current_state);
    //   // auto = distance_result.minimum_distance.link_names;
    //   // RCLCPP_INFO_STREAM(LOGGER, "\tMinimum distance between: " << min_dis_link_names[0] << " and " << min_dis_link_names[1]); // both are empty
    //   RCLCPP_INFO_STREAM(LOGGER, "\t  Minimum distance is : " << distance_result.minimum_distance.distance); //
    //   RCLCPP_INFO_STREAM(LOGGER, "\tdistance_map is " << (distance_map.empty() ? "" : "not") << "empty");                      // empty
    // }

    RCLCPP_INFO_STREAM(LOGGER, "===== END : use collision_env = planning_scene.getCollisionEnv() =====" << std::endl);
  }
  // */

  rclcpp::shutdown();
  return 0;
}