diff --git a/vda5050_core/CMakeLists.txt b/vda5050_core/CMakeLists.txt
index e68245a..2981a28 100644
--- a/vda5050_core/CMakeLists.txt
+++ b/vda5050_core/CMakeLists.txt
@@ -8,6 +8,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(PahoMqttCpp REQUIRED)
 find_package(fmt REQUIRED)
+find_package(vda5050_types REQUIRED)
 
 include(GNUInstallDirs)
 
@@ -33,11 +34,19 @@ target_include_directories(logger
     $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
 )
 
+add_library(client src/vda5050_core/state_manager/state_manager.cpp)
+target_link_libraries(client
+  vda5050_types::vda5050_types)
+target_include_directories(client
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+         $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>)
+
 add_library(vda5050_core INTERFACE)
 target_link_libraries(vda5050_core
   INTERFACE
     mqtt_client
     logger
+    client
 )
 target_include_directories(vda5050_core
   INTERFACE
@@ -54,6 +63,7 @@ install(
   TARGETS
     mqtt_client
     logger
+    client
     vda5050_core
   EXPORT export_vda5050_core
   LIBRARY DESTINATION lib
@@ -76,7 +86,12 @@ if(BUILD_TESTING)
   ament_cppcheck()
 
   find_package(ament_cmake_cpplint REQUIRED)
-  ament_cpplint()
+  set(cpplint_filters
+    "-whitespace/newline"
+  )
+  ament_cpplint(
+    FILTERS "${cpplint_filters}"
+  )
 
   find_package(ament_cmake_clang_format REQUIRED)
   ament_clang_format(
@@ -91,9 +106,10 @@ if(BUILD_TESTING)
     test/unit/mqtt_client/test_mqtt_client_interface.cpp
     test/unit/logger/test_logger.cpp
     test/unit/logger/test_default_logger.cpp
+    test/unit/state_manager/test_state_manager.cpp
     test/integration/mqtt_client/test_paho_mqtt_client.cpp
   )
-  target_link_libraries(test_mqtt_client mqtt_client logger)
+  target_link_libraries(test_mqtt_client mqtt_client logger client)
 endif()
 
 ament_package()
diff --git a/vda5050_core/include/vda5050_core/state_manager/state_manager.hpp b/vda5050_core/include/vda5050_core/state_manager/state_manager.hpp
new file mode 100644
index 0000000..6071eb3
--- /dev/null
+++ b/vda5050_core/include/vda5050_core/state_manager/state_manager.hpp
@@ -0,0 +1,262 @@
+/*
+ * Copyright (C) 2025 ROS-Industrial Consortium Asia Pacific
+ * Advanced Remanufacturing and Technology Centre
+ * A*STAR Research Entities (Co. Registration No. 199702110H)
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef VDA5050_CORE__STATE_MANAGER__STATE_MANAGER_HPP_
+#define VDA5050_CORE__STATE_MANAGER__STATE_MANAGER_HPP_
+
+#include <cstdint>
+#include <mutex>
+#include <optional>
+#include <shared_mutex>
+#include <string>
+#include <vector>
+
+#include "vda5050_types/action_state.hpp"
+#include "vda5050_types/agv_position.hpp"
+#include "vda5050_types/battery_state.hpp"
+#include "vda5050_types/error.hpp"
+#include "vda5050_types/header.hpp"
+#include "vda5050_types/info.hpp"
+#include "vda5050_types/load.hpp"
+#include "vda5050_types/operating_mode.hpp"
+#include "vda5050_types/order.hpp"
+#include "vda5050_types/safety_state.hpp"
+#include "vda5050_types/state.hpp"
+
+namespace vda5050_core {
+
+namespace state_manager {
+
+using vda5050_types::ActionState;
+using vda5050_types::ActionStatus;
+using vda5050_types::AGVPosition;
+using vda5050_types::BatteryState;
+using vda5050_types::EdgeState;
+using vda5050_types::Error;
+using vda5050_types::Header;
+using vda5050_types::Info;
+using vda5050_types::Load;
+using vda5050_types::NodeState;
+using vda5050_types::OperatingMode;
+using vda5050_types::Order;
+using vda5050_types::SafetyState;
+using vda5050_types::State;
+using vda5050_types::Velocity;
+
+class StateManager
+{
+private:
+  /// \brief the mutex protecting the data
+  mutable std::shared_mutex mutex_;
+
+  /// \brief Internal State of the AGV
+  State robot_state_;
+
+  /// \brief Clear current robot order state
+  void cleanup();
+
+public:
+  /// \brief set the AGV state header
+  /// \param header of the the AGV
+  void set_header(const Header& header);
+
+  /// \brief get the AGV State header
+  /// \return header of the AGV
+  Header get_header() const;
+
+  /// \brief set the current order_id
+  /// \param order_id of the current order
+  void set_order_id(const std::string& order_id);
+
+  /// \brief get the current order_id
+  /// \return order_id of the current order
+  std::string get_order_id() const;
+
+  /// \brief set the current order_update_id
+  /// \param order_update_id the current order update_id to set
+  void set_order_update_id(uint32_t order_update_id);
+
+  /// \brief get the current order_update_id
+  /// \return order_update_id of the current order
+  uint32_t get_order_update_id() const;
+
+  /// \brief set the current zone_set_id
+  /// \param zone_set_id zone_set_id the current order to set
+  void set_zone_set_id(const std::string& zone_set_id);
+
+  /// \brief get the current zone_set_id
+  /// \return zone_set_id of the current order
+  std::optional<std::string> get_zone_set_id() const;
+
+  /// \brief Get the nodeId of the latest node the AGV has reached.
+  /// \param last_node_id last reached node's ID.
+  void set_last_node_id(const std::string& last_node_id);
+
+  /// \brief Get the nodeId of the latest node the AGV has reached.
+  /// \return The last reached node's ID.
+  std::string get_last_node_id() const;
+
+  /// \brief Get the sequenceId of the latest node the AGV has reached.
+  /// \param last_node_sequence_id last reached node's sequence ID.
+  void set_last_node_sequence_id(uint32_t last_node_sequence_id);
+
+  /// \brief Get the sequenceId of the latest node the AGV has reached.
+  /// \return The last reached node's sequence ID.
+  uint32_t get_last_node_sequence_id() const;
+
+  /// \brief Set the current AGV position
+  /// \param agv_position the agv position
+  void set_agv_position(const AGVPosition& agv_position);
+
+  /// \brief Get the current AGV position (if set)
+  /// \return std::optional<AGVPosition> the current AGV position of std::nullopt
+  std::optional<AGVPosition> get_agv_position() const;
+
+  /// \brief Set the current velocity
+  /// \param velocity the velocity
+  void set_velocity(const Velocity& velocity);
+
+  /// \brief Get the Velocity (if set)
+  /// \return std::optional<Velocity> the velocity of std::nullopt
+  std::optional<Velocity> get_velocity() const;
+
+  /// \brief Set the driving flag of the AGV
+  /// \param driving is the agv driving?
+  void set_driving_status(bool driving);
+
+  /// \brief Set the driving flag of the AGV
+  /// \return is the agv driving?
+  bool get_driving_status() const;
+
+  /// \brief Set the distance since the last node as in vda5050
+  /// \param distance_since_last_node the new distance since the last node
+  void set_distance_since_last_node(double distance_since_last_node);
+
+  /// \brief Reset the distance since the last node
+  void reset_distance_since_last_node();
+
+  /// \brief Get the current distance since the last node.
+  /// \return The current distance since the last node, or std::nullopt if not set.
+  std::optional<double> get_distance_since_last_node() const;
+
+  /// \brief add a load to the state
+  /// \param load the load to add
+  void add_load(const Load& load);
+
+  /// \brief set a vector of load to the state
+  /// \param load the vector of load to add
+  void set_load(const std::vector<Load>& loads);
+
+  /// \brief Remove a load by it's id from the loads array
+  /// \param load_id the id of the load to remove
+  /// \return true if at least one load was removed
+  bool remove_load(std::string_view load_id);
+
+  /// \brief Get the current loads
+  /// \return const std::vector<Load>& the current loads
+  std::optional<std::vector<Load>> get_loads() const;
+
+  /// \brief Set the current operating mode of the AGV
+  /// \param operating_mode the new operating mode
+  /// \return true if the operating mode changed
+  void set_operating_mode(OperatingMode operating_mode);
+
+  /// \brief Get the current operating mode from the state
+  /// \return OperatingMode the current operating mode
+  OperatingMode get_operating_mode() const;
+
+  /// \brief Set the current battery state of the AGV
+  /// \param battery_state the battery state
+  void set_battery_state(const BatteryState& battery_state);
+
+  /// \brief Get the current battery state from the state
+  /// \return const BatteryState& the current battery state
+  BatteryState get_battery_state() const;
+
+  /// \brief Set the current safety state of the AGV
+  /// \param safety_state the safety state
+  /// \return true if the state changed
+  void set_safety_state(const SafetyState& safety_state);
+
+  /// \brief Get the current safety state from the state
+  /// \return const SafetyState& the current safety state
+  SafetyState get_safety_state() const;
+
+  /// \brief Set the request new base flag
+  /// \param request_new_base is AGV requesting for new base
+  void set_request_new_base(bool request_new_base);
+
+  /// \brief Add an error to the state
+  /// \param error the error to add
+  void add_error(const Error& error);
+
+  /// \brief Get a copy of the current errors
+  /// \return std::vector<Error>
+  std::vector<Error> get_errors() const;
+
+  /// \brief Clear list of errors
+  void clear_errors();
+
+  /// \brief Add a new information to the state
+  /// \param info the information to add
+  void add_info(const Info& info);
+
+  /// \brief Get the list of information to the state
+  /// \return AGV State list of information
+  std::optional<std::vector<Info>> get_info() const;
+
+  /// \brief Clear the list of information to the state
+  void clear_info();
+
+  /// \brief Set the AGV action status
+  void set_action_states(std::vector<ActionState> action_states);
+
+  /// \brief Get the AGV action status
+  std::vector<ActionState> get_action_states() const;
+
+  /// \brief Dump all data to a vda5050::State
+  /// \param state the state to write to
+  void dump_to(State& state);
+
+  /// \brief Check whether the maintained nodeStates array is empty.
+  /// \return True if there are zero nodes, otherwise false.
+  bool is_node_states_empty() const;
+
+  /// \brief Check if any actionStates are still executing (not FINISHED or FAILED).
+  /// \return True if at least one action is still executing, otherwise false.
+  bool are_action_states_still_executing() const;
+
+  /// \brief Clear all state related to the currently stored order.
+  void cleanup_previous_order();
+
+  /// \brief Set a new order on the vehicle (after clearing any existing order).
+  /// \param order The new order to accept and store.
+  void set_new_order(const Order& order);
+
+  /// \brief Clear the horizon nodes/edges from the current nodeStates and edgeStates.
+  void clear_horizon();
+
+  /// @brief Get the current robot state.
+  /// @return const State& the current robot state.
+  State get_state();
+};
+
+}  // namespace state_manager
+}  // namespace vda5050_core
+
+#endif  // VDA5050_CORE__STATE_MANAGER__STATE_MANAGER_HPP_
diff --git a/vda5050_core/package.xml b/vda5050_core/package.xml
index 8aacb84..003ec03 100644
--- a/vda5050_core/package.xml
+++ b/vda5050_core/package.xml
@@ -9,11 +9,14 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <build_depend>vda5050_types</build_depend>
+
   <test_depend>ament_cmake_cppcheck</test_depend>
   <test_depend>ament_cmake_cpplint</test_depend>
   <test_depend>ament_cmake_clang_format</test_depend>
   <test_depend>ament_cmake_copyright</test_depend>
   <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>vda5050_types</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/vda5050_core/src/vda5050_core/state_manager/state_manager.cpp b/vda5050_core/src/vda5050_core/state_manager/state_manager.cpp
new file mode 100644
index 0000000..d45571c
--- /dev/null
+++ b/vda5050_core/src/vda5050_core/state_manager/state_manager.cpp
@@ -0,0 +1,485 @@
+/*
+ * Copyright (C) 2025 ROS-Industrial Consortium Asia Pacific
+ * Advanced Remanufacturing and Technology Centre
+ * A*STAR Research Entities (Co. Registration No. 199702110H)
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <algorithm>
+#include <mutex>
+
+#include "vda5050_core/state_manager/state_manager.hpp"
+
+namespace vda5050_core {
+
+namespace state_manager {
+
+//=============================================================================
+void StateManager::set_header(const Header& header)
+{
+  std::unique_lock lock(this->mutex_);
+  this->robot_state_.header = header;
+}
+
+//=============================================================================
+Header StateManager::get_header() const
+{
+  std::unique_lock lock(this->mutex_);
+  return this->robot_state_.header;
+}
+
+//=============================================================================
+void StateManager::set_order_id(const std::string& order_id)
+{
+  std::unique_lock lock(this->mutex_);
+  this->robot_state_.order_id = order_id;
+}
+
+//=============================================================================
+std::string StateManager::get_order_id() const
+{
+  std::shared_lock lock(this->mutex_);
+
+  return this->robot_state_.order_id;
+}
+
+//=============================================================================
+void StateManager::set_order_update_id(uint32_t order_update_id)
+{
+  std::unique_lock lock(this->mutex_);
+  this->robot_state_.order_update_id = order_update_id;
+}
+//=============================================================================
+uint32_t StateManager::get_order_update_id() const
+{
+  std::shared_lock lock(this->mutex_);
+
+  return this->robot_state_.order_update_id;
+}
+
+//=============================================================================
+void StateManager::set_zone_set_id(const std::string& zone_set_id)
+{
+  std::unique_lock lock(this->mutex_);
+  this->robot_state_.zone_set_id = zone_set_id;
+}
+
+//=============================================================================
+std::optional<std::string> StateManager::get_zone_set_id() const
+{
+  std::shared_lock lock(this->mutex_);
+  return this->robot_state_.zone_set_id;
+}
+
+//=============================================================================
+void StateManager::set_last_node_id(const std::string& last_node_id)
+{
+  std::unique_lock lock(this->mutex_);
+  this->robot_state_.last_node_id = last_node_id;
+}
+
+//=============================================================================
+std::string StateManager::get_last_node_id() const
+{
+  std::shared_lock lock(this->mutex_);
+  return this->robot_state_.last_node_id;
+}
+
+//=============================================================================
+void StateManager::set_last_node_sequence_id(uint32_t last_node_sequence_id)
+{
+  std::unique_lock lock(this->mutex_);
+  this->robot_state_.last_node_sequence_id = last_node_sequence_id;
+}
+
+//=============================================================================
+uint32_t StateManager::get_last_node_sequence_id() const
+{
+  std::shared_lock lock(this->mutex_);
+  return this->robot_state_.last_node_sequence_id;
+}
+
+//=============================================================================
+void StateManager::set_agv_position(const AGVPosition& agv_position)
+{
+  std::unique_lock lock(this->mutex_);
+  this->robot_state_.agv_position = agv_position;
+}
+
+//=============================================================================
+std::optional<AGVPosition> StateManager::get_agv_position() const
+{
+  std::shared_lock lock(this->mutex_);
+  return this->robot_state_.agv_position;
+}
+
+//=============================================================================
+void StateManager::set_velocity(const Velocity& velocity)
+{
+  std::unique_lock lock(this->mutex_);
+  this->robot_state_.velocity = velocity;
+}
+
+//=============================================================================
+std::optional<Velocity> StateManager::get_velocity() const
+{
+  std::shared_lock lock(this->mutex_);
+  return this->robot_state_.velocity;
+}
+
+//=============================================================================
+void StateManager::set_driving_status(bool driving)
+{
+  std::unique_lock lock(this->mutex_);
+  this->robot_state_.driving = driving;
+}
+
+//=============================================================================
+bool StateManager::get_driving_status() const
+{
+  std::unique_lock lock(this->mutex_);
+  return this->robot_state_.driving;
+}
+
+//=============================================================================
+void StateManager::set_distance_since_last_node(double distance_since_last_node)
+{
+  std::unique_lock lock(this->mutex_);
+  this->robot_state_.distance_since_last_node = distance_since_last_node;
+}
+
+//=============================================================================
+void StateManager::reset_distance_since_last_node()
+{
+  std::unique_lock lock(this->mutex_);
+  this->robot_state_.distance_since_last_node.reset();
+}
+
+//=============================================================================
+std::optional<double> StateManager::get_distance_since_last_node() const
+{
+  std::shared_lock lock(this->mutex_);
+  return this->robot_state_.distance_since_last_node;
+}
+
+//=============================================================================
+void StateManager::add_load(const Load& load)
+{
+  std::unique_lock lock(this->mutex_);
+
+  if (!this->robot_state_.loads.has_value())
+  {
+    this->robot_state_.loads = {load};
+  }
+  else
+  {
+    this->robot_state_.loads->push_back(load);
+  }
+}
+
+//=============================================================================
+void StateManager::set_load(const std::vector<Load>& loads)
+{
+  std::unique_lock lock(this->mutex_);
+  this->robot_state_.loads = loads;
+}
+
+//=============================================================================
+bool StateManager::remove_load(std::string_view load_id)
+{
+  std::unique_lock lock(this->mutex_);
+
+  if (!this->robot_state_.loads.has_value())
+  {
+    return false;
+  }
+
+  auto match_id = [load_id](const Load& load) {
+    return load.load_id == load_id;
+  };
+
+  auto before_size = this->robot_state_.loads->size();
+
+  this->robot_state_.loads->erase(
+    std::remove_if(
+      this->robot_state_.loads->begin(), this->robot_state_.loads->end(),
+      match_id),
+    this->robot_state_.loads->end());
+
+  return this->robot_state_.loads->size() != before_size;
+}
+
+//=============================================================================
+std::optional<std::vector<Load>> StateManager::get_loads() const
+{
+  std::shared_lock lock(this->mutex_);
+  return this->robot_state_.loads;
+}
+
+//=============================================================================
+void StateManager::set_operating_mode(OperatingMode operating_mode)
+{
+  std::unique_lock lock(this->mutex_);
+  this->robot_state_.operating_mode = operating_mode;
+}
+
+//=============================================================================
+OperatingMode StateManager::get_operating_mode() const
+{
+  std::shared_lock lock(
+    this->mutex_);  // Ensure that mode is not being altered at the moment
+  return this->robot_state_.operating_mode;
+}
+
+//=============================================================================
+void StateManager::set_battery_state(const BatteryState& battery_state)
+{
+  std::unique_lock lock(this->mutex_);
+  this->robot_state_.battery_state = battery_state;
+}
+
+//=============================================================================
+BatteryState StateManager::get_battery_state() const
+{
+  std::shared_lock lock(this->mutex_);
+  return this->robot_state_.battery_state;
+}
+
+//=============================================================================
+void StateManager::set_safety_state(const SafetyState& safety_state)
+{
+  std::unique_lock lock(this->mutex_);
+  this->robot_state_.safety_state = safety_state;
+}
+
+//=============================================================================
+SafetyState StateManager::get_safety_state() const
+{
+  std::shared_lock lock(this->mutex_);
+  return this->robot_state_.safety_state;
+}
+
+void StateManager::set_request_new_base(bool request_new_base)
+{
+  std::unique_lock lock(this->mutex_);
+  this->robot_state_.new_base_request = request_new_base;
+}
+
+//=============================================================================
+void StateManager::add_error(const Error& error)
+{
+  std::unique_lock lock(this->mutex_);
+  this->robot_state_.errors.push_back(error);
+}
+
+//=============================================================================
+std::vector<Error> StateManager::get_errors() const
+{
+  std::shared_lock lock(this->mutex_);
+  return this->robot_state_.errors;
+}
+
+//=============================================================================
+void StateManager::clear_errors()
+{
+  std::unique_lock lock(this->mutex_);
+  this->robot_state_.errors.clear();
+}
+
+//=============================================================================
+void StateManager::add_info(const Info& info)
+{
+  std::unique_lock lock(this->mutex_);
+
+  if (!this->robot_state_.information.has_value())
+  {
+    this->robot_state_.information = std::vector<Info>{};
+  }
+
+  this->robot_state_.information->push_back(info);
+}
+
+//=============================================================================
+std::optional<std::vector<Info>> StateManager::get_info() const
+{
+  std::shared_lock lock(this->mutex_);
+  return this->robot_state_.information;
+}
+
+//=============================================================================
+void StateManager::clear_info()
+{
+  std::unique_lock lock(this->mutex_);
+  this->robot_state_.information.reset();
+}
+
+//=============================================================================
+void StateManager::set_action_states(std::vector<ActionState> action_states)
+{
+  std::unique_lock lock(this->mutex_);
+  this->robot_state_.action_states = action_states;
+}
+
+//=============================================================================
+std::vector<ActionState> StateManager::get_action_states() const
+{
+  std::unique_lock lock(this->mutex_);
+  return this->robot_state_.action_states;
+}
+
+//=============================================================================
+void StateManager::dump_to(State& state)
+{
+  std::shared_lock lock(this->mutex_);
+
+  state.header = this->robot_state_.header;
+  state.order_id = this->robot_state_.order_id;
+  state.order_update_id = this->robot_state_.order_update_id;
+  state.zone_set_id = this->robot_state_.zone_set_id;
+  state.last_node_id = this->robot_state_.last_node_id;
+  state.last_node_sequence_id = this->robot_state_.last_node_sequence_id;
+  state.node_states = this->robot_state_.node_states;
+  state.edge_states = this->robot_state_.edge_states;
+  state.agv_position = this->robot_state_.agv_position;
+  state.velocity = this->robot_state_.velocity;
+  state.loads = this->robot_state_.loads;
+  state.driving = this->robot_state_.driving;
+  state.paused = this->robot_state_.paused;
+  state.new_base_request = this->robot_state_.new_base_request;
+  state.distance_since_last_node = this->robot_state_.distance_since_last_node;
+  state.action_states = this->robot_state_.action_states;
+  state.battery_state = this->robot_state_.battery_state;
+  state.operating_mode = this->robot_state_.operating_mode;
+  state.errors = this->robot_state_.errors;
+  state.information = this->robot_state_.information;
+  state.safety_state = this->robot_state_.safety_state;
+}
+
+//=============================================================================
+bool StateManager::is_node_states_empty() const
+{
+  std::shared_lock lock(this->mutex_);
+  return this->robot_state_.node_states.empty();
+}
+
+//=============================================================================
+bool StateManager::are_action_states_still_executing() const
+{
+  std::shared_lock lock(this->mutex_);
+
+  if (this->robot_state_.action_states.empty()) return false;
+
+  for (const auto& action_state : this->robot_state_.action_states)
+  {
+    if (
+      action_state.action_status != ActionStatus::FINISHED &&
+      action_state.action_status != ActionStatus::FAILED)
+    {
+      return false;
+    }
+  }
+  return true;
+}
+
+//=============================================================================
+void StateManager::cleanup_previous_order()
+
+{
+  std::unique_lock lock(this->mutex_);
+  this->cleanup();
+}
+
+//=============================================================================
+void StateManager::set_new_order(const Order& order)
+{
+  std::unique_lock lock(this->mutex_);
+
+  this->cleanup();
+
+  this->robot_state_.order_id = order.order_id;
+  this->robot_state_.order_update_id = order.order_update_id;
+  this->robot_state_.zone_set_id = order.zone_set_id;
+
+  for (const auto& node : order.nodes)
+  {
+    NodeState node_state;
+    node_state.node_id = node.node_id;
+    node_state.sequence_id = node.sequence_id;
+    node_state.node_description = node.node_description;
+    node_state.node_position = node.node_position;
+    node_state.released = node.released;
+    this->robot_state_.node_states.push_back(node_state);
+  }
+
+  for (const auto& edge : order.edges)
+  {
+    EdgeState edge_state;
+    edge_state.edge_id = edge.edge_id;
+    edge_state.sequence_id = edge.sequence_id;
+    edge_state.edge_description = edge.edge_description;
+    edge_state.trajectory = edge.trajectory;
+    edge_state.released = edge.released;
+    this->robot_state_.edge_states.push_back(edge_state);
+  }
+}
+
+//=============================================================================
+void StateManager::clear_horizon()
+{
+  std::unique_lock lock(this->mutex_);
+  auto& nodes = this->robot_state_.node_states;
+  auto& edges = this->robot_state_.edge_states;
+
+  auto node_predicate = [](const NodeState& n) { return !n.released; };
+  auto edge_predicate = [](const EdgeState& e) { return !e.released; };
+
+  nodes.erase(
+    std::remove_if(nodes.begin(), nodes.end(), node_predicate), nodes.end());
+  edges.erase(
+    std::remove_if(edges.begin(), edges.end(), edge_predicate), edges.end());
+}
+
+State StateManager::get_state()
+{
+  std::shared_lock lock(this->mutex_);
+  return this->robot_state_;
+}
+
+void StateManager::cleanup()
+{
+  Header prev_header = this->robot_state_.header;
+  std::string last_node_id = this->robot_state_.last_node_id;
+  std::optional<AGVPosition> prev_agv_position =
+    this->robot_state_.agv_position;
+  std::optional<Velocity> prev_velocity = this->robot_state_.velocity;
+  bool prev_driving = this->robot_state_.driving;
+  std::optional<bool> prev_paused = this->robot_state_.paused;
+  BatteryState prev_battery_state = this->robot_state_.battery_state;
+  OperatingMode prev_operating_mode = this->robot_state_.operating_mode;
+  SafetyState prev_safety_state = this->robot_state_.safety_state;
+
+  this->robot_state_ = State();
+  this->robot_state_.header = prev_header;
+  this->robot_state_.last_node_id = last_node_id;
+  this->robot_state_.agv_position = prev_agv_position;
+  this->robot_state_.velocity = prev_velocity;
+  this->robot_state_.driving = prev_driving;
+  this->robot_state_.paused = prev_paused;
+  this->robot_state_.battery_state = prev_battery_state;
+  this->robot_state_.operating_mode = prev_operating_mode;
+  this->robot_state_.safety_state = prev_safety_state;
+}
+
+}  // namespace state_manager
+}  // namespace vda5050_core
diff --git a/vda5050_core/test/unit/state_manager/test_state_manager.cpp b/vda5050_core/test/unit/state_manager/test_state_manager.cpp
new file mode 100644
index 0000000..1b79ca0
--- /dev/null
+++ b/vda5050_core/test/unit/state_manager/test_state_manager.cpp
@@ -0,0 +1,586 @@
+/*
+ * Copyright (C) 2025 ROS-Industrial Consortium Asia Pacific
+ * Advanced Remanufacturing and Technology Centre
+ * A*STAR Research Entities (Co. Registration No. 199702110H)
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <gtest/gtest.h>
+#include "vda5050_core/state_manager/state_manager.hpp"
+
+using vda5050_types::ActionState;
+using vda5050_types::ActionStatus;
+using vda5050_types::AGVPosition;
+using vda5050_types::BatteryState;
+using vda5050_types::Edge;
+using vda5050_types::EdgeState;
+using vda5050_types::Error;
+using vda5050_types::ErrorLevel;
+using vda5050_types::EStop;
+using vda5050_types::Header;
+using vda5050_types::Info;
+using vda5050_types::Load;
+using vda5050_types::Node;
+using vda5050_types::NodeState;
+using vda5050_types::OperatingMode;
+using vda5050_types::Order;
+using vda5050_types::SafetyState;
+using vda5050_types::State;
+using vda5050_types::Velocity;
+
+class StateManagerTest : public ::testing::Test
+{
+protected:
+  vda5050_core::state_manager::StateManager sm;
+};
+
+//=============================================================================
+// 1. Header and Order Identification Tests
+//=============================================================================
+
+TEST_F(StateManagerTest, HeaderAndIds)
+{
+  // Test Header
+  Header h;
+  h.header_id = 999;
+  sm.set_header(h);
+  EXPECT_EQ(sm.get_header().header_id, 999);
+
+  // Test Order ID
+  sm.set_order_id("order_alpha");
+  EXPECT_EQ(sm.get_order_id(), "order_alpha");
+
+  // Test Order Update ID
+  sm.set_order_update_id(12);
+  EXPECT_EQ(sm.get_order_update_id(), 12);
+
+  // Test Zone Set ID
+  sm.set_zone_set_id("zone_beta");
+  ASSERT_TRUE(sm.get_zone_set_id());
+  EXPECT_EQ(sm.get_zone_set_id(), "zone_beta");
+}
+
+TEST_F(StateManagerTest, OrderIdEmpty)
+{
+  // Ensure order_id is empty
+  sm.set_order_id("");
+
+  sm.set_order_update_id(50);
+  sm.set_zone_set_id("zone_gamma");
+
+  EXPECT_EQ(sm.get_order_update_id(), 50);
+  EXPECT_EQ(sm.get_zone_set_id(), "zone_gamma");
+}
+
+//=============================================================================
+// 2. Navigation Tests
+//=============================================================================
+
+TEST_F(StateManagerTest, LastNodeData)
+{
+  sm.set_last_node_id("node_start");
+  EXPECT_EQ(sm.get_last_node_id(), "node_start");
+
+  sm.set_last_node_sequence_id(5);
+  EXPECT_EQ(sm.get_last_node_sequence_id(), 5);
+}
+
+TEST_F(StateManagerTest, PositionAndVelocity)
+{
+  // Position
+  AGVPosition pos;
+  pos.x = 10.0;
+  pos.y = 20.0;
+  pos.theta = 1.57;
+  pos.map_id = "map_1";
+
+  sm.set_agv_position(pos);
+  ASSERT_TRUE(sm.get_agv_position().has_value());
+  EXPECT_DOUBLE_EQ(sm.get_agv_position()->x, 10.0);
+  EXPECT_DOUBLE_EQ(sm.get_agv_position()->y, 20.0);
+  EXPECT_DOUBLE_EQ(sm.get_agv_position()->theta, 1.57);
+  EXPECT_EQ(sm.get_agv_position()->map_id, "map_1");
+
+  // Velocity
+  Velocity vel;
+  vel.vx = 2.5;
+  vel.omega = 0.5;
+
+  sm.set_velocity(vel);
+  ASSERT_TRUE(sm.get_velocity().has_value());
+  EXPECT_DOUBLE_EQ(sm.get_velocity()->vx.value(), 2.5);
+  EXPECT_DOUBLE_EQ(sm.get_velocity()->omega.value(), 0.5);
+}
+
+TEST_F(StateManagerTest, DrivingStatus)
+{
+  sm.set_driving_status(true);
+  EXPECT_TRUE(sm.get_driving_status());
+
+  sm.set_driving_status(false);
+  EXPECT_FALSE(sm.get_driving_status());
+}
+
+TEST_F(StateManagerTest, DistanceSinceLastNode)
+{
+  sm.set_distance_since_last_node(12.5);
+  ASSERT_TRUE(sm.get_distance_since_last_node().has_value());
+  EXPECT_DOUBLE_EQ(sm.get_distance_since_last_node().value(), 12.5);
+
+  sm.reset_distance_since_last_node();
+  EXPECT_FALSE(sm.get_distance_since_last_node().has_value());
+}
+
+//=============================================================================
+// 3. Load Management Tests
+//=============================================================================
+
+TEST_F(StateManagerTest, LoadOperations)
+{
+  Load load1;
+  load1.load_id = "L1";
+  load1.load_type = "pallet";
+
+  // Test Add
+  sm.add_load(load1);
+  auto loads = sm.get_loads();
+  ASSERT_TRUE(loads.has_value());
+  EXPECT_EQ(loads->size(), 1);
+  EXPECT_EQ(loads->at(0).load_id, "L1");
+
+  // Test Add Multiple
+  Load load2;
+  load2.load_id = "L2";
+  sm.add_load(load2);
+  EXPECT_EQ(sm.get_loads()->size(), 2);
+
+  // Test Set (Overwrite)
+  Load load3;
+  load3.load_id = "L3";
+  sm.set_load({load3});
+  EXPECT_EQ(sm.get_loads()->size(), 1);
+  EXPECT_EQ(sm.get_loads()->at(0).load_id, "L3");
+
+  // Test Remove
+  EXPECT_TRUE(sm.remove_load("L3"));
+  EXPECT_TRUE(sm.get_loads()->empty());
+
+  // Test Remove non-existent
+  EXPECT_FALSE(sm.remove_load("ghost_load"));
+}
+
+//=============================================================================
+// 4. System Status Tests
+//=============================================================================
+
+TEST_F(StateManagerTest, OperatingModeAndBattery)
+{
+  sm.set_operating_mode(OperatingMode::SEMIAUTOMATIC);
+  EXPECT_EQ(sm.get_operating_mode(), OperatingMode::SEMIAUTOMATIC);
+
+  BatteryState bat;
+  bat.battery_charge = 0.95;
+  bat.charging = true;
+  sm.set_battery_state(bat);
+
+  EXPECT_DOUBLE_EQ(sm.get_battery_state().battery_charge, 0.95);
+  EXPECT_TRUE(sm.get_battery_state().charging);
+}
+
+TEST_F(StateManagerTest, SafetyAndNewBase)
+{
+  SafetyState safe;
+  safe.e_stop = EStop::AUTOACK;
+  safe.field_violation = true;
+  sm.set_safety_state(safe);
+
+  EXPECT_EQ(sm.get_safety_state().e_stop, EStop::AUTOACK);
+  EXPECT_TRUE(sm.get_safety_state().field_violation);
+
+  // New Base Request
+  sm.set_request_new_base(true);
+  State s;
+  sm.dump_to(s);
+  EXPECT_TRUE(s.new_base_request.value());
+
+  sm.set_request_new_base(false);
+  sm.dump_to(s);
+  EXPECT_FALSE(s.new_base_request.value());
+}
+
+//=============================================================================
+// 5. Error and Info Tests
+//=============================================================================
+
+TEST_F(StateManagerTest, ErrorHandling)
+{
+  Error e1;
+  e1.error_type = "Hardware";
+  e1.error_level = ErrorLevel::FATAL;
+  Error e2;
+  e2.error_type = "Software";
+  e2.error_level = ErrorLevel::WARNING;
+
+  sm.add_error(e1);
+  sm.add_error(e2);
+
+  auto errors = sm.get_errors();
+  ASSERT_EQ(errors.size(), 2);
+  EXPECT_EQ(errors[0].error_type, "Hardware");
+  EXPECT_EQ(errors[1].error_type, "Software");
+
+  // Test Clear
+  sm.clear_errors();
+  EXPECT_TRUE(sm.get_errors().empty());
+}
+
+TEST_F(StateManagerTest, InfoHandling)
+{
+  Info i1;
+  i1.info_type = "Status";
+  i1.info_description = "All Good";
+
+  sm.add_info(i1);
+
+  auto infos = sm.get_info();
+  ASSERT_TRUE(infos.has_value());
+  EXPECT_EQ(infos->size(), 1);
+  EXPECT_EQ(infos->at(0).info_type, "Status");
+
+  // Test Clear
+  sm.clear_info();
+  EXPECT_FALSE(sm.get_info().has_value());
+}
+
+//=============================================================================
+// 6. Action States Tests
+//=============================================================================
+
+TEST_F(StateManagerTest, ActionStateManagement)
+{
+  ActionState as1;
+  as1.action_id = "a1";
+  as1.action_status = ActionStatus::WAITING;
+
+  ActionState as2;
+  as2.action_id = "a2";
+  as2.action_status = ActionStatus::RUNNING;
+
+  std::vector<ActionState> actions = {as1, as2};
+
+  sm.set_action_states(actions);
+
+  auto ret = sm.get_action_states();
+  ASSERT_EQ(ret.size(), 2);
+  EXPECT_EQ(ret[0].action_id, "a1");
+  EXPECT_EQ(ret[1].action_id, "a2");
+}
+
+TEST_F(StateManagerTest, AreActionsStillExecuting)
+{
+  // If no actions are executing -> return false
+  EXPECT_FALSE(sm.are_action_states_still_executing());
+
+  // If any action is NOT FINISHED and NOT FAILED -> return fralse
+  ActionState active;
+  active.action_status = ActionStatus::RUNNING;
+  sm.set_action_states({active});
+  EXPECT_FALSE(sm.are_action_states_still_executing());
+
+  ActionState waiting;
+  waiting.action_status = ActionStatus::WAITING;
+  sm.set_action_states({waiting});
+  EXPECT_FALSE(sm.are_action_states_still_executing());
+
+  // If all are FINISHED or FAILED -> return True
+  ActionState done1;
+  done1.action_status = ActionStatus::FINISHED;
+  ActionState done2;
+  done2.action_status = ActionStatus::FAILED;
+  sm.set_action_states({done1, done2});
+  EXPECT_TRUE(sm.are_action_states_still_executing());
+
+  // If an action is still active
+  sm.set_action_states({done1, done2, active});
+  EXPECT_FALSE(sm.are_action_states_still_executing());
+}
+
+//=============================================================================
+// 7. Order Lifecycle Tests
+//=============================================================================
+
+TEST_F(StateManagerTest, SetNewOrder)
+{
+  Order o;
+  o.order_id = "O1";
+  o.order_update_id = 0;
+  o.zone_set_id = "Z1";
+
+  Node n1;
+  n1.node_id = "n1";
+  n1.released = true;
+  Node n2;
+  n2.node_id = "n2";
+  n2.released = false;
+  o.nodes = {n1, n2};
+
+  Edge e1;
+  e1.edge_id = "e1";
+  e1.released = true;
+  o.edges = {e1};
+
+  sm.set_new_order(o);
+
+  EXPECT_EQ(sm.get_order_id(), "O1");
+  EXPECT_FALSE(sm.is_node_states_empty());
+
+  State s;
+  sm.dump_to(s);
+  EXPECT_EQ(s.node_states.size(), 2);
+  EXPECT_EQ(s.edge_states.size(), 1);
+}
+
+TEST_F(StateManagerTest, CleanupPreviousOrder)
+{
+  sm.set_last_node_id("keep_me");
+  sm.set_order_id("delete_me");
+  sm.set_driving_status(true);
+
+  sm.cleanup_previous_order();
+
+  // Order ID should be gone
+  EXPECT_TRUE(sm.get_order_id().empty());
+
+  // Driving status should be reset (false is default in State constructor usually)
+  EXPECT_TRUE(sm.get_driving_status());
+
+  // Last Node ID MUST persist
+  EXPECT_EQ(sm.get_last_node_id(), "keep_me");
+}
+
+TEST_F(StateManagerTest, ClearHorizon)
+{
+  // Setup order with released and unreleased elements
+  Order o;
+  Node n1;
+  n1.node_id = "rel_n";
+  n1.released = true;
+  Node n2;
+  n2.node_id = "unrel_n";
+  n2.released = false;
+  o.nodes = {n1, n2};
+
+  Edge e1;
+  e1.edge_id = "rel_e";
+  e1.released = true;
+  Edge e2;
+  e2.edge_id = "unrel_e";
+  e2.released = false;
+  o.edges = {e1, e2};
+
+  sm.set_new_order(o);
+
+  // Action
+  sm.clear_horizon();
+
+  // Assert
+  State s;
+  sm.dump_to(s);
+
+  // Only released items should remain
+  ASSERT_EQ(s.node_states.size(), 1);
+  EXPECT_EQ(s.node_states[0].node_id, "rel_n");
+
+  ASSERT_EQ(s.edge_states.size(), 1);
+  EXPECT_EQ(s.edge_states[0].edge_id, "rel_e");
+}
+
+//=============================================================================
+// 8. General Accessor Tests
+//=============================================================================
+
+TEST_F(StateManagerTest, DumpToAndGetState)
+{
+  sm.set_order_id("dump_id");
+  sm.set_last_node_sequence_id(99);
+
+  // Test dump_to
+  State s_dump;
+  sm.dump_to(s_dump);
+  EXPECT_EQ(s_dump.order_id, "dump_id");
+  EXPECT_EQ(s_dump.last_node_sequence_id, 99);
+
+  // Test get_state (const ref)
+  const State& s_ref = sm.get_state();
+  EXPECT_EQ(s_ref.order_id, "dump_id");
+  EXPECT_EQ(s_ref.last_node_sequence_id, 99);
+}
+
+TEST_F(StateManagerTest, AssignAllFieldsFromPreviousTests_DumpEqualsExpected)
+{
+  // Create a VDA5050 State
+  State expected;
+
+  Header h;
+  h.header_id = 999;
+  expected.header = h;
+  expected.order_id = "order_alpha";
+  expected.order_update_id = 12u;
+  expected.zone_set_id = std::optional<std::string>("zone_beta");
+
+  expected.last_node_id = "node_start";
+  expected.last_node_sequence_id = 5u;
+
+  AGVPosition pos;
+  pos.x = 10.0;
+  pos.y = 20.0;
+  pos.theta = 1.57;
+  pos.map_id = "map_1";
+  expected.agv_position = pos;
+
+  Velocity vel;
+  vel.vx = 2.5;
+  vel.omega = 0.5;
+  expected.velocity = vel;
+  expected.driving =
+    true;  // we'll toggle later to exercise both true/false in manager
+  expected.distance_since_last_node = std::optional<double>(12.5);
+
+  Load load1;
+  load1.load_id = "L1";
+  load1.load_type = "pallet";
+
+  Load load2;
+  load2.load_id = "L2";
+  load2.load_type = "box";
+  expected.loads = std::optional<std::vector<Load>>({load1, load2});
+  expected.operating_mode = OperatingMode::SEMIAUTOMATIC;
+
+  BatteryState bat;
+  bat.battery_charge = 0.95;
+  bat.charging = true;
+  expected.battery_state = bat;
+
+  SafetyState safe;
+  safe.e_stop = EStop::AUTOACK;
+  safe.field_violation = true;
+  expected.safety_state = safe;
+  expected.new_base_request = std::optional<bool>(true);
+
+  Error e1;
+  e1.error_type = "Hardware";
+  e1.error_level = ErrorLevel::FATAL;
+
+  Error e2;
+  e2.error_type = "Software";
+  e2.error_level = ErrorLevel::WARNING;
+  expected.errors = {e1, e2};
+
+  Info i1;
+  i1.info_type = "Status";
+  i1.info_description = "All Good";
+  expected.information = std::optional<std::vector<Info>>({i1});
+
+  ActionState as1;
+  as1.action_id = "a1";
+  as1.action_status = ActionStatus::WAITING;
+
+  ActionState as2;
+  as2.action_id = "a2";
+  as2.action_status = ActionStatus::RUNNING;
+  expected.action_states = {as1, as2};
+  expected.order_id = "O1";
+  expected.order_update_id = 0u;
+  expected.zone_set_id = std::optional<std::string>("Z1");
+
+  NodeState n1;
+  n1.node_id = "n1";
+
+  n1.released = true;
+  NodeState n2;
+
+  n2.node_id = "n2";
+  n2.released = false;
+  expected.node_states = {n1, n2};
+
+  EdgeState edge1;
+  edge1.edge_id = "e1";
+  edge1.released = true;
+  expected.edge_states = {edge1};
+  expected.last_node_id = "keep_me";
+
+  // Create a VDA50500 order
+
+  sm.set_header(h);
+
+  Order order;
+  order.order_id = "O1";
+  order.order_update_id = 0;
+  order.zone_set_id = "Z1";
+
+  Node on1;
+  on1.node_id = "n1";
+  on1.sequence_id = n1.sequence_id;
+  on1.node_description = n1.node_description;
+  on1.node_position = n1.node_position;
+  on1.released = n1.released;
+  Node on2;
+  on2.node_id = "n2";
+  on2.sequence_id = n2.sequence_id;
+  on2.node_description = n2.node_description;
+  on2.node_position = n2.node_position;
+  on2.released = n2.released;
+  order.nodes = {on1, on2};
+
+  Edge oe1;
+  oe1.edge_id = "e1";
+  oe1.sequence_id = edge1.sequence_id;
+  oe1.edge_description = edge1.edge_description;
+  oe1.trajectory = edge1.trajectory;
+  oe1.released = edge1.released;
+  order.edges = {oe1};
+
+  // Assign values rto State Manager
+  sm.set_new_order(order);
+  sm.set_last_node_id(expected.last_node_id);
+  sm.set_last_node_sequence_id(expected.last_node_sequence_id);
+  sm.set_agv_position(pos);
+  sm.set_velocity(vel);
+  sm.set_load({});
+  sm.add_load(load1);
+  sm.add_load(load2);
+  sm.set_driving_status(true);
+  sm.set_distance_since_last_node(*expected.distance_since_last_node);
+  sm.set_request_new_base(*expected.new_base_request);
+  sm.set_action_states(expected.action_states);
+  sm.set_battery_state(expected.battery_state);
+  sm.set_operating_mode(expected.operating_mode);
+  sm.clear_errors();
+  sm.add_error(e1);
+  sm.add_error(e2);
+  sm.clear_info();
+  sm.add_info(i1);
+  sm.set_safety_state(expected.safety_state);
+  State actual;
+  sm.dump_to(actual);
+
+  // Use VDA5050 type equality operator to test (check for assignment)
+  EXPECT_EQ(actual, expected)
+    << "StateManager::dump_to produced a state different from expected.";
+
+  // check get state (return by value)
+  State ref = sm.get_state();
+  EXPECT_EQ(ref, expected)
+    << "StateManager::get_state returned a state different from expected.";
+}