From e59ec5152832304461fb82f712db077bfc408501 Mon Sep 17 00:00:00 2001
From: Kalana Ratnayake <kalanaratnayake95@gmail.com>
Date: Fri, 22 May 2026 07:29:25 +0000
Subject: [PATCH 1/2] integrating libaria

---
 .gitmodules                                   |   3 +
 README.md                                     |   2 +-
 amr_core/CMakeLists.txt                       |  15 +-
 amr_core/include/amr_core/client_node.hpp     | 150 -----
 amr_core/include/amr_core/core_node.hpp       | 433 ++-----------
 .../amr_core/interfaces/arcl_interface.hpp    | 260 --------
 .../amr_core/interfaces/drive_interface.hpp   | 591 +++++++++--------
 .../amr_core/interfaces/laser_interface.hpp   | 525 ++++++++++++++--
 .../interfaces/map_file_interface.hpp         | 593 ------------------
 .../amr_core/interfaces/status_interface.hpp  | 219 +++++++
 .../include/amr_core/socket/socket_common.hpp | 111 ----
 .../include/amr_core/socket/socket_driver.hpp | 541 ----------------
 .../amr_core/socket/socket_listener.hpp       | 383 -----------
 .../amr_core/socket/socket_taskmaster.hpp     | 410 ------------
 .../amr_core/utils/libaria_runtime.hpp        |  71 +++
 amr_core/include/amr_core/utils/parser.hpp    |  96 ---
 amr_core/package.xml                          |   1 +
 amr_core/src/client_node.cpp                  |  11 -
 amr_ros/config/parameters.yaml                |  80 +--
 libaria                                       |   1 +
 20 files changed, 1198 insertions(+), 3298 deletions(-)
 create mode 100644 .gitmodules
 delete mode 100644 amr_core/include/amr_core/client_node.hpp
 delete mode 100644 amr_core/include/amr_core/interfaces/arcl_interface.hpp
 delete mode 100644 amr_core/include/amr_core/interfaces/map_file_interface.hpp
 create mode 100644 amr_core/include/amr_core/interfaces/status_interface.hpp
 delete mode 100644 amr_core/include/amr_core/socket/socket_common.hpp
 delete mode 100644 amr_core/include/amr_core/socket/socket_driver.hpp
 delete mode 100644 amr_core/include/amr_core/socket/socket_listener.hpp
 delete mode 100644 amr_core/include/amr_core/socket/socket_taskmaster.hpp
 create mode 100644 amr_core/include/amr_core/utils/libaria_runtime.hpp
 delete mode 100644 amr_core/include/amr_core/utils/parser.hpp
 delete mode 100644 amr_core/src/client_node.cpp
 create mode 160000 libaria

diff --git a/.gitmodules b/.gitmodules
new file mode 100644
index 00000000..3b2227c5
--- /dev/null
+++ b/.gitmodules
@@ -0,0 +1,3 @@
+[submodule "libaria"]
+	path = libaria
+	url = https://github.com/CollaborativeRoboticsLab/libaria.git
diff --git a/README.md b/README.md
index cd4bbbff..ca411d2a 100644
--- a/README.md
+++ b/README.md
@@ -29,7 +29,7 @@ sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup ros-humble-slam-
 Clone the repositories into the `src` folder by
 
 ```sh
-git clone https://github.com/CollaborativeRoboticsLab/omron_amr.git
+git clone --recursive https://github.com/CollaborativeRoboticsLab/omron_amr.git
 ```
 
 Build by
diff --git a/amr_core/CMakeLists.txt b/amr_core/CMakeLists.txt
index 83c2ab99..407e41c5 100644
--- a/amr_core/CMakeLists.txt
+++ b/amr_core/CMakeLists.txt
@@ -22,12 +22,16 @@ find_package(control_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(ament_index_cpp REQUIRED)
 find_package(tf2_ros REQUIRED)
+find_package(libaria CONFIG REQUIRED)
+
+set(AMR_CORE_ARIA_DIR "${CMAKE_CURRENT_SOURCE_DIR}/../../libaria")
 
 include_directories(
   include
 )
 
 add_executable(${PROJECT_NAME} src/core_node.cpp)
+target_compile_definitions(${PROJECT_NAME} PRIVATE AMR_CORE_ARIA_DIR="${AMR_CORE_ARIA_DIR}")
 ament_target_dependencies(${PROJECT_NAME} 
   rclcpp
   rclcpp_action
@@ -42,19 +46,12 @@ ament_target_dependencies(${PROJECT_NAME}
   ament_index_cpp
 
 )
-
-add_executable(client src/client_node.cpp)
-ament_target_dependencies(client
-  rclcpp
-  rclcpp_action
-  std_msgs
-  amr_msgs
-  geometry_msgs
+target_link_libraries(${PROJECT_NAME}
+  libaria::ArNetworking
 )
 
 install(TARGETS
   ${PROJECT_NAME}
-  client
   DESTINATION lib/${PROJECT_NAME}
 )
 
diff --git a/amr_core/include/amr_core/client_node.hpp b/amr_core/include/amr_core/client_node.hpp
deleted file mode 100644
index 54021d3e..00000000
--- a/amr_core/include/amr_core/client_node.hpp
+++ /dev/null
@@ -1,150 +0,0 @@
-#pragma once
-
-#include <cmath>
-#include <string>
-#include <vector>
-#include <memory>
-#include <chrono>
-
-#include <rclcpp/rclcpp.hpp>
-#include <rclcpp_action/rclcpp_action.hpp>
-
-#include <geometry_msgs/msg/pose_stamped.hpp>
-#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
-#include <amr_msgs/action/action.hpp>
-
-class ClientNode : public rclcpp::Node {
-public:
-  using ActionT = amr_msgs::action::Action;
-  using ClientT = rclcpp_action::Client<ActionT>;
-  using GoalHandle = rclcpp_action::ClientGoalHandle<ActionT>;
-
-  /**
-   * @brief constructor
-   */
-  ClientNode()
-  : rclcpp::Node("amr_client_node")
-  {}
-
-  /**
-   * 
-   */
-  void initialize()
-  {
-    // Parameters
-    this->declare_parameter<std::string>("command_type", "none");  // none|goto_goal|execute_macro|dock|undock
-    this->declare_parameter<std::string>("goal_name", "Goal1");
-    this->declare_parameter<std::string>("macro_name", "Macro1");
-    this->declare_parameter<bool>("exit_on_result", false);
-
-    // Action client
-    ac_ = rclcpp_action::create_client<ActionT>(shared_from_this(), "action_server");
-
-    // Defer optional one-shot command to allow executor to start
-    using namespace std::chrono_literals;
-    start_timer_ = this->create_wall_timer(0ms, [this]() {
-      start_timer_->cancel();
-      auto cmd = get_param<std::string>("command_type");
-      if (cmd == "goto_goal") {
-        goto_goal(get_param<std::string>("goal_name"));
-      } else if (cmd == "execute_macro") {
-        execute_macro(get_param<std::string>("macro_name"));
-      } else if (cmd == "dock") {
-        dock();
-      } else if (cmd == "undock") {
-        undock();
-      }
-    });
-  }
-
-private:
-  // ========== High-level commands ==========
-  void goto_goal(const std::string& name) {
-    ActionT::Goal goal;
-    goal.command = "goto " + name;
-    goal.identifier = { "Arrived at " + name };
-    send_goal(goal);
-  }
-
-  void execute_macro(const std::string& name) {
-    ActionT::Goal goal;
-    goal.command = "executeMacro " + name;
-    goal.identifier = { "Completed macro " + name };
-    send_goal(goal);
-  }
-
-  void dock() {
-    ActionT::Goal goal;
-    goal.command = "dock";
-    goal.identifier = { "Arrived at dock" };
-    send_goal(goal);
-  }
-
-  void undock() {
-    ActionT::Goal goal;
-    goal.command = "undock";
-    goal.identifier = { "Stopped" };
-    send_goal(goal);
-  }
-
-
-  // ========== Action helpers ==========
-  void send_goal(const ActionT::Goal& goal) {
-    // Wait briefly for server
-    using namespace std::chrono_literals;
-    if (!ac_->wait_for_action_server(2s)) {
-      RCLCPP_ERROR(this->get_logger(), "Action server not available");
-      return;
-    }
-
-    auto options = typename ClientT::SendGoalOptions();
-
-    options.feedback_callback =
-        [this](GoalHandle::SharedPtr, const std::shared_ptr<const ActionT::Feedback> feedback) {
-          RCLCPP_INFO(this->get_logger(), "%s", feedback->feed_msg.c_str());
-        };
-        
-    options.result_callback =
-        [this](const GoalHandle::WrappedResult& result) {
-          switch (result.code) {
-            case rclcpp_action::ResultCode::SUCCEEDED:
-              RCLCPP_INFO(this->get_logger(), "%s", result.result->res_msg.c_str());
-              break;
-            case rclcpp_action::ResultCode::ABORTED:
-              RCLCPP_WARN(this->get_logger(), "Aborted: %s", result.result ? result.result->res_msg.c_str() : "");
-              break;
-            case rclcpp_action::ResultCode::CANCELED:
-              RCLCPP_WARN(this->get_logger(), "Canceled");
-              break;
-            default:
-              RCLCPP_ERROR(this->get_logger(), "Unknown result code");
-              break;
-          }
-          if (get_param<bool>("exit_on_result")) {
-            rclcpp::shutdown();
-          }
-        };
-
-    options.goal_response_callback =
-        [this](const GoalHandle::SharedPtr& goal_handle) {
-          if (!goal_handle) {
-            RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
-          } else {
-            RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
-          }
-        };
-
-    ac_->async_send_goal(goal, options);
-  }
-
-  // ========== Math ==========
-  template<typename T>
-  T get_param(const std::string& name) const {
-    return this->get_parameter(name).get_parameter_value().template get<T>();
-  }
-
-private:
-  rclcpp_action::Client<ActionT>::SharedPtr ac_;
-
-  rclcpp::TimerBase::SharedPtr start_timer_;
-};
\ No newline at end of file
diff --git a/amr_core/include/amr_core/core_node.hpp b/amr_core/include/amr_core/core_node.hpp
index df55da71..86a5fd27 100644
--- a/amr_core/include/amr_core/core_node.hpp
+++ b/amr_core/include/amr_core/core_node.hpp
@@ -1,28 +1,12 @@
 #pragma once
 
 #include <rclcpp/rclcpp.hpp>
-#include <rclcpp_action/rclcpp_action.hpp>
-#include <chrono>
 #include <memory>
 #include <string>
-#include <vector>
-#include <sstream>
-#include <thread>
 
-#include <std_msgs/msg/string.hpp>
-#include <amr_msgs/msg/status.hpp>
-#include <amr_msgs/msg/location.hpp>
-
-#include "amr_core/socket/socket_listener.hpp"
-#include "amr_core/socket/socket_driver.hpp"
-#include "amr_core/socket/socket_taskmaster.hpp"
-#include "amr_core/utils/amr_exception.hpp"
-#include "amr_core/interfaces/map_file_interface.hpp"
+#include "amr_core/interfaces/status_interface.hpp"
 #include "amr_core/interfaces/laser_interface.hpp"
 #include "amr_core/interfaces/drive_interface.hpp"
-#include "amr_core/interfaces/arcl_interface.hpp"
-
-using namespace std::chrono_literals;
 
 class CoreNode : public rclcpp::Node
 {
@@ -32,389 +16,110 @@ class CoreNode : public rclcpp::Node
   }
 
   /**
-   * @brief This function initializes the core node by reading parameters, setting up publishers, services, and action
-   * servers, and establishing connections to the robot.
+   * @brief Initializes and wires the libaria-backed interfaces used by the core node.
    */
   void initialize()
   {
-    // Shared connection params (service + action)
-    ip_ = this->declare_parameter<std::string>("robot.ip", "127.0.0.1");
-    port_ = this->declare_parameter<int>("robot.port", 0);
-    passwd_ = this->declare_parameter<std::string>("robot.password", "");
-
-    // listener endpoint (defaults to same ip/port if not set)
-    host_ip_ = this->declare_parameter<std::string>("host.ip", ip_);
-    host_port_ = this->declare_parameter<int>("host.port", port_);
-
-    // Publish source data from listener
-    publish_source_ = this->declare_parameter<bool>("source_data.publish", false);
-    publish_frequency_ = this->declare_parameter<int>("source_data.frequency", 5);
-    pub_interval_ms_ = int(1000 / publish_frequency_);
-    reset_odometer_on_startup_ = this->declare_parameter<bool>("driver.reset_odometer_on_startup", true);
-
-    // SocketDriver (low-level)
-    RCLCPP_INFO(this->get_logger(), "Initializing SocketDriver..");
-    try
-    {
-      driver_ = std::make_shared<SocketDriver>(this->shared_from_this());
-      driver_->connect(ip_, static_cast<uint16_t>(port_));
-      driver_->login(passwd_);
-
-      RCLCPP_INFO(this->get_logger(), "SocketDriver connected to %s:%d", ip_.c_str(), port_);
-    }
-    catch (const amr_exception& e)
-    {
-      RCLCPP_ERROR(this->get_logger(), "SocketDriver initialization failed: %s", e.what());
-    }
-
-    // SocketTaskmaster (high-level)
-    RCLCPP_INFO(this->get_logger(), "Initializing SocketTaskmaster..");
-    try
-    {
-      task_master_ = std::make_shared<SocketTaskmaster>(this->shared_from_this());
-      task_master_->connect(ip_, port_);
-      task_master_->login(passwd_);
-
-      RCLCPP_INFO(this->get_logger(), "ScoketTaskmaster is up @ %s:%d", ip_.c_str(), port_);
-    }
-    catch (const amr_exception& ex)
-    {
-      RCLCPP_FATAL(this->get_logger(), "SocketTaskmaster initialization failed: %s", ex.what());
-    }
-
-    // SocketListener
-    RCLCPP_INFO(this->get_logger(), "Initializing SocketListener..");
-    try
-    {
-      listener_ = std::make_shared<SocketListener>(this->shared_from_this(), host_ip_, host_port_);
-      if (!listener_->begin())
-        RCLCPP_ERROR(this->get_logger(), "SocketListener begin() failed");
-      else
-        RCLCPP_INFO(this->get_logger(), "SocketListener started on %s:%d", host_ip_.c_str(), host_port_);
-    }
-    catch (const amr_exception& ex)
-    {
-      RCLCPP_ERROR(this->get_logger(), "SocketListener error: %s", ex.what());
-    }
-
-    // Publish source data from listener
-    if (publish_source_)
-    {
-      RCLCPP_INFO(this->get_logger(), "Publishing source data from listener enabled");
-
-      status_pub_ = this->create_publisher<amr_msgs::msg::Status>("amr/source/status", 10);
-      laser_pub_ = this->create_publisher<std_msgs::msg::String>("amr/source/laser", 10);
-      odom_pub_ = this->create_publisher<std_msgs::msg::String>("amr/source/odom", 10);
-      app_fault_query_pub_ = this->create_publisher<std_msgs::msg::String>("amr/source/application_fault_query", 10);
-      faults_get_pub_ = this->create_publisher<std_msgs::msg::String>("amr/source/faults_get", 10);
-      query_faults_pub_ = this->create_publisher<std_msgs::msg::String>("amr/source/query_faults", 10);
-    }
-    else
-    {
-      RCLCPP_INFO(this->get_logger(), "Publishing source data from listener is disabled");
-    }
-
-    // Publisher timer
-    main_timer =
-        this->create_wall_timer(std::chrono::milliseconds(pub_interval_ms_), std::bind(&CoreNode::main_loop, this));
-
-    // Publish map data from file
-    publish_map_ = this->declare_parameter<bool>("map_data.publish", false);
-    if (publish_map_)
-    {
-      data_point_marker_ = std::make_shared<MapFileInterface>(this->shared_from_this());
-      data_point_marker_->initialize();
-      RCLCPP_INFO(this->get_logger(), "Publishing map data from file enabled");
-    }
-
-    // Publish laser scans
-    publish_laser_scans_ = this->declare_parameter<bool>("laser_scans.publish", true);
-    if (publish_laser_scans_)
-    {
-      laser_scans_ = std::make_shared<LaserInterface>(this->shared_from_this());
-      laser_scans_->initialize();
-      RCLCPP_INFO(this->get_logger(), "Publishing laser scans enabled");
-    }
-
-    // DriverInterface
-    odom_driver_ = std::make_shared<DriverInterface>(this->shared_from_this(), driver_);
-    odom_driver_->initialize();
-
-    // ARCL service + action
-    enable_arcl_access_ = this->declare_parameter<bool>("arcl.enable", false);
-    timeout_ms_ = this->declare_parameter<int>("arcl.timeout_ms", 15000);
-    if (enable_arcl_access_)
-    {
-      RCLCPP_INFO(this->get_logger(), "ARCL interface is enabled");
-      arcl_ros = std::make_shared<ARCL_Interface>(this->shared_from_this(), driver_, task_master_, timeout_ms_);
-      arcl_ros->initialize();
-    }
-
-    if (reset_odometer_on_startup_)
-    {
-      RCLCPP_INFO(this->get_logger(), "Resetting odometer..");
-      std::string response;
-      int req_id = driver_->queue_command("odometerReset", "");
-      bool got_response = driver_->wait_for_response(req_id, response, timeout_ms_);
-      if (!got_response)
-        RCLCPP_INFO(this->get_logger(), "Reset result: %s", response.c_str());
-    }
-    else
-    {
-      RCLCPP_INFO(this->get_logger(), "Skipping odometer reset on startup");
-    }
-
-    RCLCPP_INFO(this->get_logger(), "CoreNode initialized");
-  }
-
-private:
-  // -------- Publisher (SocketListener -> topics) --------
-  /**
-   * @brief This function is called periodically by the publisher timer to poll the listener for new data and publish it
-   * to topics.
-   */
-  void main_loop()
-  {
-    // Poll listener for new data
-    try
-    {
-      while (listener_ && listener_->poll_once(0))
+    host_ = getOrDeclareParameter<std::string>("robot.ip", "127.0.0.1");
+    port_ = getOrDeclareParameter<int>("robot.port", 7272);
+    user_ = getOrDeclareParameter<std::string>("robot.user", "admin");
+    password_ = getOrDeclareParameter<std::string>("robot.password", "");
+    protocol_ = getOrDeclareParameter<std::string>("robot.protocol", "6MTX");
+    publish_status_ = getOrDeclareParameter<bool>("status.publish", true);
+    publish_main_laser_ = getOrDeclareParameter<bool>("laser.main_laser.enabled", true);
+    publish_low_laser_ = getOrDeclareParameter<bool>("laser.low_laser.enabled", false);
+
+    if (publish_status_)
+    {
+      try
       {
+        status_interface_ = std::make_shared<StatusInterface>(this->shared_from_this());
+        status_interface_->initialize(host_, port_, user_, password_, protocol_);
+        RCLCPP_INFO(this->get_logger(), "Publishing status information enabled");
       }
-    }
-    catch (const amr_exception& ex)
-    {
-      RCLCPP_ERROR(this->get_logger(), "Listener error: %s", ex.what());
-    }
-
-    pub_status();
-    pub_laser();
-    pub_odometer();
-    pub_app_fault_query();
-    pub_faults_get();
-    pub_query_faults();
-  }
-
-  /**
-   * @brief This function publishes the robot's status information to the status topic. As an example.
-   * This include parsing the following fields from the listener:
-   *
-   * ExtendedStatusForHumans: Robot lost
-   * Status: Teleop driving
-   * StateOfCharge: 79.4
-   * Location: 79031 -81597 20
-   * LocalizationScore: 0.170543
-   * Temperature: 36
-   */
-  void pub_status()
-  {
-    amr_msgs::msg::Status status_msg;
-    amr_msgs::msg::Location loc_msg;
-
-    try
-    {
-      const auto& status_status = listener_->get_response("Status");
-      const auto& status_batt = listener_->get_response("StateOfCharge");
-      const auto& status_loc = listener_->get_response("Location");
-      const auto& status_loc_sc = listener_->get_response("LocalizationScore");
-      const auto& status_temp = listener_->get_response("Temperature");
-      const auto& status_ext = listener_->get_response("ExtendedStatusForHumans");
-
-      status_msg.status = status_status.front();
-      status_msg.extended_status = status_ext.front();
-      status_msg.state_of_charge = std::stof(status_batt.front());
-      status_msg.localization_score = std::stof(status_loc_sc.front());
-      status_msg.temperature = std::stof(status_temp.front());
-
-      std::istringstream iss(status_loc.front());
-      double x = 0.0, y = 0.0, th = 0.0;
-      if (iss >> x >> y >> th)
+      catch (const std::exception& ex)
       {
-        laser_x_ = x;
-        laser_y_ = y;
-        laser_th_ = th;
-
-        loc_msg.x = x;
-        loc_msg.y = y;
-        loc_msg.theta = th;
-        status_msg.location = loc_msg;
+        status_interface_.reset();
+        RCLCPP_ERROR(this->get_logger(), "StatusInterface initialization failed: %s", ex.what());
       }
-      else
-      {
-        RCLCPP_WARN(this->get_logger(), "Value error with location coordinates. Using zeros.");
-      }
-    }
-    catch (const std::out_of_range&)
-    {
-      RCLCPP_DEBUG(this->get_logger(), "Status data not available yet");
-    }
-    catch (const std::exception& ex)
-    {
-      RCLCPP_ERROR(this->get_logger(), "Status parse error: %s", ex.what());
     }
-
-    if (publish_source_ && status_pub_)
-      status_pub_->publish(status_msg);
-  }
-
-  void pub_laser()
-  {
-    try
-    {
-      const auto& scans = listener_->get_response("RangeDeviceGetCurrent");
-      std_msgs::msg::String msg;
-      msg.data = scans.front();
-
-      if (publish_source_ && laser_pub_)
-        laser_pub_->publish(msg);
-
-      if (publish_laser_scans_)
-        laser_scans_->update(msg, laser_x_, laser_y_, laser_th_);
-    }
-    catch (const std::out_of_range&)
+    else
     {
+      RCLCPP_INFO(this->get_logger(), "Publishing status information disabled");
     }
-  }
 
-  void pub_odometer()
-  {
-    try
+    if (publish_main_laser_ || publish_low_laser_)
     {
-      const auto& odom = listener_->get_response("Odometer");
-      std::string joined;
-      for (size_t i = 0; i < odom.size(); ++i)
+      try
       {
-        if (i)
-          joined += ' ';
-        joined += odom[i];
+        laser_scans_ = std::make_shared<LaserInterface>(this->shared_from_this());
+        laser_scans_->initialize(host_, port_, user_, password_, protocol_);
+        RCLCPP_INFO(this->get_logger(), "Publishing laser scans enabled");
       }
-      std_msgs::msg::String msg;
-      msg.data = joined;
-      if (publish_source_ && odom_pub_)
-        odom_pub_->publish(msg);
-
-      if (odom_driver_)
-        odom_driver_->update(msg);
-    }
-    catch (const std::out_of_range&)
-    {
-    }
-  }
-
-  void pub_app_fault_query()
-  {
-    try
-    {
-      const auto& query = listener_->get_response("applicationFaultQuery");
-      std::string joined;
-      for (size_t i = 0; i < query.size(); ++i)
+      catch (const std::exception& ex)
       {
-        if (i)
-          joined += ' ';
-        joined += query[i];
+        laser_scans_.reset();
+        RCLCPP_ERROR(this->get_logger(), "LaserInterface initialization failed: %s", ex.what());
       }
-      std_msgs::msg::String msg;
-      msg.data = joined;
-      if (publish_source_ && app_fault_query_pub_)
-        app_fault_query_pub_->publish(msg);
     }
-    catch (const std::out_of_range&)
+    else
     {
+      RCLCPP_INFO(this->get_logger(), "Publishing laser scans disabled");
     }
-  }
 
-  void pub_faults_get()
-  {
+    // DriverInterface
     try
     {
-      const auto& faults = listener_->get_response("FaultList");
-      std::string joined;
-      for (size_t i = 0; i < faults.size(); ++i)
-      {
-        if (i)
-          joined += ' ';
-        joined += faults[i];
-      }
-      std_msgs::msg::String msg;
-      msg.data = joined;
-      if (publish_source_ && faults_get_pub_)
-        faults_get_pub_->publish(msg);
+      driver_ = std::make_shared<DriverInterface>(this->shared_from_this());
+      driver_->initialize(host_, port_, user_, password_, protocol_);
     }
-    catch (const std::out_of_range&)
+    catch (const std::exception& ex)
     {
+      driver_.reset();
+      RCLCPP_ERROR(this->get_logger(), "DriverInterface initialization failed: %s", ex.what());
     }
+
+    RCLCPP_INFO(this->get_logger(), "CoreNode initialized");
   }
 
-  void pub_query_faults()
+private:
+  /**
+   * @brief Helper function to get a parameter value or declare it with a default if it doesn't exist.
+   * @tparam T Parameter type.
+   * @param name Parameter name.
+   * @param default_value Default value to declare if parameter doesn't exist.
+   * @return Parameter value.
+   */
+  template <typename T>
+  T getOrDeclareParameter(const std::string& name, const T& default_value)
   {
-    try
-    {
-      const auto& faults = listener_->get_response("RobotFaultQuery");
-      std::string joined;
-      for (size_t i = 0; i < faults.size(); ++i)
-      {
-        if (i)
-          joined += ' ';
-        joined += faults[i];
-      }
-      std_msgs::msg::String msg;
-      msg.data = joined;
-      if (publish_source_ && query_faults_pub_)
-        query_faults_pub_->publish(msg);
-    }
-    catch (const std::out_of_range&)
+    if (!this->has_parameter(name))
     {
+      return this->declare_parameter<T>(name, default_value);
     }
+
+    T value = default_value;
+    this->get_parameter(name, value);
+    return value;
   }
 
 private:
-  // Params
-  std::string ip_;
-  int port_{ 0 };
-  std::string passwd_;
-  int publish_frequency_{ 5 };
-  int pub_interval_ms_{ 200 };
-
-  std::string host_ip_;
-  int host_port_{ 0 };
-
-  double laser_x_{ 0.0 }, laser_y_{ 0.0 }, laser_th_{ 0.0 };
-
-  bool publish_source_{ false };
-  bool reset_odometer_on_startup_{ true };
-
-  bool enable_arcl_access_{ false };
-
-  // Service (driver)
-  std::shared_ptr<SocketDriver> driver_;
-
-  // Action (taskmaster)
-  std::shared_ptr<SocketTaskmaster> task_master_;
-
-  // Publisher (listener)
-  std::shared_ptr<SocketListener> listener_;
-  rclcpp::TimerBase::SharedPtr main_timer;
-
-  // Source Pubs
-  rclcpp::Publisher<amr_msgs::msg::Status>::SharedPtr status_pub_;
-  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr laser_pub_;
-  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr odom_pub_;
-  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr app_fault_query_pub_;
-  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr faults_get_pub_;
-  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr query_faults_pub_;
-
-  // Map pubs
-  bool publish_map_{ false };
-  std::shared_ptr<MapFileInterface> data_point_marker_;  ///< Data point marker instance
+  // Status information
+  std::shared_ptr<StatusInterface> status_interface_;
 
   // Laser scans
-  bool publish_laser_scans_{ false };
   std::shared_ptr<LaserInterface> laser_scans_;
 
   // DriverInterface
-  std::shared_ptr<DriverInterface> odom_driver_;
-
-  // ARCL ROS interface
-  std::shared_ptr<ARCL_Interface> arcl_ros;
-  int timeout_ms_{ 15000 };
+  std::shared_ptr<DriverInterface> driver_;
+
+  // Parameters
+  std::string host_;
+  int port_{ 7272 };
+  std::string user_;
+  std::string password_;
+  std::string protocol_;
+  bool publish_status_{ true };
+  bool publish_main_laser_{ true };
+  bool publish_low_laser_{ false };
 };
\ No newline at end of file
diff --git a/amr_core/include/amr_core/interfaces/arcl_interface.hpp b/amr_core/include/amr_core/interfaces/arcl_interface.hpp
deleted file mode 100644
index b57cb32e..00000000
--- a/amr_core/include/amr_core/interfaces/arcl_interface.hpp
+++ /dev/null
@@ -1,260 +0,0 @@
-#include <string>
-#include <memory>
-#include <vector>
-
-#include "rclcpp/rclcpp.hpp"
-
-#include <amr_msgs/srv/arcl_api.hpp>
-#include <amr_msgs/action/action.hpp>
-
-#include "amr_core/socket/socket_driver.hpp"
-#include "amr_core/socket/socket_taskmaster.hpp"
-#include "amr_core/utils/parser.hpp"
-#include "amr_core/utils/amr_exception.hpp"
-
-/**
- * @brief ROS 2 ARCL driver node (without direct socket code).
- * Uses SocketDriver for ARCL communication.
- */
-class ARCL_Interface
-{
-public:
-  using ServiceARCL = amr_msgs::srv::ArclApi;
-  using ActionARCL = amr_msgs::action::Action;
-  using GoalHandle = rclcpp_action::ServerGoalHandle<ActionARCL>;
-
-  /**
-   * @brief Constructor. Initializes publishers, subscribers, and parameters.
-   * @param node Shared pointer to rclcpp::Node.
-   */
-  ARCL_Interface(rclcpp::Node::SharedPtr node, std::shared_ptr<SocketDriver> driver,
-           std::shared_ptr<SocketTaskmaster> taskmaster, int service_timeout_ms = 15000)
-  {
-    node_ = node;
-    socket_driver_ = driver;
-    socket_taskmaster_ = taskmaster;
-    service_timeout_ms_ = service_timeout_ms;
-  }
-
-  void initialize()
-  {
-    // Service server
-    if (!socket_driver_)
-      service_ = node_->create_service<ServiceARCL>(
-          "arcl_api_service", std::bind(&ARCL_Interface::handle_service, this, std::placeholders::_1, std::placeholders::_2));
-    else
-      RCLCPP_ERROR(node_->get_logger(), "Service server not created: driver unavailable");
-
-    // Action server
-    if (socket_taskmaster_)
-      action_server_ = rclcpp_action::create_server<ActionARCL>(
-          node_, "arcl_api_action",
-          std::bind(&ARCL_Interface::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
-          std::bind(&ARCL_Interface::handle_cancel, this, std::placeholders::_1),
-          std::bind(&ARCL_Interface::handle_accepted, this, std::placeholders::_1));
-    else
-      RCLCPP_ERROR(node_->get_logger(), "Action server not created: taskmaster unavailable");
-  }
-
-private:
-  /**
-   * @brief This function handles incoming service requests to send ARCL commands to the robot.
-   */
-  void handle_service(const std::shared_ptr<ServiceARCL::Request> req, std::shared_ptr<ServiceARCL::Response> resp)
-  {
-    std::string response;
-    bool got_response;
-
-    // Check driver availability
-    if (!socket_driver_)
-    {
-      response = "ERROR: driver not available";
-      got_response = true;
-    }
-    // Send command to robot
-    else
-    {
-      int req_id = socket_driver_->queue_command(req->command, req->line_identifier);
-      got_response = socket_driver_->wait_for_response(req_id, response, service_timeout_ms_);
-    }
-
-    // Delegate queueing and response handling to SocketDriver
-    if (got_response)
-      resp->response = response;
-    else
-      resp->response = "TIMEOUT";
-  }
-
-  // -------- Action server (SocketTaskmaster) --------
-  /**
-   * @brief This function is called when a new goal is received by the action server.
-   *
-   * @param uuid The unique identifier for the goal.
-   * @param goal The goal message containing the command and identifiers.
-   * @return rclcpp_action::GoalResponse Indicates whether the goal is accepted or rejected
-   */
-  rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID&, std::shared_ptr<const ActionARCL::Goal>)
-  {
-    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
-  }
-
-  /**
-   * @brief This function is called when a goal is requested to be canceled by the action client.
-   *
-   * @param goal_handle The handle of the goal to be canceled.
-   * @return rclcpp_action::CancelResponse Indicates whether the cancel request is accepted or rejected.
-   */
-  rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandle>)
-  {
-    return rclcpp_action::CancelResponse::ACCEPT;
-  }
-
-  /**
-   * @brief This function is called when a new goal is accepted by the action server.
-   *
-   * @param goal_handle The handle of the accepted goal.
-   */
-  void handle_accepted(const std::shared_ptr<GoalHandle> goal_handle)
-  {
-    action_thread_ = std::thread(&ARCL_Interface::execute, this, goal_handle);
-    action_thread_.detach();
-  }
-
-  /**
-   * @brief This function executes the action goal by sending commands to the robot and processing feedback.
-   *
-   * @param goal_handle The handle of the accepted goal.
-   */
-  void execute(const std::shared_ptr<GoalHandle> goal_handle)
-  {
-    // Check taskmaster availability
-    if (!socket_taskmaster_)
-    {
-      auto result = std::make_shared<ActionARCL::Result>();
-      result->res_msg = "Taskmaster unavailable";
-      goal_handle->abort(result);
-      return;
-    }
-
-    Parser parser;
-
-    // Get goal details
-    auto goal = goal_handle->get_goal();
-
-    std::vector<std::string> end_lines;
-
-    // Use first identifier as end line if available
-    if (!goal->identifier.empty())
-      end_lines.emplace_back(goal->identifier.front());
-
-    try
-    {
-      // Send command to robot
-      socket_taskmaster_->push_command(goal->command, true, end_lines);
-    }
-    catch (const amr_exception& ex)
-    {
-      // Failed to send command
-      RCLCPP_ERROR(node_->get_logger(), "push_command error: %s", ex.what());
-
-      auto result = std::make_shared<ActionARCL::Result>();
-      result->res_msg = std::string("push_command failed: ") + ex.what();
-      goal_handle->abort(result);
-      return;
-    }
-
-    auto feedback = std::make_shared<ActionARCL::Feedback>();
-    auto result = std::make_shared<ActionARCL::Result>();
-    auto start = std::chrono::steady_clock::now();
-
-    while (rclcpp::ok())
-    {
-      // Check for cancelation
-      if (goal_handle->is_canceling())
-      {
-        result->res_msg = "Canceled";
-        goal_handle->canceled(result);
-        return;
-      }
-
-      // Wait for feedback or completion
-      SocketTaskmaster::WaitResult wr{ false, "", "" };
-      try
-      {
-        wr = socket_taskmaster_->wait_command(100);
-      }
-      catch (const amr_exception& ex)
-      {
-        // Failed to get feedback
-        RCLCPP_ERROR(node_->get_logger(), "wait_command error: %s", ex.what());
-
-        result->res_msg = std::string("wait_command error: ") + ex.what();
-        goal_handle->abort(result);
-        return;
-      }
-
-      // Handle lack of feedback or no goal
-      if (!wr.feedback.empty() && wr.feedback.find("No goal") != std::string::npos)
-      {
-        result->res_msg = "No such goal";
-        goal_handle->abort(result);
-        return;
-      }
-
-      // Process feedback or completion
-      if (wr.complete)
-      {
-        feedback->feed_msg = wr.feedback;
-        goal_handle->publish_feedback(feedback);
-        result->res_msg = wr.result;
-        goal_handle->succeed(result);
-        RCLCPP_INFO(node_->get_logger(), "action_server: %s", result->res_msg.c_str());
-        return;
-      }
-      else if (!wr.feedback.empty())
-      {
-        auto pr = parser.process_arcl_server(wr.feedback);
-        feedback->feed_msg = pr.message;
-        goal_handle->publish_feedback(feedback);
-
-        if (pr.code == Parser::Code::PASS)
-        {
-          result->res_msg = pr.message;
-          goal_handle->succeed(result);
-          return;
-        }
-        else if (pr.code == Parser::Code::FAIL)
-        {
-          result->res_msg = pr.message;
-          goal_handle->abort(result);
-          return;
-        }
-      }
-      // Check for timeout
-      if (std::chrono::steady_clock::now() - start > std::chrono::minutes(10))
-      {
-        result->res_msg = "Timeout";
-        goal_handle->abort(result);
-        return;
-      }
-      std::this_thread::sleep_for(std::chrono::milliseconds(100));
-    }
-
-    if (goal_handle->is_active())
-    {
-      result->res_msg = "Node shutdown";
-      goal_handle->abort(result);
-    }
-  }
-
-  rclcpp::Node::SharedPtr node_;
-  std::shared_ptr<SocketDriver> socket_driver_;
-  std::shared_ptr<SocketTaskmaster> socket_taskmaster_;
-
-  rclcpp::Service<ServiceARCL>::SharedPtr service_;
-  rclcpp_action::Server<ActionARCL>::SharedPtr action_server_;
-
-  int service_timeout_ms_{ 15000 };
-
-  std::thread action_thread_;
-};
\ No newline at end of file
diff --git a/amr_core/include/amr_core/interfaces/drive_interface.hpp b/amr_core/include/amr_core/interfaces/drive_interface.hpp
index 6bd9f63a..9bddbf82 100644
--- a/amr_core/include/amr_core/interfaces/drive_interface.hpp
+++ b/amr_core/include/amr_core/interfaces/drive_interface.hpp
@@ -1,9 +1,17 @@
 #pragma once
 
+#include <ArNetworking/ArClientRatioDrive.h>
+#include <ArNetworking/ArNetworking.h>
+
 #include <string>
 #include <memory>
 #include <vector>
 #include <chrono>
+#include <cmath>
+#include <mutex>
+#include <algorithm>
+#include <stdexcept>
+
 #include "rclcpp/rclcpp.hpp"
 #include <tf2_ros/transform_broadcaster.h>
 #include "std_msgs/msg/empty.hpp"
@@ -13,370 +21,412 @@
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/transform_stamped.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
-#include "amr_core/socket/socket_driver.hpp"
+#include "amr_core/utils/libaria_runtime.hpp"
 
 /**
- * @brief ROS 2 ARCL driver node (without direct socket code).
- * Uses SocketDriver for ARCL communication.
+ * @brief ROS 2 drive interface backed by libaria/ArNetworking.
  */
 class DriverInterface
 {
 public:
   /**
-   * @brief Constructor. Initializes publishers, subscribers, and parameters.
+   * @brief Constructor. Initializes publishers, subscribers, and libaria callbacks.
    * @param node Shared pointer to rclcpp::Node.
    */
-  DriverInterface(rclcpp::Node::SharedPtr node, std::shared_ptr<SocketDriver> driver)
+  DriverInterface(rclcpp::Node::SharedPtr node)
+    : node_(std::move(node)), ratio_drive_(&client_), update_callback_(this, &DriverInterface::handleUpdateNumbers)
   {
-    node_ = node;
-    socket_driver_ = driver;
-
     tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(node_);
   }
+
+  ~DriverInterface()
+  {
+    client_.disconnect();
+  }
+
   /**
    * @brief Initializes parameters, publishers, and subscribers.
+   *
+   * This should be called after the node is fully constructed.
+   * @param host Robot server IP address.
+   * @param port Robot server port.
+   * @param user Username for robot server authentication.
+   * @param password Password for robot server authentication.
+   * @param protocol Optional protocol version to enforce on the robot server connection.
+   *
+   * Throws std::runtime_error if connection to the robot server fails or is rejected by the robot.
    */
-  void initialize()
+  void initialize(const std::string& host, int port, const std::string& user, const std::string& password,
+                  const std::string& protocol)
   {
-    // Parameters
-    publish_odom_ = node_->declare_parameter<bool>("driver.publish_odom", true);
-    publish_robot_tf_ = node_->declare_parameter<bool>("driver.publish_robot_tf", true);
-    subscribe_cmd_vel_ = node_->declare_parameter<bool>("driver.subscribe_cmd_vel", true);
-    subscribe_goal_pose_ = node_->declare_parameter<bool>("driver.subscribe_goal_pose", true);
-    subscribe_initial_pose_ = node_->declare_parameter<bool>("driver.subscribe_initial_pose", true);
-    subscribe_local_plan_ = node_->declare_parameter<bool>("driver.subscribe_localplan", false);
-
-    odom_topic_ = node_->declare_parameter<std::string>("driver.odom_topic", "amr/odom");
-    cmd_vel_topic_ = node_->declare_parameter<std::string>("driver.cmd_vel_topic", "amr/cmd_vel");
-    odom_reset_topic = node_->declare_parameter<std::string>("driver.odom_reset_topic", "amr/odomReset");
-    goal_pose_topic_ = node_->declare_parameter<std::string>("driver.goal_pose_topic", "goal_pose");
-    initial_pose_topic_ = node_->declare_parameter<std::string>("driver.initial_pose_topic", "initialpose");
-
-    odom_frame_ = node_->declare_parameter<std::string>("driver.odom_frame", "odom");
-    base_frame_ = node_->declare_parameter<std::string>("driver.base_frame", "base_link");
-    timeout_ms = node_->declare_parameter<int>("driver.command_timeout_ms", 15000);
-
-    expected_cmd_vel_freq_ = node_->declare_parameter<double>("driver.expected_cmd_vel_freq", 20.0);
-    min_ang_speed_ = node_->declare_parameter<double>("driver.min_angular_speed", -30);   // deg/s
-    max_ang_speed_ = node_->declare_parameter<double>("driver.max_angular_speed", 30);    // deg/s
-    min_lin_speed_ = node_->declare_parameter<double>("driver.min_linear_speed", -1000);  // mm/s
-    max_lin_speed_ = node_->declare_parameter<double>("driver.max_linear_speed", 1000);   // mm/s
-
-    unit_move_distance_ = node_->declare_parameter<double>("driver.unit_move_distance", 100.0);  // mm
-    unit_turn_angle_ = node_->declare_parameter<double>("driver.unit_turn_angle", 10.0);         // deg
+    host_ = host;
+    port_ = port;
+    user_ = user;
+    password_ = password;
+    protocol_ = protocol;
+
+    odom_topic_ = getOrDeclareParameter<std::string>("driver.odom_topic", "amr/odom");
+    cmd_vel_topic_ = getOrDeclareParameter<std::string>("driver.cmd_vel_topic", "amr/cmd_vel");
+    stop_topic_ = getOrDeclareParameter<std::string>("driver.stop_topic", "amr/stop");
+    publish_odom_ = getOrDeclareParameter<bool>("driver.publish_odom", true);
+    publish_robot_tf_ = getOrDeclareParameter<bool>("driver.publish_robot_tf", true);
+
+    odom_frame_ = getOrDeclareParameter<std::string>("driver.odom_frame", "amr/odom");
+    base_frame_ = getOrDeclareParameter<std::string>("driver.base_frame", "amr/base_link");
+
+    expected_cmd_vel_freq_ = getOrDeclareParameter<double>("driver.expected_cmd_vel_freq", 20.0);
+    min_ang_speed_ = getOrDeclareParameter<double>("driver.min_angular_speed", -60.0);  // deg/s
+    max_ang_speed_ = getOrDeclareParameter<double>("driver.max_angular_speed", 60.0);   // deg/s
+    min_lin_speed_ = getOrDeclareParameter<double>("driver.min_linear_speed", -200.0);  // mm/s
+    max_lin_speed_ = getOrDeclareParameter<double>("driver.max_linear_speed", 1200.0);  // mm/s
+    drive_throttle_pct_ = getOrDeclareParameter<double>("driver.drive_throttle_pct", 100.0);
+    unsafe_drive_ = getOrDeclareParameter<bool>("driver.unsafe_drive", true);
+    cmd_vel_timeout_sec_ = getOrDeclareParameter<double>("driver.cmd_vel_timeout_sec", 0.2);
 
     // Publishers
     if (publish_odom_)
+    {
       odom_pub_ = node_->create_publisher<nav_msgs::msg::Odometry>(odom_topic_, 10);
+    }
 
     // Subscribers
     stop_sub_ = node_->create_subscription<std_msgs::msg::Empty>(
-        "amr/stop", 10, std::bind(&DriverInterface::stopCB, this, std::placeholders::_1));
+        stop_topic_, 10, std::bind(&DriverInterface::stopCB, this, std::placeholders::_1));
 
-    odom_reset_sub_ = node_->create_subscription<std_msgs::msg::Empty>(
-        odom_reset_topic, 10, std::bind(&DriverInterface::odomResetCB, this, std::placeholders::_1));
-
-    if (subscribe_cmd_vel_ && !subscribe_local_plan_)
-      cmd_vel_sub_ = node_->create_subscription<geometry_msgs::msg::Twist>(
-          cmd_vel_topic_, 10, std::bind(&DriverInterface::cmdVelCB, this, std::placeholders::_1));
-    else if (!subscribe_cmd_vel_ && subscribe_local_plan_)
-      local_plan_sub_ = node_->create_subscription<nav_msgs::msg::Path>(
-          "/local_plan", 10, std::bind(&DriverInterface::localPlanCB, this, std::placeholders::_1));
-    else
-      RCLCPP_ERROR(node_->get_logger(), "DriverInterface: Cannot subscribe to both cmd_vel and local_plan. Please "
-                                        "choose one.");
+    cmd_vel_sub_ = node_->create_subscription<geometry_msgs::msg::Twist>(
+        cmd_vel_topic_, 10, std::bind(&DriverInterface::cmdVelCB, this, std::placeholders::_1));
 
-    if (subscribe_goal_pose_)
-      goal_pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
-          goal_pose_topic_, 10, std::bind(&DriverInterface::goalPoseCB, this, std::placeholders::_1));
+    connectClient();
+    configureHandlers();
+    ratio_drive_.setThrottle(drive_throttle_pct_);
+    configureDriveMode();
+    client_.runAsync();
 
-    if (subscribe_initial_pose_)
-      initial_pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
-          initial_pose_topic_, 10, std::bind(&DriverInterface::initialPoseCB, this, std::placeholders::_1));
+    const int watchdog_ms = std::max(50, static_cast<int>(1000.0 / std::max(expected_cmd_vel_freq_, 1.0)));
+    cmd_vel_watchdog_timer_ = node_->create_wall_timer(std::chrono::milliseconds(watchdog_ms),
+                                                       std::bind(&DriverInterface::handleCmdVelWatchdog, this));
   }
 
