FollowJointTrajectory 源码

# The joint trajectory to follow
trajectory_msgs/JointTrajectory trajectory

# Tolerances for the trajectory.  If the measured joint values fall
# outside the tolerances the trajectory goal is aborted.  Any
# tolerances that are not specified (by being omitted or set to 0) are
# set to the defaults for the action server (often taken from the
# parameter server).

# Tolerances applied to the joints as the trajectory is executed.  If
# violated, the goal aborts with error_code set to
# PATH_TOLERANCE_VIOLATED.
JointTolerance[] path_tolerance

# To report success, the joints must be within goal_tolerance of the
# final trajectory value.  The goal must be achieved by time the
# trajectory ends plus goal_time_tolerance.  (goal_time_tolerance
# allows some leeway in time, so that the trajectory goal can still
# succeed even if the joints reach the goal some time after the
# precise end time of the trajectory).
#
# If the joints are not within goal_tolerance after "trajectory finish
# time" + goal_time_tolerance, the goal aborts with error_code set to
# GOAL_TOLERANCE_VIOLATED
JointTolerance[] goal_tolerance
duration goal_time_tolerance

---
int32 error_code
int32 SUCCESSFUL = 0
int32 INVALID_GOAL = -1
int32 INVALID_JOINTS = -2
int32 OLD_HEADER_TIMESTAMP = -3
int32 PATH_TOLERANCE_VIOLATED = -4
int32 GOAL_TOLERANCE_VIOLATED = -5

# Human readable description of the error code. Contains complementary
# information that is especially useful when execution fails, for instance:
# - INVALID_GOAL: The reason for the invalid goal (e.g., the requested
#   trajectory is in the past).
# - INVALID_JOINTS: The mismatch between the expected controller joints
#   and those provided in the goal.
# - PATH_TOLERANCE_VIOLATED and GOAL_TOLERANCE_VIOLATED: Which joint
#   violated which tolerance, and by how much.
string error_string

---
Header header
string[] joint_names
trajectory_msgs/JointTrajectoryPoint desired
trajectory_msgs/JointTrajectoryPoint actual
trajectory_msgs/JointTrajectoryPoint error

---

FollowJointTrajectory 的交互流程

1. 发送轨迹目标（Goal）：
   • 控制器接收到 Goal 消息，里面包含了完整的轨迹（即若干轨迹点的集合）。
2. 执行轨迹并发送反馈（Feedback）：
   • 控制器开始按照轨迹点逐步执行运动，并以固定频率发送 Feedback 消息。
   • 每条 Feedback 消息表示当前时刻的状态（当前目标点、实际状态以及误差）。
3. 执行完成，发送结果（Result）：
   • 当整条轨迹执行完成时，控制器返回 Result 消息，表示最终执行结果（成功或失败以及执行过程中是否有问题）。

Goal接口定义如下：

trajectory_msgs/JointTrajectory trajectory :这里面传入了机器人的运行轨迹，包含了机器人各关节运行的位置、速度、加速度、运行时间等等。

JointTolerance[] path_tolerance
JointTolerance[] goal_tolerance
duration goal_time_tolerance
这几个都是时间和目标的容忍度。如果发送到服务端发现没法在规定时间内运行到响应位置，就会回传拒绝的请求。

对于JointTrajectory，格式如下：

Header header
string[] joint_names
JointTrajectoryPoint[] points

JointTrajectory 目标值

---

1. Header header

含义

• Header 是 ROS 中的一个标准化字段，用来存储关于消息的元信息（metadata）。
• Header 的类型是 std_msgs/Header，定义了以下子字段：
  • stamp: 消息的时间戳，表示这条消息的创建或发生的时间。
  • frame_id: 使用的坐标系名称，通常表示消息中的数据是基于哪个参考坐标框架。

用途

• 在机器人控制中，时间戳很重要，因为它可以帮助同步不同数据流（比如传感器数据和运动控制数据）。
• frame_id 用于指明消息在机器人运动学中的参考坐标系。

例子

Header header:
  stamp: 1234567890
  frame_id: "base_link"

• stamp 表示这条消息发生的时间，可以用于时间同步。
• frame_id 表示这些动作数据是相对于机器人底盘的坐标系来描述的。

---

2. string[] joint_names

含义

• joint_names 是一个字符串数组（string[]），用于存储关节的名称。
  • 每个名称对应一个关节，例如 "joint1", "joint2", 或 "arm_lift_joint" 等。
• 它定义了消息中的运动或状态是与哪些机器人关节相关联的。

用途

• ROS 中，机器人通常有多个关节（例如机械臂的链接节点、轮式机器人的一组电机），为了清楚地描述这些关节，使用 joint_names 来标明每个关节的唯一名字。
• 与 points 中的数据一一对应，确保轨迹中的位置、速度等能够明确分配到具体的关节上。

例子

joint_names: ["arm_lift_joint", "arm_flex_joint", "gripper_joint"]

表示轨迹的运动是针对这三个关节（arm_lift_joint、arm_flex_joint、gripper_joint）进行的。

---

3. JointTrajectoryPoint[] points

含义

• points 是一个数组，每个元素都是一个 JointTrajectoryPoint 类型。
• 每个 JointTrajectoryPoint 表示一个轨迹中的关键点（waypoint），定义了机器人在某一时刻所有关节的位置、速度、加速度等。

---

JointTrajectoryPoint 的子字段

