【MoveIt 2】示例：直接通过 C++ API 使用 MoveIt -规划场景

 规划场景 

PlanningScene 类提供了用于碰撞检查和约束检查的主要接口。在本教程中，我们将探讨该类的 C++ 接口。

 入门 

如果您还没有这样做，请确保您已完成入门中的步骤。

 整个代码 

整个代码可以在 MoveIt GitHub 项目中看到。https://github.com/moveit/moveit2_tutorials/blob/main/doc/examples/planning_scene

#include <rclcpp/rclcpp.hpp> // 包含 ROS 2 的核心库


// MoveIt
#include <moveit/robot_model_loader/robot_model_loader.h> // 包含 MoveIt 的机器人模型加载器
#include <moveit/planning_scene/planning_scene.h> // 包含 MoveIt 的规划场景


#include <moveit/kinematic_constraints/utils.h> // 包含 MoveIt 的运动学约束工具


// 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"); // 获取 "panda_joint1" 关节的位置
  return (joint_values[0] > 0.0); // 检查关节角度是否大于 0
}
// END_SUB_TUTORIAL


static const rclcpp::Logger LOGGER = rclcpp::get_logger("planning_scene_tutorial"); // 定义日志记录器


int main(int argc, char** argv)
{
  rclcpp::init(argc, argv); // 初始化 ROS 2
  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 轻松设置和配置。
  // 但是，这不是实例化 PlanningScene 的推荐方法。
  // 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 对象，并将它们传递给碰撞检测函数。
  // 请注意，机器人是否处于自碰撞状态的结果包含在结果中。自碰撞检测使用机器人的未填充版本，即直接使用 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"); // 输出自碰撞检测结果


  // 改变状态
  // ~~~~~~~~
  //
  // 现在，让我们改变机器人的当前状态。规划场景内部维护当前状态。
  // 我们可以获取它的引用并进行更改，然后检查新机器人配置的碰撞情况。
  // 特别注意，在进行新的碰撞检测请求之前，我们需要清除碰撞结果。


  moveit::core::RobotState& current_state = planning_scene.getCurrentStateNonConst(); // 获取当前状态的非 const 引用
  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 机器人的手进行碰撞检测，即检查手与机器人身体其他部分之间是否存在碰撞。
  // 我们可以通过将组名 "hand" 添加到碰撞请求中来专门请求此操作。


  collision_request.group_name = "hand"; // 设置碰撞请求的组名为 "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 手臂在给定配置下可能发生的任何碰撞的接触信息。
  // 我们可以