【中文注释】planning_scene_tutorial.cpp

planning_scene_tutorial.cpp

#include <rclcpp/rclcpp.hpp>// MoveIt 相关头文件
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>// BEGIN_SUB_TUTORIAL stateFeasibilityTestExample
//
// 用户可以为 PlanningScene 类指定自定义约束
// 通过使用 setStateFeasibilityPredicate 函数来实现自定义的回调函数。
// 以下是一个简单的示例，用于检查 Panda 机器人中的 "panda_joint1" 关节是否为正角度：
bool stateFeasibilityTestExample(const moveit::core::RobotState& robot_state, bool /*verbose*/)
{const double* joint_values = robot_state.getJointPositions("panda_joint1");return (joint_values[0] > 0.0);
}
// END_SUB_TUTORIALstatic const rclcpp::Logger LOGGER = rclcpp::get_logger("planning_scene_tutorial");int main(int argc, char** argv)
{rclcpp::init(argc, argv);rclcpp::NodeOptions node_options;node_options.automatically_declare_parameters_from_overrides(true);auto planning_scene_tutorial_node = rclcpp::Node::make_shared("planning_scene_tutorial", node_options);rclcpp::executors::SingleThreadedExecutor executor;executor.add_node(planning_scene_tutorial_node);std::thread([&executor]() { executor.spin(); }).detach();// BEGIN_TUTORIAL//// 设置// ^^^^^//// PlanningScene 类可以使用 RobotModel 或 URDF 和 SRDF 轻松设置和配置。// 不过这不是推荐的方式，建议使用 PlanningSceneMonitor 来创建和维护当前的规划场景，// 使用来自机器人关节和传感器的数据来进行维护。在这个教程中，我们将直接实例化一个// PlanningScene 类，但这种实例化方式仅用于说明。robot_model_loader::RobotModelLoader robot_model_loader(planning_scene_tutorial_node, "robot_description");const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel();planning_scene::PlanningScene planning_scene(kinematic_model);// 碰撞检测// ^^^^^^^^^^//// 自碰撞检测// ~~~~~~~~~~~//// 首先，我们检查机器人当前状态是否处于自碰撞状态，// 即当前的配置是否会导致机器人各部分互相碰撞。// 我们需要构造一个 CollisionRequest 对象和一个 CollisionResult 对象，// 然后将它们传入碰撞检测函数中。碰撞检测的结果会保存在 CollisionResult 对象中。// 自碰撞检测使用的是机器人的未加垫版本，直接使用 URDF 中提供的碰撞网格，未额外添加任何填充。collision_detection::CollisionRequest collision_request;collision_detection::CollisionResult collision_result;planning_scene.checkSelfCollision(collision_request, collision_result);RCLCPP_INFO_STREAM(LOGGER, "Test 1: Current state is " << (collision_result.collision ? "in" : "not in")<< " self collision");// 改变机器人状态// ~~~~~~~~~~~~~~~~//// 现在，我们来改变机器人的当前状态。规划场景会维护当前状态，// 我们可以获取一个引用来修改它，然后再次检查新配置下的碰撞情况。// 请注意，每次新的碰撞检测前都需要清除 CollisionResult。moveit::core::RobotState& current_state = planning_scene.getCurrentStateNonConst();current_state.setToRandomPositions();collision_result.clear();planning_scene.checkSelfCollision(collision_request, collision_result);RCLCPP_INFO_STREAM(LOGGER, "Test 2: Current state is " << (collision_result.collision ? "in" : "not in")<< " self collision");// 只检查某个组// ~~~~~~~~~~~~~~//// 接下来，我们只对 Panda 机器人的手部进行碰撞检测，// 即检查手部是否与机器人身体的其他部分发生碰撞。// 我们可以通过在 CollisionRequest 中添加组名 "hand" 来实现这一点。collision_request.group_name = "hand";current_state.setToRandomPositions();collision_result.clear();planning_scene.checkSelfCollision(collision_request, collision_result);RCLCPP_INFO_STREAM(LOGGER, "Test 3: Current state is " << (collision_result.collision ? "in" : "not in")<< " self collision");// 获取碰撞信息// ~~~~~~~~~~~~~//// 首先手动设置 Panda 机械臂到一个我们知道会发生自碰撞的位置。// 请注意，这个状态实际上已经超出了 Panda 关节的限制，我们可以直接检查这一点。std::vector<double> joint_values = { 0.0, 0.0, 0.0, -2.9, 0.0, 1.4, 0.0 };const moveit::core::JointModelGroup* joint_model_group = current_state.getJointModelGroup("panda_arm");current_state.setJointGroupPositions(joint_model_group, joint_values);RCLCPP_INFO_STREAM(LOGGER, "Test 4: Current state is "<< (current_state.satisfiesBounds(joint_model_group) ? "valid" : "not valid"));// 现在，我们可以获取 Panda 机械臂在某个给定配置下的所有碰撞信息。// 通过在 CollisionRequest 中填写相应的字段并指定返回的最大接触数量，// 我们可以请求这些碰撞信息。collision_request.contacts = true;collision_request.max_contacts = 1000;collision_result.clear();planning_scene.checkSelfCollision(collision_request, collision_result);RCLCPP_INFO_STREAM(LOGGER, "Test 5: Current state is " << (collision_result.collision ? "in" : "not in")<< " self collision");collision_detection::CollisionResult::ContactMap::const_iterator it;for (it = collision_result.contacts.begin(); it != collision_result.contacts.end(); ++it){RCLCPP_INFO(LOGGER, "Contact between: %s and %s", it->first.first.c_str(), it->first.second.c_str());}// 修改允许的碰撞矩阵（Allowed Collision Matrix, ACM）// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~//// Allowed Collision Matrix (ACM) 提供了一种机制，让碰撞检测器忽略某些对象之间的碰撞：// 包括机器人自身的部分之间的碰撞以及机器人与环境中的物体之间的碰撞。// 我们可以告诉碰撞检测器忽略前面提到的所有链接之间的碰撞，即使它们实际上是碰撞的，// 碰撞检测器也会忽略这些碰撞，返回该状态为“未发生碰撞”。// 在这个例子中，我们同时对 ACM 和当前状态进行了复制，并传入碰撞检测函数。collision_detection::AllowedCollisionMatrix acm = planning_scene.getAllowedCollisionMatrix();moveit::core::RobotState copied_state = planning_scene.getCurrentState();collision_detection::CollisionResult::ContactMap::const_iterator it2;for (it2 = collision_result.contacts.begin(); it2 != collision_result.contacts.end(); ++it2){acm.setEntry(it2->first.first, it2->first.second, true);}collision_result.clear();planning_scene.checkSelfCollision(collision_request, collision_result, copied_state, acm);RCLCPP_INFO_STREAM(LOGGER, "Test 6: Current state is " << (collision_result.collision ? "in" : "not in")<< " self collision");// 完整碰撞检测
// ~~~~~~~~~~~~~~~~~~~~~~~
//
// 虽然我们之前已经进行了自碰撞检测，但我们可以使用 checkCollision 函数，
// 该函数不仅会检测自碰撞，还会检测与环境（当前为空）的碰撞。
// 这是在路径规划中最常用的一组碰撞检测函数。注意，与环境的碰撞检测会使用机器人带有缓冲区的版本。
// 缓冲区可以帮助机器人在环境中与障碍物保持一定距离。
collision_result.clear();
planning_scene.checkCollision(collision_request, collision_result, copied_state, acm);
RCLCPP_INFO_STREAM(LOGGER, "测试 7: 当前状态 " << (collision_result.collision ? "发生" : "未发生") << " 自碰撞");// 约束检测
// ^^^^^^^^^^^^^^^^^^^
//
// PlanningScene 类中还包含便于使用的函数来检测约束。
// 约束可以有两种类型：
// (a) 从 :moveit_codedir:`KinematicConstraint<moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h>`
// 中选择的约束集：如关节约束（JointConstraint）、位置约束（PositionConstraint）、方向约束（OrientationConstraint）和可视性约束（VisibilityConstraint）。
// (b) 用户通过回调函数自定义的约束。我们将首先看一个简单的 KinematicConstraint 示例。
//
// 检测运动学约束
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
// 我们将首先对熊猫机器人（Panda robot）的 panda_arm 组的末端执行器定义一个简单的位置和方向约束。
// 注意，这里使用了一些便捷的函数来填充约束（这些函数可以在
// :moveit_codedir:`utils.h<moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h>`
// 文件中找到，该文件位于 moveit_core 的 kinematic_constraints 目录下）。std::string end_effector_name = joint_model_group->getLinkModelNames().back();geometry_msgs::msg::PoseStamped desired_pose;
desired_pose.pose.orientation.w = 1.0;
desired_pose.pose.position.x = 0.3;
desired_pose.pose.position.y = -0.185;
desired_pose.pose.position.z = 0.5;
desired_pose.header.frame_id = "panda_link0";
moveit_msgs::msg::Constraints goal_constraint =kinematic_constraints::constructGoalConstraints(end_effector_name, desired_pose);// 现在，我们可以使用 PlanningScene 类中的 isStateConstrained 函数检查某个状态是否符合此约束。copied_state.setToRandomPositions();
copied_state.update();
bool constrained = planning_scene.isStateConstrained(copied_state, goal_constraint);
RCLCPP_INFO_STREAM(LOGGER, "测试 8: 随机状态 " << (constrained ? "符合" : "不符合") << " 约束");// 当需要多次检查相同约束（例如在规划器中）时，有一种更高效的方法来检测约束。
// 我们首先构建一个 KinematicConstraintSet，预处理 ROS 约束消息并为快速处理做好设置。kinematic_constraints::KinematicConstraintSet kinematic_constraint_set(kinematic_model);
kinematic_constraint_set.add(goal_constraint, planning_scene.getTransforms());
bool constrained_2 = planning_scene.isStateConstrained(copied_state, kinematic_constraint_set);
RCLCPP_INFO_STREAM(LOGGER, "测试 9: 随机状态 " << (constrained_2 ? "符合" : "不符合") << " 约束");// 也可以直接使用 KinematicConstraintSet 类来完成此操作。kinematic_constraints::ConstraintEvaluationResult constraint_eval_result =kinematic_constraint_set.decide(copied_state);
RCLCPP_INFO_STREAM(LOGGER, "测试 10: 随机状态 " << (constraint_eval_result.satisfied ? "符合" : "不符合") << " 约束");// 用户定义的约束
// ~~~~~~~~~~~~~~~~~~~~~~~~
//
// CALL_SUB_TUTORIAL stateFeasibilityTestExample// 现在，每当调用 isStateFeasible 时，就会调用这个用户定义的回调函数。planning_scene.setStateFeasibilityPredicate(stateFeasibilityTestExample);
bool state_feasible = planning_scene.isStateFeasible(copied_state);
RCLCPP_INFO_STREAM(LOGGER, "测试 11: 随机状态 " << (state_feasible ? "可行" : "不可行"));// 每当调用 isStateValid 时，将执行三个检查：
// (a) 碰撞检测 (b) 约束检测 (c) 使用用户定义回调的可行性检测。bool state_valid = planning_scene.isStateValid(copied_state, kinematic_constraint_set, "panda_arm");
RCLCPP_INFO_STREAM(LOGGER, "测试 12: 随机状态 " << (state_valid ? "有效" : "无效"));// 注意，所有通过 MoveIt 和 OMPL 可用的规划器都会执行碰撞检测、约束检测以及使用用户定义回调的可行性检测。
// 结束教程rclcpp::shutdown();
return 0;