/*********************************************************************
* 机械臂 自动动作切换 + 笛卡尔空间控制 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_namesi);
if (it != msg->name.end()) {
size_t idx = std::distance(msg->name.begin(), it);
current_joints_i = msg->positionidx;
} else {
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
"关节 %s 未在 joint_states 中找到", expected_namesi.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_pose0, current_pose1, current_pose2,
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;
}