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

修正sleep时间2

Browse Source
This commit is contained in:
zj
2025-10-30 17:42:13 +08:00
parent 95a6d6de9d
commit bc6f8b99dd
1 changed files with 4 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
rm_arm_control/src/rm_arm_node.cpp
View File

@@ -387,6 +387,7 @@ void RmArmNode::ArmActionCheck()
// Execute the goal
while (rclcpp::ok()) {
if (!busy) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Arm action check loop is idle, wait time %ld ns.", timeout.tv_nsec);
int result = sem_timedwait(&sem_, &timeout);
if (result == 0) {
if (leftArmHandler_ != NULL && (leftArmHandler_->armStatus == ARM_STATUS_READY)) {
@@ -395,6 +396,8 @@ void RmArmNode::ArmActionCheck()
if (rightArmHandler_ != NULL && (rightArmHandler_->armStatus == ARM_STATUS_READY)) {
StartNewGoal(rightArmHandler_);
}
} else {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Arm action check loop wait timeout.");
}
}
int64_t currentTimeNs = get_clock()->now().nanoseconds();
@@ -439,7 +442,7 @@ void RmArmNode::ArmActionCheck()
usedTime);
}
} else {
timeout.tv_nsec += nextCheckPoint - afterCheckTimeNs;
timeout.tv_nsec += ARM_FOLLOWING_CHECKING;
if (timeout.tv_nsec >= 1e9) {
timeout.tv_sec += timeout.tv_nsec / (int64_t)(1e9);
timeout.tv_nsec = timeout.tv_nsec % (int64_t)(1e9);