#include "arm_define.h"
// #include "rm_arm_simulator.hpp"
#include "rm_arm_node.hpp"
#include "col_simulator.hpp"

// static ArmSimulator *armSimulator_;
static CollisionSimulator *colSimulator_;

using ArmGoalSharedPtr =
        std::shared_ptr<const interfaces::action::Arm::Goal>;

namespace ArmCommandHash
{
    using ArmCommand = int(*)(RmArmHandler *handle, const ArmGoalSharedPtr goal);

    static int AngleStepOn(RmArmHandler *handle, const ArmGoalSharedPtr goal)
    {
        if (goal->data_length != USED_ARM_DOF) {
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "AngleStepOn data length error.");
            return -1;
        }
        float points[GOAL_DATA_LENGTH] = {0};
        std::string speeds = "aim speed ";
        for (int i = 0; i < USED_ARM_DOF; i++) {
            points[i] = goal->data_array[i];
            speeds += std::to_string(points[i]) + " ";
        }
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%s", speeds.c_str());
        int result = handle->AddNextPoint(points, goal->command_id, GOAL_TYPE_STEPPING);
        return result;
    }

    static int AngleDirectMove(RmArmHandler *handle, std::shared_ptr<const ArmAction::Goal> goal)
    {
        if (goal->data_length != USED_ARM_DOF) {
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "AngleDirectMove data length error.");
            return -1;
        }
        float points[GOAL_DATA_LENGTH] = {0};
        for (int i = 0; i < USED_ARM_DOF; i++) {
            points[i] = goal->data_array[i];
        }
        if (colSimulator_->CheckArmJointsAngleLimit(points) != 0) {
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "AngleDirectMove joints limit error.");
            return ARM_AIM_CANNOT_REACH;
        }
        int result = handle->AddNextPoint(points, goal->command_id, GOAL_TYPE_MOVING);
        return result;
    }

    static int PoseStepOn(RmArmHandler *handle, std::shared_ptr<const ArmAction::Goal> goal)
    {
        if (goal->data_length != POSE_DIMENSION) {
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "PoseStepOn data length error.");
            return -1;
        }
        float points[GOAL_DATA_LENGTH] = {0};
        for (int i = 0; i < POSE_DIMENSION; i++) {
            points[i] = goal->data_array[i];
        }
        return handle->AddNextPointFromPose(points, goal->command_id, GOAL_TYPE_STEPPING);
    }

    static int PoseDirectMove(RmArmHandler *handle, std::shared_ptr<const ArmAction::Goal> goal)
    {
        if (goal->data_length != POSE_DIMENSION) {
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "PoseDirectMove data length error.");
            return -1;
        }
        float points[GOAL_DATA_LENGTH] = {0};
        for (int i = 0; i < POSE_DIMENSION; i++) {
            points[i] = goal->data_array[i];
        }
        return handle->AddNextPointFromPose(points, goal->command_id, GOAL_TYPE_MOVING);
    }

    const std::unordered_map<int, ArmCommand> armCommandMap = {
        {ARM_COMMAND_TYPE_ANGLE_STEP_ON, AngleStepOn},
        {ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE, AngleDirectMove},
        {ARM_COMMAND_TYPE_POSE_STEP_ON, PoseStepOn},
        {ARM_COMMAND_TYPE_POSE_DIRECT_MOVE, PoseDirectMove}
    };
}

static RmArmNode *localNodePtr = NULL;
RmArmHandler *RmArmNode::leftArmHandler_;
RmArmHandler *RmArmNode::rightArmHandler_;
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr RmArmNode::armStatesPub_;
sensor_msgs::msg::JointState RmArmNode::armStates;

static RmArmHandler *GetArmHandler(RmArmNode *node, int body_id)
{
    RmArmHandler *handle = NULL;
    if (body_id == LEFT_ARM) {
        handle = node->leftArmHandler_;
    } else if (body_id == RIGHT_ARM) {
        handle = node->rightArmHandler_;
    }
    return handle;
}