JointTrajectoryPoint 的定义在 trajectory_msgs/JointTrajectoryPoint 中，包含了以下内容：

• positions[]: 浮点数数组，每个关节的目标位置。
• velocities[]: 浮点数数组，每个关节的目标速度（rad/s或m/s）。
• accelerations[]: 浮点数数组，每个关节的目标加速度。
• effort[]: 浮点数数组，每个关节的施加力（Nm）。
• time_from_start: 表示定点相对于轨迹起始时间的时刻。

用途

• 用来表示机器人运动轨迹。这些点按照时间顺序排列，每一个点定义了机器人在某一时刻所有关节的状态。
• 该字段通常用于驱动机器人执行某些动作。例如机器人从起始位置执行到达目标姿势的动作。

例子

假设我们优化一个机械臂运动轨迹，通过三个关键点定义的轨迹：

points: 
  - positions: [0.0, 1.0, 0.5] 
    velocities: [0.5, 0.5, 0.0]
    accelerations: [0.1, 0.1, 0.0]
    time_from_start: 1.0
  - positions: [1.5, 0.5, 0.3]
    velocities: [0.5, 0.0, 0.0]
    accelerations: [0.2, 0.2, 0.0]
    time_from_start: 2.0
  - positions: [2.0, 0.0, 0.0]
    velocities: [0.0, 0.0, 0.0]
    accelerations: [0.0, 0.0, 0.0]
    time_from_start: 3.0

以上例子定义了一段轨迹：

1. 第一时刻（1 秒），关节分别达到位置 [0.0, 1.0, 0.5]。
2. 第二时刻（2 秒），关节分别达到位置 [1.5, 0.5, 0.3]。
3. 第三时刻（3 秒），关节分别达到位置 [2.0, 0.0, 0.0]。

---

我们可以构造一个例子用来描述机器人运动：

Header:
  stamp: 1666000000
  frame_id: "base_link"
joint_names: ["joint1", "joint2", "joint3"]
points:
  - positions: [0.0, 1.0, 0.5]
    velocities: [0.5, 0.5, 0.0]
    time_from_start: 1.0
  - positions: [1.5, 0.5, 0.3]
    velocities: [0.5, 0.0, 0.0]
    time_from_start: 2.0

该消息的意义：

1. 机器人基于 base_link 的参考系。
2. 控制关节 "joint1", "joint2", 和 "joint3"。
3. 定义了一段运动轨迹：
   • 从 0 秒 开始，在第 1 秒达到特定的目标位置 [0.0, 1.0, 0.5]。
   • 在第 2 秒移动到新的目标位置 [1.5, 0.5, 0.3]。

该消息会被发布到机器人控制器（如机械臂的运动控制器），从而驱动执行对应的动作。

feedback反馈

---

Header header
string[] joint_names
trajectory_msgs/JointTrajectoryPoint desired
trajectory_msgs/JointTrajectoryPoint actual
trajectory_msgs/JointTrajectoryPoint error

反馈回的值和传入的一样，需求目标点（desired），实际目标点（actual），二者插值（error）。

#include "rclcpp/rclcpp.hpp"
#include "control_msgs/action/follow_joint_trajectory.hpp"
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
#include <vector>
#include <memory> // std::shared_ptr 和 std::make_shared

void create_feedback_message() {
    // Step 1: 初始化 feedback 对象
    auto feedback = std::make_shared<control_msgs::action::FollowJointTrajectory::Feedback>();

    // Step 2: 填充 joint_names
    std::vector<std::string> joint_names = {"joint1", "joint2", "joint3"}; // 示例关节名称
    feedback->joint_names = joint_names;

    // Step 3: 填充 desired.positions
    trajectory_msgs::msg::JointTrajectoryPoint desired_point;
    desired_point.positions = {1.0, -1.5, 0.6}; // 示例目标关节的位置
    feedback->desired = desired_point;

    // Step 4: 填充 actual.positions
    trajectory_msgs::msg::JointTrajectoryPoint actual_point;
    actual_point.positions = {0.9, -1.3, 0.5}; // 示例当前关节的位置
    feedback->actual = actual_point;

    // Step 5: 计算 error.positions 并填充
    trajectory_msgs::msg::JointTrajectoryPoint error_point;
    error_point.positions.resize(joint_names.size()); // 根据关节数初始化 error.positions 的大小
    for (size_t i = 0; i < joint_names.size(); ++i) {
        error_point.positions[i] = feedback->actual.positions[i] - feedback->desired.positions[i];
    }
    feedback->error = error_point;

    // 模拟打印，输出检查
    for (size_t i = 0; i < joint_names.size(); ++i) {
        RCLCPP_INFO(rclcpp::get_logger("FeedbackLogger"),
                    "Joint [%s]: Desired=%.2f, Actual=%.2f, Error=%.2f",
                    joint_names[i].c_str(),
                    feedback->desired.positions[i],
                    feedback->actual.positions[i],
                    feedback->error.positions[i]);
    }
}

它只表示某一时刻的状态，Feedback 的 desired 仅指向当前目标点，而不是整个轨迹的点集合。

另外，无需提前显式定义 feedback，只需要在 execute 函数内创建并使用一个局部变量即可。这是因为 feedback 在 Action 中只是用来临时保存数据，然后通过 goal_handle->publish_feedback(feedback) 进行发布，并不会持久地用到。也就是说，每个轨迹点处理时，会单独创建并填充一个新的 feedback 对象。