+private:
   /**
-   * @brief Update odometry based on incoming string message.
-   * @param msg String message containing odometry data.
-   *
-   * Example input: "Odometer: <distance> mm <heading> deg <time> sec"
+   * @brief Helper function to get a parameter value or declare it with a default if it doesn't exist.
+   * @tparam T Parameter type.
+   * @param name Parameter name.
+   * @param default_value Default value to declare if parameter doesn't exist.
+   * @return Parameter value.
    */
-  void update(const std_msgs::msg::String& msg)
+  template <typename T>
+  T getOrDeclareParameter(const std::string& name, const T& default_value)
   {
-    // Prepare odometry message
-    nav_msgs::msg::Odometry odom_msg;
-    odom_msg.header.stamp = node_->now();
-    odom_msg.header.frame_id = odom_frame_;
-    odom_msg.child_frame_id = base_frame_;
+    if (!node_->has_parameter(name))
+    {
+      return node_->declare_parameter<T>(name, default_value);
+    }
 
-    // Parse the string for distance, heading, and time
-    std::istringstream iss(msg.data);
-    std::string token, previous_token;
-    double distance = 0.0, heading = 0.0, time = 0.0;
+    T value = default_value;
+    node_->get_parameter(name, value);
+    return value;
+  }
 
-    while (iss >> token)
+  /**
+   * @brief Connects to the robot server using libaria and handles connection errors.
+   * @throws std::runtime_error if connection fails or is rejected by the robot.
+   */
+  void connectClient()
+  {
+    if (!protocol_.empty())
     {
-      if (token == "Odometer:")
-      {
-        previous_token = token;
-        continue;
-      }
-      if (token == "mm")
-      {
-        previous_token = token;
-        continue;
-      }
-      if (token == "deg")
-      {
-        previous_token = token;
-        continue;
-      }
-      if (token == "sec")
-      {
-        previous_token = token;
-        continue;
-      }
+      client_.enforceProtocolVersion(protocol_.c_str());
+    }
 
-      if (previous_token == "Odometer:")
-      {
-        distance = std::stod(token);
-        continue;
-      }
-      if (previous_token == "mm")
-      {
-        heading = std::stod(token);
-        continue;
-      }
-      if (previous_token == "deg")
+    const char* password = password_.empty() ? nullptr : password_.c_str();
+    if (!client_.blockingConnect(host_.c_str(), port_, true, user_.c_str(), password))
+    {
+      if (client_.wasRejected())
       {
-        time = std::stod(token);
-        continue;
+        throw std::runtime_error("Robot rejected the drive interface connection.");
       }
+      throw std::runtime_error("Could not connect drive interface to Omron robot server.");
     }
 
-    // Fill odometry message
-    odom_msg.pose.pose.position.x = distance / 1000.0;  // meters
-    odom_msg.pose.pose.position.y = 0.0;
-    odom_msg.pose.pose.position.z = 0.0;
+    client_.setRobotName(host_.c_str());
+    RCLCPP_INFO(node_->get_logger(), "DriverInterface connected to %s:%d using libaria", host_.c_str(), port_);
+  }
+
+  /**
+   * @brief Configures handlers for the robot server.
+   */
+  void configureHandlers()
+  {
+    if (!client_.dataExists("updateNumbers"))
+    {
+      RCLCPP_WARN(node_->get_logger(), "Server does not advertise updateNumbers; odometry will not update");
+      return;
+    }
+
+    client_.addHandler("updateNumbers", &update_callback_);
+    client_.request("updateNumbers", 50);
+  }
 