#define LEFT_ARM_JOINT_START_INDEX 9
#define RIGHT_ARM_JOINT_START_INDEX 15
#define JOINT_ALL_CNT 29
// 机器人控制器构造函数
RmArmNode::RmArmNode(ArmDriver *armDriver, Kinematics *kinematics) : Node("rm_arm_control"), armDriver_(armDriver)
{
    localNodePtr = this;
    sem_init(&sem_, 0, 0);

    KinematicsManager *leftTrajectory = new KinematicsManager(kinematics, LEFT_ARM);
    KinematicsManager *rightTrajectory = new KinematicsManager(kinematics, RIGHT_ARM);

    leftArmHandler_ = new RmArmHandler(armDriver_, LEFT_ARM, &sem_, leftTrajectory);
    if (leftArmHandler_ != NULL && leftArmHandler_->armStatus == ARM_STATUS_READY) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Left arm handler initialization success.");
    } else {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Left arm handler initialization failed.");
    }

    rightArmHandler_ = new RmArmHandler(armDriver_, RIGHT_ARM, &sem_, rightTrajectory);
    if (rightArmHandler_ != NULL && rightArmHandler_->armStatus == ARM_STATUS_READY) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Right arm handler initialization success.");
    } else {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Right arm handler initialization failed.");
    }

    armStatesPub_ = this->create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);

    this->action_server_ = rclcpp_action::create_server<ArmAction>(
        this,
        "ArmAction",
        std::bind(&RmArmNode::ArmActionGoal, this, std::placeholders::_1, std::placeholders::_2),
        std::bind(&RmArmNode::ArmActionCancel, this, std::placeholders::_1),
        std::bind(&RmArmNode::ArmActionAccepted, this, std::placeholders::_1));

    this->declare_parameter<std::string>("robot_description", "");
    std::string urdf_string = this->get_parameter("robot_description").as_string();

    this->declare_parameter<std::string>("robot_description_semantic", "");
    std::string srdf_string = this->get_parameter("robot_description_semantic").as_string();
    if (srdf_string == "") {
        throw std::runtime_error("robot_description_semantic parameter is not valid.");
    }

    this->declare_parameter<std::string>("robot_description_xml", "");
    std::string xml_string = this->get_parameter("robot_description_xml").as_string();
    if (xml_string == "") {
        throw std::runtime_error("robot_description_xml parameter is not valid.");
    }

    /*rightArmHandler_->nowJoints_[0] = 92.56;
    rightArmHandler_->nowJoints_[1] = 105.44;
    rightArmHandler_->nowJoints_[2] = 27.5;*/
    
    armStates.name = {
        "left_behind_hip_joint","left_behind_leg_joint","left_behind_wheel_joint","right_behind_hip_joint",
        "right_behind_leg_joint","right_behind_wheel_joint","body_joint","body_2_joint","head_joint",
        "left_joint1","left_joint2","left_joint3","left_joint4","left_joint5", "left_joint6",
        "right_joint1","right_joint2","right_joint3","right_joint4","right_joint5", "right_joint6",
        "left_front_roll_joint","left_front_hip_joint","left_front_knee_joint",
        "left_front_wheel_joint","right_front_roll_joint","right_front_hip_joint","right_front_knee_joint","right_front_wheel_joint"};
    armStates.position.resize(JOINT_ALL_CNT, 0.0);
    armStates.header.frame_id = "base_link";
    colSimulator_ = new CollisionSimulator(xml_string, srdf_string, 
        (float**)(std::vector<float*>{leftArmHandler_->nowJoints_, rightArmHandler_->nowJoints_}.data()),
        (KinematicsManager**)(std::vector<KinematicsManager*>{leftArmHandler_->trapezoidalTrajectory_, rightArmHandler_->trapezoidalTrajectory_}.data()));

    std::thread{std::bind(&RmArmNode::ArmActionCheck, this)}.detach();
    std::thread{std::bind(&RmArmNode::ArmActionExecute, this)}.detach();

    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Robot arm control node initialized.");
}

