add ClearJointError and GetJointErrorAndBrake

rm_arm_control/include/arm_driver.hpp

@@ -58,6 +58,18 @@ public:
         return UNKNOWN_ERR;
     }
 
+    static int ClearJointError(int armId, int joint_num) {
+        (void)armId; (void)joint_num;
+        return 0; // Simulation always success
+    }
+
+    static int GetJointErrorAndBrake(int armId, uint16_t *err_flag, uint16_t *brake_state) {
+        (void)armId;
+        if (err_flag) *err_flag = 0;
+        if (brake_state) *brake_state = 0;
+        return 0;
+    }
+
     void PublicThreadUpdate();
 
 private:

rm_arm_control/include/rm_arm_node.hpp

@@ -5,6 +5,8 @@
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "interfaces/action/arm.hpp"
 #include <std_msgs/msg/string.hpp>
+#include "interfaces/msg/arm_error.hpp"
+#include "interfaces/srv/clear_arm_error.hpp"
 #include "arm_driver_define.hpp"
 #include "rm_arm_handler.hpp"
 
@@ -46,7 +48,15 @@ private:
     static rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr armStatesPub_;
     std::unordered_map<int64_t, std::shared_ptr<GoalHandleArm>> goal_handles_;
 
+    // NEW: Service and Topic
+    rclcpp::Service<interfaces::srv::ClearArmError>::SharedPtr clear_error_service_;
+    rclcpp::Publisher<interfaces::msg::ArmError>::SharedPtr arm_error_pub_;
+    rclcpp::TimerBase::SharedPtr error_timer_;
+
     void CallbackGetRobotDescription(const std_msgs::msg::String::SharedPtr msg);
+    void ClearArmErrorCallback(const std::shared_ptr<interfaces::srv::ClearArmError::Request> request,
+                               std::shared_ptr<interfaces::srv::ClearArmError::Response> response);
+    void ErrorTimerCallback();
 };
 
 #endif // RM_ARM_NODE_HPP

rm_arm_control/include/rm_driver_kinematics.hpp

@@ -56,6 +56,9 @@ public:
     int ForwardKinematics(float *jointAngles, float *endEffectorPose);
     int InverseKinematics(const float *endEffectorPose, float *jointAngles);
 
+    static int ClearJointError(int armId, int joint_num);
+    static int GetJointErrorAndBrake(int armId, uint16_t *err_flag, uint16_t *brake_state);
+
     static RM_Service *rmService_;
 
 private:

rm_arm_control/simulator/rm_driver_kinematics.cpp

@@ -240,7 +240,7 @@ int RmArmDriverAndKinematics::computeMovingTrajectory(NodataTrajectory *newTraje
     for (int i = 0; i < USED_ARM_DOF; ++i) {
         joints[i] = end[i];
     }
-    return rm_movej(robotHandle, joints, 18, 1, 0, 0);
+    return rm_movej(robotHandle, joints, 40, 1, 0, 0);
     // return 0;
 }
 
@@ -386,4 +386,35 @@ int RmArmDriverAndKinematics::InverseKinematics(const float *endEffectorPose, fl
     return 0;
 }
 
+int RmArmDriverAndKinematics::ClearJointError(int armId, int joint_num)
+{
+    if (rmService_ == nullptr) {
+        return UNKNOWN_ERR;
+    }
+    rm_robot_handle *handle = (armId == LEFT_ARM) ? leftRobotHandle_ : rightRobotHandle_;
+    if (handle == nullptr) {
+        return UNKNOWN_ERR;
+    }
+    //clear error
+    int ret = rmService_->rm_set_joint_clear_err(handle, joint_num);
+    if (ret == 0) {
+        //enable joint
+        rmService_->rm_set_joint_en_state(handle, joint_num, 1);
+    }
+    
+    return ret;
+}
+
+int RmArmDriverAndKinematics::GetJointErrorAndBrake(int armId, uint16_t *err_flag, uint16_t *brake_state)
+{
+    if (rmService_ == nullptr) {
+        return UNKNOWN_ERR;
+    }
+    rm_robot_handle *handle = (armId == LEFT_ARM) ? leftRobotHandle_ : rightRobotHandle_;
+    if (handle == nullptr) {
+        return UNKNOWN_ERR;
+    }
+    return rmService_->rm_get_joint_err_flag(handle, err_flag, brake_state);
+}
+
 

rm_arm_control/src/rm_arm_node.cpp

@@ -127,6 +127,14 @@ RmArmNode::RmArmNode(ArmDriver *armDriver, Kinematics *kinematics) : Node("rm_ar
 
     armStatesPub_ = this->create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
 
+    clear_error_service_ = this->create_service<interfaces::srv::ClearArmError>(
+        "clear_arm_error", std::bind(&RmArmNode::ClearArmErrorCallback, this, std::placeholders::_1, std::placeholders::_2));
+    
+    arm_error_pub_ = this->create_publisher<interfaces::msg::ArmError>("arm_errors", 10);
+    
+    error_timer_ = this->create_wall_timer(
+        500ms, std::bind(&RmArmNode::ErrorTimerCallback, this));
+
     this->action_server_ = rclcpp_action::create_server<ArmAction>(
         this,
         "ArmAction",
@@ -508,3 +516,41 @@ void RmArmNode::CallbackRealtimeArmJointState(ArmStatusCallbackData *data)
         armStatesPub_->publish(armStates);
     }
 }
+
+void RmArmNode::ClearArmErrorCallback(const std::shared_ptr<interfaces::srv::ClearArmError::Request> request,
+                                     std::shared_ptr<interfaces::srv::ClearArmError::Response> response)
+{
+    RCLCPP_INFO(this->get_logger(), "Clearing error for arm %d, joint %d", request->arm_id, request->joint_num);
+    int result = ArmDriver::ClearJointError(request->arm_id, request->joint_num);
+    if (result == 0) {
+        response->success = true;
+        response->message = "Clear error success";
+        RCLCPP_INFO(this->get_logger(), "Clear error success");
+    } else {
+        response->success = false;
+        response->message = "Clear error failed with code: " + std::to_string(result);
+        RCLCPP_ERROR(this->get_logger(), "Clear error failed with code: %d", result);
+    }
+}
+
+void RmArmNode::ErrorTimerCallback()
+{
+    auto publish_error = [&](int arm_id) {
+        uint16_t err_flags[ARM_DOF] = {0};
+        uint16_t brake_states[ARM_DOF] = {0};
+        
+        int result = ArmDriver::GetJointErrorAndBrake(arm_id, err_flags, brake_states);
+        if (result == 0) {
+            interfaces::msg::ArmError msg;
+            msg.arm_id = arm_id;
+            for(int i=0; i<ARM_DOF; ++i) {
+                msg.err_flag.push_back(err_flags[i]);
+                msg.brake_state.push_back(brake_states[i]);
+            }
+            arm_error_pub_->publish(msg);
+        }
+    };
+
+    publish_error(LEFT_ARM);
+    publish_error(RIGHT_ARM);
+}