ROS2_Moveit2_Ur5e_Grasp项目详解（九）：u5re

在前面的cpp操作文件中可以看到都引用了头文件u5re_gripper.h，那么我们来详细了解一下这个头文件。

按照惯例，先贴代码：

using GripperCommand = control_msgs::action::GripperCommand;
using GoalHandleGripperCommand = rclcpp_action::ClientGoalHandle<GripperCommand>;

class UR5eGripper : public rclcpp::Node {
public:
  explicit UR5eGripper(const rclcpp::NodeOptions &options);
  void init();

  void get_target_pose_list(std::vector<std::vector<double>> &target_pose_list);
  void get_joint_target_positions(
      moveit::planning_interface::MoveGroupInterfacePtr move_group,
      const std::vector<double> &target_pose, const std::string &reference_frame,
      std::vector<double> &joint_target_positions);
  bool plan_and_execute(const std::vector<double> &target_pose);
  bool grasp(double gripper_position);
  void get_cube_pose(const std::string &from_frame, const std::string &to_frame,
                    std::vector<double> &cube_pose);
  void go_to_ready_position();

private:
  void goal_response_callback(const GoalHandleGripperCommand::SharedPtr &goal_handle);
  void feedback_callback(GoalHandleGripperCommand::SharedPtr,
                        const std::shared_ptr<const GripperCommand::Feedback> feedback);
  void result_callback(const GoalHandleGripperCommand::WrappedResult &result);
  void str_list_2_double_list(const std::vector<std::string> &str_list,
                              std::vector<std::vector<double>> &double_list);

  std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface_;
  rclcpp_action::Client<GripperCommand>::SharedPtr gripper_action_client_;
  rclcpp_action::Client<GripperCommand>::SendGoalOptions send_goal_options_;
  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

  std::vector<std::vector<double>> target_pose_list_;
  std::string gripper_action_name_ = "/gripper_controller/gripper_cmd";
  const std::string PLANNING_GROUP = "ur_manipulator";
};

cpp

类定义：UR5eGripper : public rclcpp::Node