RmArmNode::~RmArmNode() 
{
    armStatesPub_.reset();
    delete leftArmHandler_;
    delete rightArmHandler_;
    // delete armSimulator_;
}

void RmArmNode::CallbackGetRobotDescription(const std_msgs::msg::String::SharedPtr msg)
{
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Received robot_description message.");
    /*if (armSimulator_ == NULL) {
        armStates.name = {
            "left_behind_hip_joint","left_behind_leg_joint","left_behind_wheel_joint","right_behind_hip_joint",
            "right_behind_leg_joint","right_behind_wheel_joint","body_joint","body_2_joint","head_joint",
            "left_joint1","left_joint2","left_joint3","left_joint4","left_joint5", "left_joint6",
            "right_joint1","right_joint2","right_joint3","right_joint4","right_joint5", "right_joint6",
            "left_front_roll_joint","left_front_hip_joint","left_front_knee_joint",
            "left_front_wheel_joint","right_front_roll_joint","right_front_hip_joint","right_front_knee_joint","right_front_wheel_joint"};
        armStates.position.resize(JOINT_ALL_CNT, 0.0);
        armStates.header.frame_id = "base_link";
        armSimulator_ = new ArmSimulator(msg, leftArmHandler_->nowJoints_, rightArmHandler_->nowJoints_,
            leftArmHandler_->trapezoidalTrajectory_, rightArmHandler_->trapezoidalTrajectory_);
    }*/
}

rclcpp_action::GoalResponse RmArmNode::ArmActionGoal(
    const rclcpp_action::GoalUUID & uuid,
    std::shared_ptr<const ArmAction::Goal> goal)
{
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Received goal request.");
    // Accept the goal
    if (ArmCommandHash::armCommandMap.find(goal->data_type) == ArmCommandHash::armCommandMap.end()) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Received goal with invalid command id: %d", goal->data_type);
        return rclcpp_action::GoalResponse::REJECT;
    }
    RmArmHandler *handle = GetArmHandler(this, goal->body_id);
    if (handle == NULL) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "arm validation error.");
        return rclcpp_action::GoalResponse::REJECT;
    }
    if (handle->armStatus == ARM_STATUS_ERROR || handle->armStatus == ARM_STATUS_INIT) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Arm in error status.");
        return rclcpp_action::GoalResponse::REJECT;
    }
    int result = ArmCommandHash::armCommandMap.at(goal->data_type)(handle, goal);
    if (result != 0) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to process the goal.");
        return rclcpp_action::GoalResponse::REJECT;
    }
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}

rclcpp_action::CancelResponse RmArmNode::ArmActionCancel(
    const std::shared_ptr<GoalHandleArm> goal_handle)
{
    ArmGoalSharedPtr goal = goal_handle->get_goal();
    int64_t index = goal->command_id;
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Received request to cancel goal. Index: %ld", index);
    int result = 0;
    RmArmHandler *handle = GetArmHandler(this, goal->body_id);
    if (handle == NULL) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "arm validation error.");
        return rclcpp_action::CancelResponse::REJECT;
    }
    result = handle->DeletePoint(goal->command_id);
    if (result != 0) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to cancel the goal.");
        return rclcpp_action::CancelResponse::REJECT;
    }
    // Accept the cancel request
    return rclcpp_action::CancelResponse::ACCEPT;
}

