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

optimize robot working info

Browse Source
This commit is contained in:
david
2025-10-22 21:21:52 +08:00
parent 8e670bfcc8
commit 3dd9e995fe
2 changed files with 148 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
src/brain/include/brain/cerebrum_node.hpp
View File

@@ -219,6 +219,7 @@ private:
std::mutex exec_mutex_;
rclcpp::Publisher<interfaces::msg::RobotWorkInfo>::SharedPtr robot_work_info_pub_;
interfaces::msg::RobotWorkInfo robot_work_info_;
struct PerNodeExecInfo
{
@@ -338,8 +339,25 @@ private:
*/
bool LoadRobotConfiguration();
/**
* @brief Publish robot work information.
*/
void RobotWorkInfoPublish();
/**
* @brief Simulate battery capacity.
* @return int
*/
int SimulatedBatteryCapacity();
/**
* @brief Get the Future Time object
*
* @param seconds
* @return std::string
*/
std::string GetFutureTime(int32_t seconds);
// Temporary override used so the generic ExecuteBtAction goal builder can
// send a single-skill goal when a per-skill BT node starts.
std::optional<std::string> single_skill_goal_override_;

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

@@ -108,7 +108,13 @@ CerebrumNode::CerebrumNode(const rclcpp::NodeOptions & options)
RegisterActionClient();
bt_timer_ = this->create_wall_timer(10ms, [this]() {ExecuteBehaviorTree();});
task_timer_ = this->create_wall_timer(10000ms, [this]() {CerebrumTask();});
if (!bt_timer_) {
RCLCPP_ERROR(this->get_logger(), "Failed to create BT timer");
}
// task_timer_ = this->create_wall_timer(10000ms, [this]() {CerebrumTask();});
// if (!task_timer_) {
// RCLCPP_ERROR(this->get_logger(), "Failed to create task timer");
// }
CreateServices();
@@ -187,6 +193,7 @@ void CerebrumNode::RegisterActionClient()
g.calls.push_back(BuildSkillCallForSkill(s, std::nullopt));
}
}
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) {
@@ -268,6 +275,7 @@ 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");
}
@@ -311,6 +319,16 @@ void CerebrumNode::CerebrumTask()
}
BuildBehaviorTreeFromFile(path_param.config);
current_bt_config_params_path_ = path_param;
//update working info
auto scheduled = path_param.config;
scheduled = scheduled.substr(scheduled.find_last_of('/')+1, scheduled.find_last_of('.') - scheduled.find_last_of('/')-1);
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);
RCLCPP_WARN(this->get_logger(), "CerebrumTask Switching to BT config file path: %s", path_param.config.c_str());
return;
}
@@ -340,7 +358,15 @@ void CerebrumNode::ExecuteBehaviorTree()
auto status = tree_.rootNode()->executeTick();
if (status != BT::NodeStatus::RUNNING) {
bt_pending_run_ = false;
RCLCPP_INFO(this->get_logger(), "BT finished status=%s", BT::toStr(status, true).c_str());
//update working info
auto scheduled = current_bt_config_params_path_.config;
scheduled = scheduled.substr(scheduled.find_last_of('/')+1, scheduled.find_last_of('.') - scheduled.find_last_of('/')-1);
if (robot_work_info_.task.size() > 2) {
robot_work_info_.task[0] = (scheduled);
robot_work_info_.task[2] = "None";
}
robot_work_info_.working_state = BT::toStr(status, false);
RCLCPP_INFO(this->get_logger(), "BT finished status=%s, scheduled=%s", BT::toStr(status, true).c_str(), scheduled.c_str());
}
}
@@ -608,6 +634,10 @@ void CerebrumNode::DeclareBtActionParamsForSkillInstance(
current_bt_config_params_path_.param.c_str(), instance_name.c_str());
} else {
instance_params = *params;
//update working info
robot_work_info_.skill = skill_name;
robot_work_info_.action_name = instance_name;
robot_work_info_.instance_params = instance_params;
RCLCPP_INFO(this->get_logger(), "Loaded BT params for %s, instance_params: %s",
instance_name.c_str(), instance_params.c_str());
}
@@ -1039,11 +1069,22 @@ void CerebrumNode::CreateServices()
}
BuildBehaviorTreeFromFile(path_param.config);
current_bt_config_params_path_ = path_param;
//update working info
auto scheduled = path_param.config;
scheduled = scheduled.substr(scheduled.find_last_of('/')+1, scheduled.find_last_of('.') - scheduled.find_last_of('/')-1);
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);
RCLCPP_WARN(this->get_logger(), "cerebrum/rebuild_now Service Switching to BT config file path: %s", path_param.config.c_str());
return;
break;
}
resp->success = true;
resp->message = "Rebuild triggered";
return;
});
}
@@ -1077,36 +1118,101 @@ bool CerebrumNode::LoadRobotConfiguration()
return true;
}
/**
* @brief Simulate battery capacity.
*
* @return int
*/
int CerebrumNode::SimulatedBatteryCapacity()
{
static int simulated_battery_capacity = 85;
static rclcpp::Time last_battery_update = rclcpp::Time(0, 0, this->get_clock()->get_clock_type());
static constexpr double kBatteryDecrementIntervalSec = 60.0; // 1 minute
// Initialize last_battery_update on first run
if (last_battery_update.nanoseconds() == 0) {
last_battery_update = this->now();
}
double elapsed = (this->now() - last_battery_update).seconds();
if (elapsed >= kBatteryDecrementIntervalSec) {
if (simulated_battery_capacity > 0) {
--simulated_battery_capacity;
RCLCPP_INFO(this->get_logger(), "Battery simulator: decreased to %d%%", simulated_battery_capacity);
}
last_battery_update = this->now();
}
return simulated_battery_capacity;
}
/**
* @brief Publish robot work information.
*
*/
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_pub_ = this->create_publisher<
interfaces::msg::RobotWorkInfo>("/robot_work_info", rclcpp::QoS(10).best_effort());
pub_robot_work_info_timer_ = this->create_wall_timer(1000ms, [this]() {
static int64_t robot_work_info_pub_msg_id = 0;
interfaces::msg::RobotWorkInfo msg;
msg.msg_id = robot_work_info_pub_msg_id++;
msg.working_state = "Working";
msg.battery_capacity = 95;
msg.working_time = 1.5f;
msg.nav_state = "Normal";
msg.comm_quality = "Good";
msg.task = {"StockIn", "StockOut", "None"};
msg.expt_completion_time = "2024-12-31T23:59:59Z";
msg.work_log = "No log";
msg.skill = "MoveWaist_H";
msg.action_name = "ExecuteBtAction";
msg.instance_params = "{}";
msg.bt_node_status = "RUNNING";
msg.smacc_state = "StExecuting";
if (robot_work_info_pub_) {
robot_work_info_pub_->publish(msg);
RCLCPP_WARN(this->get_logger(), "Publishing robot work info id: %ld", msg.msg_id);
}
if (!robot_work_info_pub_) {
RCLCPP_ERROR(this->get_logger(), "Failed to create robot work info publisher");
return;
}
pub_robot_work_info_timer_ = this->create_wall_timer(100ms, [this]() {
static int64_t robot_work_info_pub_msg_id = 0;
interfaces::msg::RobotWorkInfo msg { robot_work_info_ };
msg.msg_id = robot_work_info_pub_msg_id++;
msg.battery_capacity = SimulatedBatteryCapacity();
msg.working_time = 1.5f;
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";
if (robot_work_info_pub_) {
robot_work_info_pub_->publish(msg);
// RCLCPP_WARN(this->get_logger(), "Publishing robot work info id: %ld", msg.msg_id);
}
});
if (!pub_robot_work_info_timer_) {
RCLCPP_ERROR(this->get_logger(), "Failed to create robot work info timer");
}
}
/**
* @brief Get the future time as a string.
*
* @param seconds Number of seconds to add to the current time.
* @return std::string Formatted future time string.
*/
std::string CerebrumNode::GetFutureTime(int seconds)
{
auto finish = this->now() + rclcpp::Duration::from_seconds(seconds);
auto tp = std::chrono::system_clock::time_point(std::chrono::nanoseconds(finish.nanoseconds()));
std::time_t tt = std::chrono::system_clock::to_time_t(tp);
std::tm tm{};
#if defined(_MSC_VER)
localtime_s(&tm, &tt);
#else
localtime_r(&tt, &tm);
#endif
char buf[20] = {0}; // "YYYY-MM-DD HH:MM:SS"
if (std::strftime(buf, sizeof(buf), "%Y-%m-%d %H:%M:%S", &tm)) {
return std::string(buf);
} else {
return std::string("");
}
}
} // namespace brain