add robot work info pub

src/brain/include/brain/cerebrum_node.hpp

@@ -21,7 +21,7 @@
 #include "brain_interfaces/msg/skill_call.hpp"
 #include <rclcpp/node_interfaces/node_parameters_interface.hpp>
 #include "brain/robot_config.hpp"
-
+#include "interfaces/msg/robot_work_info.hpp"
 
 namespace brain
 {
@@ -206,6 +206,7 @@ private:
   std::filesystem::file_time_type bt_xml_last_write_time_{}; // last observed mod time (for hot reload)
   rclcpp::TimerBase::SharedPtr task_timer_;
   rclcpp::TimerBase::SharedPtr bt_timer_;
+  rclcpp::TimerBase::SharedPtr pub_robot_work_info_timer_;
   // Service to trigger immediate rebuild of sequence + BT
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr rebuild_service_;
   std::atomic<bool> bt_updating_{false};
@@ -216,7 +217,8 @@ private:
   std::vector<std::string> current_sequence_skills_;
   // Mutex that protects both per_node_exec_ and epoch_skills_ updates.
   std::mutex exec_mutex_;
-  // Removed legacy global pending_bt_action_name_: each node now stores its own skill_name.
+
+  rclcpp::Publisher<interfaces::msg::RobotWorkInfo>::SharedPtr robot_work_info_pub_;
 
   struct PerNodeExecInfo
   {
@@ -336,6 +338,8 @@ private:
    */
   bool LoadRobotConfiguration();
 
+  void RobotWorkInfoPublish();
+
   // 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_;

src/brain/src/cerebrum_node.cpp

@@ -112,6 +112,8 @@ CerebrumNode::CerebrumNode(const rclcpp::NodeOptions & options)
 
   CreateServices();
 
+  RobotWorkInfoPublish();
+
   RCLCPP_INFO(this->get_logger(), "CerebrumNode initialized (timeout=%.2f random_pick=%zu)", 
     node_timeout_sec_, random_skill_pick_count_);
 }
@@ -1075,4 +1077,36 @@ bool CerebrumNode::LoadRobotConfiguration()
   return true;
 }
 
+/**
+ * @brief Publish robot work information.
+ * 
+ */
+void CerebrumNode::RobotWorkInfoPublish()
+{
+  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);
+      }
+  });
+}
+
 } // namespace brain