-    double heading_rad = heading * M_PI / 180.0;
-    odom_msg.pose.pose.orientation.x = 0.0;
-    odom_msg.pose.pose.orientation.y = 0.0;
-    odom_msg.pose.pose.orientation.z = sin(heading_rad / 2.0);
-    odom_msg.pose.pose.orientation.w = cos(heading_rad / 2.0);
+  /**
+   * @brief Configures the drive mode for the robot.
+   */
+  void configureDriveMode()
+  {
+    if (!client_.dataExists("setSafeDrive"))
+    {
+      RCLCPP_WARN(node_->get_logger(), "Server does not advertise setSafeDrive; leaving drive mode unchanged");
+      return;
+    }
 
-    if (time > 0.0)
+    if (unsafe_drive_)
     {
-      odom_msg.twist.twist.linear.x = (distance / 1000.0) / time;  // m/s
-      odom_msg.twist.twist.angular.z = (heading_rad) / time;       // rad/s
+      ratio_drive_.unsafeDrive();
+      RCLCPP_WARN(node_->get_logger(), "Unsafe drive enabled for cmd_vel control");
     }
     else
     {
-      odom_msg.twist.twist.linear.x = 0.0;
-      odom_msg.twist.twist.angular.z = 0.0;
+      ratio_drive_.safeDrive();
+      RCLCPP_INFO(node_->get_logger(), "Safe drive enabled for cmd_vel control");
+    }
+  }
+
+  /**
+   * @brief Handles update numbers from the robot server.
+   * @param packet The packet containing the update numbers.
+   */
+  void handleUpdateNumbers(ArNetPacket* packet)
+  {
+    // The packet structure is assumed to be:
+    // byte: update type (ignored)
+    // byte4: x in mm
+    // byte4: y in mm
+    // byte2: theta in degrees
+    // byte2: x velocity in mm/s
+    // byte2: theta velocity in deg/s
+    // byte2: y velocity in mm/s
+
+    packet->bufToByte2();
+    const double x = static_cast<double>(packet->bufToByte4()) / 1000.0;
+    const double y = static_cast<double>(packet->bufToByte4()) / 1000.0;
+    const double theta_deg = static_cast<double>(packet->bufToByte2());
+    const double x_vel_mps = static_cast<double>(packet->bufToByte2()) / 1000.0;
+    const double theta_vel_rad_s = static_cast<double>(packet->bufToByte2()) * M_PI / 180.0;
+    const double y_vel_mps = static_cast<double>(packet->bufToByte2()) / 1000.0;
+    packet->bufToByte();
+
+    // Update internal pose state within a mutex to ensure thread safety with the ROS callbacks that may access the pose.
+    {
+      std::lock_guard<std::mutex> lock(pose_mutex_);
+      pose_x_ = x;
+      pose_y_ = y;
+      pose_theta_deg_ = theta_deg;
+      have_pose_ = true;
+    }
+
+    // Capture the first pose as the odom origin. Subsequent updates are transformed into that initial frame,
+    // so the published odom is a proper relative pose even when the robot starts with a non-zero heading.
+    if (!initial_pose_set_)
+    {
+      initial_pose_x_ = pose_x_;
+      initial_pose_y_ = pose_y_;
+      initial_pose_theta_deg_ = pose_theta_deg_;
+      initial_pose_set_ = true;
+      return;
     }
 
-    // Publish odometry
+    const double initial_theta_rad = initial_pose_theta_deg_ * M_PI / 180.0;
+    const double delta_x = pose_x_ - initial_pose_x_;
+    const double delta_y = pose_y_ - initial_pose_y_;
+    const double cos_initial = std::cos(initial_theta_rad);
+    const double sin_initial = std::sin(initial_theta_rad);
+
+    const double relative_x = cos_initial * delta_x + sin_initial * delta_y;
+    const double relative_y = -sin_initial * delta_x + cos_initial * delta_y;
+    const double relative_theta_rad = std::atan2(std::sin((pose_theta_deg_ - initial_pose_theta_deg_) * M_PI / 180.0),
+                                                 std::cos((pose_theta_deg_ - initial_pose_theta_deg_) * M_PI / 180.0));
+    const double relative_x_vel_mps = cos_initial * x_vel_mps + sin_initial * y_vel_mps;
+    const double relative_y_vel_mps = -sin_initial * x_vel_mps + cos_initial * y_vel_mps;
+
+    theta_rad_ = relative_theta_rad;
+
+    nav_msgs::msg::Odometry odom_msg;
+    odom_msg.header.stamp = node_->now();
+    odom_msg.header.frame_id = odom_frame_;
+    odom_msg.child_frame_id = base_frame_;
+    odom_msg.pose.pose.position.x = relative_x;
+    odom_msg.pose.pose.position.y = relative_y;
+    odom_msg.pose.pose.position.z = 0.0;
+
+    odom_msg.pose.pose.orientation.z = std::sin(theta_rad_ / 2.0);
+    odom_msg.pose.pose.orientation.w = std::cos(theta_rad_ / 2.0);
+
+    odom_msg.twist.twist.linear.x = relative_x_vel_mps;
+    odom_msg.twist.twist.linear.y = relative_y_vel_mps;
+    odom_msg.twist.twist.angular.z = theta_vel_rad_s;
+
     if (publish_odom_ && odom_pub_)
+    {
       odom_pub_->publish(odom_msg);
+    }
 
     if (publish_robot_tf_)
     {
       geometry_msgs::msg::TransformStamped tf_msg;
-      tf_msg.header.stamp = odom_msg.header.stamp;
-      tf_msg.header.frame_id = odom_frame_;
+      tf_msg.header = odom_msg.header;
       tf_msg.child_frame_id = base_frame_;
-      tf_msg.transform.translation.x = odom_msg.pose.pose.position.x;
-      tf_msg.transform.translation.y = odom_msg.pose.pose.position.y;
-      tf_msg.transform.translation.z = odom_msg.pose.pose.position.z;
+
+      tf_msg.transform.translation.x = relative_x;
+      tf_msg.transform.translation.y = relative_y;
+      tf_msg.transform.translation.z = 0.0;
+
       tf_msg.transform.rotation = odom_msg.pose.pose.orientation;
 
       tf_broadcaster_->sendTransform(tf_msg);
     }
   }
 
-private:
+  /**
+   * @brief Callback for stop messages. Stops the robot immediately.
+   * @param msg The received stop message (empty).
+   */
   void stopCB(const std_msgs::msg::Empty& /*msg*/)
   {
-    std::string response;
-    int req_id = socket_driver_->queue_command("stop", "");
-    bool got_response = socket_driver_->wait_for_response(req_id, response, timeout_ms);
-    if (!got_response)
-      RCLCPP_INFO(node_->get_logger(), "Stopping result: %s", response.c_str());
-  }
-
-  void odomResetCB(const std_msgs::msg::Empty& /*msg*/)
-  {
-    std::string response;
-    int req_id = socket_driver_->queue_command("odometerReset", "");
-    bool got_response = socket_driver_->wait_for_response(req_id, response, timeout_ms);
-    if (!got_response)
-      RCLCPP_INFO(node_->get_logger(), "Reset result: %s", response.c_str());
+    cmd_vel_active_ = false;
+    stop_sent_ = true;
+    ratio_drive_.stop();
+    if (client_.dataExists("stop"))
+    {
+      client_.requestOnce("stop");
+    }
   }
 
+  /**
+   * @brief Callback for cmd_vel messages. Converts Twist messages to drive commands and sends them to the robot.
+   * Also updates the last command time for the watchdog.
+   * 
+   * @param msg The received cmd_vel message.
+   */
   void cmdVelCB(const geometry_msgs::msg::Twist& msg)
   {
-    // Convert into mm/s and deg/s for ARCL commands since twist is in m/s and rad/s
-    double lin = msg.linear.x * 1000;                // m/s to mm/s
-    double ang = msg.angular.z * (180.0 / 3.14159);  // rad/s to deg/s
-
-    // Clamp values to min/max
-    lin = std::clamp(lin, min_lin_speed_, max_lin_speed_);
-    ang = std::clamp(ang, min_ang_speed_, max_ang_speed_);
-
-    // Deadband
-    if (std::abs(lin) < 20)
-      lin = 0;
-    if (std::abs(ang) < 5)
-      ang = 0;
-
-    // If angular velocity is non-zero, send turn command first
-    if (std::abs(ang) > 0)
+    last_cmd_vel_time_ = node_->now();
+    stop_sent_ = false;
+    cmd_vel_active_ = std::fabs(msg.linear.x) > 1e-3 || std::fabs(msg.angular.z) > 1e-3;
+    if (!cmd_vel_active_)
     {
-      int angle = (ang > 0) ? unit_turn_angle_ : -unit_turn_angle_;
-      int speed = std::abs(ang);
-
-      std::string cmd = "doTask deltaheading " + std::to_string(angle) + " " + std::to_string(speed);
-
-      int idx_ = socket_driver_->queue_command(cmd, "Completed doing task deltaHeading");
-      bool got = socket_driver_->wait_for_response(idx_, response_, timeout_ms);
-      if (!got)
-        RCLCPP_ERROR(node_->get_logger(), "Robot did not respond to angular cmd_vel command: %s", response_.c_str());
+      ratio_drive_.stop();
+      return;
     }
 
-    // IF linear velocity is non-zero, send move command
-    if (std::abs(lin) > 0)
+    if (!client_.dataExists("ratioDrive"))
     {
-      int distance = (lin > 0) ? unit_move_distance_ : -unit_move_distance_;
-      int speed = std::abs(lin);
+      RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, "Server does not advertise ratioDrive");
+      return;
+    }
 
-      std::string cmd = "doTask move " + std::to_string(distance) + " " + std::to_string(speed);
+    const double max_linear_mps = std::max(std::fabs(min_lin_speed_), std::fabs(max_lin_speed_)) / 1000.0;
+    const double max_angular_rad_s = std::max(std::fabs(min_ang_speed_), std::fabs(max_ang_speed_)) * M_PI / 180.0;
+    const double trans_ratio = toPercent(msg.linear.x, max_linear_mps);
+    const double rot_ratio = toPercent(msg.angular.z, max_angular_rad_s);
 
-      int idx_ = socket_driver_->queue_command(cmd, "Completed doing task move");
-      bool got = socket_driver_->wait_for_response(idx_, response_, timeout_ms);
-      if (!got)
-        RCLCPP_ERROR(node_->get_logger(), "Robot did not respond to linear cmd_vel command: %s", response_.c_str());
-    }
+    ratio_drive_.setThrottle(drive_throttle_pct_);
+    ratio_drive_.setLatVelRatio(0.0);
+    ratio_drive_.setTransVelRatio(trans_ratio);
+    ratio_drive_.setRotVelRatio(rot_ratio);
   }
 
-  void localPlanCB(const nav_msgs::msg::Path::SharedPtr msg)
+  void handleCmdVelWatchdog()
   {
-    if (msg->poses.empty())
+    if (!cmd_vel_active_)
+    {
       return;
+    }
 
-    // Get the last pose in the path as the goal
-    const auto& p = msg->poses.back().pose.position;
-    const auto& q = msg->poses.back().pose.orientation;
-
-    // int x_mm = static_cast<int>(std::lround(p.x * 1000.0));
-    // int y_mm = static_cast<int>(std::lround(p.y * 1000.0));
-    int deg = static_cast<int>(std::lround(yaw_deg(q.w, q.x, q.y, q.z)));
-    if (deg > 180)
-      deg -= 360;
-
-    std::string cmd = "doTask setHeading " + std::to_string(deg) + " " + std::to_string(std::abs(deg));
-    std::string identifier = { "Completed doing task setHeading" };
-
-    RCLCPP_INFO(node_->get_logger(), "Sending local plan heading command: %s", cmd.c_str());
-
-    int req_id = socket_driver_->queue_command(cmd, identifier);
-    bool got = socket_driver_->wait_for_response(req_id, response_, timeout_ms);
-
-    if (!got)
-			RCLCPP_ERROR(node_->get_logger(), "Robot did not respond to local plan command: %s", response_.c_str());
-    else
-      RCLCPP_INFO(node_->get_logger(), "Local plan command response: %s", response_.c_str());
+    if ((node_->now() - last_cmd_vel_time_).seconds() > cmd_vel_timeout_sec_)
+    {
+      cmd_vel_active_ = false;
+      if (!stop_sent_)
+      {
+        ratio_drive_.stop();
+        stop_sent_ = true;
+      }
+    }
   }
 
-  // Pose-based commands (from subscriptions)
-  void goalPoseCB(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
-  {
-    const auto& p = msg->pose.position;
-    const auto& q = msg->pose.orientation;
-
-    int x_mm = static_cast<int>(std::lround(p.x * 1000.0));
-    int y_mm = static_cast<int>(std::lround(p.y * 1000.0));
-    int deg = static_cast<int>(std::lround(yaw_deg(q.w, q.x, q.y, q.z)));
-    if (deg > 180)
-      deg -= 360;
-
-    std::string cmd =
-        "doTask gotoPoint " + std::to_string(x_mm) + " " + std::to_string(y_mm) + " " + std::to_string(deg);
-    std::string identifier = { "Going to point" };
-
-    std::string response;
-    int req_id = socket_driver_->queue_command(cmd, identifier);
-    bool got_response = socket_driver_->wait_for_response(req_id, response, timeout_ms);
-    return;
-  }
 
-  void initialPoseCB(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
+private:
+  /**
+   * @brief Helper function to convert a velocity value to a percentage of the maximum, clamped to [-100, 100].
+   * @param value The velocity value to convert.
+   * @param max_abs_value The maximum absolute velocity corresponding to 100%.
+   * @return The velocity as a percentage of the maximum, clamped to [-100, 100].
+   */
+  static double toPercent(double value, double max_abs_value)
   {
-    const auto& p = msg->pose.pose.position;
-    const auto& q = msg->pose.pose.orientation;
-
-    int x_mm = static_cast<int>(std::lround(p.x * 1000.0));
-    int y_mm = static_cast<int>(std::lround(p.y * 1000.0));
-    int deg = static_cast<int>(std::lround(yaw_deg(q.w, q.x, q.y, q.z)));
-    if (deg > 180)
-      deg -= 360;
-
-    // xySpread=0 angleSpread=0
-    std::string cmd =
-        "localizetopoint " + std::to_string(x_mm) + " " + std::to_string(y_mm) + " " + std::to_string(deg) + " 0 0";
-    std::string identifier = { "Localizing at point" };
-
-    std::string response;
-    int req_id = socket_driver_->queue_command(cmd, identifier);
-    bool got_response = socket_driver_->wait_for_response(req_id, response, timeout_ms);
-    return;
-  }
+    if (max_abs_value <= 0.0)
+    {
+      return 0.0;
+    }
 
-  static double yaw_deg(double w, double x, double y, double z)
-  {
-    const double t3 = +2.0 * (w * z + x * y);
-    const double t4 = +1.0 - 2.0 * (y * y + z * z);
-    const double yaw = std::atan2(t3, t4);  // radians
-    return yaw * 180.0 / M_PI;
+    const double percent = (value / max_abs_value) * 100.0;
+    return std::clamp(percent, -100.0, 100.0);
   }
 
-private:
+  // ROS node and libaria client
   rclcpp::Node::SharedPtr node_;
-  std::shared_ptr<SocketDriver> socket_driver_;
+
+  // Libaria runtime manager to ensure proper initialization and shutdown of libaria
+  amr_core::LibAriaRuntime aria_runtime_;
+
+  // Libaria client and drive interface
+  ArClientBase client_;
+  ArClientRatioDrive ratio_drive_;
+
+  //
+  ArFunctor1C<DriverInterface, ArNetPacket*> update_callback_;
 
   // Parameters
-  bool publish_odom_{ true };
-  bool publish_robot_tf_{ true };
-  bool subscribe_cmd_vel_{ true };
-  bool subscribe_goal_pose_{ true };
-  bool subscribe_initial_pose_{ true };
-  bool subscribe_local_plan_{ false };
+  std::string host_;
+  int port_{ 7272 };
+  std::string user_;
+  std::string password_;
+  std::string protocol_;
 
   std::string odom_topic_{ "amr/odom" };
   std::string cmd_vel_topic_{ "amr/cmd_vel" };
-  std::string odom_reset_topic{ "amr/odomReset" };
-  std::string goal_pose_topic_{ "goal_pose" };
-  std::string initial_pose_topic_{ "initialpose" };
+  std::string stop_topic_{ "amr/stop" };
+  bool publish_odom_{ true };
+  bool publish_robot_tf_{ true };
 
   std::string odom_frame_{ "odom" };
   std::string base_frame_{ "base_link" };
-  int timeout_ms{ 15000 };
-
-  int expected_cmd_vel_freq_{ 20 };  // Hz
-  double min_lin_speed_{ -1000 };    // mm/s
-  double max_lin_speed_{ 1000 };     // mm/s
-  double min_ang_speed_{ -30 };      // deg/s
-  double max_ang_speed_{ 30 };       // deg/s
-
-  int unit_move_distance_{ 50 };  // mm
-  int unit_turn_angle_{ 5 };      // deg
-
-  std::string response_;
-  std::mutex cmd_mutex_;
 
-  int avg_lin_speed_{ 0 };
-  int avg_ang_speed_{ 0 };
-  int iterations_{ 0 };
+  double expected_cmd_vel_freq_{ 20.0 };
+  double min_lin_speed_{ -1000.0 };
+  double max_lin_speed_{ 1000.0 };
+  double min_ang_speed_{ -30.0 };
+  double max_ang_speed_{ 30.0 };
+  double drive_throttle_pct_{ 100.0 };
+  double cmd_vel_timeout_sec_{ 0.2 };
+  bool unsafe_drive_{ true };
+
+  std::mutex pose_mutex_;
+  
+  double pose_x_{ 0.0 };
+  double pose_y_{ 0.0 };
+  double pose_theta_deg_{ 0.0 };
+
+  double initial_pose_x_{ 0.0 };
+  double initial_pose_y_{ 0.0 };
+  double initial_pose_theta_deg_{ 0.0 };
+  double theta_rad_{ 0.0 };
+
+  bool have_pose_{ false };
+  bool cmd_vel_active_{ false };
+  bool stop_sent_{ false };
+  bool initial_pose_set_{ false };
+
+  rclcpp::Time last_cmd_vel_time_{ 0, 0, RCL_ROS_TIME };
 
   // Subscribers
   rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr stop_sub_;
@@ -389,4 +439,5 @@ class DriverInterface
   // Publishers
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
   std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+  rclcpp::TimerBase::SharedPtr cmd_vel_watchdog_timer_;
 };
diff --git a/amr_core/include/amr_core/interfaces/laser_interface.hpp b/amr_core/include/amr_core/interfaces/laser_interface.hpp
index 7425d8d2..a4a154a4 100644
--- a/amr_core/include/amr_core/interfaces/laser_interface.hpp
+++ b/amr_core/include/amr_core/interfaces/laser_interface.hpp
@@ -1,102 +1,507 @@
 #pragma once
 
+#include <ArNetworking/ArNetworking.h>
+
+#include <geometry_msgs/msg/transform_stamped.hpp>
 #include "rclcpp/rclcpp.hpp"
-#include "sensor_msgs/msg/point_cloud.hpp"
+#include "sensor_msgs/msg/laser_scan.hpp"
 #include "std_msgs/msg/string.hpp"
-#include <tf2_ros/transform_broadcaster.h>
-#include <geometry_msgs/msg/transform_stamped.hpp>
-#include <sstream>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
 #include <string>
-#include <vector>
 #include <memory>
-#include <chrono>
-#include <cmath>  // added
+#include <mutex>
+#include <cmath>
+#include <stdexcept>
+#include <vector>
+#include <limits>
+
+#include "amr_core/utils/libaria_runtime.hpp"
 
 /**
- * @brief Node for publishing LaserScan messages from string data and broadcasting TF.
+ * @brief Node for publishing LaserScan messages from libaria laser packets while preserving amr_core frame correction.
  */
 class LaserInterface
 {
 public:
+  /**
+   * @brief Configuration for a laser device, including parameters for interpreting laser packets and the publisher
+   * for the resulting LaserScan messages.
+   *
+   * @param frame_id TF frame ID to use for the LaserScan messages.
+   * @param topic_name ROS topic name to publish the LaserScan messages on.
+   * @param request_name Name of the laser data request to send to the robot server.
+   * @param request_period_ms Period in milliseconds to request laser data from the robot server.
+   * @param angle_min Minimum angle of the laser scan in radians.
+   * @param angle_max Maximum angle of the laser scan in radians.
+   * @param angle_increment Angle increment between laser scan readings in radians.
+   * @param range_min Minimum valid range value for the laser scan in meters.
+   * @param range_max Maximum valid range value for the laser scan in meters.
+   * @param publisher ROS publisher for the resulting LaserScan messages.
+   */
+  struct LaserDeviceConfig
+  {
+    std::string frame_id;
+    std::string topic_name;
+    std::string request_name;
+    int request_period_ms{ 200 };
+    double angle_min{ -M_PI };
+    double angle_max{ M_PI };
+    double angle_increment{ M_PI / 360.0 };
+    double range_min{ 0.02 };
+    double range_max{ 30.0 };
+    rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr publisher;
+  };
+
   /**
    * @brief Constructor. Initializes node pointer.
    * @param node Shared pointer to rclcpp::Node.
    */
   LaserInterface(rclcpp::Node::SharedPtr node)
+    : node_(std::move(node))
+    , primary_laser_callback_(this, &LaserInterface::handlePrimaryLaser)
+    , low_laser_callback_(this, &LaserInterface::handleLowLaser)
+    , update_callback_(this, &LaserInterface::handleUpdateNumbers)
+  {
+  }
+
+  ~LaserInterface()
   {
-    node_ = node;
+    client_.disconnect();
   }
 
   /**
-   * @brief Initializes the LaserScan publisher, subscriptions, and parameters.
+   * @brief Initializes parameters, publishers, and subscribers.
+   *
    * This should be called after the node is fully constructed.
+   * @param host Robot server IP address.
+   * @param port Robot server port.
+   * @param user Username for robot server authentication.
+   * @param password Password for robot server authentication.
+   * @param protocol Optional protocol version to enforce on the robot server connection.
+   *
+   * Throws std::runtime_error if connection to the robot server fails or is rejected by the robot.
+   */
+  void initialize(const std::string& host, int port, const std::string& user, const std::string& password,
+                  const std::string& protocol)
+  {
+    host_ = host;
+    port_ = port;
+    user_ = user;
+    password_ = password;
+    protocol_ = protocol;
+    enable_primary_laser_ = getOrDeclareParameter<bool>("laser.main_laser.enabled", true);
+    enable_low_laser_ = getOrDeclareParameter<bool>("laser.low_laser.enabled", false);
+
+    primary_laser_ = loadLaserDeviceConfig("laser.main_laser", LaserDeviceConfig{
+                                                                   "laser_frame",
+                                                                   "scan",
+                                                                   "Laser_1Current",
+                                                                   200,
+                                                                   -2.0 * M_PI / 3.0,
+                                                                   2.0 * M_PI / 3.0,
+                                                                   M_PI / 360.0,
+                                                                   0.02,
+                                                                   15.0,
+                                                                   nullptr,
+                                                               });
+
+    validateLaserDeviceConfig(primary_laser_, "laser.main_laser");
+    if (enable_primary_laser_)
+    {
+      primary_laser_.publisher = node_->create_publisher<sensor_msgs::msg::LaserScan>(primary_laser_.topic_name, 10);
+    }
+
+    low_laser_ = loadLaserDeviceConfig("laser.low_laser", LaserDeviceConfig{
+                                                              "laser_frame_low",
+                                                              "scan_low",
+                                                              "Laser_2Current",
+                                                              200,
+                                                              -63.0 * M_PI / 180.0,
+                                                              63.0 * M_PI / 180.0,
+                                                              M_PI / 360.0,
+                                                              0.02,
+                                                              4.0,
+                                                              nullptr,
+                                                          });
+    validateLaserDeviceConfig(low_laser_, "laser.low_laser");
+
+    base_frame_ = getOrDeclareParameter<std::string>("driver.base_frame", "base_link");
+    tf_buffer_ = std::make_unique<tf2_ros::Buffer>(node_->get_clock());
+    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, node_, false);
+
+    if (enable_low_laser_)
+    {
+      low_laser_.publisher = node_->create_publisher<sensor_msgs::msg::LaserScan>(low_laser_.topic_name, 10);
+    }
+
+    connectClient();
+    configureHandlers();
+    client_.runAsync();
+  }
+
+private:
+  /**
+   * @brief Helper function to get a parameter value or declare it with a default if it doesn't exist.
+   * This simplifies the process of loading parameters with defaults and ensures that all parameters are declared on the
+   * node.
+   * @param name Name of the parameter to get or declare.
+   * @param default_value Default value to use if the parameter does not already exist.
+   * @return The value of the parameter, either existing or newly declared with the default.
+   */
+  template <typename T>
+  T getOrDeclareParameter(const std::string& name, const T& default_value)
+  {
+    if (!node_->has_parameter(name))
+    {
+      return node_->declare_parameter<T>(name, default_value);
+    }
+
+    T value = default_value;
+    node_->get_parameter(name, value);
+    return value;
+  }
+
+  /**
+   * @brief Loads laser device configuration from parameters with a given prefix, using defaults for any missing
+   * parameters.
+   * @param prefix Parameter name prefix for the laser device configuration.
+   * @param defaults Default configuration values to use for any parameters that are not set.
+   * @return Loaded LaserDeviceConfig with parameters from the node or defaults.
    */
-  void initialize()
+  LaserDeviceConfig loadLaserDeviceConfig(const std::string& prefix, const LaserDeviceConfig& defaults)
   {
-    // Declare and get parameters for scan angles and other LaserScan fields
-    frame_id_ = node_->declare_parameter<std::string>("laser_scans.frame_id", "laser_frame");
-    topic_name_ = node_->declare_parameter<std::string>("laser_scans.topic", "scan");
+    LaserDeviceConfig config = defaults;
+    config.frame_id = getOrDeclareParameter<std::string>(prefix + ".frame_id", defaults.frame_id);
+    config.topic_name = getOrDeclareParameter<std::string>(prefix + ".topic", defaults.topic_name);
+    config.request_name = getOrDeclareParameter<std::string>(prefix + ".request", defaults.request_name);
+    config.request_period_ms = getOrDeclareParameter<int>(prefix + ".request_period_ms", defaults.request_period_ms);
+    config.angle_min = getOrDeclareParameter<double>(prefix + ".angle_min", defaults.angle_min);
+    config.angle_max = getOrDeclareParameter<double>(prefix + ".angle_max", defaults.angle_max);
+    config.angle_increment = getOrDeclareParameter<double>(prefix + ".angle_increment", defaults.angle_increment);
+    config.range_min = getOrDeclareParameter<double>(prefix + ".range_min", defaults.range_min);
+    config.range_max = getOrDeclareParameter<double>(prefix + ".range_max", defaults.range_max);
+    return config;
+  }
 
-    point_cloud_pub = node_->create_publisher<sensor_msgs::msg::PointCloud>(topic_name_, 10);
+  bool lookupLaserTransform(const LaserDeviceConfig& config, double& sensor_x, double& sensor_y, double& sensor_yaw)
+  {
+    if (!tf_buffer_)
+    {
+      return false;
+    }
+
+    try
+    {
+      const geometry_msgs::msg::TransformStamped transform =
+          tf_buffer_->lookupTransform(base_frame_, config.frame_id, rclcpp::Time(0));
+      sensor_x = transform.transform.translation.x;
+      sensor_y = transform.transform.translation.y;
+
+      const auto& rotation = transform.transform.rotation;
+      const double siny_cosp = 2.0 * (rotation.w * rotation.z + rotation.x * rotation.y);
+      const double cosy_cosp = 1.0 - 2.0 * (rotation.y * rotation.y + rotation.z * rotation.z);
+      sensor_yaw = std::atan2(siny_cosp, cosy_cosp);
+      return true;
+    }
+    catch (const std::exception& ex)
+    {
+      RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, "Could not resolve TF from %s to %s: %s",
+                           base_frame_.c_str(), config.frame_id.c_str(), ex.what());
+      return false;
+    }
+  }
+
+  /**
+   * @brief Validates laser device configuration parameters and applies defaults or corrections if necessary, while
+   * logging warnings for any issues found.
+   *
+   * @param config LaserDeviceConfig to validate and correct if necessary.
+   * @param prefix Parameter name prefix to use in warning messages.
+   *
+   * This function checks for the following issues:
+   * - angle_increment must be positive; if not, it defaults to 0.5 degree (pi/360 radians).
+   * - angle_max must be greater than angle_min; if not, it defaults to angle_min + angle_increment.
+   * - range_max must be greater than range_min; if not, range_min defaults to 0.02 m and range_max defaults to 30.0 m.
+   */
+  void validateLaserDeviceConfig(LaserDeviceConfig& config, const char* prefix)
+  {
+    if (config.angle_increment <= 0.0)
+    {
+      RCLCPP_WARN(node_->get_logger(), "%s.angle_increment must be positive; defaulting to 0.5 degree", prefix);
+      config.angle_increment = M_PI / 360.0;
+    }
+    if (config.angle_max <= config.angle_min)
+    {
+      RCLCPP_WARN(node_->get_logger(), "%s.angle_max must exceed angle_min; defaulting to full configured FOV", prefix);
+      config.angle_max = config.angle_min + config.angle_increment;
+    }
+    if (config.range_max <= config.range_min)
+    {
+      RCLCPP_WARN(node_->get_logger(), "%s.range_max must exceed range_min; defaulting to 30.0 m", prefix);
+      config.range_min = 0.02;
+      config.range_max = 30.0;
+    }
   }
 