class UR5eGripper : public rclcpp::Node {

cpp

含义：UR5eGripper 是一个继承自 rclcpp::Node 的类

构造函数：explicit UR5eGripper(…)

explicit UR5eGripper(const rclcpp::NodeOptions &options);

cpp

这是一个构造函数。

explicit：防止隐式类型转换（安全编程习惯）。
参数：接收一个 NodeOptions 对象，允许在创建节点时配置选项（如自动声明参数、上下文、QoS 等）。
作用：初始化这个 ROS 2 节点，比如设置节点名称（通常在 main() 中传入）。

init() 函数

void init();

cpp

作用：初始化类内部组件。
为什么不在构造函数中完成？因为有些资源（如 MoveGroupInterface）需要在节点完全启动后才能安全创建。

get_target_pose_list(…)

void get_target_pose_list(std::vector<std::vector<double>> &target_pose_list);

cpp

功能：获取预设的“放置目标位姿”列表。
参数：输出参数，返回一组目标位姿。
每个位姿格式：[x, y, z, roll, pitch, yaw]（单位：米 + 弧度）
用途：用于将物体放到不同位置（比如 target_pose_list[0] 是第一个放置点）

get_joint_target_positions(…)

void get_joint_target_positions(
      moveit::planning_interface::MoveGroupInterfacePtr move_group,
      const std::vector<double> &target_pose, const std::string &reference_frame,
      std::vector<double> &joint_target_positions);

cpp

功能：给定一个末端执行器的目标位姿（target_pose），求解对应的关节角度目标值（逆运动学）。
参数说明：
- move_group：MoveIt 的运动组接口。
- target_pose：目标位姿 [x, y, z, roll, pitch, yaw]。
- reference_frame：参考坐标系（如 “base_link”）。
- joint_target_positions：输出的关节角（弧度）。
用途：为后续轨迹规划提供输入。

plan_and_execute(…)

bool plan_and_execute(const std::vector<double> &target_pose);

cpp

功能：规划并执行一条到达目标位姿的轨迹。
参数：目标位姿 [x, y, z, roll, pitch, yaw]。
返回值：true 表示成功，false 表示失败（如无解、碰撞、超时）。
内部流程：
- 调用 get_joint_target_positions 求解目标关节角。
- 使用 MoveGroupInterface 进行轨迹规划。
- 执行规划好的轨迹（发送到控制器）。
这是最核心的运动控制函数之一。

grasp(…)

bool grasp(double gripper_position);

cpp

功能：控制夹爪开合。
参数：gripper_position —— 目标开合度（单位：米或归一化值，取决于夹爪驱动方式）。
- 例如：0.0 表示闭合，1 表示完全打开。
实现方式：通过 ROS 2 Action 发送目标命令。
返回值：是否成功发送命令或等待完成。

get_cube_pose(…)

void get_cube_pose(const std::string &from_frame, const std::string &to_frame,
                    std::vector<double> &cube_pose);

cpp

功能：通过 TF（Transform）系统获取两个坐标系之间的相对位姿。
参数：
- from_frame：源坐标系（如 “base_link”）
- to_frame：目标坐标系（如 “cube1”）
- cube_pose：输出位姿 [x, y, z, roll, pitch, yaw]
实现方式：
- 使用 tf_buffer_->lookupTransform(…) 获取 geometry_msgs::TransformStamped
- 将四元数转换为欧拉角（roll, pitch, yaw）
用途：获取立方体在机器人基座坐标系下的位置，用于抓取。

go_to_ready_position()

void go_to_ready_position();

cpp

功能：让机器人回到一个预设的“安全起始位置”（Ready Pose / Home Pose）。
实现方式：
- 可能是通过关节目标（Joint Target）直接移动。
- 或者调用 plan_and_execute 到某个预设位姿。
目的：
- 避免奇异点
- 防止碰撞
- 作为每次操作前的初始化姿态

接下来介绍private私有函数，这些是类内部使用的资源和回调函数。

goal_response_callback(…)

void goal_response_callback(const GoalHandleGripperCommand::SharedPtr &goal_handle);

cpp

作用：当向夹爪 Action Server 发送目标后，服务器返回是否接受该目标。
典型行为：
- 如果目标被接受，继续监听反馈。
- 如果被拒绝，记录警告或重试。
用途：异步处理 Action 请求的响应。

feedback_callback(…)

void feedback_callback(GoalHandleGripperCommand::SharedPtr,
                        const std::shared_ptr<const GripperCommand::Feedback> feedback);

cpp

作用：在夹爪运动过程中，持续接收反馈信息。
反馈内容可能包括：
- 当前开合度
- 施加的力
- 运动状态（moving, stalled）
用途：监控夹爪状态，可用于检测是否夹紧物体。

result_callback(…)

void result_callback(const GoalHandleGripperCommand::WrappedResult &result);

cpp

作用：Action 执行完成后，接收最终结果。
结果状态可能为：
- SUCCEEDED
- ABORTED
- CANCELED
用途：判断夹爪动作是否成功完成。

str_list_2_double_list(…)

void str_list_2_double_list(const std::vector<std::string> &str_list,
                              std::vector<std::vector<double>> &double_list);

cpp

功能：将字符串列表（如参数服务器读取的字符串数组）转换为双精度浮点数二维数组。
典型用途：
- 从 YAML 文件读取 [“0.1,0.2,0.3”, “0.4,0.5,0.6”]
- 解析成 [[0.1,0.2,0.3], [0.4,0.5,0.6]]
使用场景：加载 target_pose_list_ 时，把参数字符串转为数值。

最后介绍私有成员变量

std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;

cpp

MoveIt 的核心接口，用于与机器人运动规划器交互。
提供 setPoseTarget, setJointValueTarget, plan(), move() 等方法。
对应的运动组是 “ur_manipulator”。

moveit::planning_interface::PlanningSceneInterface planning_scene_interface_;

cpp

用于与规划场景（Planning Scene）交互。
可添加/删除障碍物、设置碰撞对象等。

rclcpp_action::Client<GripperCommand>::SharedPtr gripper_action_client_;

cpp

夹爪动作客户端，用于向夹爪控制器发送命令。
Action 类型：GripperCommand（通常是 control_msgs::action::GripperCommand）
目标话题：/gripper_controller/gripper_cmd

rclcpp_action::Client<GripperCommand>::SendGoalOptions send_goal_options_;

cpp

配置 Action 客户端发送目标时的回调函数（即上面三个回调）。

在 init() 中会设置：

send_goal_options_.goal_response = std::bind(&UR5eGripper::goal_response_callback, this, _1);
send_goal_options_.feedback = std::bind(&UR5eGripper::feedback_callback, this, _1, _2);
send_goal_options_.result = std::bind(&UR5eGripper::result_callback, this, _1);

cpp

std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

cpp

TF2 系统组件：
- tf_buffer_：存储所有坐标变换的历史数据。
- tf_listener_：自动订阅 /tf 和 /tf_static 话题，填充 buffer。
用于 get_cube_pose() 中查询坐标变换。

std::vector<std::vector<double>> target_pose_list_;

cpp

存储所有预设的放置目标位姿。
通过 get_target_pose_list() 填充。
每个元素是一个 6 维向量 [x, y, z, roll, pitch, yaw]。

std::string gripper_action_name_ = "/gripper_controller/gripper_cmd";

cpp

定义夹爪 Action 的话题名称。
可通过参数修改（更灵活）。

const std::string PLANNING_GROUP = "ur_manipulator";

cpp

定义 MoveIt 中的运动组名称。
在 ur5e_moveit_config 中定义，通常包含所有机械臂关节。

总结，这个类的作用：在这里插入图片描述

捋一下demo.launch.py、 demo.cpp、 ur5e_gripper.cpp 和 ur5e_gripper.h 四个文件之间的关系。

整体架构关系

这四个文件构成了一个完整的ROS 2机器人控制系统的不同层次：

demo.launch.py (启动配置层)
    ↓
demo.cpp (应用主程序层)
    ↓
ur5e_gripper.cpp (功能实现层)
    ↓
ur5e_gripper.h (接口定义层)

cpp

详细调用关系分析：

demo.launch.py → demo.cpp

cpp

demo.launch.py是启动文件，负责配置和启动demo.cpp编译生成的可执行文件：

Node(
    package='ur5e_gripper_control',    ## 指定功能包
    executable='demo',                 ## 指定可执行文件名
    name='demo_node',                  ## 节点名称
    parameters=[                       ## 传递参数
        ## ...
    ], 
    output='screen'
)

python

当运行ros2 launch ur5e_gripper_control demo.launch.py时，系统会：

a. 加载ur5e_gripper_control包 b. 查找并执行名为demo的可执行文件 c. 将配置参数传递给该可执行文件

demo.cpp → ur5e_gripper.cpp

cpp

demo.cpp是应用程序的主入口，它使用UR5eGripper类实现具体功能：

// 创建UR5eGripper对象实例
auto node = std::make_shared<UR5eGripper>(node_options);

// 调用UR5eGripper类的方法
node->init();
node->get_target_pose_list(target_pose_list);
node->get_cube_pose(from_frame, to_frame_list[i], cube_pose);
node->plan_and_execute(cube_pose_list[i]);
node->grasp(0.36);
node->go_to_ready_position();

cpp

demo.cpp中调用了ur5e_gripper.cpp中实现的多个方法来完成机器人控制任务。

ur5e_gripper.cpp ↔ ur5e_gripper.h

cpp

ur5e_gripper.h是UR5eGripper类的头文件，定义了类的接口：

class UR5eGripper : public rclcpp::Node {
public:
  explicit UR5eGripper(const rclcpp::NodeOptions &options);
  void init();
  void get_target_pose_list(std::vector<std::vector<double>> &target_pose_list);
  bool plan_and_execute(const std::vector<double> &target_pose);
  bool grasp(double gripper_position);
  // ... 其他公共方法
};

cpp

ur5e_gripper.cpp是实现文件，包含了这些方法的具体实现。

编译和链接关系

在CMakeLists.txt中定义了编译规则：

## 将demo.cpp和ur5e_gripper.cpp编译成一个可执行文件
add_executable(demo src/demo.cpp src/ur5e_gripper.cpp)

## 链接所需的库
ament_target_dependencies(demo
  rclcpp moveit_ros_planning_interface tf2 
  moveit_core moveit_ros_planning control_msgs)

cpp

这表明demo.cpp和ur5e_gripper.cpp被编译成同一个可执行文件demo。

数据流向

a. 配置数据流向：target_pose_list.yaml → demo.launch.py → demo.cpp → UR5eGripper构造函数 b. 控制指令流向：demo.cpp → UR5eGripper方法 → MoveIt/ROS 2系统 → 机器人硬件 c. 反馈数据流向：机器人硬件 → ROS 2系统 → UR5eGripper方法 → demo.cpp

各文件职责总结 a. demo.launch.py：负责配置节点启动参数，是系统的入口点 b. demo.cpp：实现应用逻辑，协调各个功能模块完成抓取任务 c. ur5e_gripper.cpp：实现具体的机器人控制功能，如运动规划、夹爪控制等 d. ur5e_gripper.h：定义UR5eGripper类的公共接口，为上层应用提供调用入口

ROS2_Moveit2_Ur5e_Grasp项目详解（九）：u5re_gripper.h详解