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); } }