搞定了，有需要的自己看吧
#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 ¤t_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;
}