void RmArmNode::ArmActionFeedback(int coll)
{
    float leftPose[POSE_DIMENSION] = {0};
    float rightPose[POSE_DIMENSION] = {0};
    RmArmHandler *leftHandle = GetArmHandler(this, LEFT_ARM);
    RmArmHandler *rightHandle = GetArmHandler(this, RIGHT_ARM);
    if (leftHandle != NULL && leftHandle->armStatus != ARM_STATUS_ERROR && leftHandle->armStatus != ARM_STATUS_INIT) {
        leftHandle->trapezoidalTrajectory_->armKinematics_->ForwardKinematics(leftHandle->nowJoints_, leftPose);
    }
    if (rightHandle != NULL && rightHandle->armStatus != ARM_STATUS_ERROR && rightHandle->armStatus != ARM_STATUS_INIT) {
        rightHandle->trapezoidalTrajectory_->armKinematics_->ForwardKinematics(rightHandle->nowJoints_, rightPose);
    }
    for (auto it = goal_handles_.begin(); it != goal_handles_.end(); ++it) {
        auto goal_handle = it->second;
        ArmGoalSharedPtr goal = goal_handle->get_goal();
        RmArmHandler *handle = GetArmHandler(this, goal->body_id);
        if (handle == NULL) {
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "arm validation error.");
            continue;
        }
        if (handle->armStatus == ARM_STATUS_ERROR || handle->armStatus == ARM_STATUS_INIT) {
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Arm in error status.");
            continue;
        }
        float *pose = (goal->body_id == LEFT_ARM) ? leftPose : rightPose;
        
        auto feedback = std::make_shared<ArmAction::Feedback>();
        feedback->command_id = goal->command_id;
        feedback->int_lenth = 1;
        feedback->float_length = POSE_DIMENSION;
        feedback->int_data_array = {coll};
        feedback->float_data_array = {pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], pose[6]};
        // RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Publishing feedback for command id: %ld, joints: %f, %f, %f, %f, %f, %f, %f",
        //     feedback->command_id, feedback->float_data_array[0], feedback->float_data_array[1],
        //     feedback->float_data_array[2], feedback->float_data_array[3],
        //     feedback->float_data_array[4], feedback->float_data_array[5], feedback->float_data_array[6]);
        goal_handle->publish_feedback(feedback);
    }
}

void RmArmNode::ArmActionFinish(int64_t index, int result)
{
    rclcpp::Time endColTime = this->get_clock()->now();
    auto goal_handle_pair = goal_handles_.find(index);
    if (goal_handle_pair != goal_handles_.end()) {
        auto goal_handle = goal_handle_pair->second;
        auto goalResult = std::make_shared<ArmAction::Result>();
        float pose[POSE_DIMENSION] = {0};
        RmArmHandler *handle = GetArmHandler(this, goal_handle->get_goal()->body_id);
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Finishing goal joints: %f, %f, %f, %f, %f, %f",
            handle->nowJoints_[0], handle->nowJoints_[1], handle->nowJoints_[2],
            handle->nowJoints_[3], handle->nowJoints_[4], handle->nowJoints_[5]);
        if (handle != NULL && handle->armStatus != ARM_STATUS_ERROR && handle->armStatus != ARM_STATUS_INIT) {
            handle->trapezoidalTrajectory_->armKinematics_->ForwardKinematics(handle->nowJoints_, pose);
        }
        goalResult->end_time = endColTime.nanoseconds();
        goalResult->result = result;
        goalResult->command_id = index;
        goalResult->pose.position.x = pose[POSE_POSITION_X];
        goalResult->pose.position.y = pose[POSE_POSITION_Y];
        goalResult->pose.position.z = pose[POSE_POSITION_Z];
        goalResult->pose.orientation.x = pose[POSE_QUATERNION_X];
        goalResult->pose.orientation.y = pose[POSE_QUATERNION_Y];
        goalResult->pose.orientation.z = pose[POSE_QUATERNION_Z];
        goalResult->pose.orientation.w = pose[POSE_QUATERNION_W];
        if (goal_handle->is_canceling()) {
            goal_handle->canceled(goalResult);
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Goal %ld canceled.", index);
        } else if (result == OK){
            goal_handle->succeed(goalResult);
        } else {
            goal_handle->abort(goalResult);
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Goal %ld aborted.", index);
        }
        goal_handles_.erase(index);
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Goal %ld finished. now aim cnt %ld.", index, goal_handles_.size());
    } else {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Goal handle not found for index %ld.", index);
    }
}

