From 87576a53a1e07ef0b459399367d04b339befb02f Mon Sep 17 00:00:00 2001 From: NuoDaJia02 Date: Wed, 5 Nov 2025 15:17:11 +0800 Subject: [PATCH] =?UTF-8?q?=E5=B1=8F=E8=94=BD=E6=97=A5=E5=BF=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- rm_arm_control/src/rm_arm_handler.cpp | 41 ++++++++++++++++----------- rm_arm_control/src/rm_arm_node.cpp | 8 +++--- 2 files changed, 29 insertions(+), 20 deletions(-) diff --git a/rm_arm_control/src/rm_arm_handler.cpp b/rm_arm_control/src/rm_arm_handler.cpp index 375ca9d..275951f 100644 --- a/rm_arm_control/src/rm_arm_handler.cpp +++ b/rm_arm_control/src/rm_arm_handler.cpp @@ -30,12 +30,15 @@ void PrintData(ArmStatusCallbackData *data) { RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "==================MYCALLBACK=================="); RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "errCode: %d", data->errCode); - /*RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "current:"); + //RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "current:"); + std::string current = "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]); } @@ -43,8 +46,8 @@ void PrintData(ArmStatusCallbackData *data) RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "error code:"); for (size_t i = 0; i < USED_ARM_DOF; i++){ RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d ", data->jointErrCode[i]); - } - */ + }*/ + std::string posStr = "joints position: "; for (size_t i = 0; i < USED_ARM_DOF; i++){ posStr += std::to_string(data->jointAngle[i]) + " "; @@ -54,18 +57,23 @@ void PrintData(ArmStatusCallbackData *data) /*RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "speed:"); for (size_t i = 0; i < USED_ARM_DOF; i++){ RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointSpeed[i]); - } - - 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]); - } - - 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]); }*/ + posStr = "temperature: "; + //RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "temperature:"); + for (size_t i = 0; i < USED_ARM_DOF; 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()); + + 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]); + 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 +98,8 @@ void RmArmHandler::ArmDealCallBackInfo(ArmStatusCallbackData *data) armStatus = ARM_STATUS_WAITING; } if (temp != ARM_STATUS_READY){ - PrintData(data); + // if (armType_ == 1) + // PrintData(data); } } diff --git a/rm_arm_control/src/rm_arm_node.cpp b/rm_arm_control/src/rm_arm_node.cpp index cdbc4e6..2cc860b 100644 --- a/rm_arm_control/src/rm_arm_node.cpp +++ b/rm_arm_control/src/rm_arm_node.cpp @@ -279,10 +279,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); } }