driver

/*********************************************************************
 * 机械臂 自动动作切换 + 笛卡尔空间控制 Demo
 * 功能：
 *   - 自动序列模式（原有速度动作循环）
 *   - 笛卡尔目标模式（通过逆解 + 速度闭环控制末端位姿）
 *   - 实时打印当前关节角和末端笛卡尔位姿
 *********************************************************************/
#define _USE_MATH_DEFINES   // 确保 M_PI 可用
#include <rclcpp/rclcpp.hpp>
#include <control_msgs/msg/joint_jog.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <signal.h>
#include <vector>
#include <map>
#include <thread>
#include <chrono>
#include <cmath>
#include <mutex>
#include <algorithm>        // for std::find
#include "ArmKinematics.hpp"

// 话题名称
const std::string JOINT_TOPIC = "/servo_demo_node/delta_joint_cmds";
const std::string JOINT_STATES_TOPIC = "/joint_states";
const size_t ROS_QUEUE_SIZE = 10;

using JointAction = std::map<std::string, double>;

void quit(int sig) {
    (void)sig;
    rclcpp::shutdown();
    exit(0);
}

class ArmAutoDemo : public rclcpp::Node {
public:
    ArmAutoDemo() : Node("arm_auto_demo"), kin_() {
        // ---- 创建发布器 ----
        joint_pub_ = this->create_publisher<control_msgs::msg::JointJog>(JOINT_TOPIC, ROS_QUEUE_SIZE);
        
        // ---- 订阅关节状态 ----
        joint_state_sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
            JOINT_STATES_TOPIC, 10,
            std::bind(&ArmAutoDemo::jointStateCallback, this, std::placeholders::_1));

        // ---- 预设关节空间动作序列（与之前相同） ----
        actions_.clear();
        actions_.push_back({{"JM0", 0.15/3.0}, {"JM1-2", 0.25/3.0}, {"JM5", 0.10/3.0}});
        actions_.push_back({{"JM0", -0.30/3.0}, {"JM1-2", -0.05/3.0}, {"JM4-3", 0.10/3.0}, {"JM4", 0.05/3.0}, {"JM5", 0.05/3.0}});
        actions_.push_back({{"JM0", 0.25/3.0}, {"JM1-2", -0.30/3.0}, {"JM4-3", -0.15/3.0}, {"JM4", 0.05/3.0}, {"JM5", -0.20/3.0}, {"YB", 0.05/3.0}});
        actions_.push_back({{"JM0", -0.10/3.0}, {"JM1-2", 0.10/3.0}, {"JM4-3", 0.05/3.0}, {"JM4", -0.10/3.0}, {"JM5", 0.05/3.0}, {"YB", -0.05/3.0}});
        actions_.push_back({});   // 停驻（索引4）

        action_duration_ = 3.0;
        current_action_idx_ = 0;
        action_start_time_ = this->now();
        last_print_time_ = 0.0;

        // ---- 笛卡尔目标模式相关 ----
        cartesian_mode_active_ = false;
        target_joints_.assign(6, 0.0);
        target_pose_.assign(6, 0.0);   // 新增：存储目标笛卡尔位姿
        current_joints_.assign(6, 0.0);
        joint_states_received_ = false;

        RCLCPP_INFO(this->get_logger(), "===== 机械臂自动动作 + 笛卡尔控制 Demo 启动 =====");
        RCLCPP_INFO(this->get_logger(), "关节空间序列共 %zu 组动作，每组 %.1f 秒", actions_.size(), action_duration_);
        RCLCPP_INFO(this->get_logger(), "笛卡尔控制模式默认关闭，可通过 setCartesianTarget() 激活");

        // ---- 启动线程 ----
        std::thread(&ArmAutoDemo::publishLoop, this).detach();
        std::thread(&ArmAutoDemo::actionSwitchLoop, this).detach();