void RmArmNode::StartNewGoal(RmArmHandler *handle)
{
    if (goal_handles_.size() == 0) {
        return;
    }
    int ret;
    int64_t index = handle->GetCurrentRunOffGoalIndex();
    if (index != -1) {
        ret = handle->GoalReached();
        ArmActionFinish(index, ret);
    }
    index = handle->GetCurrentGoalIndex();
    if (goal_handles_.find(index) == goal_handles_.end()) {
        return;
    }
    do {
        ret = handle->StartNewTrajectory();
        if (ret == 0) {
            // ret = armSimulator_->UpdateCollisionForNewGoal(handle->armType_);
            ret = colSimulator_->UpdateCollisionForNewGoal(handle->armType_);
            if (ret == SAFE) {
                handle->ArmMove();
            } else {
                handle->ArmStopStart(ARM_COLLISION);
                ret = ARM_COLLISION;
            }
        }
        if (ret != ARM_NOW_NO_GOAL && ret != 0) {
            index = handle->GetCurrentRunOffGoalIndex();
            handle->GoalReached();
            ArmActionFinish(index, ret);
            RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to start new goal with error code: %d.", ret);
        }
    } while (ret != 0 && ret != ARM_NOW_NO_GOAL);
}

int g_checkCnt = 0;
int64_t g_totalColTime = 0;
int64_t g_maxColTime = 0;

void RmArmNode::ArmActionCheck()
{
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Checking\n");
    rclcpp::Time startTime = get_clock()->now();
    int64_t nextCheckPoint = startTime.nanoseconds() + ARM_FOLLOWING_CHECKING;
    struct timespec timeout;
    clock_gettime(CLOCK_REALTIME, &timeout);
    timeout.tv_nsec += ARM_FOLLOWING_CHECKING;
    if (timeout.tv_nsec >= 1e9) {
        timeout.tv_sec += timeout.tv_nsec / (int64_t)(1e9);
        timeout.tv_nsec = timeout.tv_nsec % (int64_t)(1e9);
    }
    int step = 0;
    bool busy = false;

    int64_t maxUsedTime = 0;

    // Execute the goal
    while (rclcpp::ok()) {
        if (!busy) {
            int result = sem_timedwait(&sem_, &timeout);
            if (!rclcpp::ok()) {
                break;
            }

            if (leftArmHandler_ != NULL && (leftArmHandler_->armStatus == ARM_STATUS_READY)) {
                StartNewGoal(leftArmHandler_);
            }
            if (rightArmHandler_ != NULL && (rightArmHandler_->armStatus == ARM_STATUS_READY)) {
                StartNewGoal(rightArmHandler_);
            }
            
        }
        int64_t currentTimeNs = get_clock()->now().nanoseconds();
        if (currentTimeNs >= nextCheckPoint) {
            step++;
            nextCheckPoint += ARM_FOLLOWING_CHECKING;
            int coll = colSimulator_->CheckCollision(this); //armSimulator_->CheckCollision();

            g_checkCnt++;
            int64_t durationCol = (get_clock()->now().nanoseconds() - currentTimeNs);
            g_totalColTime += durationCol;
            if (g_maxColTime < durationCol) {
                g_maxColTime = durationCol;
            }
            if (g_checkCnt % 2000 == 0) {
                RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()),
                    "Collision check avg time: %ld us, max time: %ld us",
                    g_totalColTime / g_checkCnt / 1000, g_maxColTime / 1000);
                g_checkCnt = 0;
                g_totalColTime = 0;
            }

            if (coll != SAFE) {
            // if (coll == ARM_AIM_CANNOT_REACH) {
                leftArmHandler_->ArmStop(ARM_COLLISION);
                rightArmHandler_->ArmStop(ARM_COLLISION);
                ArmActionFeedback(coll);
                step = 0;
            } else if (step >= 5) {
                ArmActionFeedback(coll);
                step = 0;
            }
        }

        int64_t afterCheckTimeNs = get_clock()->now().nanoseconds();

        if (afterCheckTimeNs >= nextCheckPoint) {
            busy = true;
            int64_t usedTime = afterCheckTimeNs - currentTimeNs;
            if (usedTime > 10 * 1e6) {
                RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "arm action check loop is too slow!, %ld",
                    usedTime);
            }
        } else {
            timeout.tv_nsec += ARM_FOLLOWING_CHECKING;
            if (timeout.tv_nsec >= 1e9) {
                timeout.tv_sec += timeout.tv_nsec / (int64_t)(1e9);
                timeout.tv_nsec = timeout.tv_nsec % (int64_t)(1e9);
            }
            busy = false;
        }
    }
}