-  void update(const std_msgs::msg::String& msg, double laser_x, double laser_y, double laser_theta)
+  /**
+   * @brief Connects to the robot server using the configured host, port, user, password, and protocol. Throws
+   * std::runtime_error if the connection fails or is rejected by the robot.
+   */
+  void connectClient()
   {
-    std::string raw_resp = msg.data;
-    std::string rng_device = "Laser_1";
-    std::string::size_type pos = raw_resp.find(rng_device);
-    if (pos != std::string::npos)
+    if (!protocol_.empty())
     {
-      std::string vals_str;
-      try
+      client_.enforceProtocolVersion(protocol_.c_str());
+    }
+
+    const char* password = password_.empty() ? nullptr : password_.c_str();
+    if (!client_.blockingConnect(host_.c_str(), port_, true, user_.c_str(), password))
+    {
+      if (client_.wasRejected())
       {
-        // +1 to exclude the following space.
-        vals_str = raw_resp.substr(pos + rng_device.length() + 1);
+        throw std::runtime_error("Robot rejected the laser interface connection.");
       }
-      catch (const std::out_of_range&)
+      throw std::runtime_error("Could not connect laser interface to Omron robot server.");
+    }
+
+    client_.setRobotName(host_.c_str());
+    RCLCPP_INFO(node_->get_logger(), "LaserInterface connected to %s:%d using libaria", host_.c_str(), port_);
+  }
+
+  /**
+   * @brief Configures data handlers for the robot server client based on the loaded laser device configurations. This
+   * includes requesting the appropriate data packets from the robot server at the configured periods and setting up
+   * the corresponding handler functions to process incoming data.
+   */
+  void configureHandlers()
+  {
+    if (client_.dataExists("updateNumbers"))
+    {
+      client_.addHandler("updateNumbers", &update_callback_);
+      client_.request("updateNumbers", 50);
+    }
+
+    if (enable_primary_laser_)
+    {
+      configureLaserHandler(primary_laser_, primary_laser_callback_);
+    }
+    if (enable_low_laser_)
+    {
+      configureLaserHandler(low_laser_, low_laser_callback_);
+    }
+  }
+
+  /**
+   * @brief Helper function to configure a laser data handler for a given LaserDeviceConfig and callback. This checks if
+   * the requested data exists on the robot server, adds the handler for it, and sends the request to the robot server
+   * at the configured period. If the requested data does not exist, a warning is logged.
+   */
+  void configureLaserHandler(LaserDeviceConfig& config, ArFunctor1C<LaserInterface, ArNetPacket*>& callback)
+  {
+    if (!client_.dataExists(config.request_name.c_str()))
+    {
+      RCLCPP_WARN(node_->get_logger(), "Server does not advertise %s", config.request_name.c_str());
+      return;
+    }
+
+    client_.addHandler(config.request_name.c_str(), &callback);
+    client_.request(config.request_name.c_str(), config.request_period_ms);
+  }
+
+  /**
+   * @brief Handler function for processing "updateNumbers" packets from the robot server. This extracts the robot's
+   * current pose (x, y, theta) from the packet and updates the internal state of the LaserInterface. The pose is stored
+   * in millimeters for x and y, and degrees for theta, as provided by the robot server. This information is used to
+   * correct the laser scan data for the robot's current position and orientation.
+   * @param packet Incoming ArNetPacket containing the updateNumbers data from the robot server.
+   *
+   * The packet is expected to have the following structure based on the robot server's data format:
+   * - Byte2: Some header or identifier (ignored in this handler)
+   * - Byte4: Robot's x position in millimeters (signed integer)
+   * - Byte4: Robot's y position in millimeters (signed integer)
+   * - Byte2: Robot's orientation (theta) in degrees (signed integer)
+   * - Byte2: Additional data (ignored in this handler)
+   * - Byte2: Additional data (ignored in this handler)
+   * - Byte2: Additional data (ignored in this handler)
+   * - Byte: Additional data (ignored in this handler)
+   */
+  void handleUpdateNumbers(ArNetPacket* packet)
+  {
+    packet->bufToByte2();
+    const double x = static_cast<double>(packet->bufToByte4());
+    const double y = static_cast<double>(packet->bufToByte4());
+    const double theta_deg = static_cast<double>(packet->bufToByte2());
+    packet->bufToByte2();
+    packet->bufToByte2();
+    packet->bufToByte2();
+    packet->bufToByte();
+
+    std::lock_guard<std::mutex> lock(pose_mutex_);
+    pose_x_mm_ = x;
+    pose_y_mm_ = y;
+    pose_theta_deg_ = theta_deg;
+    have_pose_ = true;
+  }
+
+  /**
+   * @brief Handler function for processing primary laser data packets from the robot server. This extracts the laser
+   * scan data from the packet, applies corrections based on the robot's current pose, and publishes a LaserScan message
+   * on the configured topic. The laser scan data is expected to be in the form of (x, y) coordinates of detected points
+   * relative to the robot, which are then transformed into range and angle measurements for the LaserScan message.
+   */
+  void handlePrimaryLaser(ArNetPacket* packet)
+  {
+    handleLaserPacket(packet, primary_laser_);
+  }
+
+  /**
+   * @brief Handler function for processing low laser data packets from the robot server. This functions similarly to
+   * the primary laser handler, but uses the low laser configuration for interpreting the packet and publishing the
+   * LaserScan message. This allows for handling a second laser device with different parameters and topic.
+   */
+  void handleLowLaser(ArNetPacket* packet)
+  {
+    handleLaserPacket(packet, low_laser_);
+  }
+
+  /**
+   * @brief Common handler function for processing laser data packets from the robot server. This function is used by
+   * both the primary and low laser handlers to extract laser scan data from the packet, apply pose corrections, and
+   * publish a LaserScan message. The specific configuration for interpreting the packet and publishing is determined by
+   * the provided LaserDeviceConfig.
+   *
+   * The packet is expected to have the following structure based on the robot server's data format for laser scans:
+   * - Byte4: Number of laser readings (N)
+   * - For each reading (repeated N times):
+   *   - Byte4: x coordinate of the detected point in millimeters (signed integer)
+   *   - Byte4: y coordinate of the detected point in millimeters (signed integer)
+   */
+  void handleLaserPacket(ArNetPacket* packet, const LaserDeviceConfig& config)
+  {
+    const int num_readings = packet->bufToByte4();
+    if (num_readings <= 0)
+    {
+      RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, "Received empty laser packet");
+      return;
+    }
+
+    if (!have_pose_)
+    {
+      RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000,
+                           "Skipping laser packet because robot pose is not available yet");
+      return;
+    }
+
+    double sensor_x = 0.0;
+    double sensor_y = 0.0;
+    double sensor_yaw = 0.0;
+    if (!lookupLaserTransform(config, sensor_x, sensor_y, sensor_yaw))
+    {
+      return;
+    }
+
+    double laser_x = 0.0;
+    double laser_y = 0.0;
+    double laser_theta = 0.0;
+    {
+      std::lock_guard<std::mutex> lock(pose_mutex_);
+      laser_x = pose_x_mm_;
+      laser_y = pose_y_mm_;
+      laser_theta = pose_theta_deg_;
+    }
+
+    const double bounded_angle_max = std::max(config.angle_max, config.angle_min + config.angle_increment);
+    const std::size_t bin_count =
+        static_cast<std::size_t>(std::llround((bounded_angle_max - config.angle_min) / config.angle_increment)) + 1U;
+    const double effective_angle_increment = config.angle_increment;
+
+    sensor_msgs::msg::LaserScan scan_msg;
+    scan_msg.header.stamp = node_->now();
+    scan_msg.header.frame_id = config.frame_id;
+    scan_msg.angle_min = static_cast<float>(config.angle_min);
+    scan_msg.angle_max =
+        static_cast<float>(config.angle_min + effective_angle_increment * static_cast<double>(bin_count - 1U));
+    scan_msg.angle_increment = static_cast<float>(effective_angle_increment);
+    scan_msg.range_min = static_cast<float>(config.range_min);
+    scan_msg.range_max = static_cast<float>(config.range_max);
+    scan_msg.scan_time = config.request_period_ms / 1000.0f;
+    scan_msg.time_increment = bin_count > 1 ? scan_msg.scan_time / static_cast<float>(bin_count - 1U) : 0.0f;
+    scan_msg.ranges.assign(bin_count, std::numeric_limits<float>::infinity());
+
+    const double theta_rad = laser_theta * M_PI / 180.0;
+    const double c = std::cos(theta_rad);
+    const double s = std::sin(theta_rad);
+
+    for (int i = 0; i < num_readings; ++i)
+    {
+      const double x_mm = static_cast<double>(packet->bufToByte4());
+      const double y_mm = static_cast<double>(packet->bufToByte4());
+      const double dx = x_mm - laser_x;
+      const double dy = y_mm - laser_y;
+      const double point_x_base = c * dx + s * dy;
+      const double point_y_base = -s * dx + c * dy;
+      const double point_x_sensor = point_x_base / 1000.0 - sensor_x;
+      const double point_y_sensor = point_y_base / 1000.0 - sensor_y;
+
+      const double sensor_c = std::cos(sensor_yaw);
+      const double sensor_s = std::sin(sensor_yaw);
+      const double point_x_laser = sensor_c * point_x_sensor + sensor_s * point_y_sensor;
+      const double point_y_laser = -sensor_s * point_x_sensor + sensor_c * point_y_sensor;
+
+      const double range = std::hypot(point_x_laser, point_y_laser);
+      if (range < config.range_min || range > config.range_max)
+      {
+        continue;
+      }
+
+      const double angle = std::atan2(point_y_laser, point_x_laser);
+      if (angle < config.angle_min || angle > scan_msg.angle_max)
       {
-        vals_str.clear();
+        continue;
       }
 
-      // Build PointCloud in a frame with laser pose removed
-      sensor_msgs::msg::PointCloud cloud_msg;
-      cloud_msg.header.stamp = node_->now();
-      cloud_msg.header.frame_id = frame_id_;  // Keep configured frame
-
-      // Convert laser pose units and compute inverse transform
-      // Inputs are mm (x,y) and deg (theta). We remove laser pose effect:
-      // p_local = R(-theta) * ([x,y] - [laser_x, laser_y])
-      const double theta_rad = laser_theta * M_PI / 180.0;
-      const double c = std::cos(theta_rad);
-      const double s = std::sin(theta_rad);
-
-      std::istringstream iss(vals_str);
-      double x_mm, y_mm;
-      while (iss >> x_mm >> y_mm)
+      const std::size_t index =
+          static_cast<std::size_t>(std::floor((angle - config.angle_min) / effective_angle_increment));
+      if (index >= scan_msg.ranges.size())
       {
-        // Translate: world -> laser origin
-        const double dx = x_mm - laser_x;
-        const double dy = y_mm - laser_y;
-
-        // Rotate by -theta (inverse of laser heading)
-        const double lx_mm = c * dx + s * dy;
-        const double ly_mm = -s * dx + c * dy;
-
-        geometry_msgs::msg::Point32 pt;
-        pt.x = static_cast<float>(lx_mm / 1000.0);  // mm -> m
-        pt.y = static_cast<float>(ly_mm / 1000.0);  // mm -> m
-        pt.z = 0.0f;
-        cloud_msg.points.push_back(pt);
+        continue;
       }
-      
-      point_cloud_pub->publish(cloud_msg);
+
+      scan_msg.ranges[index] = std::min(scan_msg.ranges[index], static_cast<float>(range));
+    }
+
+    if (config.publisher)
+    {
+      config.publisher->publish(scan_msg);
     }
   }
 
-private:
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud>::SharedPtr point_cloud_pub;  ///< Publisher for PointCloud
   rclcpp::Node::SharedPtr node_;
+  amr_core::LibAriaRuntime aria_runtime_;
+  ArClientBase client_;
+  ArFunctor1C<LaserInterface, ArNetPacket*> primary_laser_callback_;
+  ArFunctor1C<LaserInterface, ArNetPacket*> low_laser_callback_;
+  ArFunctor1C<LaserInterface, ArNetPacket*> update_callback_;
+
+  std::mutex pose_mutex_;
+  double pose_x_mm_{ 0.0 };
+  double pose_y_mm_{ 0.0 };
+  double pose_theta_deg_{ 0.0 };
+  bool have_pose_{ false };
+
+  std::string host_;
+  int port_{ 7272 };
+  std::string user_;
+  std::string password_;
+  std::string protocol_;
+  std::string base_frame_{ "base_link" };
+
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 
-  std::string frame_id_{ "laser_frame" };
-  std::string topic_name_{ "scan" };
+  LaserDeviceConfig primary_laser_;
+  LaserDeviceConfig low_laser_;
+  bool enable_primary_laser_{ true };
+  bool enable_low_laser_{ false };
 };