        // ---- 可选：演示示例，10秒后自动切换到一个笛卡尔目标（可取消注释测试） ----
        // std::thread([this]() {
        //     std::this_thread::sleep_for(std::chrono::seconds(10));
        //     this->setCartesianTarget(0.3, 0.1, 0.4, 0.0, 0.0, 0.0);
        // }).detach();
    }

    /**
     * @brief 设置笛卡尔空间目标，内部自动调用 ik 并切换至笛卡尔控制模式
     */
    void setCartesianTarget(double x, double y, double z,
                            double roll, double pitch, double yaw) {
        std::lock_guard<std::mutex> lock(action_mutex_);
        // 调用逆运动学
        target_joints_ = kin_.ik(x, y, z, roll, pitch, yaw);
        target_pose_ = {x, y, z, roll, pitch, yaw};   // 保存目标位姿以便打印
        cartesian_mode_active_ = true;
        RCLCPP_INFO(this->get_logger(),
            "🔷 切换到笛卡尔控制模式，目标位姿: (%.3f, %.3f, %.3f) RPY(%.2f, %.2f, %.2f)",
            x, y, z, roll, pitch, yaw);
        RCLCPP_INFO(this->get_logger(),
            "   对应关节目标: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]",
            target_joints_[0], target_joints_[1], target_joints_[2],
            target_joints_[3], target_joints_[4], target_joints_[5]);
        // 重置时间戳，使打印从0开始
        action_start_time_ = this->now();
        last_print_time_ = 0.0;
    }

    /**
     * @brief 退出笛卡尔模式，回到自动关节序列
     */
    void disableCartesianMode() {
        std::lock_guard<std::mutex> lock(action_mutex_);
        cartesian_mode_active_ = false;
        RCLCPP_INFO(this->get_logger(), "🔶 退出笛卡尔模式，恢复关节序列");
        action_start_time_ = this->now();
        last_print_time_ = 0.0;
    }

