【Moveit2】MoveGroupInterface设置目标姿态，然后创建一个计划到该姿态的运动路径，static_cast＜bool＞与std::make_pair函数用法

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。

具体解释：

1. ok 是一个布尔值 (bool)，表示运动规划是否成功（true 表示成功，false 表示失败）。
2. 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>，避免手动指定类型的麻烦。