Plan and Execute using MoveGroupInterface
// Create the MoveIt MoveGroup Interface
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, "panda_arm");
// Set a target Pose
auto const target_pose = []{
geometry_msgs::msg::Pose msg;
msg.orientation.w = 1.0;
msg.position.x = 0.28;
msg.position.y = -0.2;
msg.position.z = 0.5;
return msg;
}();
move_group_interface.setPoseTarget(target_pose);
// Create a plan to that target pose
auto const [success, plan] = [&move_group_interface]{
moveit::planning_interface::MoveGroupInterface::Plan msg;
auto const ok = static_cast<bool>(move_group_interface.plan(msg));
return std::make_pair(ok, msg);
}();
// Execute the plan
if(success) {
move_group_interface.execute(plan);
} else {
RCLCPP_ERROR(logger, "Planing failed!");
}
总结：
- 代码的功能是通过 MoveIt 进行机器人手臂的运动规划与执行。首先，设置目标姿态，然后创建一个计划到该姿态的运动路径，最后如果规划成功，则执行该计划。如果规划失败，输出错误信息。
代码解释
// Create the MoveIt MoveGroup Interface
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, "panda_arm");
解释：
MoveGroupInterface是 MoveIt 的接口类，专门用于规划和执行机器人运动。- 这里创建了一个
move_group_interface对象，初始化时传入两个参数：node: 该参数是 ROS 2 节点，用于与 ROS 系统通信。"panda_arm": 这是控制机器人手臂的运动组的名称，通常与 URDF 文件中的运动规划组名称对应。
// Set a target Pose
auto const target_pose = []{
geometry_msgs::msg::Pose msg;
msg.orientation.w = 1.0;
msg.position.x = 0.28;
msg.position.y = -0.2;
msg.position.z = 0.5;
return msg;
}();
解释：
- 定义了一个
target_pose（目标姿态），用于设定机器人手臂需要到达的目标位置和姿态。 - 使用 C++ Lambda 表达式创建了
target_pose：geometry_msgs::msg::Pose: ROS 消息类型，用于表示位置和姿态。msg.orientation.w = 1.0: 设置四元数的w分量，表示没有旋转（默认朝向）。msg.position.x,msg.position.y,msg.position.z: 分别设置目标点的 x, y, z 坐标。- Lambda 表达式返回这个
msg作为目标姿态。
move_group_interface.setPoseTarget(target_pose);
解释：
setPoseTarget函数将前面定义的target_pose设定为机器人的目标姿态，机器人会试图移动手臂到达该位置和姿态。target_pose: 函数的输入参数，代表机器人希望达到的位姿目标。
源代码-Setting a pose target (goal)
// Create a plan to that target pose
auto const [success, plan] = [&move_group_interface]{
moveit::planning_interface::MoveGroupInterface::Plan msg;
auto const ok = static_cast<bool>(move_group_interface.plan(msg));
return std::make_pair(ok, msg);
}();
解释：
- 这里定义了一个 Lambda 表达式，用于创建机器人移动到目标姿态的运动规划。
moveit::planning_interface::MoveGroupInterface::Plan msg: 定义一个Plan对象，用来存储运动规划信息。move_group_interface.plan(msg): 调用plan函数生成一个移动计划，将结果存入msg。该函数返回一个布尔值，指示规划是否成功。std::make_pair(ok, msg): 使用make_pair返回一个包含布尔值（规划成功与否）和运动规划的元组。
- 最终
success变量表示规划是否成功，plan变量包含具体的规划信息。
// Execute the plan
if(success) {
move_group_interface.execute(plan);
} else {
RCLCPP_ERROR(logger, "Planning failed!");
}
解释：
- 检查
success是否为true，如果运动规划成功，则执行该规划：move_group_interface.execute(plan): 执行之前生成的运动规划plan，让机器人按规划的路径移动到目标位姿。
- 如果规划失败，则使用
RCLCPP_ERROR打印错误信息，表示运动规划失败。
static_cast<bool> 是 C++ 中的一种类型转换，使用 static_cast 运算符将一个值强制转换为 bool 类型。
作用：
static_cast<bool> 将一个非布尔类型的值转换为布尔类型（bool），其行为是：
- 当值为非零时，转换结果为
true。 - 当值为零时，转换结果为
false。
具体场景：
在你的代码中，move_group_interface.plan(msg) 返回的值是一个整型值（int），其中可能使用 1 表示成功，0 表示失败。通过 static_cast<bool> 将这个整型值转换为布尔值，使得 1 被转换为 true，0 被转换为 false。
代码示例：
auto const ok = static_cast<bool>(move_group_interface.plan(msg));
在这行代码中：
move_group_interface.plan(msg)返回的是一个数值，表示规划成功与否。static_cast<bool>将这个数值转换为布尔类型，以便在后续的判断或逻辑操作中直接使用布尔值，比如if(success)。
使用 static_cast 的原因：
static_cast 是一种编译期的类型安全转换，确保类型转换发生时能够遵循明确的规则。相比于传统的 C 风格强制转换，static_cast 提供了更安全的类型转换方式，避免了潜在的类型不匹配或不明确的转换行为。
std::make_pair 是 C++ 标准库中的一个函数，用于创建一个 std::pair 对象。std::pair 是一种数据结构，可以存储两个相关的值，类型可以相同也可以不同。std::make_pair 提供了一种方便的方式来创建和初始化这种 pair。
作用：
std::make_pair 的作用是创建一个 std::pair，其中包含两个值，并将这两个值打包在一起。这两个值可以是任何类型，而且可以是不同类型。使用 std::make_pair 可以自动推导出 pair 中的类型，而无需显式指定。
语法：
std::make_pair(value1, value2);
value1:pair的第一个值（类型可以自动推导）。value2:pair的第二个值（类型可以自动推导）。
示例：
std::pair<int, std::string> myPair = std::make_pair(1, "hello");
这行代码使用 std::make_pair 创建了一个 std::pair<int, std::string> 对象，其中 1 是 pair 的第一个元素，"hello" 是第二个元素。std::make_pair 会根据传入的值自动推导出 pair 中元素的类型。
在Moveit2 教程代码中：
auto const [success, plan] = std::make_pair(ok, msg);
std::make_pair 的功能是将 ok 和 msg 这两个变量打包成一个 std::pair 对象，并将其返回。然后利用 C++17 引入的 结构化绑定，将 std::pair 中的两个值分别赋给 success 和 plan。
具体解释：
ok是一个布尔值 (bool)，表示运动规划是否成功（true表示成功，false表示失败）。msg是一个MoveGroupInterface::Plan对象，包含了生成的运动计划。
通过 std::make_pair(ok, msg)：
ok和msg会被打包成一个std::pair<bool, MoveGroupInterface::Plan>对象。
然后，利用 C++17 的结构化绑定：
success将会被赋值为pair中的第一个元素，即ok（表示是否成功的布尔值）。plan将会被赋值为pair中的第二个元素，即msg（表示运动规划的对象）。
作用：
std::make_pair在这里的作用是简化代码，将ok和msg一次性打包在一起并返回，避免手动创建std::pair对象的麻烦。- 通过 C++17 的结构化绑定，代码显得更加简洁，同时获取
pair中的两个值，分别赋给success和plan。
等效代码：
在不使用 std::make_pair 和结构化绑定的情况下，代码可以这样写：
std::pair<bool, moveit::planning_interface::MoveGroupInterface::Plan> result;
result.first = ok;
result.second = msg;
bool success = result.first;
auto plan = result.second;
但是使用 std::make_pair 和结构化绑定的代码更加简洁和清晰，直接返回和解包 pair 中的值。
std::make_pair 在 C++ 中的功能主要体现在以下几个方面：
1. 简化代码书写
std::make_pair 可以根据传入的两个参数，自动推导 std::pair 中的类型，而无需显式指定类型，减少了代码的冗余。这样可以让代码更加简洁、易读。
例如：
std::pair<int, std::string> myPair = std::make_pair(1, "hello");
相比直接构造 std::pair<int, std::string>(1, "hello")，std::make_pair 会自动推导类型，减少手动指定类型的步骤。
2. 打包两种类型的数据
std::make_pair 将两个可能是不同类型的数据打包在一起，形成一个 std::pair 对象。这对于希望同时返回多个不同类型的值的场景非常有用，例如在函数返回值中需要返回多个不同类型的数据。
例如：
std::pair<bool, int> result = std::make_pair(true, 42);
可以返回一个布尔值和一个整数的组合。
3. 作为函数返回值
std::make_pair 常用于函数中返回多个值的情况。由于 C++ 函数无法直接返回多个值，通过 std::pair 或 std::tuple 可以有效实现类似的功能。例如：
std::pair<int, std::string> getValue() {
return std::make_pair(42, "value");
}
这种方式可以方便地将多个数据组合起来作为函数的返回值，简化调用时的操作。
4. 在关联容器中使用
在 C++ 标准库的关联容器（如 std::map 或 std::unordered_map）中，std::pair 用于表示键值对。std::make_pair 提供了一种简便的方式来创建这些键值对，常用于将元素插入这些容器。
例如：
std::map<int, std::string> myMap;
myMap.insert(std::make_pair(1, "one"));
这里使用 std::make_pair 插入键值对 (1, "one") 到 map 容器中。
5. 结合 C++17 的结构化绑定
在 C++17 中引入了结构化绑定，允许从 std::pair 中解包值。使用 std::make_pair 可以配合结构化绑定简化解包代码。
例如：
auto [key, value] = std::make_pair(1, "one");
这样可以直接解包 pair 中的元素，分别赋给 key 和 value，使代码更简洁易读。
6. 参数类型自动推导
std::make_pair 能自动推导出参数类型，所以即使传入复杂的类型，也不必显式指定类型。例如：
auto myPair = std::make_pair(1.5, std::string("example"));
这行代码自动生成一个 std::pair<double, std::string>，避免手动指定类型的麻烦。