void RmArmNode::ArmActionExecute()
{
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Executing\n");
    rclcpp::Time startTime = get_clock()->now();
    int64_t startTimeNs = startTime.nanoseconds();
    // Execute the goal
    while (rclcpp::ok()) {
        if (colSimulator_ != NULL) {
            if (leftArmHandler_ != NULL) {
                leftArmHandler_->ArmRun(startTimeNs);
            }
            if (rightArmHandler_ != NULL) {
                rightArmHandler_->ArmRun(startTimeNs);
            }
        }
        startTimeNs += ARM_FOLLOWING_PERIOD;
        int64_t currentTimeNs = get_clock()->now().nanoseconds();
        int64_t sleepTime = (startTimeNs - (currentTimeNs)) / 1e3;
        if (sleepTime > 0) {
            std::this_thread::sleep_for(std::chrono::microseconds(sleepTime));
        } else {
            if (sleepTime < -10 * 1e3) {
                RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "arm action execute loop is too slow!, %ld",
                    -sleepTime);
                startTimeNs = currentTimeNs + ARM_FOLLOWING_PERIOD;
            }
        }
    }
}

void RmArmNode::ArmActionAccepted(const std::shared_ptr<GoalHandleArm> goal_handle)
{
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Goal accepted. Index: %ld", goal_handle->get_goal()->command_id);
    // Start executing the goal
    goal_handles_[goal_handle->get_goal()->command_id] = goal_handle;
}

void RmArmNode::CallbackRealtimeArmJointState(ArmStatusCallbackData *data) 
{
    if (data->armId == LEFT_ARM && leftArmHandler_ != NULL) {
        leftArmHandler_->ArmDealCallBackInfo(data);
    }
    else if (data->armId == RIGHT_ARM && rightArmHandler_ != NULL) {
        rightArmHandler_->ArmDealCallBackInfo(data);
    }
    if (armStatesPub_ != NULL && colSimulator_ != NULL) {
        for (int i = LEFT_ARM_JOINT_START_INDEX; i < LEFT_ARM_JOINT_START_INDEX + USED_ARM_DOF; i++) {
            armStates.position[i] = leftArmHandler_->nowJoints_[i - LEFT_ARM_JOINT_START_INDEX] * M_PI / 180.0f;
        }
        for (int i = RIGHT_ARM_JOINT_START_INDEX; i < RIGHT_ARM_JOINT_START_INDEX + USED_ARM_DOF; i++) {
            armStates.position[i] = rightArmHandler_->nowJoints_[i - RIGHT_ARM_JOINT_START_INDEX] * M_PI / 180.0f;
        }
        armStates.header.stamp = localNodePtr->get_clock()->now();
        armStates.velocity = {};
        armStates.effort = {};
        // 发布消息
        armStatesPub_->publish(armStates);
    }
}
