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:
zj
2025-10-27 17:10:52 +08:00
parent cac66111db
commit 1b37132764
1 changed files with 9 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
rm_arm_control/src/rm_arm_handler.cpp
View File

@@ -30,7 +30,7 @@ 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:");
for (size_t i = 0; i < USED_ARM_DOF; i++){
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointCurrent[i]);
}
@@ -44,13 +44,14 @@ void PrintData(ArmStatusCallbackData *data)
for (size_t i = 0; i < USED_ARM_DOF; i++){
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d ", data->jointErrCode[i]);
}
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "position:");
*/
std::string posStr = "joints position: ";
for (size_t i = 0; i < USED_ARM_DOF; i++){
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointAngle[i]);
posStr += std::to_string(data->jointAngle[i]) + " ";
}
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%s", posStr.c_str());
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "speed:");
/*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]);
}
@@ -63,7 +64,7 @@ void PrintData(ArmStatusCallbackData *data)
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]);
}
}*/
}
@@ -88,9 +89,9 @@ void RmArmHandler::ArmDealCallBackInfo(ArmStatusCallbackData *data)
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ARM_STATUS_WAITING.");
armStatus = ARM_STATUS_WAITING;
}
/*if (temp != ARM_STATUS_READY){
if (temp != ARM_STATUS_READY){
PrintData(data);
}*/
}
}
int RmArmHandler::AddNextPoint(float points[GOAL_DATA_LENGTH], const int64_t &index, int type)