\ No newline at end of file
diff --git a/amr_core/include/amr_core/interfaces/map_file_interface.hpp b/amr_core/include/amr_core/interfaces/map_file_interface.hpp
deleted file mode 100644
index 0e75f730..00000000
--- a/amr_core/include/amr_core/interfaces/map_file_interface.hpp
+++ /dev/null
@@ -1,593 +0,0 @@
-#pragma once
-
-#include "rclcpp/rclcpp.hpp"
-#include <ament_index_cpp/get_package_share_directory.hpp>
-#include "visualization_msgs/msg/marker.hpp"
-#include "visualization_msgs/msg/marker_array.hpp"
-#include "geometry_msgs/msg/quaternion.hpp"
-#include "geometry_msgs/msg/point.hpp"
-#include "nav_msgs/msg/occupancy_grid.hpp"
-#include "tf2/LinearMath/Quaternion.h"
-#include <fstream>
-#include <sstream>
-#include <string>
-#include <vector>
-#include <cmath>
-#include <limits>
-
-/**
- * @brief Publishes map grid created by the robot and forbidden areas as MarkerArray for RViz 
- * visualization.
- */
-class MapFileInterface
-{
-public:
-  /**
-   * @brief Constructor. Initializes publishers, loads map data, and starts timer.
-   */
-  MapFileInterface(rclcpp::Node::SharedPtr node)
-  {
-    node_ = node;
-  }
-
-  /**
-   * @brief Initializes the occupancy grid and forbidden areas.
-   */
-  void initialize()
-  {
-    map_pub_frequency_ = node_->declare_parameter<int>("map_data.frequency", 2);
-    map_file_ = node_->declare_parameter<std::string>("map_data.file", "data.map");
-    map_topic_ = node_->declare_parameter<std::string>("map_data.topic", "map_grid");
-
-    map_pub_interval_ms_ = int(1000 / map_pub_frequency_);
-
-    map_pub = node_->create_publisher<nav_msgs::msg::OccupancyGrid>(map_topic_, 10);
-    fa_pub = node_->create_publisher<visualization_msgs::msg::MarkerArray>("forbidden_areas", 10);
-    timer = node_->create_wall_timer(std::chrono::milliseconds(map_pub_interval_ms_),
-                                     std::bind(&MapFileInterface::publish_map, this));
-
-    set_forbidden_area_params();
-
-    if (!get_map_data(map_file_))
-    {
-      RCLCPP_ERROR(node_->get_logger(), "Reading map failed");
-    }
-
-    get_min_max_coordinates();
-    set_grid_attributes();
-    initialize_map();
-    fill_map();
-
-    RCLCPP_INFO(node_->get_logger(), "MapFileInterface initialized");
-  }
-
-  /**
-   * @brief This function publishes the occupancy grid and forbidden areas to their respective topics.
-   */
-  void publish_map()
-  {
-    map_pub->publish(grid);
-  }
-
-  /**
-   * @brief Creates a quaternion msg given only yaw.
-   * @param yaw Yaw angle in radians.
-   * @return Quaternion message.
-   */
-  geometry_msgs::msg::Quaternion createQuaternionMsgFromYaw(double yaw)
-  {
-    tf2::Quaternion tfq;
-    tfq.setRPY(0, 0, yaw);
-    geometry_msgs::msg::Quaternion gmq;
-
-    double roll = 0;
-    double pitch = 0;
-
-    double cy = cos(yaw * 0.5);
-    double sy = sin(yaw * 0.5);
-    double cp = cos(pitch * 0.5);
-    double sp = sin(pitch * 0.5);
-    double cr = cos(roll * 0.5);
-    double sr = sin(roll * 0.5);
-
-    gmq.w = cy * cp * cr + sy * sp * sr;
-    gmq.x = cy * cp * sr - sy * sp * cr;
-    gmq.y = sy * cp * sr + cy * sp * cr;
-    gmq.z = sy * cp * cr - cy * sp * sr;
-
-    return gmq;
-  }
-
-  /**
-   * @brief Extracts yaw from a quaternion message.
-   * @param quaternion Quaternion message.
-   * @return Yaw angle in radians.
-   */
-  double getYawFromQuaternionMsg(geometry_msgs::msg::Quaternion quaternion)
-  {
-    double varA = +2.0 * ((quaternion.w * quaternion.z) + (quaternion.x * quaternion.y));
-    double varB = +1.0 - 2.0 * ((quaternion.y * quaternion.y) + (quaternion.z * quaternion.z));
-    return std::atan2(varA, varB);
-  }
-
-private:
-  /**
-   * @brief Gets minimum and maximum coordinates from the points vector.
-   */
-  void get_min_max_coordinates()
-  {
-    minx = std::numeric_limits<double>::max();
-    miny = std::numeric_limits<double>::max();
-    maxx = -std::numeric_limits<double>::max();
-    maxy = -std::numeric_limits<double>::max();
-    for (const auto& pt : points)
-    {
-      if (pt.x < minx)
-        minx = pt.x;
-      if (pt.x > maxx)
-        maxx = pt.x;
-      if (pt.y < miny)
-        miny = pt.y;
-      if (pt.y > maxy)
-        maxy = pt.y;
-    }
-  }
-
-  /**
-   * @brief Gets the number of points from a map file line.
-   * @param line Line from map file.
-   * @param ignore_head Unused header string.
-   * @param num_of_points Output number of points.
-   */
-  void get_points_count(std::string* line, std::string* ignore_head, int* num_of_points)
-  {
-    if (line->find("NumPoints") != std::string::npos)
-    {
-      std::istringstream iss(*line);
-      iss >> *ignore_head;
-      iss >> *num_of_points;
-    }
-  }
-
-  /**
-   * @brief Gets the number of lines from a map file line.
-   * @param line Line from map file.
-   * @param ignore_head Unused header string.
-   * @param num_of_lines Output number of lines.
-   */
-  void get_lines_count(std::string* line, std::string* ignore_head, int* num_of_lines)
-  {
-    if (line->find("NumLines") != std::string::npos)
-    {
-      std::istringstream iss(*line);
-      iss >> *ignore_head;
-      iss >> *num_of_lines;
-    }
-  }
-
-  /**
-   * @brief Adds a point to the points vector.
-   * @param x X coordinate (mm).
-   * @param y Y coordinate (mm).
-   */
-  void set_point(int x, int y)
-  {
-    geometry_msgs::msg::Point p;
-
-    // Values are in millimeter.
-    p.x = static_cast<double>(x) / 1000.0;
-    p.y = static_cast<double>(y) / 1000.0;
-    p.z = 0;
-    points.push_back(p);
-  }
-
-  /**
-   * @brief Adds a line (two points) to the lines vector.
-   * @param x1 X1 coordinate (mm).
-   * @param y1 Y1 coordinate (mm).
-   * @param x2 X2 coordinate (mm).
-   * @param y2 Y2 coordinate (mm).
-   */
-  void set_line(int x1, int y1, int x2, int y2)
-  {
-    geometry_msgs::msg::Point p1, p2;
-
-    // Values are in millimeter.
-    p1.x = static_cast<double>(x1) / 1000.0;
-    p1.y = static_cast<double>(y1) / 1000.0;
-    p1.z = 0;
-    p2.x = static_cast<double>(x2) / 1000.0;
-    p2.y = static_cast<double>(y2) / 1000.0;
-    p2.z = 0;
-
-    // Needs two consecutive points to form a line.
-    lines.push_back(p1);
-    lines.push_back(p2);
-  }
-
-  /**
-   * @brief Reads forbidden area information from a map file line.
-   * @param line Line from map file.
-   * @param dt_theta Output orientation (radians).
-   * @param x1 Output X1.
-   * @param y1 Output Y1.
-   * @param x2 Output X2.
-   * @param y2 Output Y2.
-   */
-  void get_forbidden_area_information(std::string line, double* dt_theta, double* x1, double* y1, double* x2,
-                                      double* y2)
-  {
-    std::istringstream iss(line);
-    std::string dummy;
-
-    // The first 8 pieces of that line are irrelevant except for the 5th piece which is the angle/orientation
-    // of the forbidden area.
-    for (int i = 0; i < 8; i++)
-    {
-      if (i == 4)
-      {
-        iss >> *dt_theta;
-        *dt_theta *= 0.01745329;
-      }
-      else
-      {
-        iss >> dummy;
-      }
-    }
-    if (!(iss >> *y1 >> *x1 >> *y2 >> *x2))
-    {
-      RCLCPP_ERROR(node_->get_logger(), "Error reading forbidden area from file");
-    }
-  }
-
-  /**
-   * @brief Sets forbidden area marker and adds to marker array.
-   * @param dt_theta Orientation (radians).
-   * @param x1 X1.
-   * @param y1 Y1.
-   * @param x2 X2.
-   * @param y2 Y2.
-   */
-  void set_forbidden_area_information(double dt_theta, double x1, double y1, double x2, double y2)
-  {
-    // Compute the actual forbidden area from the transformed area.
-    // The raw info in the map file is a transformed coordinates of the actual forbidden area.
-    // We transform back to the actual area.
-    double cx = (x1 + x2) / 2000.0;  // Centre of transformed X coord.
-    double cy = (y1 + y2) / 2000.0;  // Centre of transformed Y coord.
-
-    // Distance from centre of transformed area to origin.
-    double r = sqrt((cx * cx) + (cy * cy));  // Polar coordinates, r component.
-    double theta = atan2(cy, cx);            // Polar coordinates, theta component.
-
-    // Get back the centre point of actual forbidden area.
-    double o_theta = theta - dt_theta;
-    double ocx = r * sin(o_theta);
-    double ocy = r * cos(o_theta);
-    dt_theta -= M_PI_2;
-
-    geometry_msgs::msg::Quaternion q = createQuaternionMsgFromYaw(dt_theta);
-
-    // Push to vector that will be published.
-    fa.pose.position.x = ocx;
-    fa.pose.position.y = ocy;
-    fa.pose.orientation = q;
-    fa.scale.x = (x2 - x1) / 1000.0;
-    fa.scale.y = (y2 - y1) / 1000.0;
-    f_areas.markers.push_back(fa);
-    fa.id += 1;
-  }
-
-  /**
-   * @brief Sets default parameters for forbidden area markers.
-   */
-  void set_forbidden_area_params()
-  {
-    fa.header.frame_id = "pose";
-    fa.action = visualization_msgs::msg::Marker::ADD;
-    fa.ns = "forbidden_areas";
-    fa.pose.orientation.w = 1.0;
-    fa.pose.position.z = 0;
-    fa.type = visualization_msgs::msg::Marker::CUBE;
-    fa.id = 0;
-    fa.scale.z = 0.25;
-    fa.color.a = 0.5;
-    fa.color.r = 1.0;
-    fa.color.g = 0.0;
-    fa.color.b = 0.0;
-  }
-
-  /**
-   * @brief Loads map data from file and populates points, lines, and forbidden areas.
-   * @param filename Map file name.
-   * @return True if successful, false otherwise.
-   */
-  bool get_map_data(std::string filename)
-  {
-    // Read map file
-    std::string map_path = ament_index_cpp::get_package_share_directory("amr_ros") + "/map/" + filename;
-    std::ifstream map_file(map_path);
-
-    if (map_file.fail())
-    {
-      return false;
-    }
-
-    int num_of_points = 0;
-    int num_of_lines = 0;
-    std::string line, ignore_head;
-    while (std::getline(map_file, line))
-    {
-      get_points_count(&line, &ignore_head, &num_of_points);
-      get_lines_count(&line, &ignore_head, &num_of_lines);
-
-      // We have seen the points data, collect them.
-      if (line.find("DATA") != std::string::npos)
-      {
-        int x, y;
-        for (int i = 0; i < num_of_points; i++)
-        {
-          std::getline(map_file, line);
-          std::istringstream iss(line);
-          if (!(iss >> x >> y))
-          {
-            RCLCPP_ERROR(node_->get_logger(), "Error reading points from file.");
-          }
-          set_point(x, y);
-        }
-      }
-
-      // We have seen the lines data, collect them.
-      if (line.find("LINES") != std::string::npos)
-      {
-        int x1, y1, x2, y2;
-        for (int i = 0; i < num_of_lines; i++)
-        {
-          std::getline(map_file, line);
-          std::istringstream iss(line);
-          if (!(iss >> x1 >> y1 >> x2 >> y2))
-          {
-            RCLCPP_ERROR(node_->get_logger(), "Error reading lines from file.");
-          }
-          set_line(x1, y1, x2, y2);
-        }
-      }
-
-      // node_ line contains forbidden area information, collect them.
-      if (line.find("Cairn: ForbiddenArea") != std::string::npos)
-      {
-        // Read the raw info
-        double x1, y1, x2, y2, dt_theta;
-        get_forbidden_area_information(line, &dt_theta, &x1, &y1, &x2, &y2);
-        set_forbidden_area_information(dt_theta, x1, y1, x2, x2);
-      }
-    }
-    return true;
-  }
-
-  /**
-   * @brief Gets the normal angle for calculating width. Assumes -180 to +180 degrees from 0 heading.
-   * @param theta Input angle in radians.
-   * @return Normal angle in radians.
-   */
-  double get_normal(double theta)
-  {
-    double pi = 3.141592654;
-    if (theta >= pi / 2)
-      theta -= pi / 2;
-    else
-      theta += pi / 2;
-    return theta;
-  }
-
-  /**
-   * @brief Returns coordinate difference for a given heading and distance.
-   * @param theta Heading in radians.
-   * @param distance Distance.
-   * @return Point difference.
-   */
-  geometry_msgs::msg::Point get_extrapolated_coordinates(double theta, double distance)
-  {
-    // sin and cos functions take in radians
-    double pi = 3.141592654;
-    geometry_msgs::msg::Point difference;
-
-    // 0 to 90 degrees
-    if (theta >= 0 && theta <= (pi / 2))
-    {
-      difference.x = distance * cos(theta);
-      difference.y = distance * sin(theta);
-    }
-    // 90 to 180 degrees
-    else if (theta > (pi / 2) && theta <= pi)
-    {
-      difference.x = distance * cos(pi - theta);
-      difference.y = -(distance * sin(pi - theta));
-    }
-    // 0 to -90 degrees
-    else if (theta < 0 && theta >= -(pi / 2))
-    {
-      difference.x = -(distance * cos(theta));
-      difference.y = distance * sin(theta);
-    }
-    // -90 to -180 degrees
-    else if (theta < -(pi / 2) && theta >= -pi)
-    {
-      // assumes that theta is a negative value
-      difference.x = -(distance * cos(theta + pi));
-      difference.y = -(distance * sin(theta + pi));
-    }
-    return difference;
-  }
-
-  /**
-   * @brief Checks if a point is within the map array and colours it.
-   * @param coordinate Point coordinate.
-   * @param occupancy_value Value to set in grid.
-   */
-  void check_and_colour_grid(geometry_msgs::msg::Point coordinate, int occupancy_value)
-  {
-    float relx = coordinate.x - minx;
-    float rely = coordinate.y - miny;
-    rely /= grid.info.resolution;
-    relx /= grid.info.resolution;
-
-    // Check if point is within map bounds
-    if (rely <= grid.info.height && relx <= grid.info.width && rely >= 0 && relx >= 0)
-    {
-      // node_ is 2D array represented by 1D. correct position would be (height_position * width) + width
-      int position_on_grid = int((int(rely) * grid.info.width) + relx);
-      grid.data[position_on_grid] = occupancy_value;
-    }
-  }
-
-  /**
-   * @brief Fills the occupancy grid with points, lines, and forbidden areas.
-   */
-  void fill_map()
-  {
-    // For each forbidden area, get number of steps on each axis of rectangle
-    for (size_t i = 0; i < f_areas.markers.size(); i++)
-    {
-      // Get number of steps to calculate to cover the full length and breadth of forbidden area
-      int step_count_length = (f_areas.markers[i].scale.x / grid.info.resolution) + 1;
-      int step_count_breadth = (f_areas.markers[i].scale.y / grid.info.resolution) + 1;
-
-      // not sure if node_ should be radians or degree.
-      double yaw = getYawFromQuaternionMsg(f_areas.markers[i].pose.orientation);
-
-      // Get the x and y difference in step for length of forbidden area
-      // The resolution here is divided by 2 to increase fidelity of the forbidden area
-      auto step_length = get_extrapolated_coordinates(yaw, grid.info.resolution / 2);
-
-      // Get the x and y difference in step for breadh of forbidden area. The angle node_ is trigonometrically
-      // calculated from is the normal of the yaw of the original orientation
-      auto step_breadth = get_extrapolated_coordinates(get_normal(yaw), grid.info.resolution / 2);
-
-      // Since we have an origin point, begin filling rectangle from origin.
-      // For each row in the forbidden area, get their starting coordinates
-      for (int j = 0; j < step_count_breadth; j++)
-      {
-        geometry_msgs::msg::Point starting_position_positive, starting_position_negative;
-
-        starting_position_positive.x = f_areas.markers[i].pose.position.x + (step_breadth.x * j);
-        starting_position_positive.y = f_areas.markers[i].pose.position.y + (step_breadth.y * j);
-
-        starting_position_negative.x = f_areas.markers[i].pose.position.x - (step_breadth.x * j);
-        starting_position_negative.y = f_areas.markers[i].pose.position.y - (step_breadth.y * j);
-
-        // get the position of every point in a row to check and colour the grid
-        for (int k = 0; k < step_count_length; k++)
-        {
-          // Occupancy value here is set to 85 just to distinguish between forbidden areas and actual map obstacles
-          // They should be set to 100 if map is used for path planning (We are only using it for visualization)
-          geometry_msgs::msg::Point starting_position_positive_positive;
-          starting_position_positive_positive.x = starting_position_positive.x + (step_length.x * k);
-          starting_position_positive_positive.y = starting_position_positive.y + (step_length.y * k);
-          check_and_colour_grid(starting_position_positive_positive, 85);
-
-          geometry_msgs::msg::Point starting_position_positive_negative;
-          starting_position_positive_negative.x = starting_position_positive.x - (step_length.x * k);
-          starting_position_positive_negative.y = starting_position_positive.y - (step_length.y * k);
-          check_and_colour_grid(starting_position_positive_negative, 85);
-
-          geometry_msgs::msg::Point starting_position_negative_positive;
-          starting_position_negative_positive.x = starting_position_negative.x + (step_length.x * k);
-          starting_position_negative_positive.y = starting_position_negative.y + (step_length.y * k);
-          check_and_colour_grid(starting_position_negative_positive, 85);
-
-          geometry_msgs::msg::Point starting_position_negative_negative;
-          starting_position_negative_negative.x = starting_position_negative.x - (step_length.x * k);
-          starting_position_negative_negative.y = starting_position_negative.y - (step_length.y * k);
-          check_and_colour_grid(starting_position_negative_negative, 85);
-        }
-      }
-    }
-
-    // for each point in point vector, convert to relative coordinates compared to grid origin
-    // if point lies within a grid, colour it black
-    for (const auto& pt : points)
-    {
-      check_and_colour_grid(pt, 100);
-    }
-
-    for (size_t i = 0; i < lines.size(); i++)
-    {
-      // for each 2 points in line vector, convert to relative coordinates compared to grid origin
-      // break each line up into as many points as grid resolution allows
-      // check if points lie within a grid and colour it black
-      if (i % 2 == 1)
-        continue;
-
-      // gets vector from 1 point to another
-      double vector_x = lines[i + 1].x - lines[i].x;
-      double vector_y = lines[i + 1].y - lines[i].y;
-
-      // calculates magnitude of the vector
-      double distance = std::hypot(vector_x, vector_y);
-
-      // break the distance into points based on grid resolution
-      int steps = distance / grid.info.resolution;
-      float step_x = vector_x / steps;
-      float step_y = vector_y / steps;
-
-      // incrementally checks each point from the start of the line where they lie on the grid
-      for (int j = 0; j < steps; j++)
-      {
-        geometry_msgs::msg::Point step_temp;
-        step_temp.x = lines[i].x + (step_x * j);
-        step_temp.y = lines[i].y + (step_y * j);
-        check_and_colour_grid(step_temp, 100);
-      }
-    }
-  }
-
-  /**
-   * @brief Sets entire OccupancyGrid to -1 for unknown value in grid.
-   */
-  void initialize_map()
-  {
-    for (int i = 0; i < int(grid.info.height * grid.info.width); i++)
-    {
-      grid.data.push_back(-1);
-    }
-  }
-
-  /**
-   * @brief Sets the necessary attributes in the occupancy grid to show up in RViz.
-   */
-  void set_grid_attributes()
-  {
-    grid.header.stamp = node_->get_clock()->now();
-    grid.info.resolution = 0.02;
-    grid.info.origin.orientation = createQuaternionMsgFromYaw(0);
-    grid.header.frame_id = "map";
-
-    geometry_msgs::msg::Point origin;
-    origin.x = minx;
-    origin.y = miny;
-    origin.z = 0;
-    grid.info.origin.position = origin;
-
-    grid.info.width = int((maxx - minx) / grid.info.resolution + 1);
-    grid.info.height = int((maxy - miny) / grid.info.resolution + 1);
-  }
-
-  std::vector<geometry_msgs::msg::Point> points;  ///< Map points
-  std::vector<geometry_msgs::msg::Point> lines;   ///< Map lines
-  visualization_msgs::msg::MarkerArray f_areas;   ///< Forbidden areas marker array
-  visualization_msgs::msg::Marker fa;             ///< Single forbidden area marker
-  nav_msgs::msg::OccupancyGrid grid;              ///< Occupancy grid
-  double minx, miny, maxx, maxy;                  ///< Map bounds
-
-  rclcpp::Node::SharedPtr node_;  ///< Node pointer
-  std::string map_file_;          ///< Map file name
-  std::string map_topic_;         ///< Map Topic
-
-  int map_pub_frequency_{ 2 };
-  int map_pub_interval_ms_{ 500 };
-
-  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub;         ///< Publisher for occupancy grid
-  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr fa_pub;  ///< Publisher for forbidden areas
-  rclcpp::TimerBase::SharedPtr timer;                                         ///< Timer for periodic publishing
-};
\ No newline at end of file
diff --git a/amr_core/include/amr_core/interfaces/status_interface.hpp b/amr_core/include/amr_core/interfaces/status_interface.hpp
new file mode 100644
index 00000000..f21db540
--- /dev/null
+++ b/amr_core/include/amr_core/interfaces/status_interface.hpp
@@ -0,0 +1,219 @@
+#pragma once
+
+#include <ArNetworking/ArNetworking.h>
+
+#include <amr_msgs/msg/location.hpp>
+#include <amr_msgs/msg/status.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/battery_state.hpp>
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+#include <memory>
+#include <mutex>
+#include <stdexcept>
+#include <string>
+
+#include "amr_core/utils/libaria_runtime.hpp"
+
+class StatusInterface
+{
+public:
+  explicit StatusInterface(rclcpp::Node::SharedPtr node)
+    : node_(std::move(node))
+    , update_numbers_callback_(this, &StatusInterface::handleUpdateNumbers)
+    , update_strings_callback_(this, &StatusInterface::handleUpdateStrings)
+  {
+  }
+
+  ~StatusInterface()
+  {
+    client_.disconnect();
+  }
+
+  /**
+   * @brief Initializes parameters, publishers, and subscribers.
+   *
+   * This should be called after the node is fully constructed.
+   * @param host Robot server IP address.
+   * @param port Robot server port.
+   * @param user Username for robot server authentication.
+   * @param password Password for robot server authentication.
+   * @param protocol Optional protocol version to enforce on the robot server connection.
+   *
+   * Throws std::runtime_error if connection to the robot server fails or is rejected by the robot.
+   */
+  void initialize(const std::string& host, int port, const std::string& user, const std::string& password,
+                  const std::string& protocol)
+  {
+    host_ = host;
+    port_ = port;
+    user_ = user;
+    password_ = password;
+    protocol_ = protocol;
+
+    topic_name_ = getOrDeclareParameter<std::string>("status.topic", "amr/status");
+    battery_topic_name_ = getOrDeclareParameter<std::string>("status.battery_topic", "battery_state");
+    publish_period_ms_ = getOrDeclareParameter<int>("status.publish_period_ms", 200);
+
+    status_pub_ = node_->create_publisher<amr_msgs::msg::Status>(topic_name_, 10);
+    battery_pub_ = node_->create_publisher<sensor_msgs::msg::BatteryState>(battery_topic_name_, 10);
+
+    connectClient();
+    configureHandlers();
+    client_.runAsync();
+
+    publish_timer_ = node_->create_wall_timer(std::chrono::milliseconds(publish_period_ms_),
+                                              std::bind(&StatusInterface::publishStatus, this));
+  }
+
+private:
+  template <typename T>
+  T getOrDeclareParameter(const std::string& name, const T& default_value)
+  {
+    if (!node_->has_parameter(name))
+    {
+      return node_->declare_parameter<T>(name, default_value);
+    }
+
+    T value = default_value;
+    node_->get_parameter(name, value);
+    return value;
+  }
+
+  void connectClient()
+  {
+    if (!protocol_.empty())
+    {
+      client_.enforceProtocolVersion(protocol_.c_str());
+    }
+
+    const char* password = password_.empty() ? nullptr : password_.c_str();
+    if (!client_.blockingConnect(host_.c_str(), port_, true, user_.c_str(), password))
+    {
+      if (client_.wasRejected())
+      {
+        throw std::runtime_error("Robot rejected the status interface connection.");
+      }
+      throw std::runtime_error("Could not connect status interface to Omron robot server.");
+    }
+
+    client_.setRobotName(host_.c_str());
+    RCLCPP_INFO(node_->get_logger(), "StatusInterface connected to %s:%d using libaria", host_.c_str(), port_);
+  }
+
+  void configureHandlers()
+  {
+    if (client_.dataExists("updateNumbers"))
+    {
+      client_.addHandler("updateNumbers", &update_numbers_callback_);
+      client_.request("updateNumbers", 50);
+    }
+    else
+    {
+      RCLCPP_WARN(node_->get_logger(), "Server does not advertise updateNumbers; status pose fields will stay default");
+    }
+
+    if (client_.dataExists("updateStrings"))
+    {
+      client_.addHandler("updateStrings", &update_strings_callback_);
+      client_.request("updateStrings", -1);
+    }
+    else
+    {
+      RCLCPP_WARN(node_->get_logger(), "Server does not advertise updateStrings; status text fields will stay default");
+    }
+  }
+
+  void handleUpdateNumbers(ArNetPacket* packet)
+  {
+    const double state_of_charge = static_cast<double>(packet->bufToByte2()) / 10.0;
+    const double x = static_cast<double>(packet->bufToByte4());
+    const double y = static_cast<double>(packet->bufToByte4());
+    const double theta = static_cast<double>(packet->bufToByte2());
+    packet->bufToByte2();
+    packet->bufToByte2();
+    packet->bufToByte2();
+    packet->bufToByte();
+
+    std::lock_guard<std::mutex> lock(status_mutex_);
+    latest_status_.state_of_charge = static_cast<float>(state_of_charge);
+    latest_status_.location.x = x;
+    latest_status_.location.y = y;
+    latest_status_.location.theta = theta;
+  }
+
+  void handleUpdateStrings(ArNetPacket* packet)
+  {
+    char status_buffer[256] = { 0 };
+    char extended_buffer[256] = { 0 };
+    packet->bufToStr(status_buffer, sizeof(status_buffer));
+    packet->bufToStr(extended_buffer, sizeof(extended_buffer));
+
+    std::lock_guard<std::mutex> lock(status_mutex_);
+    latest_status_.status = status_buffer;
+    latest_status_.extended_status = extended_buffer;
+  }
+
+  void publishStatus()
+  {
+    if (!status_pub_)
+    {
+      return;
+    }
+
+    amr_msgs::msg::Status msg;
+    {
+      std::lock_guard<std::mutex> lock(status_mutex_);
+      msg = latest_status_;
+    }
+    status_pub_->publish(msg);
+
+    if (battery_pub_)
+    {
+      sensor_msgs::msg::BatteryState battery_msg;
+      battery_msg.header.stamp = node_->now();
+      battery_msg.present = true;
+      battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN;
+      battery_msg.power_supply_health = sensor_msgs::msg::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN;
+      battery_msg.power_supply_technology = sensor_msgs::msg::BatteryState::POWER_SUPPLY_TECHNOLOGY_UNKNOWN;
+      battery_msg.voltage = std::numeric_limits<float>::quiet_NaN();
+      battery_msg.temperature = msg.temperature;
+      battery_msg.current = std::numeric_limits<float>::quiet_NaN();
+      battery_msg.charge = std::numeric_limits<float>::quiet_NaN();
+      battery_msg.capacity = std::numeric_limits<float>::quiet_NaN();
+      battery_msg.design_capacity = std::numeric_limits<float>::quiet_NaN();
+      battery_msg.percentage = std::numeric_limits<float>::quiet_NaN();
+
+      if (std::isfinite(msg.state_of_charge) && msg.state_of_charge >= 0.0f)
+      {
+        battery_msg.percentage = std::clamp(msg.state_of_charge / 100.0f, 0.0f, 1.0f);
+      }
+
+      battery_pub_->publish(battery_msg);
+    }
+  }
+
+  rclcpp::Node::SharedPtr node_;
+  amr_core::LibAriaRuntime aria_runtime_;
+  ArClientBase client_;
+  ArFunctor1C<StatusInterface, ArNetPacket*> update_numbers_callback_;
+  ArFunctor1C<StatusInterface, ArNetPacket*> update_strings_callback_;
+
+  std::mutex status_mutex_;
+  amr_msgs::msg::Status latest_status_;
+
+  std::string host_;
+  int port_{ 7272 };
+  std::string user_;
+  std::string password_;
+  std::string protocol_;
+  std::string topic_name_{ "amr/source/status" };
+  std::string battery_topic_name_{ "battery_state" };
+  int publish_period_ms_{ 200 };
+
+  rclcpp::Publisher<amr_msgs::msg::Status>::SharedPtr status_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr battery_pub_;
+  rclcpp::TimerBase::SharedPtr publish_timer_;
+};
diff --git a/amr_core/include/amr_core/socket/socket_common.hpp b/amr_core/include/amr_core/socket/socket_common.hpp
deleted file mode 100644
index 89e3f474..00000000
--- a/amr_core/include/amr_core/socket/socket_common.hpp
+++ /dev/null
@@ -1,111 +0,0 @@
-#pragma once
-#include <string>
-#include <vector>
-#include <cerrno>
-#include <cstring>
-#include <unistd.h>
-#include <fcntl.h>
-#include <sys/socket.h>
-
-#include "amr_core/utils/amr_exception.hpp"
-
-namespace amr::net
-{
-
-// Set non-blocking on a fd
-inline void set_nonblocking(int fd)
-{
-  int flags = fcntl(fd, F_GETFL, 0);
-  if (flags < 0 || fcntl(fd, F_SETFL, flags | O_NONBLOCK) < 0)
-  {
-    throw amr_exception(std::string("fcntl(O_NONBLOCK) failed: ") + std::strerror(errno));
-  }
-}
-
-// Close fd if valid and reset ref
-inline void close_fd(int& fd)
-{
-  if (fd >= 0)
-  {
-    ::shutdown(fd, SHUT_RDWR);
-    ::close(fd);
-    fd = -1;
-  }
-}
-
-// Read SO_ERROR from a connected socket
-inline int get_so_error(int fd)
-{
-  int err = 0;
-  socklen_t len = sizeof(err);
-  if (::getsockopt(fd, SOL_SOCKET, SO_ERROR, &err, &len) < 0)
-    return errno;
-  return err;
-}
-
-// Drain recv into buffer (append), non-blocking; returns true if peer closed
-inline bool recv_append_nb(int fd, std::string& buf)
-{
-  char tmp[4096];
-  for (;;)
-  {
-    ssize_t n = ::recv(fd, tmp, sizeof(tmp), 0);
-    if (n > 0)
-    {
-      buf.append(tmp, static_cast<size_t>(n));
-    }
-    else if (n == 0)
-    {
-      return true;  // peer closed
-    }
-    else
-    {
-      if (errno == EAGAIN || errno == EWOULDBLOCK)
-        return false;
-      throw amr_exception(std::string("recv() failed: ") + std::strerror(errno));
-    }
-  }
-}
-
-// Send from buffer, erase sent prefix, non-blocking; returns false if would-block
-inline bool send_erase_nb(int fd, std::string& buf)
-{
-  while (!buf.empty())
-  {
-    ssize_t n = ::send(fd, buf.data(), buf.size(), 0);
-    if (n > 0)
-    {
-      buf.erase(0, static_cast<size_t>(n));
-    }
-    else if (n < 0)
-    {
-      if (errno == EAGAIN || errno == EWOULDBLOCK)
-        return false;
-      throw amr_exception(std::string("send() failed: ") + std::strerror(errno));
-    }
-    else
-    {
-      // n == 0: treat as closed
-      throw amr_exception("send() returned 0 (connection closed)");
-    }
-  }
-  return true;
-}
-
-// Extract CRLF-terminated lines, without CRLF
-inline size_t extract_crlf_lines(std::string& buf, std::vector<std::string>& out)
-{
-  size_t count = 0;
-  for (;;)
-  {
-    auto pos = buf.find("\r\n");
-    if (pos == std::string::npos)
-      break;
-    out.emplace_back(buf.substr(0, pos));
-    buf.erase(0, pos + 2);
-    ++count;
-  }
-  return count;
-}
-
-}  // namespace amr::net
\ No newline at end of file
diff --git a/amr_core/include/amr_core/socket/socket_driver.hpp b/amr_core/include/amr_core/socket/socket_driver.hpp
deleted file mode 100644
index a5e735c2..00000000
--- a/amr_core/include/amr_core/socket/socket_driver.hpp
+++ /dev/null
@@ -1,541 +0,0 @@
-#pragma once
-
-#include <string>
-#include <vector>
-#include <chrono>
-#include <cerrno>
-#include <cstring>
-#include <mutex>
-#include <condition_variable>
-#include <unordered_map>
-#include <memory>
-#include <queue>
-
-#include <unistd.h>
-#include <fcntl.h>
-#include <poll.h>
-#include <sys/socket.h>
-#include <netinet/in.h>
-#include <arpa/inet.h>
-
-#include <rclcpp/rclcpp.hpp>
-#include "amr_core/utils/amr_exception.hpp"
-#include "amr_core/socket/socket_common.hpp"
-
-/**
- * @brief Non-blocking TCP socket driver for simple client/server use.
- *
- * - Client mode: connect() to host:port (non-blocking, EINPROGRESS-safe).
- * - Server mode: begin_listen() binds, listens, and accepts a single client.
- * - Poll-based I/O with internal send/receive buffers.
- * - CRLF line helpers: send_line(), read_lines().
- * - rclcpp logging; errors throw amr_exception.
- */
-class SocketDriver
-{
-public:
-  /// Receive buffer chunk size
-  static constexpr size_t RECV_CHUNK = 4096;
-
-  /// Operating mode of the driver
-  enum class Mode
-  {
-    None,
-    Client,
-    Server
-  };
-
-  struct CommandEntry
-  {
-    int id;
-    std::string command;
-    std::string line_identifier;
-    std::string response;
-    bool newLine{ false };
-    bool done{ false };
-  };
-
-  /**
-   * @brief Construct a new SocketDriver.
-   * @param node ROS 2 node for logging.
-   */
-  explicit SocketDriver(const rclcpp::Node::SharedPtr& node) : node_(node)
-  {
-  }
-
-  /**
-   * @brief Destructor closes any open sockets.
-   */
-  ~SocketDriver()
-  {
-    close();
-  }
-
-  /**
-   * @brief Handle readiness flags from poll().
-   * @param revents Returned event mask.
-   * @throws amr_exception on socket error conditions.
-   */
-  void process_events(short revents)
-  {
-    if (revents & (POLLERR | POLLHUP | POLLNVAL))
-    {
-      int soerr = amr::net::get_so_error(sock_fd_);
-      std::string msg = "socket error: " + std::string(std::strerror(soerr));
-      close();
-      throw amr_exception(msg);
-    }
-
-    if (connecting_ && (revents & POLLOUT))
-    {
-      int soerr = amr::net::get_so_error(sock_fd_);
-      if (soerr != 0)
-      {
-        std::string msg = "connect completion error: " + std::string(std::strerror(soerr));
-        close();
-        throw amr_exception(msg);
-      }
-      connecting_ = false;
-      RCLCPP_INFO(node_->get_logger(), "Connect completed");
-    }
-
-    if (revents & POLLIN)
-      read_ready();
-    if (revents & POLLOUT)
-      write_ready();
-  }
-
-  /**
-   * @brief Initiate a non-blocking client connection to addr:port (IPv4).
-   * @param addr Dotted-quad IP (e.g., "127.0.0.1").
-   * @param port TCP port.
-   * @return true if connect initiated (connected or in progress).
-   * @throws amr_exception on socket creation or configuration failure.
-   *
-   * If connect returns EINPROGRESS, completion is detected on POLLOUT in poll_once().
-   */
-  bool connect(const std::string& addr, uint16_t port)
-  {
-    close();
-    mode_ = Mode::Client;
-
-    RCLCPP_INFO(node_->get_logger(), "Connecting to %s:%u", addr.c_str(), port);
-
-    sock_fd_ = ::socket(AF_INET, SOCK_STREAM, 0);
-    if (sock_fd_ < 0)
-    {
-      throw amr_exception(std::string("socket() failed: ") + std::strerror(errno));
-    }
-
-    // Non-blocking
-    amr::net::set_nonblocking(sock_fd_);
-
-    sockaddr_in sa{};
-    sa.sin_family = AF_INET;
-    sa.sin_port = htons(port);
-    if (::inet_pton(AF_INET, addr.c_str(), &sa.sin_addr) != 1)
-    {
-      close();
-      throw amr_exception("Invalid IPv4 address: " + addr);
-    }
-
-    int rc = ::connect(sock_fd_, reinterpret_cast<sockaddr*>(&sa), sizeof(sa));
-    if (rc == 0)
-    {
-      connecting_ = false;
-      RCLCPP_INFO(node_->get_logger(), "Connected immediately");
-      return true;
-    }
-    if (rc < 0 && errno == EINPROGRESS)
-    {
-      connecting_ = true;
-      RCLCPP_DEBUG(node_->get_logger(), "Connect in progress (EINPROGRESS)");
-      return true;
-    }
-
-    int err = errno;
-    close();
-    throw amr_exception(std::string("connect() failed: ") + std::strerror(err));
-  }
-
-  /**
-   * @brief Send password and wait for "End of commands".
-   * @param passwd Password to send (appends CRLF).
-   * @throws amr_exception on I/O failure.
-   */
-  void login(const std::string& passwd)
-  {
-    send_line(passwd, true);
-    wait_until_login();
-  }
-
-  /**
-   * @brief Block until "End of commands" is received.
-   * @throws amr_exception on I/O failure or timeout (15s).
-   *
-   * This method polls the socket every 100ms, reading any available data
-   * and checking for the login completion string.
-   */
-  void wait_until_login()
-  {
-    auto start = std::chrono::steady_clock::now();
-    std::vector<std::string> lines;
-    for (;;)
-    {
-      poll_once(100);
-      lines.clear();
-      read_lines(lines);
-      for (const auto& ln : lines)
-      {
-        if (ln.find("End of commands") != std::string::npos)
-          return;
-      }
-      if (std::chrono::steady_clock::now() - start > std::chrono::seconds(15))
-      {
-        throw amr_exception("Login timeout");
-      }
-    }
-  }
-
-  /**
-   * @brief Bind, listen, and accept a single client (server mode).
-   * @param addr Bind address (e.g., "127.0.0.1" or "0.0.0.0").
-   * @param port TCP port to bind.
-   * @param backlog Listen backlog (default 1).
-   * @return true on success.
-   * @throws amr_exception on any socket error.
-   *
-   * This method blocks in accept() until a client connects, then sets the
-   * accepted socket to non-blocking mode and closes the listening socket.
-   */
-  bool begin_listen(const std::string& addr, uint16_t port, int backlog = 1)
-  {
-    close();
-    mode_ = Mode::Server;
-
-    RCLCPP_INFO(node_->get_logger(), "Listening on %s:%u", addr.c_str(), port);
-
-    listen_fd_ = ::socket(AF_INET, SOCK_STREAM, 0);
-    if (listen_fd_ < 0)
-    {
-      throw amr_exception(std::string("socket() failed: ") + std::strerror(errno));
-    }
-
-    int yes = 1;
-    if (::setsockopt(listen_fd_, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(yes)) < 0)
-    {
-      int err = errno;
-      close();
-      throw amr_exception(std::string("setsockopt(SO_REUSEADDR) failed: ") + std::strerror(err));
-    }
-
-    sockaddr_in sa{};
-    sa.sin_family = AF_INET;
-    sa.sin_port = htons(port);
-    if (::inet_pton(AF_INET, addr.c_str(), &sa.sin_addr) != 1)
-    {
-      close();
-      throw amr_exception("Invalid bind address: " + addr);
-    }
-
-    if (::bind(listen_fd_, reinterpret_cast<sockaddr*>(&sa), sizeof(sa)) < 0)
-    {
-      int err = errno;
-      close();
-      throw amr_exception(std::string("bind() failed: ") + std::strerror(err));
-    }
-
-    if (::listen(listen_fd_, backlog) < 0)
-    {
-      int err = errno;
-      close();
-      throw amr_exception(std::string("listen() failed: ") + std::strerror(err));
-    }
-
-    sockaddr_in peer{};
-    socklen_t plen = sizeof(peer);
-    sock_fd_ = ::accept(listen_fd_, reinterpret_cast<sockaddr*>(&peer), &plen);
-    if (sock_fd_ < 0)
-    {
-      int err = errno;
-      close();
-      throw amr_exception(std::string("accept() failed: ") + std::strerror(err));
-    }
-
-    // We only serve one client; close the listener
-    amr::net::close_fd(listen_fd_);
-
-    // Non-blocking on accepted socket
-    amr::net::set_nonblocking(sock_fd_);
-
-    connecting_ = false;
-
-    char addrbuf[64]{};
-    ::inet_ntop(AF_INET, &peer.sin_addr, addrbuf, sizeof(addrbuf));
-    RCLCPP_INFO(node_->get_logger(), "Accepted client %s:%u", addrbuf, ntohs(peer.sin_port));
-    return true;
-  }
-
-  /**
-   * @brief Poll for readiness and process I/O once.
-   * @param timeout_ms Timeout in milliseconds (-1 to block).
-   * @param extra_events Additional events to poll for (default POLLIN|POLLOUT).
-   * @return true if any events processed; false on timeout.
-   * @throws amr_exception on poll or I/O error.
-   */
-  bool poll_once(int timeout_ms, short extra_events = (POLLIN | POLLOUT))
-  {
-    if (sock_fd_ < 0)
-      return false;
-
-    struct pollfd pfd{};
-    pfd.fd = sock_fd_;
-    pfd.events = POLLIN;
-    if (!send_buf_.empty() || connecting_)
-      pfd.events |= POLLOUT;
-    pfd.events |= (extra_events & ~(POLLIN | POLLOUT));  // allow caller extras
-
-    int rc = ::poll(&pfd, 1, timeout_ms);
-    if (rc < 0)
-    {
-      throw amr_exception(std::string("poll() failed: ") + std::strerror(errno));
-    }
-    if (rc == 0)
-      return false;
-
-    process_events(pfd.revents);
-    return true;
-  }
-
-  /**
-   * @brief Queue raw bytes for sending.
-   * @param data Pointer to bytes.
-   * @param len Number of bytes.
-   */
-  void send_bytes(const void* data, size_t len)
-  {
-    const char* p = static_cast<const char*>(data);
-    send_buf_.append(p, p + len);
-  }
-
-  /**
-   * @brief Queue a CRLF-terminated line for sending.
-   * @param s Line content (CRLF appended).
-   * @param newline If true, append CRLF (default true).
-   */
-  void send_line(const std::string& s, bool newline = true)
-  {
-    send_buf_.append(s);
-    if (newline)
-      send_buf_.append("\r\n");
-  }
-
-  /**
-   * @brief Extract complete CRLF-terminated lines from the receive buffer.
-   * @param out_lines Appended with parsed lines (without CRLF).
-   * @return number of lines extracted.
-   */
-  size_t read_lines(std::vector<std::string>& out_lines)
-  {
-    return amr::net::extract_crlf_lines(recv_buf_, out_lines);
-  }
-
-  /**
-   * @brief Close the active (and listening) sockets and reset state.
-   */
-  void close()
-  {
-    amr::net::close_fd(sock_fd_);
-    amr::net::close_fd(listen_fd_);
-    connecting_ = false;
-    send_buf_.clear();
-    // keep any partial recv_buf_ for inspection if desired
-    mode_ = Mode::None;
-  }
-
-  /**
-   * @brief Check if the data socket is open.
-   */
-  bool is_open() const
-  {
-    return sock_fd_ >= 0;
-  }
-
-  /**
-   * @brief Check if the client connection is still in progress.
-   */
-  bool is_connecting() const
-  {
-    return connecting_;
-  }
-
-  /**
-   * @brief Current mode.
-   */
-  Mode mode() const
-  {
-    return mode_;
-  }
-
-  // --- ARCL API Service Command Queue ---
-  /**
-   * @brief Send a command line immediately to the socket (no response wait).
-   * @param command Command string to send.
-   * @param newline If true, appends CRLF.
-   */
-  void send_command(const std::string& command, bool newline = true)
-  {
-    send_line(command, newline);
-
-    // flush the buffer immediately:
-    poll_once(0);
-  }
-
-  /**
-   * @brief Queue a command for sending and response matching.
-   * @param command Command string to send.
-   * @param line_identifier Substring to match in response line.
-   * @param newline If true, appends CRLF.
-   * @return unique command ID for wait_for_response().
-   *
-   * The command is queued for sending; the next available command in the
-   * queue is sent on each poll_and_process() call. Responses are matched
-   * by searching for line_identifier in received lines.
-   */
-  int queue_command(const std::string& command, const std::string& line_identifier, bool newline = true)
-  {
-    std::lock_guard<std::mutex> lock(queue_mutex_);
-
-    int current_id = 0;
-    for (const auto& id : id_tracker_)
-    {
-      if (current_id == id)
-        current_id += 1;
-      else
-        break;
-    }
-
-    id_tracker_.push_back(current_id);
-    std::sort(id_tracker_.begin(), id_tracker_.end());
-
-    auto entry = std::make_shared<CommandEntry>();
-    entry->id = current_id;
-    entry->command = command;
-    entry->line_identifier = line_identifier;
-    entry->newLine = newline;
-    command_map_[current_id] = entry;
-    command_queue_.push(entry);
-    return current_id;
-  }
-
-  /**
-   * @brief Wait for a queued command's response line or timeout.
-   * @param id Command ID returned by queue_command().
-   * @param response On success, set to the matched response line.
-   * @param timeout_ms Timeout in milliseconds.
-   * @return true if response received; false on timeout.
-   *
-   * This method polls and processes I/O until the command's response
-   * is received or the timeout expires. On success, the matched response
-   * line is returned (without CRLF) via the response parameter.
-   */
-  bool wait_for_response(int id, std::string& response, int timeout_ms)
-  {
-    auto start = std::chrono::steady_clock::now();
-    std::unique_lock<std::mutex> lock(queue_mutex_);
-    while (true)
-    {
-      if (command_map_.count(id) && command_map_[id]->done)
-      {
-        response = command_map_[id]->response;
-        command_map_.erase(id);
-        return true;
-      }
-      lock.unlock();
-      poll_and_process();
-      lock.lock();
-      auto now = std::chrono::steady_clock::now();
-      if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() > timeout_ms)
-        break;
-      std::this_thread::sleep_for(std::chrono::milliseconds(20));
-    }
-    command_map_.erase(id);
-    return false;
-  }
-
-private:
-  void read_ready()
-  {
-    if (sock_fd_ < 0)
-      return;
-    bool peer_closed = amr::net::recv_append_nb(sock_fd_, recv_buf_);
-    if (peer_closed)
-    {
-      RCLCPP_INFO(node_->get_logger(), "Peer closed connection");
-      close();
-    }
-  }
-
-  void write_ready()
-  {
-    if (sock_fd_ < 0 || send_buf_.empty() || connecting_)
-      return;
-    // send_erase_nb returns false when EAGAIN/EWOULDBLOCK; just try later
-    (void)amr::net::send_erase_nb(sock_fd_, send_buf_);
-  }
-
-  void poll_and_process()
-  {
-    // Send next command if available
-    {
-      std::lock_guard<std::mutex> lock(queue_mutex_);
-      if (!command_queue_.empty())
-      {
-        auto entry = command_queue_.front();
-        send_line(entry->command, entry->newLine);
-        command_queue_.pop();
-      }
-    }
-
-    // Poll socket and process events
-    poll_once(20);
-
-    // Read lines and match responses
-    std::vector<std::string> lines;
-    read_lines(lines);
-
-    std::lock_guard<std::mutex> lock(queue_mutex_);
-    for (auto& [id, entry] : command_map_)
-    {
-      if (!entry->done)
-      {
-        for (const auto& line : lines)
-        {
-          if (line.find(entry->line_identifier) != std::string::npos)
-          {
-            entry->response = line;
-            entry->done = true;
-            break;
-          }
-        }
-      }
-    }
-  }
-
-private:
-  rclcpp::Node::SharedPtr node_;
-  Mode mode_{ Mode::None };
-  int listen_fd_{ -1 };
-  int sock_fd_{ -1 };
-  bool connecting_{ false };
-
-  std::string recv_buf_;
-  std::string send_buf_;
-
-  // --- ARCL API Service Command Queue ---
-  std::mutex queue_mutex_;
-  std::vector<int> id_tracker_;
-  std::unordered_map<int, std::shared_ptr<CommandEntry>> command_map_;
-  std::queue<std::shared_ptr<CommandEntry>> command_queue_;
-  int next_id_{ 1 };
-};
\ No newline at end of file
diff --git a/amr_core/include/amr_core/socket/socket_listener.hpp b/amr_core/include/amr_core/socket/socket_listener.hpp
deleted file mode 100644
index 25597695..00000000
--- a/amr_core/include/amr_core/socket/socket_listener.hpp
+++ /dev/null
@@ -1,383 +0,0 @@
-#pragma once
-
-#include <string>
-#include <vector>
-#include <unordered_map>
-#include <mutex>
-#include <cstring>
-#include <thread>
-#include <chrono>
-
-#include <poll.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <sys/socket.h>
-#include <netinet/in.h>
-#include <arpa/inet.h>
-
-#include <rclcpp/rclcpp.hpp>
-
-#include "amr_core/utils/amr_exception.hpp"
-#include "amr_core/socket/socket_common.hpp"
-
-class SocketListener
-{
-public:
-  static constexpr size_t RECV_BUFFER = 4096;
-
-  /**
-   * @brief Construct a new Socket Listener object
-   * @param node Shared pointer to the ROS2 node for logging
-   * @param addr Address to bind the socket to (e.g., "0.0.0.0")
-   * @param port Port number to listen on
-   */
-  explicit SocketListener(const rclcpp::Node::SharedPtr& node, const std::string& addr, int port)
-    : node_(node), addr_(addr), port_(port)
-  {
-  }
-
-  // Destructor closes any open sockets
-  ~SocketListener()
-  {
-    close();
-  }
-
-  /**
-   * @brief Begin listening for incoming socket connections
-   * @return true if successfully listening and accepted a connection
-   * @throws amr_exception on socket errors
-   *
-   * This method creates a socket, sets the SO_REUSEADDR option, and binds to the specified address and port. If
-   * the bind fails, it retries every 3 seconds until successful. Once bound, it listens for incoming connections
-   * and accepts a single client connection. The accepted socket is set to non-blocking mode. If any socket operation
-   * fails, an amr_exception is thrown with an appropriate error message.
-   */
-  bool begin()
-  {
-    RCLCPP_INFO(node_->get_logger(), "Attempt to listen for incoming on %s at port %d", addr_.c_str(), port_);
-
-    // Create, set SO_REUSEADDR, and bind (retry bind on failure)
-    for (;;)
-    {
-      listen_fd_ = ::socket(AF_INET, SOCK_STREAM, 0);
-      if (listen_fd_ < 0)
-      {
-        throw amr_exception(std::string("socket() failed: ") + std::strerror(errno));
-      }
-
-      int yes = 1;
-      if (::setsockopt(listen_fd_, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(yes)) < 0)
-      {
-        amr::net::close_fd(listen_fd_);
-        throw amr_exception(std::string("setsockopt(SO_REUSEADDR) failed: ") + std::strerror(errno));
-      }
-
-      sockaddr_in sa{};
-      sa.sin_family = AF_INET;
-      sa.sin_port = htons(static_cast<uint16_t>(port_));
-      if (::inet_pton(AF_INET, addr_.c_str(), &sa.sin_addr) != 1)
-      {
-        amr::net::close_fd(listen_fd_);
-        throw amr_exception("Invalid listen address: " + addr_);
-      }
-
-      if (::bind(listen_fd_, reinterpret_cast<sockaddr*>(&sa), sizeof(sa)) < 0)
-      {
-        RCLCPP_INFO(node_->get_logger(), "Socket listener connection failed: %s", std::strerror(errno));
-        RCLCPP_INFO(node_->get_logger(), "Retrying");
-        amr::net::close_fd(listen_fd_);
-        std::this_thread::sleep_for(std::chrono::seconds(3));
-        continue;
-      }
-      break;
-    }
-
-    if (::listen(listen_fd_, 1) < 0)
-    {
-      amr::net::close_fd(listen_fd_);
-      throw amr_exception(std::string("listen() failed: ") + std::strerror(errno));
-    }
-
-    sockaddr_in peer{};
-    socklen_t plen = sizeof(peer);
-    conn_fd_ = ::accept(listen_fd_, reinterpret_cast<sockaddr*>(&peer), &plen);
-    if (conn_fd_ < 0)
-    {
-      amr::net::close_fd(listen_fd_);
-      throw amr_exception(std::string("accept() failed: ") + std::strerror(errno));
-    }
-
-    // Close the listening socket (single-connection behavior to match Python)
-    amr::net::close_fd(listen_fd_);
-
-    char addrbuf[64]{};
-    ::inet_ntop(AF_INET, &peer.sin_addr, addrbuf, sizeof(addrbuf));
-    RCLCPP_INFO(node_->get_logger(), "Listening to new socket on %s at port %d", addrbuf,
-                static_cast<int>(ntohs(peer.sin_port)));
-
-    // Make the accepted socket non-blocking
-    amr::net::set_nonblocking(conn_fd_);
-
-    return true;
-  }
-
-  /**
-   * @brief Poll the socket for incoming data once
-   * @param timeout_ms Timeout in milliseconds for the poll operation
-   * @return true if data was received and processed, false if timeout occurred
-   * @throws amr_exception on poll errors
-   *
-   * This method uses the poll() system call to check for incoming data on the connected socket. If data is
-   * available, it reads the data and processes it. If the poll operation times out without any events, it
-   * returns false. If an error occurs during the poll operation, an amr_exception is thrown with an appropriate
-   * error message.
-   */
-  bool poll_once(int timeout_ms)
-  {
-    if (conn_fd_ < 0)
-      return false;
-
-    struct pollfd pfd{};
-    pfd.fd = conn_fd_;
-    pfd.events = POLLIN;
-
-    int rc = ::poll(&pfd, 1, timeout_ms);
-    if (rc < 0)
-    {
-      throw amr_exception(std::string("poll() failed: ") + std::strerror(errno));
-    }
-    if (rc == 0)
-      return false;
-
-    process_events(pfd.revents);
-    return true;
-  }
-
-  /**
-   * @brief Process events indicated by poll()
-   * @param revents The event flags returned by poll()
-   */
-  void process_events(short revents)
-  {
-    if ((revents & POLLIN) != 0)
-    {
-      _read();
-    }
-    sort_data();
-  }
-
-  /**
-   * @brief Close any open sockets and clean up resources
-   *
-   * This method shuts down and closes both the connection and listening sockets if they are open. It also
-   * resets their file descriptors to -1.
-   */
-  void close()
-  {
-    std::lock_guard<std::mutex> lk(mtx_);
-    amr::net::close_fd(conn_fd_);
-    amr::net::close_fd(listen_fd_);
-  }
-
-  /**
-   * @brief Retrieve stored responses for a given key
-   * @param key The key for which to retrieve responses
-   * @return A reference to a vector of response strings associated with the key
-   * @throws std::out_of_range if the key does not exist or has no associated responses
-   *
-   * This method looks up the provided key in the internal response storage and returns a reference to the
-   * vector of response strings associated with that key. If the key is not found or if there are no responses
-   * stored for that key, it throws a std::out_of_range exception.
-   */
-  const std::vector<std::string>& get_response(const std::string& key) const
-  {
-    std::lock_guard<std::mutex> lk(mtx_);
-    auto it = responses_.find(key);
-    if (it == responses_.end() || it->second.empty())
-    {
-      throw std::out_of_range("No response for key: " + key);
-    }
-    return it->second;
-  }
-
-  /**
-   * @brief Public method to read data from the socket
-   */
-  void read()
-  {
-    _read();
-  }
-
-private:
-  /**
-   * @brief Read incoming data from the socket and append to the receive buffer
-   * @throws amr_exception on recv errors
-   *
-   * Uses amr::net::recv_append_nb to fetch bytes into a temporary buffer, then
-   * appends to the guarded recv_buffer_. Closes on peer shutdown.
-   */
-  void _read()
-  {
-    if (conn_fd_ < 0)
-      return;
-
-    std::string tmp;
-    bool peer_closed = amr::net::recv_append_nb(conn_fd_, tmp);
-    if (!tmp.empty())
-    {
-      std::lock_guard<std::mutex> lk(mtx_);
-      recv_buffer_.append(tmp);
-    }
-    if (peer_closed)
-    {
-      close();
-    }
-  }
-
-  /**
-   * @brief Trim leading and trailing whitespace from a string
-   * @param s The input string to trim
-   * @return A new string with leading and trailing whitespace removed
-   */
-  static std::string trim_copy(const std::string& s)
-  {
-    size_t start = s.find_first_not_of(" \t\r\n");
-    if (start == std::string::npos)
-      return {};
-    size_t end = s.find_last_not_of(" \t\r\n");
-    return s.substr(start, end - start + 1);
-  }
-
-  /**
-   * @brief Parse complete lines from the receive buffer and store key-value pairs
-   *
-   * Extracts complete CRLF lines using amr::net::extract_crlf_lines while
-   * holding the buffer lock, then parses and stores them.
-   */
-  void sort_data()
-  {
-    for (;;)
-    {
-      std::vector<std::string> lines;
-      {
-        std::lock_guard<std::mutex> lk(mtx_);
-        if (amr::net::extract_crlf_lines(recv_buffer_, lines) == 0)
-        {
-          return;
-        }
-      }
-      for (const auto& line : lines)
-      {
-        // Parse "Key: value" or bare key
-        std::string key, value;
-        auto colon = line.find(':');
-        if (colon == std::string::npos)
-        {
-          key = line;
-          store(key, std::string{});
-        }
-        else
-        {
-          key = line.substr(0, colon);
-          value = trim_copy(line.substr(colon + 1));
-          store(key, value);
-        }
-      }
-    }
-  }
-
-  /**
-   * @brief Store a key-value pair in the response storage with special handling for certain keys
-   * @param key The key to store
-   * @param value The value associated with the key
-   */
-  void store(const std::string& key, const std::string& value)
-  {
-    std::lock_guard<std::mutex> lk(mtx_);
-
-    if (key == "Goal")
-    {
-      if (goal_f_)
-      {
-        responses_[key].push_back(value);
-      }
-      else
-      {
-        responses_[key] = { value };
-        goal_f_ = true;
-      }
-    }
-    else if (key == "End of goals")
-    {
-      goal_f_ = false;
-    }
-    else if (key == "ApplicationFaultQuery")
-    {
-      if (app_fault_f_)
-      {
-        responses_[key].push_back(value);
-      }
-      else
-      {
-        responses_[key] = { value };
-        app_fault_f_ = true;
-      }
-    }
-    else if (key == "End of ApplicationFaultQuery")
-    {
-      app_fault_f_ = false;
-    }
-    else if (key == "FaultList")
-    {
-      if (faults_get_f_)
-      {
-        responses_[key].push_back(value);
-      }
-      else
-      {
-        responses_[key] = { value };
-        faults_get_f_ = true;
-      }
-    }
-    else if (key == "End of FaultList")
-    {
-      faults_get_f_ = false;
-    }
-    else if (key == "RobotFaultQuery")
-    {
-      if (query_faults_f_)
-      {
-        responses_[key].push_back(value);
-      }
-      else
-      {
-        responses_[key] = { value };
-        query_faults_f_ = true;
-      }
-    }
-    else if (key == "EndQueryFaults")
-    {
-      query_faults_f_ = false;
-    }
-    else
-    {
-      responses_[key] = { value };
-    }
-  }
-
-private:
-  rclcpp::Node::SharedPtr node_;
-  int listen_fd_{ -1 };
-  int conn_fd_{ -1 };
-  std::string addr_;
-  int port_{ 0 };
-
-  mutable std::mutex mtx_;
-  std::string recv_buffer_;
-  std::unordered_map<std::string, std::vector<std::string>> responses_;
-
-  bool goal_f_{ false };
-  bool app_fault_f_{ false };
-  bool faults_get_f_{ false };
-  bool query_faults_f_{ false };
-};
\ No newline at end of file
diff --git a/amr_core/include/amr_core/socket/socket_taskmaster.hpp b/amr_core/include/amr_core/socket/socket_taskmaster.hpp
deleted file mode 100644
index db43675b..00000000
--- a/amr_core/include/amr_core/socket/socket_taskmaster.hpp
+++ /dev/null
@@ -1,410 +0,0 @@
-#pragma once
-
-#include <string>
-#include <vector>
-#include <algorithm>
-#include <cerrno>
-#include <cstring>
-
-#include <unistd.h>
-#include <fcntl.h>
-#include <poll.h>
-#include <sys/socket.h>
-#include <arpa/inet.h>
-
-#include <rclcpp/rclcpp.hpp>
-
-#include "amr_core/utils/amr_exception.hpp"
-#include "amr_core/socket/socket_common.hpp"
-
-/**
- * @brief Non-blocking TCP client for ARCL-like command/response workflows.
- *
- * SocketTaskmaster manages a single TCP connection using poll-based I/O.
- * It supports sending a command (optionally CRLF-terminated), collecting
- * feedback lines (CRLF-terminated), and detecting completion by matching
- * one or more configured end-line substrings.
- *
- * Uses rclcpp for logging and throws amr_exception on errors.
- */
-class SocketTaskmaster
-{
-public:
-  static constexpr size_t RECV_BUFFER = 4096;
-
-  /**
-   * @brief Construct a new SocketTaskmaster
-   * @param node rclcpp node for logging
-   */
-  explicit SocketTaskmaster(const rclcpp::Node::SharedPtr& node) : node_(node)
-  {
-  }
-
-  /**
-   * @brief Destructor (closes the socket if open)
-   */
-  ~SocketTaskmaster()
-  {
-    close_socket();
-  }
-
-  /**
-   * @brief Connect to addr:port using a non-blocking socket (IPv4).
-   * @param addr Dotted-quad IP (e.g., "127.0.0.1")
-   * @param port TCP port
-   * @return true if connect started (connected or EINPROGRESS)
-   * @throws amr_exception on socket setup failures
-   *
-   * If connect() returns EINPROGRESS, completion is finalized on POLLOUT
-   * inside process_events() via SO_ERROR.
-   */
-  bool connect(const std::string& addr, int port)
-  {
-    close_socket();
-    connecting_ = false;
-
-    RCLCPP_INFO(node_->get_logger(), "Starting connection to %s:%d", addr.c_str(), port);
-
-    sock_fd_ = ::socket(AF_INET, SOCK_STREAM, 0);
-    if (sock_fd_ < 0)
-    {
-      throw amr_exception(std::string("socket() failed: ") + std::strerror(errno));
-    }
-
-    amr::net::set_nonblocking(sock_fd_);
-
-    sockaddr_in sa{};
-    sa.sin_family = AF_INET;
-    sa.sin_port = htons(static_cast<uint16_t>(port));
-    if (::inet_pton(AF_INET, addr.c_str(), &sa.sin_addr) != 1)
-    {
-      int err = errno;
-      close_socket();
-      throw amr_exception(std::string("inet_pton failed: ") + std::strerror(err));
-    }
-
-    int rc = ::connect(sock_fd_, reinterpret_cast<sockaddr*>(&sa), sizeof(sa));
-    if (rc == 0)
-    {
-      connecting_ = false;
-      RCLCPP_INFO(node_->get_logger(), "Connected immediately");
-      return true;
-    }
-    if (rc < 0 && errno == EINPROGRESS)
-    {
-      connecting_ = true;
-      RCLCPP_DEBUG(node_->get_logger(), "Connect in progress (EINPROGRESS)");
-      return true;
-    }
-
-    int err = errno;
-    close_socket();
-    throw amr_exception(std::string("connect() failed: ") + std::strerror(err));
-  }
-
-  /**
-   * @brief Send password and wait for "End of commands".
-   * @param passwd Password to send (appends CRLF).
-   * @throws amr_exception on I/O failure.
-   */
-  void login(const std::string& passwd)
-  {
-    push_command(passwd, /*newline=*/true, /*end_lines=*/{ "End of commands" });
-    wait_until_login();
-  }
-
-  /**
-   * @brief Process ready I/O events indicated by poll().
-   * @param revents Bitmask of returned poll events.
-   * @throws amr_exception on socket errors or I/O failures.
-   */
-  void process_events(short revents)
-  {
-    if (revents & (POLLERR | POLLHUP | POLLNVAL))
-    {
-      int soerr = amr::net::get_so_error(sock_fd_);
-      std::string msg = "socket error: " + std::string(std::strerror(soerr));
-      close_socket();
-      throw amr_exception(msg);
-    }
-
-    if (connecting_ && (revents & POLLOUT))
-    {
-      int soerr = amr::net::get_so_error(sock_fd_);
-      if (soerr != 0)
-      {
-        std::string msg = "connect completion error: " + std::string(std::strerror(soerr));
-        close_socket();
-        throw amr_exception(msg);
-      }
-      connecting_ = false;
-      RCLCPP_INFO(node_->get_logger(), "Connect completed");
-    }
-
-    if (revents & POLLIN)
-      read();
-    if (revents & POLLOUT)
-      write();
-  }
-
-  /**
-   * @brief Queue a command to send and configure completion criteria.
-   * @param command Command content to send.
-   * @param newline If true, appends "\r\n" to the command.
-   * @param end_lines List of substrings that indicate command completion if seen in any parsed line.
-   *
-   * A leading "\r\n" is prepended to mimic the original Python behavior.
-   * Any currently available incoming bytes are drained first.
-   */
-  void push_command(const std::string& command, bool newline = false, const std::vector<std::string>& end_lines = {})
-  {
-    std::string cmd = command;
-    if (newline)
-      cmd += "\r\n";
-
-    _recv_buffer.clear();
-    _send_buffer = "\r\n" + cmd;  // send a blank line first
-    _check_end = end_lines;
-    _result.clear();
-    _feedback.clear();
-
-    clear_recv_buf();  // drain stale incoming data
-  }
-
-  /**
-   * @brief Result struct returned by wait_command().
-   */
-  struct WaitResult
-  {
-    bool complete;         ///< True if a completion line was matched
-    std::string result;    ///< The matched line (without CRLF)
-    std::string feedback;  ///< Accumulated feedback lines joined with CRLF
-  };
-
-  /**
-   * @brief Poll once for I/O and process data.
-   * @param poll_timeout_ms Timeout in milliseconds (-1 blocks indefinitely).
-   * @return WaitResult with complete=true when an end_line is matched.
-   * @throws amr_exception on poll or I/O failure.
-   *
-   * When complete, the result and feedback are returned and cleared internally.
-   */
-  WaitResult wait_command(int poll_timeout_ms = -1)
-  {
-    if (!is_open())
-    {
-      return { false, "", _feedback };
-    }
-
-    struct pollfd pfd{};
-    pfd.fd = sock_fd_;
-    pfd.events = POLLIN;
-    if (connecting_ || !_send_buffer.empty())
-      pfd.events |= POLLOUT;
-
-    int rc = ::poll(&pfd, 1, poll_timeout_ms);
-    if (rc < 0)
-    {
-      throw amr_exception(std::string("poll() failed: ") + std::strerror(errno));
-    }
-    if (rc > 0)
-    {
-      process_events(pfd.revents);
-    }
-
-    if (_result.empty())
-    {
-      return { false, "", _feedback };
-    }
-    else
-    {
-      auto res = _result;
-      auto feed = _feedback;
-      _result.clear();
-      _feedback.clear();
-      return { true, res, feed };
-    }
-  }
-
-  /**
-   * @brief Block until one of the end markers is detected.
-   *
-   * Polls indefinitely. Logs errors via rclcpp; on fatal I/O errors it
-   * closes the socket and returns.
-   */
-  void wait_until_login()
-  {
-    while (true)
-    {
-      if (!is_open())
-        break;
-
-      struct pollfd pfd{};
-      pfd.fd = sock_fd_;
-      pfd.events = POLLIN | POLLOUT;
-
-      int rc = ::poll(&pfd, 1, -1);
-      if (rc < 0)
-      {
-        RCLCPP_ERROR(node_->get_logger(), "poll() during login failed: %s", std::strerror(errno));
-        continue;
-      }
-      if (rc > 0)
-      {
-        try
-        {
-          process_events(pfd.revents);
-        }
-        catch (const amr_exception& ex)
-        {
-          RCLCPP_ERROR(node_->get_logger(), "Login processing error: %s", ex.what());
-          break;
-        }
-      }
-
-      if (has_match_in_result())
-      {
-        _result.clear();
-        _feedback.clear();
-        _check_end.clear();
-        break;
-      }
-    }
-  }
-
-  /**
-   * @brief Check if the socket is open.
-   */
-  bool is_open() const
-  {
-    return sock_fd_ >= 0;
-  }
-
-private:
-  /**
-   * @brief Read from socket into receive buffer (non-blocking).
-   * @throws amr_exception on recv error (other than EAGAIN/EWOULDBLOCK).
-   */
-  void read()
-  {
-    if (!is_open())
-      return;
-
-    std::string tmp;
-    bool peer_closed = amr::net::recv_append_nb(sock_fd_, tmp);
-    if (!tmp.empty())
-    {
-      _recv_buffer.append(tmp);
-      if (!_check_end.empty())
-      {
-        extract_resp();
-      }
-    }
-    if (peer_closed)
-    {
-      close_socket();
-    }
-  }
-
-  /**
-   * @brief Write pending bytes from send buffer (non-blocking).
-   * @throws amr_exception on send error (other than EAGAIN/EWOULDBLOCK).
-   */
-  void write()
-  {
-    if (!is_open() || _send_buffer.empty() || connecting_)
-      return;
-    // send_erase_nb returns false if would-block; un-sent tail remains in _send_buffer.
-    (void)amr::net::send_erase_nb(sock_fd_, _send_buffer);
-  }
-
-  /**
-   * @brief Drain any currently available incoming bytes (used before sending new command).
-   * @throws amr_exception on recv error (other than EAGAIN/EWOULDBLOCK).
-   */
-  void clear_recv_buf()
-  {
-    if (!is_open())
-      return;
-    std::string tmp;
-    bool peer_closed = amr::net::recv_append_nb(sock_fd_, tmp);
-    // discard tmp (stale data)
-    if (peer_closed)
-    {
-      close_socket();
-    }
-  }
-
-  /**
-   * @brief Parse _recv_buffer by CRLF lines and update _feedback/_result.
-   *
-   * Moves complete lines from _recv_buffer into _feedback until a line containing
-   * any of the configured end-line substrings is encountered. That line is stored
-   * in _result (without CRLF).
-   */
-  void extract_resp()
-  {
-    std::vector<std::string> lines;
-    if (amr::net::extract_crlf_lines(_recv_buffer, lines) == 0)
-      return;
-
-    for (const auto& line : lines)
-    {
-      if (any_end_in(line))
-      {
-        _result = line;
-        return;
-      }
-      else
-      {
-        if (!_feedback.empty())
-          _feedback += "\r\n";
-        _feedback += line;
-      }
-    }
-  }
-
-  /**
-   * @brief Test whether any end-line substring appears in the provided line.
-   */
-  bool any_end_in(const std::string& line) const
-  {
-    for (const auto& chk : _check_end)
-    {
-      if (!chk.empty() && line.find(chk) != std::string::npos)
-      {
-        return true;
-      }
-    }
-    return false;
-  }
-
-  /**
-   * @brief Return true if the stored result matches any end-line substring.
-   */
-  bool has_match_in_result() const
-  {
-    return any_end_in(_result);
-  }
-
-  /**
-   * @brief Close the socket if open.
-   */
-  void close_socket()
-  {
-    amr::net::close_fd(sock_fd_);
-    connecting_ = false;
-  }
-
-private:
-  rclcpp::Node::SharedPtr node_;
-  int sock_fd_{ -1 };
-  bool connecting_{ false };
-
-  // Buffers and state
-  std::string _recv_buffer;
-  std::string _send_buffer;
-  std::vector<std::string> _check_end;
-  std::string _result;
-  std::string _feedback;
-};
\ No newline at end of file
diff --git a/amr_core/include/amr_core/utils/libaria_runtime.hpp b/amr_core/include/amr_core/utils/libaria_runtime.hpp
new file mode 100644
index 00000000..1b002d26
--- /dev/null
+++ b/amr_core/include/amr_core/utils/libaria_runtime.hpp
@@ -0,0 +1,71 @@
+#pragma once
+
+#include <Aria/Aria.h>
+#include <ArNetworking/ArNetworking.h>
+
+#include <cstdlib>
+#include <mutex>
+
+namespace amr_core
+{
+namespace detail
+{
+inline std::mutex& ariaMutex()
+{
+  static std::mutex mutex;
+  return mutex;
+}
+
+inline int& ariaRefCount()
+{
+  static int ref_count = 0;
+  return ref_count;
+}
+
+inline void configureAriaDirectory()
+{
+  if (std::getenv("ARIA") != nullptr)
+  {
+    return;
+  }
+
+#ifdef AMR_CORE_ARIA_DIR
+  ::setenv("ARIA", AMR_CORE_ARIA_DIR, 0);
+#endif
+}
+}  // namespace detail
+
+class LibAriaRuntime
+{
+public:
+  LibAriaRuntime()
+  {
+    std::lock_guard<std::mutex> lock(detail::ariaMutex());
+    if (detail::ariaRefCount() == 0)
+    {
+      detail::configureAriaDirectory();
+      Aria::init();
+      ArLog::init(ArLog::StdOut, ArLog::Normal);
+    }
+    ++detail::ariaRefCount();
+  }
+
+  ~LibAriaRuntime()
+  {
+    std::lock_guard<std::mutex> lock(detail::ariaMutex());
+    if (detail::ariaRefCount() == 0)
+    {
+      return;
+    }
+
+    --detail::ariaRefCount();
+    if (detail::ariaRefCount() == 0)
+    {
+      Aria::shutdown();
+    }
+  }
+
+  LibAriaRuntime(const LibAriaRuntime&) = delete;
+  LibAriaRuntime& operator=(const LibAriaRuntime&) = delete;
+};
+}  // namespace amr_core
\ No newline at end of file
diff --git a/amr_core/include/amr_core/utils/parser.hpp b/amr_core/include/amr_core/utils/parser.hpp
deleted file mode 100644
index b9d485dc..00000000
--- a/amr_core/include/amr_core/utils/parser.hpp
+++ /dev/null
@@ -1,96 +0,0 @@
-#pragma once
-
-#include <string>
-
-/**
- * @brief Parses ARCL server messages and classifies them.
- *
- * Returns one of:
- *  - PASS (0)
- *  - FAIL (1)
- *  - CONTINUE (2)
- * Along with a human-readable message.
- */
-class Parser
-{
-public:
-  enum class Code : int
-  {
-    PASS = 0,
-    FAIL = 1,
-    CONTINUE = 2
-  };
-
-  struct Result
-  {
-    Code code;
-    std::string message;
-  };
-
-  Parser() = default;
-
-  /**
-   * @brief Process a single ARCL server line and classify it.
-   * @param input The input message.
-   * @return Result classification and message.
-   */
-  Result process_arcl_server(const std::string& input)
-  {
-    if (input.find(" Docked") != std::string::npos)
-    {
-      return { Code::PASS, "Docked successfully." };
-    }
-    else if (input.find("Localized to point") != std::string::npos)
-    {
-      return { Code::PASS, "Localization complete." };
-    }
-    else if (input.find("Completed doing task") != std::string::npos)
-    {
-      return { Code::PASS, "Reached point" };
-    }
-    else if (input.find("Completed macro") != std::string::npos)
-    {
-      return { Code::PASS, "Macro execution complete" };
-    }
-    else if (input.find("Stopped") != std::string::npos)
-    {
-      return { Code::FAIL, "Command was interrupted." };
-    }
-    else if (input.find("Failed going to goal") != std::string::npos)
-    {
-      return { Code::FAIL, "Failed going to goal." };
-    }
-    else if (input.find("Interrupted") != std::string::npos)
-    {
-      return { Code::FAIL, "Interrupted." };
-    }
-    else if (input.find("Failed") != std::string::npos)
-    {
-      return { Code::FAIL, "Failed" };
-    }
-    else if (input.find("Error") != std::string::npos)
-    {
-      return { Code::FAIL, std::string("An Error has occurred:") + input };
-    }
-    else if (input.find("Going to") != std::string::npos)
-    {
-      return { Code::CONTINUE, input };
-    }
-    else if (input.find(" Docking ") != std::string::npos)
-    {
-      return { Code::CONTINUE, "Docking." };
-    }
-    else if (input.find("Will do task") != std::string::npos)
-    {
-      return { Code::CONTINUE, input };
-    }
-    else if (input.find("Executing macro") != std::string::npos)
-    {
-      return { Code::CONTINUE, input };
-    }
-    else
-    {
-      return { Code::CONTINUE, input };
-    }
-  }
-};
\ No newline at end of file
diff --git a/amr_core/package.xml b/amr_core/package.xml
index ac2837e4..dd2d390f 100644
--- a/amr_core/package.xml
+++ b/amr_core/package.xml
@@ -23,6 +23,7 @@
   <depend>visualization_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>ament_index_cpp</depend>
