Feature/improvements

botbrain_ws/src/bot_localization/bot_localization/launch/rtabmap.launch.py

@@ -20,12 +20,8 @@ def generate_launch_description():
     robot_package_name = f"{robot_model}_pkg"
     default_map = _raw_robot.get('default_map') or 'rtabmap.db'
 
-    # Define the database path centrally
-    database_path = os.path.join(
-        get_package_share_directory(robot_package_name),
-        'maps',
-        default_map
-    )
+    # Define the database path
+    database_path = os.path.join(workspace_dir, 'src', robot_package_name, 'maps', default_map)
 
     camera_cfg_file = os.path.join(
         get_package_share_directory(robot_package_name),

botbrain_ws/src/bot_state_machine/include/lifecycle_manager.hpp

@@ -74,6 +74,12 @@ class LifecycleManager : public rclcpp_lifecycle::LifecycleNode
     std::unordered_map<std::string,
         rclcpp::Subscription<lifecycle_msgs::msg::TransitionEvent>::SharedPtr> transition_subs_;
 
+    // Cached lifecycle state label per node, updated from transition events.
+    std::unordered_map<std::string, std::string> state_cache_;
+
+    // Protects state_cache_ from concurrent reads/writes.
+    std::mutex cache_mutex_;
+
 public:
     // Constructor.
     LifecycleManager();

botbrain_ws/src/bot_state_machine/src/lifecycle_manager.cpp

@@ -92,6 +92,10 @@ LifecycleManager::on_cleanup(const rclcpp_lifecycle::State & previous_state)
     transition_subs_.clear();
     change_state_srvs_.clear();
     get_state_srvs_.clear();
+    {
+        std::lock_guard<std::mutex> lock(cache_mutex_);
+        state_cache_.clear();
+    }
     nodes_.clear();
     status_pub_.reset();
 
@@ -195,6 +199,10 @@ void LifecycleManager::command_srv_callback(
 void LifecycleManager::transition_callback(const std::string& node_name,
     const lifecycle_msgs::msg::TransitionEvent::SharedPtr msg)
 {
+    {
+        std::lock_guard<std::mutex> lock(cache_mutex_);
+        state_cache_[node_name] = msg->goal_state.label;
+    }
     const uint8_t goal = msg->goal_state.id;
     if (controller_)
         controller_->add_event(PendingEvent{node_name, goal});
@@ -259,6 +267,12 @@ void LifecycleManager::create_comms()
         get_state_srvs_.emplace(n.name, get_cli);
     }
 
+    {
+        std::lock_guard<std::mutex> lock(cache_mutex_);
+        for (const auto &n : nodes_)
+            state_cache_[n.name] = "unknown";
+    }
+
     print_info("Watching " + std::to_string(transition_subs_.size()) + " transition_event topics.");
 }
 
@@ -274,21 +288,15 @@ void LifecycleManager::publish_callback()
     bot_custom_interfaces::msg::StatusArray msg;
     msg.header.stamp = this->now();
 
+    std::lock_guard<std::mutex> lock(cache_mutex_);
     for (const auto &n : nodes_)
     {
         bot_custom_interfaces::msg::Status item;
         item.name = n.name;
         item.display_name = n.display_name;
 
-        auto st = get_state(n.name);  // poll current lifecycle state
-        if(!st)
-        {
-            item.status = "ERROR";
-        }
-        else
-        {
-            item.status = st->label;
-        }
+        auto it = state_cache_.find(n.name);
+        item.status = (it != state_cache_.end()) ? it->second : "unknown";
 
         msg.containers.push_back(item);
     }