HiveCoreRD/hivecore_robot_arm
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

屏蔽日志

Browse Source
This commit is contained in:
NuoDaJia02
2025-11-05 15:17:11 +08:00
parent 21d9952922
commit 87576a53a1
2 changed files with 29 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
rm_arm_control/src/rm_arm_handler.cpp
View File

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

8
rm_arm_control/src/rm_arm_node.cpp
View File

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