+  <depend>libaria</depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/amr_core/src/client_node.cpp b/amr_core/src/client_node.cpp
deleted file mode 100644
index ed3c0639..00000000
--- a/amr_core/src/client_node.cpp
+++ /dev/null
@@ -1,11 +0,0 @@
-#include <rclcpp/rclcpp.hpp>
-#include "amr_core/client_node.hpp"
-
-int main(int argc, char** argv) {
-  rclcpp::init(argc, argv);
-  auto node = std::make_shared<ClientNode>();
-  node->initialize();
-  rclcpp::spin(node);
-  rclcpp::shutdown();
-  return 0;
-}
\ No newline at end of file
diff --git a/amr_ros/config/parameters.yaml b/amr_ros/config/parameters.yaml
index 4473bc02..74150bf3 100644
--- a/amr_ros/config/parameters.yaml
+++ b/amr_ros/config/parameters.yaml
@@ -2,52 +2,54 @@ amr_core:
   ros__parameters:
     robot:
       ip: "192.168.1.1"
-      port: 7171
-      password: "omron"
+      port: 7272
+      user: "admin"
+      password: "admin"
+      protocol: "6MTX"
 
-    host:
-      ip: "192.168.1.50"
-      port: 7179
-
-    source_data:
-      publish: true
-      frequency: 5
-
-    map_data:
-      publish: false
-      topic: "map_grid"
-      frequency: 2
-      file: "data.map"
-
-    laser_scans:
+    status:
       publish: true