private:
    // ----- 成员变量 -----
    rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr joint_pub_;
    rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;

    std::vector<JointAction> actions_;
    int current_action_idx_;
    double action_duration_;
    std::mutex action_mutex_;

    rclcpp::Time action_start_time_;
    double last_print_time_ = 0.0;

    // 运动学对象
    ArmKinematics kin_;

    // 关节状态与目标
    std::vector<double> current_joints_;    // 当前实际关节角
    std::vector<double> target_joints_;     // 目标关节角（笛卡尔模式）
    std::vector<double> target_pose_;       // 目标笛卡尔位姿 [x,y,z,roll,pitch,yaw]
    bool joint_states_received_ = false;
    bool cartesian_mode_active_ = false;

    // ----- 关节状态回调（按名称匹配，增强鲁棒性） -----
    void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg) {
        std::lock_guard<std::mutex> lock(action_mutex_);
        // 定义期望的关节顺序
        std::vector<std::string> expected_names = {"JM0", "JM1-2", "JM4-3", "JM4", "JM5", "YB"};
        for (size_t i = 0; i < expected_names.size(); ++i) {
            auto it = std::find(msg->name.begin(), msg->name.end(), expected_names[i]);
            if (it != msg->name.end()) {
                size_t idx = std::distance(msg->name.begin(), it);
                current_joints_[i] = msg->position[idx];
            } else {
                RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
                    "关节 %s 未在 joint_states 中找到", expected_names[i].c_str());
            }
        }
        joint_states_received_ = true;
    }

    // ----- 发布循环（100 Hz） -----
    void publishLoop() {
        rclcpp::Rate rate(100);
        while (rclcpp::ok()) {
            std::lock_guard<std::mutex> lock(action_mutex_);
            auto msg = control_msgs::msg::JointJog();
            msg.header.stamp = this->now();

            if (cartesian_mode_active_ && joint_states_received_) {
                // ---------- 笛卡尔模式：速度 P 控制 ----------
                const double Kp = 0.5;          // 比例增益
                const double VEL_LIMIT = 0.5;   // 关节速度限幅 (rad/s)
                msg.joint_names = {"JM0", "JM1-2", "JM4-3", "JM4", "JM5", "YB"};

                bool reached = true;
                for (size_t i = 0; i < 6; ++i) {
                    double error = target_joints_[i] - current_joints_[i];
                    if (std::fabs(error) > 0.005) reached = false;
                    double vel = Kp * error;
                    vel = std::max(-VEL_LIMIT, std::min(VEL_LIMIT, vel));
                    msg.velocities.push_back(vel);
                }

                // 如果到达目标，自动退出笛卡尔模式并切换到停驻动作
                if (reached) {
                    RCLCPP_INFO(this->get_logger(), "✅ 笛卡尔目标已到达，自动退出模式并进入停驻");
                    cartesian_mode_active_ = false;
                    // 将序列索引设置为停驻动作（最后一个）
                    current_action_idx_ = static_cast<int>(actions_.size()) - 1;
                    // 发送零速度停止
                    msg.velocities.assign(6, 0.0);
                    action_start_time_ = this->now();
                    last_print_time_ = 0.0;
                }

                // ----- 打印笛卡尔模式下的进度 -----
                double now = this->now().seconds();
                double elapsed = now - action_start_time_.seconds();
                if (elapsed - last_print_time_ >= 0.2 || elapsed <= 0.05) {
                    last_print_time_ = elapsed;
                    auto current_pose = kin_.fk(current_joints_);
                    RCLCPP_INFO(this->get_logger(),
                        "📍 笛卡尔模式 | 当前末端: (%.3f, %.3f, %.3f) | 目标: (%.3f, %.3f, %.3f)",
                        current_pose[0], current_pose[1], current_pose[2],
                        target_pose_[0], target_pose_[1], target_pose_[2]);
                }
            } else {
                // ---------- 关节空间自动序列模式 ----------
                for (const auto& joint : actions_[current_action_idx_]) {
                    msg.joint_names.push_back(joint.first);
                    msg.velocities.push_back(joint.second);
                }

                // ----- 打印序列模式信息（与原逻辑相同）-----
                double now = this->now().seconds();
                double elapsed = now - action_start_time_.seconds();
                if (elapsed - last_print_time_ >= 0.2 || elapsed <= 0.05) {
                    last_print_time_ = elapsed;
                    RCLCPP_INFO(this->get_logger(),
                        "→ 动作 %d | 已运行: %.1f/%.1f s",
                        current_action_idx_ + 1, elapsed, action_duration_);
                    for (const auto& joint : actions_[current_action_idx_]) {
                        double angle_deg = joint.second * elapsed * 180 / M_PI;
                        RCLCPP_INFO(this->get_logger(),
                            "   └─ %s : 速度 %.2f rad/s | 累计角度: %.2f°",
                            joint.first.c_str(), joint.second, angle_deg);
                    }
                    if (actions_[current_action_idx_].empty())
                        RCLCPP_INFO(this->get_logger(), "   └─ 所有关节停止");
                }
            }

            joint_pub_->publish(msg);
            rate.sleep();
        }
    }

    // ----- 动作切换线程（仅序列模式有效） -----
    void actionSwitchLoop() {
        while (rclcpp::ok()) {
            std::this_thread::sleep_for(std::chrono::milliseconds((int)(action_duration_ * 1000)));
            std::lock_guard<std::mutex> lock(action_mutex_);
            // 如果处于笛卡尔模式，不切换序列索引
            if (!cartesian_mode_active_) {
                current_action_idx_ = (current_action_idx_ + 1) % actions_.size();
                action_start_time_ = this->now();
                last_print_time_ = 0.0;
            }
        }
    }
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    signal(SIGINT, quit);

    auto node = std::make_shared<ArmAutoDemo>();
    rclcpp::spin(node);

    rclcpp::shutdown();
    return 0;
}