修正初始链接，去掉注释

rm_arm_control/simulator/col_simulator.cpp

@@ -11,7 +11,7 @@ float g_leftArmJoints[COLLISION_CHECK_SIMPLE_CNT][USED_ARM_DOF] = {0};
 float g_rightArmJoints[COLLISION_CHECK_SIMPLE_CNT][USED_ARM_DOF] = {0};
 float g_otherJoints[COLLISION_CHECK_SIMPLE_CNT][USED_OTHER_DOF] = {0};
 
-const std::string g_bodyStartName[BODY_CNT] = {"LeftLink1", "RightLink1", "base_link"};
+const std::string g_bodyStartName[BODY_CNT] = {"base_link_leftarm", "base_link_rightarm", "base_link"};
 
 static float zeroFixedAngle = 0;
 
@@ -607,7 +607,6 @@ int CollisionSimulator::CheckBodyAngleLimit(int simple, int bodyType)
 int CollisionSimulator::CheckArmJointsAngleLimit(float *joints)
 {
     CollisionStructureInfo *colStruct = &collision_structures_[g_linkStartIndex[BODY_TYPE_LEFT_ARM]];
-
     std::vector<CollisionStructureInfo *> child = {colStruct};
     int childIndex = 0;
     int childCnt = 1;

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, 5, 1, 0, 0);
+    return rm_movej(robotHandle, joints, 20, 1, 0, 0);
 }
 
 int RmArmDriverAndKinematics::computeSteppingTrajectory(NodataTrajectory *newTrajectory, const float *start, const float *speed, int armId)

rm_arm_control/src/rm_arm_handler.cpp

@@ -32,10 +32,12 @@ void PrintData(ArmStatusCallbackData *data)
     RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "errCode: %d", data->errCode);
     RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "current:");
     for (size_t i = 0; i < USED_ARM_DOF; i++){
-        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointCurrent[i]);
+        current += std::to_string(data->jointCurrent[i]) + " ";
+        //RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointCurrent[i]);
     }
+    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%s", current.c_str());
     
-    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "enable:");
+    /*RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "enable:");
     for (size_t i = 0; i < USED_ARM_DOF; i++){
         RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d ", data->jointEnFlag[i]);
     }
@@ -56,16 +58,21 @@ void PrintData(ArmStatusCallbackData *data)
         RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointSpeed[i]);
     }
     
-    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "temperature:");
+    posStr = "temperature: ";
+    //RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "temperature:");
     for (size_t i = 0; i < USED_ARM_DOF; i++){
-        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointTemp[i]);
+        posStr += std::to_string(data->jointTemp[i]) + " ";
+        //RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointTemp[i]);
     }
+    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%s", posStr.c_str());
     
-    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "voltage:");
+    posStr = "voltage: ";
+    //RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "voltage:");
     for (size_t i = 0; i < USED_ARM_DOF; i++){
-        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointVoltage[i]);
-    }*/
-    
+        //RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointVoltage[i]);
+        posStr += std::to_string(data->jointVoltage[i]) + " ";
+    }
+    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%s", posStr.c_str());*/
 }
 
 void RmArmHandler::ArmDealCallBackInfo(ArmStatusCallbackData *data)
@@ -90,7 +97,8 @@ void RmArmHandler::ArmDealCallBackInfo(ArmStatusCallbackData *data)
         armStatus = ARM_STATUS_WAITING;
     }
     if (temp != ARM_STATUS_READY){
-        PrintData(data);
+        // if (armType_ == 1)
+        //     PrintData(data);
     }
 }
 

rm_arm_control/src/rm_arm_node.cpp

@@ -278,10 +278,10 @@ void RmArmNode::ArmActionFeedback(int coll)
         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]);
+        // 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);
     }
 }