-      topic: "scan"
-      frame_id: "laser_frame"
+      topic: "amr/status"
+      battery_topic: "amr/battery_state"
+      publish_period_ms: 200
 
-    arcl:
-      enable: false
-      timeout_ms: 15000
+    laser:
+      main_laser:
+        enabled: true
+        topic: "scan"
+        frame_id: "laser_frame"
+        request: "Laser_1Current"
+        request_period_ms: 200
+        angle_min: -2.09439510239
+        angle_max: 2.09439510239
+        angle_increment: 0.00872664626
+        range_min: 0.02
+        range_max: 40.0
+      low_laser:
+        enabled: true
+        topic: "scan_low"
+        frame_id: "laser_frame_low"
+        request: "Laser_2Current"
+        request_period_ms: 200
+        angle_min: -1.09955742876
+        angle_max: 1.09955742876
+        angle_increment: 0.00872664626
+        range_min: 0.02
+        range_max: 4.0
 
     driver:
+      odom_topic: "/odom"
+      cmd_vel_topic: "/cmd_vel"
+      stop_topic: "/stop"
       publish_odom: true
       publish_robot_tf: true
-      subscribe_cmd_vel: true
-      subscribe_goal_pose: true
-      subscribe_initial_pose: true
-      subscribe_localplan: false
-      odom_topic: "/amr/odom"
-      cmd_vel_topic: "/amr/cmd_vel"
-      odom_reset_topic: "/amr/odomReset"
-      goal_pose_topic: "goal_pose"
-      initial_pose_topic: "initialpose"
       odom_frame: "odom"
       base_frame: "base_link"
-      reset_odometer_on_startup: true
       expected_cmd_vel_freq: 20.0
-      command_timeout_ms: 15000
-      min_linear_speed: -1000.0  # mm/s
-      max_linear_speed: 1000.0   # mm/s
-      min_angular_speed: -30.0    # deg/s
-      max_angular_speed: 30.0     # deg/s
-      unit_move_distance: 50.0   # mm
-      unit_turn_angle: 5.0       # deg
+      min_linear_speed: -200.0
+      max_linear_speed: 1200.0
+      min_angular_speed: -60.0
+      max_angular_speed: 60.0
+      drive_throttle_pct: 100.0
+      unsafe_drive: true
+      cmd_vel_timeout_sec: 0.2
\ No newline at end of file
diff --git a/libaria b/libaria
new file mode 160000
index 00000000..818cc722
--- /dev/null
+++ b/libaria
@@ -0,0 +1 @@
+Subproject commit 818cc72211620171acce8defc24d5676875a9990

From ce7ac086d6ddb92db77f14f1f5b0f761c4213a86 Mon Sep 17 00:00:00 2001
From: Kalana Ratnayake <kalanaratnayake95@gmail.com>
Date: Fri, 22 May 2026 12:57:40 +0000
Subject: [PATCH 2/2] added libaria to the humble

---
 README.md                            |  52 ++++++++-----
 amr_msgs/CMakeLists.txt              |   3 -
 amr_msgs/action/Action.action        |  11 ---
 amr_msgs/srv/ArclApi.srv             |   4 -
 amr_msgs/srv/ArclListen.srv          |   3 -
 amr_nav2/launch/ld250.launch.py      |   2 +-
 amr_ros/config/ld250_parameters.yaml |  55 ++++++++++++++
 amr_ros/launch/amr_core.launch.py    |  17 +++--
 amr_ros/launch/ld250.launch.py       | 104 +++++++++++++++++++++++++
 amr_test/CMakeLists.txt              |  71 -----------------
 amr_test/include/amr_test/utils.hpp  |  29 -------
 amr_test/package.xml                 |  21 ------
 amr_test/src/demo_execute_macro.cpp  | 109 ---------------------------
 amr_test/src/demo_get_macros.cpp     |  55 --------------
 docs/digital-twin.md                 |  42 ++++++-----
 docs/parameters.md                   | 109 ++++++++++++++-------------
 16 files changed, 284 insertions(+), 403 deletions(-)
 delete mode 100644 amr_msgs/action/Action.action
 delete mode 100644 amr_msgs/srv/ArclApi.srv
 delete mode 100644 amr_msgs/srv/ArclListen.srv
 create mode 100644 amr_ros/config/ld250_parameters.yaml
 create mode 100644 amr_ros/launch/ld250.launch.py
 delete mode 100644 amr_test/CMakeLists.txt
 delete mode 100644 amr_test/include/amr_test/utils.hpp
 delete mode 100644 amr_test/package.xml
 delete mode 100644 amr_test/src/demo_execute_macro.cpp
 delete mode 100644 amr_test/src/demo_get_macros.cpp

diff --git a/README.md b/README.md
index ca411d2a..0da28e37 100644
--- a/README.md
+++ b/README.md
@@ -1,11 +1,11 @@
 # Omron AMR package
 
-To view the original Readme.md [click here](./docs/original_readme.md)
+To view the original Readme.md [click here](./docs/old/original_readme.md)
 
