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

optimize working state

Browse Source
This commit is contained in:
david
2025-10-30 08:44:24 +08:00
parent 8c6bb8cd6e
commit 1eb2597583
2 changed files with 8 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/brain/src/cerebellum_node.cpp
View File

@@ -125,10 +125,6 @@ void CerebellumNode::ConfigureArmHooks()
RCLCPP_INFO(this->get_logger(), "[%s]Received Arm goal: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld, data_array=",
skill_name.c_str(), goal.body_id, goal.data_type, goal.data_length, goal.command_id, goal.frame_time_stamp);
for (auto & val : goal.data_array) {
RCLCPP_INFO(this->get_logger(), "%.6f", static_cast<double>(val));
}
goal.body_id = (tls_arm_body_id == LEFT_ARM ||
tls_arm_body_id == RIGHT_ARM) ? tls_arm_body_id : LEFT_ARM;
goal.command_id = ++command_id_;
@@ -158,9 +154,9 @@ void CerebellumNode::ConfigureArmHooks()
}
}
RCLCPP_INFO(this->get_logger(), "[%s] Control params: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld, data_array=[%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f]",
RCLCPP_INFO(this->get_logger(), "[%s] Control params: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld, data_array=[%.6f, %.6f, %.6f, %.6f, %.6f, %.6f]",
skill_name.c_str(), goal.body_id, goal.data_type, goal.data_length, goal.command_id, goal.frame_time_stamp,
goal.data_array[0], goal.data_array[1], goal.data_array[2], goal.data_array[3], goal.data_array[4], goal.data_array[5], goal.data_array[6]);
goal.data_array[0], goal.data_array[1], goal.data_array[2], goal.data_array[3], goal.data_array[4], goal.data_array[5]);
return goal;
};

6
src/brain/src/cerebrum_node.cpp
View File

@@ -1294,6 +1294,12 @@ void CerebrumNode::RobotWorkInfoPublish()
}
}
msg.task[1] =
(msg.task[2] == "StockIn")? "StockOut" :
(msg.task[2] == "StockOut")? "StockIn" :
(msg.task[0] == "StockIn")? "StockOut" :
(msg.task[0] == "StockOut")? "StockIn" : "None";
msg.working_state = (msg.bt_node_status == "RUNNING")? "Working" :
(msg.bt_node_status == "FAILURE")? "Fault" : "Standby";