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

optimize robot work info

Browse Source
This commit is contained in:
david
2025-10-29 09:39:43 +08:00
parent 9858019bee
commit 6e2f06b26a
1 changed files with 18 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -194,7 +194,7 @@ void CerebrumNode::RegisterActionClient()
g.calls.push_back(BuildSkillCallForSkill(s, std::nullopt));
}
}
robot_work_info_.bt_node_status = "Running";
robot_work_info_.bt_node_status = "RUNNING";
// Debug: log outgoing calls being sent to Cerebellum
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction] Sending goal with calls_count=%zu", g.calls.size());
for (size_t i = 0; i < g.calls.size(); ++i) {
@@ -276,7 +276,6 @@ void CerebrumNode::RegisterActionClient()
info.awaiting_result = false;
info.result = ok; // authoritative final
info.result_code = ok ? ExecResultCode::SUCCESS : ExecResultCode::FAILURE;
robot_work_info_.bt_node_status = info.result ? "SUCCESS" : "FAILURE";
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction][result] Finalizing node %s result=%s",
kv.first.c_str(), info.result ? "SUCCESS" : "FAILURE");
}
@@ -327,7 +326,6 @@ void CerebrumNode::CerebrumTask()
if (robot_work_info_.task.size() > 2) {
robot_work_info_.task[2] = (scheduled); //current
}
robot_work_info_.working_state = "RUNNING";
robot_work_info_.expt_completion_time = GetFutureTime(300);
robot_work_time_.start = rclcpp::Clock().now();
@@ -367,7 +365,7 @@ void CerebrumNode::ExecuteBehaviorTree()
robot_work_info_.task[0] = (scheduled);
robot_work_info_.task[2] = "None";
}
robot_work_info_.working_state = BT::toStr(status, false);
robot_work_info_.bt_node_status = BT::toStr(status, false);
robot_work_time_.total_seconds += (rclcpp::Clock().now() - robot_work_time_.start).seconds();
RCLCPP_INFO(this->get_logger(), "BT finished status=%s, scheduled=%s", BT::toStr(status, true).c_str(), scheduled.c_str());
@@ -1078,7 +1076,6 @@ void CerebrumNode::CreateServices()
// Update work info for successful rebuilds
if (resp->success) {
robot_work_info_.working_state = "RUNNING";
robot_work_info_.expt_completion_time = GetFutureTime(300);
robot_work_time_.start = rclcpp::Clock().now();
}
@@ -1249,6 +1246,7 @@ void CerebrumNode::RobotWorkInfoPublish()
robot_work_info_.task.push_back("None"); //last task
robot_work_info_.task.push_back("None"); //next task
robot_work_info_.task.push_back("None"); //current task
robot_work_info_.bt_node_status = "IDLE";
robot_work_info_pub_ = this->create_publisher<
interfaces::msg::RobotWorkInfo>("/robot_work_info", 10);
@@ -1267,13 +1265,22 @@ void CerebrumNode::RobotWorkInfoPublish()
msg.working_time = GetWorkingTime();
msg.nav_state = "Normal";
msg.comm_quality = "Good";
msg.expt_completion_time = robot_work_info_.expt_completion_time;
msg.work_log = "No log";
msg.skill = robot_work_info_.skill;
msg.action_name = robot_work_info_.action_name;
msg.instance_params = robot_work_info_.instance_params;
msg.bt_node_status = robot_work_info_.bt_node_status;
msg.smacc_state = (robot_work_info_.bt_node_status == "Running")? "StExecuting" : "StCompleted";
for (auto & task : msg.task) {
if (task == "bt_carry_boxes_sch1") {
task = "StockIn";
} else if (task == "bt_carry_boxes_sch2") {
task = "StockOut";
}
}
msg.working_state = (msg.bt_node_status == "RUNNING")? "Working" :
(msg.bt_node_status == "FAILURE")? "Fault" : "Standby";
msg.smacc_state = (msg.bt_node_status == "RUNNING")? "StExecuting" :
(msg.bt_node_status == "SUCCESS")? "StCompleted" :
(msg.bt_node_status == "FAILURE")? "StFailed" : "StIdle";
if (robot_work_info_pub_) {
robot_work_info_pub_->publish(msg);