spray_path_follower.cpp

#include <trajectory_msgs/msg/joint_trajectory.h>

rcl_subscription_t subscriber;
trajectory_msgs__msg__JointTrajectory traj_msg;
std::vector<TrajectoryPoint> points;

void traj_callback(const void *msgin) {
  const trajectory_msgs__msg__JointTrajectory *msg = (const trajectory_msgs__msg__JointTrajectory *)msgin;

  // TrajectoryPointsの受信処理
  for (size_t i = 0; i < msg->points.size; i++) {
    TrajectoryPoint pt;
    pt.positions = msg->points.data[i].positions.data;
    pt.time_from_start = msg->points.data[i].time_from_start.sec + msg->points.data[i].time_from_start.nanosec / 1e9;
    points.push_back(pt);
  }
}

void loop() {
  float now = millis() / 1000.0;
  for (int i = 0; i < points.size() - 1; ++i) {
    if (now >= points[i].time_from_start && now < points[i+1].time_from_start) {
      float ratio = (now - points[i].time_from_start) / (points[i+1].time_from_start - points[i].time_from_start);
      std::vector<float> interpolated;
      for (int j = 0; j < points[i].positions.size(); ++j) {
        float value = points[i].positions[j] * (1 - ratio) + points[i+1].positions[j] * ratio;
        interpolated.push_back(value);
      }
      send_joint_command(interpolated); // サーボへ送信
    }
  }
}