runtime/rbs_skill_servers/src/mtp_cart.cpp

#include "Eigen/Dense"
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <rclcpp/logging.hpp>

// Action libs
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "rbs_skill_servers/base_skill.hpp"

namespace rbs_skill_actions {

using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
using GoalHandleMoveitSendPose =
    rclcpp_action::ServerGoalHandle<MoveitSendPose>;

class CartesianMoveToPose : public SkillBase<MoveitSendPose> {
public:
  explicit CartesianMoveToPose(
      const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
      : SkillBase<MoveitSendPose>("mtp_cart", options), m_current_step(0) {

    // Initialize publisher for target poses
    auto cbg =
        this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);

    rclcpp::PublisherOptions p_options;
    p_options.callback_group = cbg;

    m_publisher = this->create_publisher<geometry_msgs::msg::PoseStamped>(
        "cartesian_motion_controller/target_frame", 10, p_options);

    m_current_pose = std::make_shared<geometry_msgs::msg::PoseStamped>();

    rclcpp::SubscriptionOptions s_options;
    s_options.callback_group = cbg;

    m_current_pose_subscriber =
        this->create_subscription<geometry_msgs::msg::PoseStamped>(
            "cartesian_motion_controller/current_pose", 10,
            std::bind(&CartesianMoveToPose::on_pose_callback, this,
                      std::placeholders::_1),
            s_options);
  }

protected:
  std::string requiredActionController() override {
    return "cartesian_motion_controller";
  }

  std::vector<std::string> requiredParameters() override {
    return {"end_effector_link", "robot_base_link"};
  }

  void executeAction() override {
    m_base_link = this->get_parameter("robot_base_link").as_string();

    m_trajectory =
        interpolate(m_current_pose->pose, m_current_goal->target_pose);
    m_current_step = 0;

    m_total_time = m_current_goal->duration;
    m_step_time =
        m_total_time / static_cast<double>(m_trajectory.size()) * 1000.0;

    m_trajectory_timer = this->create_wall_timer(
        std::chrono::milliseconds(static_cast<int>(m_step_time)),
        [this]() { adjust_and_publish_pose(); });
  }

private:
  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr m_publisher;
  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr
      m_current_pose_subscriber;
  geometry_msgs::msg::PoseStamped::SharedPtr m_current_pose;
  std::string m_base_link;
  double m_threshold_distance = 0.02;

  std::vector<geometry_msgs::msg::Pose> m_trajectory;
  size_t m_current_step;
  rclcpp::TimerBase::SharedPtr m_trajectory_timer;
  double m_total_time;
  double m_step_time;

  void adjust_and_publish_pose() {
    if (m_current_step < m_trajectory.size()) {
      publish_pose(m_trajectory[m_current_step++]);
    } else {
      double distance =
          calculate_distance(m_current_pose->pose, m_current_goal->target_pose);
      if (distance <= m_threshold_distance) {
        m_current_result->success = true;
        m_current_goal_handle->succeed(m_current_result);
        RCLCPP_INFO(this->get_logger(), "Goal successfully completed");
        m_trajectory_timer->cancel();
      } else {
        publish_pose(m_trajectory.back());
      }
    }
  }

  double calculate_distance(const geometry_msgs::msg::Pose &p1,
                            const geometry_msgs::msg::Pose &p2) {
    Eigen::Vector3d pos1(p1.position.x, p1.position.y, p1.position.z);
    Eigen::Vector3d pos2(p2.position.x, p2.position.y, p2.position.z);

    double position_distance = (pos1 - pos2).norm();

    Eigen::Quaterniond orient1 = normalize_orientation(p1.orientation);
    Eigen::Quaterniond orient2 = normalize_orientation(p2.orientation);

    return position_distance + orient1.angularDistance(orient2);
  }

  Eigen::Quaterniond
  normalize_orientation(const geometry_msgs::msg::Quaternion &q) {
    Eigen::Quaterniond orientation(q.w, q.x, q.y, q.z);
    orientation.normalize();

    if (orientation.norm() == 0) {
      RCLCPP_FATAL(this->get_logger(),
                   "Quaternion has zero norm; normalization failed.");
      throw std::runtime_error("Quaternion normalization failed.");
    }

    return orientation;
  }

  std::vector<geometry_msgs::msg::Pose>
  interpolate(const geometry_msgs::msg::Pose &start_pose,
              const geometry_msgs::msg::Pose &end_pose) {
    std::vector<geometry_msgs::msg::Pose> trajectory;
    Eigen::Vector3d start_position(start_pose.position.x, start_pose.position.y,
                                   start_pose.position.z);
    Eigen::Vector3d end_position(end_pose.position.x, end_pose.position.y,
                                 end_pose.position.z);

    Eigen::Quaterniond start_orientation =
        normalize_orientation(start_pose.orientation);
    Eigen::Quaterniond end_orientation =
        normalize_orientation(end_pose.orientation);

    double distance = (end_position - start_position).norm();
    int num_steps = static_cast<int>(distance / 0.01);

    for (int i = 0; i <= num_steps; ++i) {
      double t = static_cast<double>(i) / num_steps;

      Eigen::Vector3d interpolated_position =
          start_position + t * (end_position - start_position);
      Eigen::Quaterniond interpolated_orientation =
          start_orientation.slerp(t, end_orientation);

      geometry_msgs::msg::Pose interpolated_pose;
      interpolated_pose.position.x = interpolated_position.x();
      interpolated_pose.position.y = interpolated_position.y();
      interpolated_pose.position.z = interpolated_position.z();
      interpolated_pose.orientation.x = interpolated_orientation.x();
      interpolated_pose.orientation.y = interpolated_orientation.y();
      interpolated_pose.orientation.z = interpolated_orientation.z();
      interpolated_pose.orientation.w = interpolated_orientation.w();

      trajectory.push_back(interpolated_pose);
    }

    return trajectory;
  }

  void publish_pose(const geometry_msgs::msg::Pose &pose) {
    geometry_msgs::msg::PoseStamped tp;
    tp.pose = pose;
    tp.header.stamp = this->now();
    tp.header.frame_id = m_base_link;
    m_publisher->publish(tp);
  }

  void on_pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
    m_current_pose = msg;
  }
};

} // namespace rbs_skill_actions

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::CartesianMoveToPose);