-This package is a restructuring of [OmronAPAC/Omron_AMR_ROS2](https://github.com/OmronAPAC/Omron_AMR_ROS2)
+This package is a restructuring of [OmronAPAC/Omron_AMR_ROS2](https://github.com/OmronAPAC/Omron_AMR_ROS2) with a libaria-backed `amr_core` hardware interface.
 
-View [New Developer's Guide](https://github.com/CollaborativeRoboticsLab/omron_amr/blob/master/docs/old/DeveloperGuide_updated.adoc).
-View [Old Developer's Guide](https://github.com/CollaborativeRoboticsLab/omron_amr/blob/master/docs/old/DeveloperGuide.adoc).
+View [New Developer's Guide](./docs/old/DeveloperGuide_updated.adoc).
+View [Old Developer's Guide](./docs/old/DeveloperGuide.adoc).
 
 More information
 - [amr_core parameters](./docs/parameters.md)
@@ -39,26 +39,47 @@ cd ..
 colcon build
 ```
 
-## Usage 
+## Usage
 
 ### Initialization
 
-1. [Establish Remote connection to AMR Robot](https://github.com/CollaborativeRoboticsLab/omron_amr/blob/main/docs/DeveloperGuide.adoc#231-set-up-user-ethernet)
+1. [Establish Remote connection to AMR Robot](./docs/old/DeveloperGuide.adoc)
 
-2. [Configure AMR robot via Mobile Planner](https://github.com/CollaborativeRoboticsLab/omron_amr/blob/main/docs/DeveloperGuide_updated.adoc#332-set-up-arcl)
+2. [Configure AMR robot via Mobile Planner](./docs/old/DeveloperGuide_updated.adoc)
 
+### Start only the hardware interface
 
-### Connect with the robot base
+```sh
+source install/setup.bash
+ros2 launch amr_ros amr_core.launch.py
+```
+
+### Start the LD250 wrapper
 
-Run the following command to connect to the robot.
+This launches the LD250 hardware parameters and can optionally include Nav2 and RViz.
 
 ```sh
 source install/setup.bash
-ros2 launch amr_ros amr_core.launch.py
+ros2 launch amr_ros ld250.launch.py
 ```
-### Start visualization
 
-Run the following command to visualize robot. Swap `ld250` with `ld90` or `amr_platform` for other robot models.
+Example with Nav2 and RViz:
+
+```sh
+source install/setup.bash
+ros2 launch amr_ros ld250.launch.py use_nav2:=true use_slam:=true rviz:=true
+```
+
+Digital twin overlay example:
+
+```sh
+source install/setup.bash
+ros2 launch amr_ros ld250.launch.py extra_params_file:=/absolute/path/to/override.yaml
+```
+
+### Start visualization only
+
+Run the following command to visualize the robot. Swap `ld250` with `ld90` or `amr_platform` for other robot models.
 
 ```sh
 source install/setup.bash
@@ -72,10 +93,3 @@ source install/setup.bash
 ros2 launch amr_teleop amr_joyop.launch.py
 ```
 
-## To Do List
-
-- [x] Replace AMR_Core with a cpp package with support for standard ros2 interface (/cmd_vel, /tf and /odom)
-- [ ] Extend the interface with standard ros2 messages for battery and other optional data
-- [x] Add support for NAV2
-- [x] Create cascadeing launch files for AMR core and RVIz
-
diff --git a/amr_msgs/CMakeLists.txt b/amr_msgs/CMakeLists.txt
index 2f05f577..d801beae 100644
--- a/amr_msgs/CMakeLists.txt
+++ b/amr_msgs/CMakeLists.txt
@@ -23,9 +23,6 @@ find_package(rosidl_default_generators REQUIRED)
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Location.msg"
   "msg/Status.msg"
-  "srv/ArclApi.srv"
-  "srv/ArclListen.srv"
-  "action/Action.action"
 )
 # uncomment the following section in order to fill in
 # further dependencies manually.
diff --git a/amr_msgs/action/Action.action b/amr_msgs/action/Action.action
deleted file mode 100644
index 58a4f566..00000000
--- a/amr_msgs/action/Action.action
+++ /dev/null
@@ -1,11 +0,0 @@
-# Goal/Request
-string command
-string[] identifier
-
----
-# Result
-string res_msg
-
----
-# Feedback
-string feed_msg
diff --git a/amr_msgs/srv/ArclApi.srv b/amr_msgs/srv/ArclApi.srv
deleted file mode 100644
index 10584d10..00000000
--- a/amr_msgs/srv/ArclApi.srv
+++ /dev/null
@@ -1,4 +0,0 @@
-string command
-string line_identifier
----
-string response
\ No newline at end of file
diff --git a/amr_msgs/srv/ArclListen.srv b/amr_msgs/srv/ArclListen.srv
deleted file mode 100644
index 6ca1d5b8..00000000
--- a/amr_msgs/srv/ArclListen.srv
+++ /dev/null
@@ -1,3 +0,0 @@
-string resp_header
----
-string resp_text
\ No newline at end of file
diff --git a/amr_nav2/launch/ld250.launch.py b/amr_nav2/launch/ld250.launch.py
index 6a860451..d5142db7 100644
--- a/amr_nav2/launch/ld250.launch.py
+++ b/amr_nav2/launch/ld250.launch.py
@@ -125,7 +125,7 @@ def generate_launch_description():
                               'params_file': params_file}.items()),
 
         IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(os.path.join(package_dir, 'launch', 'navigation.launch.py')),
+            PythonLaunchDescriptionSource(os.path.join(package_dir, 'launch', 'nav2.launch.py')),
             launch_arguments={'namespace': namespace,
                               'use_sim_time': use_sim_time,
                               'autostart': autostart,
diff --git a/amr_ros/config/ld250_parameters.yaml b/amr_ros/config/ld250_parameters.yaml
new file mode 100644
index 00000000..74150bf3
--- /dev/null
+++ b/amr_ros/config/ld250_parameters.yaml
@@ -0,0 +1,55 @@
+amr_core:
+  ros__parameters:
+    robot:
+      ip: "192.168.1.1"
+      port: 7272
+      user: "admin"
+      password: "admin"
+      protocol: "6MTX"
+
+    status:
+      publish: true
+      topic: "amr/status"
+      battery_topic: "amr/battery_state"
+      publish_period_ms: 200
+
+    laser:
+      main_laser:
+        enabled: true
+        topic: "scan"
+        frame_id: "laser_frame"
+        request: "Laser_1Current"
+        request_period_ms: 200
+        angle_min: -2.09439510239
+        angle_max: 2.09439510239
+        angle_increment: 0.00872664626
+        range_min: 0.02
+        range_max: 40.0
+      low_laser:
+        enabled: true
+        topic: "scan_low"
+        frame_id: "laser_frame_low"
+        request: "Laser_2Current"
+        request_period_ms: 200
+        angle_min: -1.09955742876
+        angle_max: 1.09955742876
+        angle_increment: 0.00872664626
+        range_min: 0.02
+        range_max: 4.0
+
+    driver:
+      odom_topic: "/odom"
+      cmd_vel_topic: "/cmd_vel"
+      stop_topic: "/stop"
+      publish_odom: true
+      publish_robot_tf: true
+      odom_frame: "odom"
+      base_frame: "base_link"
+      expected_cmd_vel_freq: 20.0
+      min_linear_speed: -200.0
+      max_linear_speed: 1200.0
+      min_angular_speed: -60.0
+      max_angular_speed: 60.0
+      drive_throttle_pct: 100.0
+      unsafe_drive: true
+      cmd_vel_timeout_sec: 0.2
\ No newline at end of file
diff --git a/amr_ros/launch/amr_core.launch.py b/amr_ros/launch/amr_core.launch.py
index 5d0c8e70..ce4cde31 100644
--- a/amr_ros/launch/amr_core.launch.py
+++ b/amr_ros/launch/amr_core.launch.py
@@ -1,12 +1,9 @@
-import sys
 import os
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, OpaqueFunction
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command
+from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 from launch.conditions import UnlessCondition
-from launch_ros.parameter_descriptions import ParameterValue
-from launch_ros.substitutions import FindPackageShare
 from ament_index_python.packages import get_package_share_directory
 
 def load_file(package_name, file_path):
@@ -21,6 +18,7 @@ def load_file(package_name, file_path):
 
 
 def _create_nodes(context):
+    params_file = LaunchConfiguration('params_file').perform(context)
     robot_description_override = LaunchConfiguration('robot_description_override')
     extra_params_file = LaunchConfiguration('extra_params_file').perform(context)
 
@@ -35,7 +33,7 @@ def _create_nodes(context):
         condition=UnlessCondition(robot_description_override)
     )
 
-    parameter_files = [os.path.join(get_package_share_directory('amr_ros'), 'config', 'parameters.yaml')]
+    parameter_files = [params_file]
     if extra_params_file:
         parameter_files.append(extra_params_file)
 
@@ -51,6 +49,14 @@ def _create_nodes(context):
 
 
 def generate_launch_description():
+    default_core_params = os.path.join(get_package_share_directory('amr_ros'), 'config', 'parameters.yaml')
+
+    declare_params_file = DeclareLaunchArgument(
+        'params_file',
+        default_value=default_core_params,
+        description='Base parameter file passed to amr_core.',
+    )
+
     declare_robot_description_override = DeclareLaunchArgument(
         'robot_description_override',
         default_value='false',
@@ -64,6 +70,7 @@ def generate_launch_description():
     )
 
     return LaunchDescription([
+        declare_params_file,
         declare_robot_description_override,
         declare_extra_params_file,
         OpaqueFunction(function=_create_nodes),
diff --git a/amr_ros/launch/ld250.launch.py b/amr_ros/launch/ld250.launch.py
new file mode 100644
index 00000000..4ee0bfbf
--- /dev/null
+++ b/amr_ros/launch/ld250.launch.py
@@ -0,0 +1,104 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    rviz = LaunchConfiguration('rviz')
+    use_nav2 = LaunchConfiguration('use_nav2')
+    use_slam = LaunchConfiguration('use_slam')
+    extra_params_file = LaunchConfiguration('extra_params_file')
+    robot_description_override = LaunchConfiguration('robot_description_override')
+    nav2_params_file = LaunchConfiguration('nav2_params_file')
+
+    declare_rviz = DeclareLaunchArgument(
+        'rviz',
+        default_value='false',
+        description='Launch RViz with the LD250 configuration.',
+    )
+
+    declare_use_nav2 = DeclareLaunchArgument(
+        'use_nav2',
+        default_value='false',
+        description='Launch the Nav2 stack for the LD250 platform.',
+    )
+
+    declare_use_slam = DeclareLaunchArgument(
+        'use_slam',
+        default_value='true',
+        description='Launch SLAM inside the LD250 Nav2 bringup.',
+    )
+
+    declare_robot_description_override = DeclareLaunchArgument(
+        'robot_description_override',
+        default_value='false',
+        description='Disable loading the default LD250 robot description.',
+    )
+
+    declare_nav2_params_file = DeclareLaunchArgument(
+        'nav2_params_file',
+        default_value=PathJoinSubstitution([FindPackageShare('amr_nav2'), 'config', 'ld250_nav2.yaml']),
+        description='Nav2 parameter file to pass to the LD250 navigation launch.',
+    )
+
+    declare_extra_params_file = DeclareLaunchArgument(
+        'extra_params_file',
+        default_value='',
+        description='Optional extra parameter file layered on top of the LD250 hardware parameters.',
+    )
+
+    core_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution([
+                FindPackageShare('amr_ros'),
+                'launch',
+                'amr_core.launch.py',
+            ])
+        ),
+        launch_arguments={
+            'params_file': PathJoinSubstitution([FindPackageShare('amr_ros'), 'config', 'ld250_parameters.yaml']),
+            'extra_params_file': extra_params_file,
+            'robot_description_override': robot_description_override,
+        }.items(),
+    )
+
+    rviz_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution([
+                FindPackageShare('amr_ros'),
+                'launch',
+                'ld250_rviz.launch.py',
+            ])
+        ),
+        condition=IfCondition(rviz),
+    )
+
+    nav2_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution([
+                FindPackageShare('amr_nav2'),
+                'launch',
+                'ld250.launch.py',
+            ])
+        ),
+        condition=IfCondition(use_nav2),
+        launch_arguments={
+            'params_file': nav2_params_file,
+            'slam': use_slam,
+        }.items(),
+    )
+
+    return LaunchDescription([
+        declare_rviz,
+        declare_use_nav2,
+        declare_use_slam,
+        declare_robot_description_override,
+        declare_nav2_params_file,
+        declare_extra_params_file,
+        core_launch,
+        nav2_launch,
+        rviz_launch,
+    ])
\ No newline at end of file
diff --git a/amr_test/CMakeLists.txt b/amr_test/CMakeLists.txt
deleted file mode 100644
index 344257ee..00000000
--- a/amr_test/CMakeLists.txt
+++ /dev/null
@@ -1,71 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(amr_test)
-
-# Default to C99
-if(NOT CMAKE_C_STANDARD)
-  set(CMAKE_C_STANDARD 99)
-endif()
-
-# Default to C++14
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(amr_msgs REQUIRED)
-find_package(rclcpp_action REQUIRED)
-find_package(rclcpp_components REQUIRED)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # uncomment the line when a copyright and license is not present in all source files
-  #set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # uncomment the line when this package is not in a git repo
-  #set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-add_executable(get_macros src/demo_get_macros.cpp)
-target_include_directories(get_macros
-  PUBLIC
-    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-    $<INSTALL_INTERFACE:include>)
-
-ament_target_dependencies(get_macros
-  rclcpp amr_msgs)
-
-add_library(action_client SHARED
-  src/demo_execute_macro.cpp)
-target_include_directories(action_client PRIVATE
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
-target_compile_definitions(action_client
-  PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL")
-
- ament_target_dependencies(action_client
-  "amr_msgs"
-  "rclcpp"
-  "rclcpp_action"
-  "rclcpp_components")
-rclcpp_components_register_node(action_client PLUGIN "demo::ActionClient" EXECUTABLE execute_macro)
-
-install(TARGETS
-  action_client
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin)
-
-
-install(TARGETS
-  get_macros
-  DESTINATION lib/${PROJECT_NAME})
-
-ament_package()
diff --git a/amr_test/include/amr_test/utils.hpp b/amr_test/include/amr_test/utils.hpp
deleted file mode 100644
index c19a6ba2..00000000
--- a/amr_test/include/amr_test/utils.hpp
+++ /dev/null
@@ -1,29 +0,0 @@
-#include <string>
-#include <vector>
-#include <iostream>
-#include <functional>
-
-using namespace std;
-
-
-// Utility function to split string based on a delimiter
-template <typename T>
-void split(const basic_string<T>& s, T c, vector<basic_string<T>>& v) {
-    auto i = 0;
-    auto j = s.find(c);
-
-    while (j != basic_string<T>::npos) {
-        v.push_back(s.substr(i, j-i));
-        i = ++j;
-        j = s.find(c, j);
-
-        if (j == basic_string<T>::npos)
-            v.push_back(s.substr(i, s.length()));
-    }
-}
-
-// Utility function to extract macro name 
-template<typename T>
-basic_string<T> extract_macro_name(const basic_string<T>& s) {
-    return s.substr(7);
-}
\ No newline at end of file
diff --git a/amr_test/package.xml b/amr_test/package.xml
deleted file mode 100644
index b5a476ad..00000000
--- a/amr_test/package.xml
+++ /dev/null
@@ -1,21 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>amr_test</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="linhns59@gmail.com">zeon</maintainer>
-  <license>TODO: License declaration</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <depend>rclcpp</depend>
-  <depend>amr_msgs</depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>
diff --git a/amr_test/src/demo_execute_macro.cpp b/amr_test/src/demo_execute_macro.cpp
deleted file mode 100644
index c8d39ba7..00000000
--- a/amr_test/src/demo_execute_macro.cpp
+++ /dev/null
@@ -1,109 +0,0 @@
-#include <functional>
-#include <future>
-#include <memory>
-#include <string>
-#include <sstream>
-#include <string>
-
-#include "amr_msgs/action/action.hpp"
-
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp_action/rclcpp_action.hpp"
-#include "rclcpp_components/register_node_macro.hpp"
-
-/*
-This example shows how to make calls to the ARCL server using ROS2 action
-to execute a macro and view the feedback during execution
-*/
-namespace demo
-{
-  class ActionClient : public rclcpp::Node
-  {
-  public:
-    using Action = amr_msgs::action::Action;
-    using GoalHandleAction = rclcpp_action::ClientGoalHandle<Action>;
-
-    explicit ActionClient(const rclcpp::NodeOptions &options)
-        : Node("execute_macro", options)
-    {
-      this->client_ptr_ = rclcpp_action::create_client<Action>(this, "action_server");
-
-      this->timer_ = this->create_wall_timer(
-          std::chrono::milliseconds(500),
-          std::bind(&ActionClient::send_goal, this));
-    }
-
-    void send_goal()
-    {
-      using namespace std::placeholders;
-
-      this->timer_->cancel();
-
-      if (!this->client_ptr_->wait_for_action_server())
-      {
-        RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
-        rclcpp::shutdown();
-      }
-
-      auto goal_msg = Action::Goal();
-      goal_msg.command = "executeMacro Macro4";
-      goal_msg.identifier = std::vector<std::string>{"Completed macro Macro4"};
-
-      RCLCPP_INFO(this->get_logger(), "Sending goal");
-
-      auto send_goal_options = rclcpp_action::Client<Action>::SendGoalOptions();
-      send_goal_options.goal_response_callback =
-          std::bind(&ActionClient::goal_response_callback, this, _1);
-      send_goal_options.feedback_callback =
-          std::bind(&ActionClient::feedback_callback, this, _1, _2);
-      send_goal_options.result_callback =
-          std::bind(&ActionClient::result_callback, this, _1);
-      this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
-    }
-
-  private:
-    rclcpp_action::Client<Action>::SharedPtr client_ptr_;
-    rclcpp::TimerBase::SharedPtr timer_;
-
-    void goal_response_callback(GoalHandleAction::SharedPtr goal_handle)
-    {
-      if (!goal_handle)
-      {
-        RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
-      }
-      else
-      {
-        RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
-      }
-    }
-
-    void feedback_callback(GoalHandleAction::SharedPtr,
-                           const std::shared_ptr<const Action::Feedback> feedback)
-    {
-      RCLCPP_INFO(this->get_logger(), feedback->feed_msg.c_str());
-    }
-
-    void result_callback(const GoalHandleAction::WrappedResult &result)
-    {
-      switch (result.code)
-      {
-      case rclcpp_action::ResultCode::SUCCEEDED:
-        break;
-      case rclcpp_action::ResultCode::ABORTED:
-        RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
-        return;
-      case rclcpp_action::ResultCode::CANCELED:
-        RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
-        return;
-      default:
-        RCLCPP_ERROR(this->get_logger(), "Unknown result code");
-        return;
-      }
-      RCLCPP_INFO(this->get_logger(), result.result->res_msg.c_str());
-      rclcpp::shutdown();
-    }
-  }; // class ActionClient
-
-} // namespace demo
-
-RCLCPP_COMPONENTS_REGISTER_NODE(demo::ActionClient)
\ No newline at end of file
diff --git a/amr_test/src/demo_get_macros.cpp b/amr_test/src/demo_get_macros.cpp
deleted file mode 100644
index 5970f5c3..00000000
--- a/amr_test/src/demo_get_macros.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-#include <vector>
-
-#include <chrono>
-#include <cstdlib>
-#include <memory>
-
-#include "rclcpp/rclcpp.hpp"
-#include "amr_msgs/srv/arcl_api.hpp"
-#include "amr_msgs/srv/arcl_listen.hpp"
-#include "amr_test/utils.hpp"
-
-
-using namespace std::chrono_literals;
-
-/*
-This example shows how to make calls to the ARCL server using ROS2 service and client 
-to get all defined macros in the robot
-*/
-
-int main(int argc, char const *argv[]) {
-    rclcpp::init(argc, argv);
-    std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("get_macros");
-    auto srv_name = "arcl_api_service";
-    auto client = node->create_client<amr_msgs::srv::ArclApi>(srv_name);
-
-    auto request = std::make_shared<amr_msgs::srv::ArclApi::Request>();
-    request->command = "getMacros";
-    request->line_identifier = "End of macros";
-
-    while (!client->wait_for_service(1s)) {
-        if (!rclcpp::ok()) {
-            RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service. Exiting.");
-            return 0;
-        }
-        RCLCPP_INFO(node->get_logger(), "service not available, waiting again...");
-    }
-
-    auto result = client->async_send_request(request);
-    // Wait for the result.
-    if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) {
-        std::vector<std::string> macros{};
-        auto resp = result.get()->response;
-        split(resp, '\n', macros);
-        RCLCPP_INFO(node->get_logger(), "Macros:");
-
-        // Skip the last two lines (identifier and empty line)
-        for (auto iter = macros.begin(); iter != std::prev(std::prev(macros.end())); iter++) {
-            std::cout << extract_macro_name(*iter) << std::endl;
-        }
-        
-    } else {
-        RCLCPP_ERROR(node->get_logger(), "Failed to call service %s", srv_name);
-    }
-    return 0;
-}
diff --git a/docs/digital-twin.md b/docs/digital-twin.md
index 8e473266..d8f710a5 100644
--- a/docs/digital-twin.md
+++ b/docs/digital-twin.md
@@ -1,43 +1,45 @@
 # Digital Twin Override Pattern
 
-For mixed real-hardware and simulation workflows, keep the generic AMR defaults in [../amr_ros/config/parameters.yaml](../amr_ros/config/parameters.yaml) and layer package-specific overrides on top.
+For mixed real-hardware and simulation workflows, keep the generic AMR defaults in [../amr_ros/config/parameters.yaml](../amr_ros/config/parameters.yaml) or [../amr_ros/config/ld250_parameters.yaml](../amr_ros/config/ld250_parameters.yaml) and layer package-specific overrides on top.
 
-## Why use an override file
+## Launch layering
 
-The base AMR package should preserve normal robot behavior:
+[../amr_ros/launch/amr_core.launch.py](../amr_ros/launch/amr_core.launch.py) accepts:
 
-- publishes odometry
-- publishes `odom -> base_link` TF
-- optionally publishes listener-derived laser and source topics
-- resets the odometer on startup
+- `params_file`: the base hardware parameter file
+- `extra_params_file`: an optional overlay file applied after `params_file`
 
-That is the right default for a standalone AMR integration, but it conflicts with a digital twin that already publishes the authoritative robot state.
+The LD250 wrapper [../amr_ros/launch/ld250.launch.py](../amr_ros/launch/ld250.launch.py) forwards the same `extra_params_file` argument.
 
-## Recommended pattern
+## Twin-safe controls
 
-1. Keep hardware defaults in [../amr_ros/config/parameters.yaml](../amr_ros/config/parameters.yaml).
-2. Add a package-specific override file in the integration package.
-3. Pass that file into [../amr_ros/launch/amr_core.launch.py](../amr_ros/launch/amr_core.launch.py) with the `extra_params_file` launch argument.
+For a simulation-authoritative setup, the libaria-backed driver can remain command-capable while simulation owns robot state. The main controls are:
 
-## An example
+- `status.publish`
+- `laser.main_laser.enabled`
+- `laser.low_laser.enabled`
+- `driver.publish_odom`
+- `driver.publish_robot_tf`
 
-Omron Handsolo uses [handsolo_ros/config/handsolo-amr.yaml](https://github.com/CollaborativeRoboticsLab/omron_handsolo/tree/humble/handsolo_ros/config/handsolo-amr.yaml) to disable ROS-side state publication from `amr_core` while keeping the command path active.
+## Example override
 
-The relevant override values are:
+Omron Handsolo can layer an override file that looks like this:
 
 ```yaml
 amr_core:
   ros__parameters:
-    source_data:
+    status:
       publish: false
 
-    laser_scans:
-      publish: false
+    laser:
+      main_laser:
+        enabled: false
+      low_laser:
+        enabled: false
 
     driver:
       publish_odom: false
       publish_robot_tf: false
-      reset_odometer_on_startup: false
 ```
 
 With that arrangement:
@@ -46,4 +48,4 @@ With that arrangement:
 - simulation owns scan publication
 - `amr_core` still receives filtered velocity commands and forwards them to the AMR
 
-This keeps the authority over robot state in one place instead of splitting it across simulation and hardware interfaces.
\ No newline at end of file
+This keeps robot-state authority in one place instead of splitting it across simulation and hardware interfaces.
\ No newline at end of file
diff --git a/docs/parameters.md b/docs/parameters.md
index eccc3720..9d691606 100644
--- a/docs/parameters.md
+++ b/docs/parameters.md
@@ -1,79 +1,84 @@
 # `amr_core` Parameters
 
-The default parameters for `amr_core` live in [amr_ros/config/parameters.yaml](../amr_ros/config/parameters.yaml).
-Launch files can layer an additional parameter file on top of those defaults with the `extra_params_file` argument in [amr_ros/launch/amr_core.launch.py](../amr_ros/launch/amr_core.launch.py).
+The default hardware parameters live in [amr_ros/config/parameters.yaml](../amr_ros/config/parameters.yaml).
+The LD250 wrapper launch uses [amr_ros/config/ld250_parameters.yaml](../amr_ros/config/ld250_parameters.yaml) as its base file.
 
-## Common groups
+[amr_ros/launch/amr_core.launch.py](../amr_ros/launch/amr_core.launch.py) accepts:
 
-### `robot.*`
+- `params_file`: the base parameter file passed to `amr_core`
+- `extra_params_file`: an optional overlay file applied after `params_file`
 
-- `robot.ip`: target AMR IP address.
-- `robot.port`: ARCL port used by the driver connection.
-- `robot.password`: ARCL password.
+## `robot.*`
 
-### `host.*`
+- `robot.ip`: target robot IP address.
+- `robot.port`: controller port used by the libaria client.
+- `robot.user`: login user for the robot controller.
+- `robot.password`: login password for the robot controller.
+- `robot.protocol`: robot protocol profile, for example `6MTX`.
 
-- `host.ip`: local host IP for the AMR listener socket.
-- `host.port`: local host port for the AMR listener socket.
+## `status.*`
 
-### `source_data.*`
+- `status.publish`: enables the status interface.
+- `status.topic`: topic for the published AMR status message.
+- `status.battery_topic`: topic for the battery state publisher.
+- `status.publish_period_ms`: status polling and publish period in milliseconds.
 
-- `source_data.publish`: publishes raw listener-derived status, odom, laser, and fault topics under `amr/source/*`.
-- `source_data.frequency`: polling and publish rate for listener-driven source data.
+## `laser.main_laser.*`
 
-### `map_data.*`
+- `laser.main_laser.enabled`: enables the primary front laser publisher.
+- `laser.main_laser.topic`: output topic, normally `/scan`.
+- `laser.main_laser.frame_id`: frame used in the published scan.
+- `laser.main_laser.request`: libaria request name used to subscribe to the laser packet.
+- `laser.main_laser.request_period_ms`: packet request period in milliseconds.
+- `laser.main_laser.angle_min`: minimum scan angle in radians.
+- `laser.main_laser.angle_max`: maximum scan angle in radians.
+- `laser.main_laser.angle_increment`: fallback angle increment in radians.
+- `laser.main_laser.range_min`: minimum valid range in meters.
+- `laser.main_laser.range_max`: maximum valid range in meters.
 
-- `map_data.publish`: enables map file publication.
-- `map_data.topic`: output topic for map data.
-- `map_data.frequency`: publication rate.
-- `map_data.file`: map file name.
+## `laser.low_laser.*`
 
-### `laser_scans.*`
+- `laser.low_laser.enabled`: enables the low front laser publisher.
+- `laser.low_laser.topic`: output topic, normally `/scan_low`.
+- `laser.low_laser.frame_id`: frame used in the published scan.
+- `laser.low_laser.request`: libaria request name used to subscribe to the laser packet.
+- `laser.low_laser.request_period_ms`: packet request period in milliseconds.
+- `laser.low_laser.angle_min`: minimum scan angle in radians.
+- `laser.low_laser.angle_max`: maximum scan angle in radians.
+- `laser.low_laser.angle_increment`: fallback angle increment in radians.
+- `laser.low_laser.range_min`: minimum valid range in meters.
+- `laser.low_laser.range_max`: maximum valid range in meters.
 
-- `laser_scans.publish`: enables ROS laser scan publication from AMR listener data.
-- `laser_scans.topic`: laser scan topic name.
-- `laser_scans.frame_id`: frame used for published scans.
-
-### `arcl.*`
-
-- `arcl.enable`: enables the ARCL ROS interface.
-- `arcl.timeout_ms`: timeout used by ARCL service/action calls.
+The published angle increment is inferred from the packet when enough points are available. The configured increment is only used as a fallback.
 
 ## `driver.*`
 
-- `driver.publish_odom`: publishes the AMR odometry topic.
-- `driver.publish_robot_tf`: broadcasts the `odom_frame -> base_frame` transform from `amr_core`.
-- `driver.subscribe_cmd_vel`: enables velocity command subscription.
-- `driver.subscribe_goal_pose`: enables goal pose subscription.
-- `driver.subscribe_initial_pose`: enables initial pose subscription.
-- `driver.subscribe_localplan`: subscribes to `/local_plan` instead of `cmd_vel` when enabled.
-- `driver.odom_topic`: odometry topic name used by `amr_core`.
-- `driver.cmd_vel_topic`: command velocity topic name consumed by `amr_core`.
-- `driver.odom_reset_topic`: topic that triggers odometer reset.
-- `driver.goal_pose_topic`: goal pose topic.
-- `driver.initial_pose_topic`: initial pose topic.
+- `driver.odom_topic`: odometry topic name.
+- `driver.cmd_vel_topic`: velocity command topic consumed by `amr_core`.
+- `driver.stop_topic`: stop command topic.
+- `driver.publish_odom`: enables odometry publication from `amr_core`.
+- `driver.publish_robot_tf`: enables the `odom -> base_link` TF publisher.
 - `driver.odom_frame`: frame id assigned to published odometry.
-- `driver.base_frame`: child frame id assigned to published odometry / TF.
-- `driver.reset_odometer_on_startup`: resets the AMR odometer during node startup.
-- `driver.expected_cmd_vel_freq`: expected command rate used by the driver logic.
-- `driver.command_timeout_ms`: timeout for command/response interactions with the AMR.
+- `driver.base_frame`: child frame id assigned to published odometry and TF.
+- `driver.expected_cmd_vel_freq`: expected command rate used by the watchdog.
 - `driver.min_linear_speed`: minimum linear speed clamp in mm/s.
 - `driver.max_linear_speed`: maximum linear speed clamp in mm/s.
 - `driver.min_angular_speed`: minimum angular speed clamp in deg/s.
 - `driver.max_angular_speed`: maximum angular speed clamp in deg/s.
-- `driver.unit_move_distance`: linear move step sent per command in mm.
-- `driver.unit_turn_angle`: angular turn step sent per command in deg.
+- `driver.drive_throttle_pct`: throttle scaling applied to outgoing drive commands.
+- `driver.unsafe_drive`: enables libaria unsafe drive mode.
+- `driver.cmd_vel_timeout_sec`: timeout for stopping the robot when `cmd_vel` goes stale.
 
-## Digital twin overrides
+## Digital twin overlays
 
-For a simulation-authoritative setup, keep the normal robot defaults in [amr_ros/config/parameters.yaml](../amr_ros/config/parameters.yaml) and place override values in a separate package-specific file.
+For simulation-authoritative setups, keep the hardware defaults in [../amr_ros/config/parameters.yaml](../amr_ros/config/parameters.yaml) or [../amr_ros/config/ld250_parameters.yaml](../amr_ros/config/ld250_parameters.yaml), then layer a package-specific override file with `extra_params_file`.
 
-The Handsolo twin configuration does this in [omron_handsolo/handsolo_ros/config/handsolo-amr.yaml](../../omron_handsolo/handsolo_ros/config/handsolo-amr.yaml), where it disables:
+The override usually disables:
 
-- raw source-data publication
-- laser scan publication from `amr_core`
-- AMR odometry publication
-- AMR TF publication
-- odometer reset on startup
+- `status.publish`
+- `laser.main_laser.enabled`
+- `laser.low_laser.enabled`
+- `driver.publish_odom`
+- `driver.publish_robot_tf`
 
 That leaves `amr_core` acting as the command bridge while simulation owns TF, odometry, and sensors.