diff --git a/dynamixel_ros_control/CMakeLists.txt b/dynamixel_ros_control/CMakeLists.txt
index 6e62f30..d2a0996 100644
--- a/dynamixel_ros_control/CMakeLists.txt
+++ b/dynamixel_ros_control/CMakeLists.txt
@@ -20,6 +20,8 @@ find_package(hardware_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(dynamixel_sdk REQUIRED)
 find_package(yaml-cpp REQUIRED)
+find_package(diagnostic_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(ament_index_cpp REQUIRED)
@@ -27,9 +29,11 @@ find_package(transmission_interface REQUIRED)
 find_package(hector_transmission_interface REQUIRED)
 find_package(hector_transmission_interface_msgs REQUIRED)
 find_package(controller_orchestrator REQUIRED)
+find_package(realtime_tools REQUIRED)
 
 set(HEADERS
     include/${PROJECT_NAME}/common.hpp
+    include/${PROJECT_NAME}/diagnostic_state.hpp
     include/${PROJECT_NAME}/joint.hpp
     include/${PROJECT_NAME}/dynamixel.hpp
     include/${PROJECT_NAME}/control_table.hpp
@@ -42,6 +46,7 @@ set(HEADERS
 
 set(SOURCES
     src/common.cpp
+    src/diagnostic_state.cpp
     src/joint.cpp
     src/dynamixel.cpp
     src/control_table.cpp
@@ -64,13 +69,16 @@ ament_target_dependencies(
   pluginlib
   dynamixel_sdk
   yaml-cpp
+  diagnostic_msgs
+  sensor_msgs
   std_msgs
   std_srvs
   ament_index_cpp
   transmission_interface
   hector_transmission_interface
   hector_transmission_interface_msgs
-  controller_orchestrator)
+  controller_orchestrator
+  realtime_tools)
 
 pluginlib_export_plugin_description_file(hardware_interface
                                          dynamixel_ros_control.xml)
@@ -154,6 +162,12 @@ if(BUILD_TESTING)
   set_tests_properties(test_hw_mimic PROPERTIES TIMEOUT 180)
   target_link_libraries(test_hw_mimic ${PROJECT_NAME})
   ament_target_dependencies(test_hw_mimic ${HW_TEST_DEPS})
+
+  # Test: Diagnostics and joint state publishing
+  ament_add_gtest(test_hw_diagnostics test/test_hw_diagnostics.cpp)
+  set_tests_properties(test_hw_diagnostics PROPERTIES TIMEOUT 300)
+  target_link_libraries(test_hw_diagnostics ${PROJECT_NAME})
+  ament_target_dependencies(test_hw_diagnostics ${HW_TEST_DEPS})
 endif()
 
 # Install library and include folder
diff --git a/dynamixel_ros_control/include/dynamixel_ros_control/diagnostic_state.hpp b/dynamixel_ros_control/include/dynamixel_ros_control/diagnostic_state.hpp
new file mode 100644
index 0000000..f9672f9
--- /dev/null
+++ b/dynamixel_ros_control/include/dynamixel_ros_control/diagnostic_state.hpp
@@ -0,0 +1,39 @@
+#ifndef DYNAMIXEL_ROS_CONTROL_DIAGNOSTIC_STATE_H
+#define DYNAMIXEL_ROS_CONTROL_DIAGNOSTIC_STATE_H
+
+#include <map>
+#include <string>
+
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
+
+namespace dynamixel_ros_control {
+
+/// @brief Lightweight snapshot of diagnostic values for cheap change detection (no heap allocations).
+struct DiagnosticState
+{
+  bool hw_ok{true};
+  bool e_stop_active{false};
+  bool torque_enabled{false};
+  unsigned int read_errors{0};
+  unsigned int write_errors{0};
+  std::map<std::string, int32_t> joint_hw_errors;  ///< Per-joint hardware error status (only non-OK entries).
+
+  bool operator==(const DiagnosticState& o) const
+  {
+    return hw_ok == o.hw_ok && e_stop_active == o.e_stop_active && torque_enabled == o.torque_enabled &&
+           read_errors == o.read_errors && write_errors == o.write_errors && joint_hw_errors == o.joint_hw_errors;
+  }
+  bool operator!=(const DiagnosticState& o) const
+  {
+    return !(*this == o);
+  }
+
+  /// @brief Convert to a DiagnosticStatus message. Performs string allocations — only call when publishing.
+  diagnostic_msgs::msg::DiagnosticStatus toMsg(const std::string& name) const;
+
+  /// @brief Convert a hardware error status bitfield to a human-readable string.
+  static std::string hardwareErrorToString(int32_t error_status);
+};
+
+}  // namespace dynamixel_ros_control
+#endif
diff --git a/dynamixel_ros_control/include/dynamixel_ros_control/dynamixel_hardware_interface.hpp b/dynamixel_ros_control/include/dynamixel_ros_control/dynamixel_hardware_interface.hpp
index 6b3c4db..429ef71 100644
--- a/dynamixel_ros_control/include/dynamixel_ros_control/dynamixel_hardware_interface.hpp
+++ b/dynamixel_ros_control/include/dynamixel_ros_control/dynamixel_hardware_interface.hpp
@@ -4,6 +4,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <mutex>
 
+#include "diagnostic_state.hpp"
 #include "joint.hpp"
 #include "sync_read_manager.hpp"
 #include "sync_write_manager.hpp"
@@ -14,6 +15,9 @@
 #include <hector_transmission_interface/adjustable_offset_manager.hpp>
 #include <controller_orchestrator/controller_orchestrator.hpp>
 #include <hector_transmission_interface/adjustable_offset_transmission_loader.hpp>
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+#include <realtime_tools/realtime_publisher.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
 #include <std_msgs/msg/bool.hpp>
 #include <std_srvs/srv/set_bool.hpp>
 #include <std_srvs/srv/trigger.hpp>
@@ -110,6 +114,12 @@ class DynamixelHardwareInterface : public hardware_interface::SystemInterface
   void setJointLED(const std::string& joint_name, const std::string& color);  ///< Set LED color for a specific motor.
   void updateErrorLEDs();                                                     ///< Set red LED for motors with errors.
 
+  /// @brief Publish diagnostics via RealtimePublisher. Only publishes when content changes.
+  void publishDiagnostics();
+
+  /// @brief Publish goal joint states via RealtimePublisher.
+  void publishGoalJointStates();
+
   /// @brief Activate E-Stop: switch to position mode and hold current positions.
   bool activateEStop();
 
@@ -146,6 +156,13 @@ class DynamixelHardwareInterface : public hardware_interface::SystemInterface
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_torque_service_;
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr reboot_service_;
   rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr soft_e_stop_subscription_;
+  // Realtime-safe publishers (publish from control thread, actual ROS publish on internal thread)
+  std::shared_ptr<realtime_tools::RealtimePublisher<diagnostic_msgs::msg::DiagnosticArray>> rt_diagnostics_pub_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<sensor_msgs::msg::JointState>> rt_goal_joint_state_pub_;
+
+  DiagnosticState last_diag_state_;          ///< Cached raw values for cheap change detection.
+  bool first_diagnostics_published_{false};  ///< Ensures at least one message is published.
+
   rclcpp::executors::MultiThreadedExecutor::SharedPtr exe_;
   std::thread exe_thread_;
   std::mutex dynamixel_comm_mutex_;  ///< Protects all Dynamixel communication.
diff --git a/dynamixel_ros_control/package.xml b/dynamixel_ros_control/package.xml
index 3733231..c7e4f2a 100644
--- a/dynamixel_ros_control/package.xml
+++ b/dynamixel_ros_control/package.xml
@@ -12,6 +12,7 @@
 
   <depend>ament_index_cpp</depend>
   <depend>controller_orchestrator</depend>
+  <depend>diagnostic_msgs</depend>
   <depend>dynamixel_sdk</depend>
   <depend>hardware_interface</depend>
   <depend>hector_transmission_interface</depend>
@@ -20,6 +21,8 @@
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>
+  <depend>realtime_tools</depend>
+  <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
   <depend>transmission_interface</depend>
@@ -37,7 +40,6 @@
   <test_depend>joint_state_broadcaster</test_depend>
   <test_depend>joint_trajectory_controller</test_depend>
   <test_depend>position_controllers</test_depend>
-  <test_depend>realtime_tools</test_depend>
   <test_depend>velocity_controllers</test_depend>
 
   <export>
diff --git a/dynamixel_ros_control/src/diagnostic_state.cpp b/dynamixel_ros_control/src/diagnostic_state.cpp
new file mode 100644
index 0000000..0b7e76d
--- /dev/null
+++ b/dynamixel_ros_control/src/diagnostic_state.cpp
@@ -0,0 +1,69 @@
+#include "dynamixel_ros_control/diagnostic_state.hpp"
+
+#include "dynamixel_ros_control/dynamixel.hpp"
+
+namespace dynamixel_ros_control {
+
+diagnostic_msgs::msg::DiagnosticStatus DiagnosticState::toMsg(const std::string& name) const
+{
+  diagnostic_msgs::msg::DiagnosticStatus status;
+  status.name = name;
+  status.hardware_id = name;
+
+  if (!hw_ok) {
+    status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+    status.message = "Hardware error detected";
+  } else if (e_stop_active) {
+    status.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    status.message = "E-Stop active";
+  } else {
+    status.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+    status.message = "OK";
+  }
+
+  status.values.resize(4 + joint_hw_errors.size());
+  status.values[0].key = "e_stop_active";
+  status.values[0].value = e_stop_active ? "true" : "false";
+  status.values[1].key = "torque_enabled";
+  status.values[1].value = torque_enabled ? "true" : "false";
+  status.values[2].key = "consecutive_read_errors";
+  status.values[2].value = std::to_string(read_errors);
+  status.values[3].key = "consecutive_write_errors";
+  status.values[3].value = std::to_string(write_errors);
+
+  size_t i = 4;
+  for (const auto& [joint_name, error_status] : joint_hw_errors) {
+    status.values[i].key = joint_name + "/hardware_error";
+    status.values[i].value = hardwareErrorToString(error_status);
+    ++i;
+  }
+
+  return status;
+}
+
+std::string DiagnosticState::hardwareErrorToString(int32_t error_status)
+{
+  if (error_status == OK) {
+    return "ok";
+  }
+  std::string result;
+  if (error_status & VOLTAGE_ERROR)
+    result += "Voltage, ";
+  if (error_status & HALL_SENSOR_ERROR)
+    result += "Hall Sensor, ";
+  if (error_status & OVERHEATING_ERROR)
+    result += "Overheating, ";
+  if (error_status & MOTOR_ENCODER_ERROR)
+    result += "Motor Encoder, ";
+  if (error_status & ELECTRICAL_SHOCK_ERROR)
+    result += "Electrical Shock, ";
+  if (error_status & OVERLOAD_ERROR)
+    result += "Overload, ";
+  // Remove trailing ", "
+  if (result.size() >= 2) {
+    result.erase(result.size() - 2);
+  }
+  return result;
+}
+
+}  // namespace dynamixel_ros_control
diff --git a/dynamixel_ros_control/src/dynamixel_hardware_interface.cpp b/dynamixel_ros_control/src/dynamixel_hardware_interface.cpp
index f0e9f8e..2c78463 100644
--- a/dynamixel_ros_control/src/dynamixel_hardware_interface.cpp
+++ b/dynamixel_ros_control/src/dynamixel_hardware_interface.cpp
@@ -204,6 +204,12 @@ DynamixelHardwareInterface::on_init(const hardware_interface::HardwareComponentI
   // setup controller orchestrator
   controller_orchestrator_ = std::make_shared<controller_orchestrator::ControllerOrchestrator>(node_);
 
+  // set up realtime-safe publishers (actual ROS publish happens on internal threads, not control thread)
+  rt_diagnostics_pub_ = std::make_shared<realtime_tools::RealtimePublisher<diagnostic_msgs::msg::DiagnosticArray>>(
+      node_->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("~/diagnostics", rclcpp::QoS(1).transient_local()));
+  rt_goal_joint_state_pub_ = std::make_shared<realtime_tools::RealtimePublisher<sensor_msgs::msg::JointState>>(
+      node_->create_publisher<sensor_msgs::msg::JointState>("~/goal_joint_states", 1));
+
   // set up e-stop subscription
   std::string topic = ns != "/" ? ns + "/soft_e_stop" : "/soft_e_stop";
   soft_e_stop_subscription_ = node_->create_subscription<std_msgs::msg::Bool>(
@@ -585,6 +591,10 @@ hardware_interface::return_type DynamixelHardwareInterface::read(const rclcpp::T
 
   first_read_successful_ = true;
   last_successful_read_time_ = time;
+
+  publishDiagnostics();
+  publishGoalJointStates();
+
   return hardware_interface::return_type::OK;
 }
 
@@ -637,6 +647,63 @@ hardware_interface::return_type DynamixelHardwareInterface::write(const rclcpp::
   return hardware_interface::return_type::OK;
 }
 
+void DynamixelHardwareInterface::publishDiagnostics()
+{
+  // Compare raw values to detect changes
+  DiagnosticState current;
+  current.hw_ok = isHardwareOk();
+  current.e_stop_active = e_stop_active_.load();
+  current.torque_enabled = is_torqued_.load();
+  current.read_errors = read_manager_.getErrorCount();
+  current.write_errors = control_write_manager_.getErrorCount();
+  for (const auto& [name, joint] : joints_) {
+    auto status = joint.dynamixel->hardware_error_status;
+    if (status != OK) {
+      current.joint_hw_errors[name] = status;
+    }
+  }
+
+  if (first_diagnostics_published_ && current == last_diag_state_) {
+    return;
+  }
+
+  if (!rt_diagnostics_pub_->trylock()) {
+    return;
+  }
+
+  auto& msg = rt_diagnostics_pub_->msg_;
+  msg.header.stamp = node_->now();
+  msg.status = {current.toMsg(get_name())};
+
+  rt_diagnostics_pub_->unlockAndPublish();
+  last_diag_state_ = current;
+  first_diagnostics_published_ = true;
+}
+
+void DynamixelHardwareInterface::publishGoalJointStates()
+{
+  if (rt_goal_joint_state_pub_->trylock()) {
+    auto& msg = rt_goal_joint_state_pub_->msg_;
+    msg.header.stamp = node_->now();
+    msg.name.resize(joint_names_.size());
+    msg.position.resize(joint_names_.size());
+    msg.velocity.resize(joint_names_.size());
+    msg.effort.resize(joint_names_.size());
+
+    for (size_t i = 0; i < joint_names_.size(); ++i) {
+      const auto& joint = joints_.at(joint_names_[i]);
+      msg.name[i] = joint_names_[i];
+      auto gpos_it = joint.joint_state.goal.find(hardware_interface::HW_IF_POSITION);
+      msg.position[i] = gpos_it != joint.joint_state.goal.end() ? gpos_it->second : 0.0;
+      auto gvel_it = joint.joint_state.goal.find(hardware_interface::HW_IF_VELOCITY);
+      msg.velocity[i] = gvel_it != joint.joint_state.goal.end() ? gvel_it->second : 0.0;
+      auto geff_it = joint.joint_state.goal.find(hardware_interface::HW_IF_EFFORT);
+      msg.effort[i] = geff_it != joint.joint_state.goal.end() ? geff_it->second : 0.0;
+    }
+    rt_goal_joint_state_pub_->unlockAndPublish();
+  }
+}
+
 bool DynamixelHardwareInterface::loadTransmissionConfiguration()
 {
   auto simple_transmission_loader = transmission_interface::SimpleTransmissionLoader();
diff --git a/dynamixel_ros_control/test/test_hw_diagnostics.cpp b/dynamixel_ros_control/test/test_hw_diagnostics.cpp
new file mode 100644
index 0000000..a394d79
--- /dev/null
+++ b/dynamixel_ros_control/test/test_hw_diagnostics.cpp
@@ -0,0 +1,290 @@
+// Copyright (c) 2024 Team Hector, TU Darmstadt
+// SPDX-License-Identifier: BSD-3-Clause
+
+#include "test_hardware_interface_common.hpp"
+
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+
+namespace dynamixel_ros_control::test {
+
+// ============================================================================
+// Diagnostics Tests
+// ============================================================================
+
+TEST_F(HardwareInterfaceTest, Diagnostics_PublishedOnTopic)
+{
+  diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg;
+  auto sub = tester_node_->create_subscription<diagnostic_msgs::msg::DiagnosticArray>(
+      "/athena_arm_interface/diagnostics", rclcpp::QoS(10).transient_local(),
+      [&msg](diagnostic_msgs::msg::DiagnosticArray::SharedPtr m) { msg = m; });
+
+  auto deadline = std::chrono::steady_clock::now() + 10s;
+  while (!msg && std::chrono::steady_clock::now() < deadline) {
+    executor_->spin_some();
+    std::this_thread::sleep_for(50ms);
+  }
+
+  ASSERT_NE(msg, nullptr) << "No diagnostics message received";
+  ASSERT_FALSE(msg->status.empty());
+  EXPECT_EQ(msg->status[0].level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+  EXPECT_EQ(msg->status[0].message, "OK");
+}
+
+TEST_F(HardwareInterfaceTest, Diagnostics_ContainsExpectedFields)
+{
+  diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg;
+  auto sub = tester_node_->create_subscription<diagnostic_msgs::msg::DiagnosticArray>(
+      "/athena_arm_interface/diagnostics", rclcpp::QoS(10).transient_local(),
+      [&msg](diagnostic_msgs::msg::DiagnosticArray::SharedPtr m) { msg = m; });
+
+  auto deadline = std::chrono::steady_clock::now() + 10s;
+  while (!msg && std::chrono::steady_clock::now() < deadline) {
+    executor_->spin_some();
+    std::this_thread::sleep_for(50ms);
+  }
+
+  ASSERT_NE(msg, nullptr);
+  ASSERT_FALSE(msg->status.empty());
+
+  const auto& values = msg->status[0].values;
+  std::set<std::string> found_keys;
+  for (const auto& kv : values) {
+    found_keys.insert(kv.key);
+  }
+  EXPECT_TRUE(found_keys.count("e_stop_active")) << "Missing e_stop_active";
+  EXPECT_TRUE(found_keys.count("torque_enabled")) << "Missing torque_enabled";
+  EXPECT_TRUE(found_keys.count("consecutive_read_errors")) << "Missing consecutive_read_errors";
+  EXPECT_TRUE(found_keys.count("consecutive_write_errors")) << "Missing consecutive_write_errors";
+}
+
+TEST_F(HardwareInterfaceTest, Diagnostics_ReflectsEStopState)
+{
+  // Initially should be OK with e_stop_active=false
+  diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg;
+  auto sub = tester_node_->create_subscription<diagnostic_msgs::msg::DiagnosticArray>(
+      "/athena_arm_interface/diagnostics", rclcpp::QoS(10).transient_local(),
+      [&msg](diagnostic_msgs::msg::DiagnosticArray::SharedPtr m) { msg = m; });
+
+  auto deadline = std::chrono::steady_clock::now() + 10s;
+  while (!msg && std::chrono::steady_clock::now() < deadline) {
+    executor_->spin_some();
+    std::this_thread::sleep_for(50ms);
+  }
+  ASSERT_NE(msg, nullptr);
+  EXPECT_EQ(msg->status[0].level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+  std::string e_stop_value;
+  for (const auto& kv : msg->status[0].values) {
+    if (kv.key == "e_stop_active")
+      e_stop_value = kv.value;
+  }
+  EXPECT_EQ(e_stop_value, "false");
+
+  // Activate e-stop
+  auto estop_pub = createEStopPublisher();
+  std_msgs::msg::Bool estop_msg;
+  estop_msg.data = true;
+  estop_pub->publish(estop_msg);
+  std::this_thread::sleep_for(500ms);
+
+  // Wait for diagnostics to reflect e-stop
+  deadline = std::chrono::steady_clock::now() + 5s;
+  while (std::chrono::steady_clock::now() < deadline) {
+    executor_->spin_some();
+    std::this_thread::sleep_for(50ms);
+    if (msg && !msg->status.empty() && msg->status[0].level == diagnostic_msgs::msg::DiagnosticStatus::WARN) {
+      break;
+    }
+  }
+
+  ASSERT_NE(msg, nullptr);
+  ASSERT_FALSE(msg->status.empty());
+  EXPECT_EQ(msg->status[0].level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+  EXPECT_EQ(msg->status[0].message, "E-Stop active");
+
+  for (const auto& kv : msg->status[0].values) {
+    if (kv.key == "e_stop_active") {
+      EXPECT_EQ(kv.value, "true");
+    }
+  }
+
+  // Cleanup
+  estop_msg.data = false;
+  estop_pub->publish(estop_msg);
+  std::this_thread::sleep_for(300ms);
+}
+
+TEST_F(HardwareInterfaceTest, Diagnostics_ReflectsTorqueState)
+{
+  // Initially torque should be enabled
+  diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg;
+  auto sub = tester_node_->create_subscription<diagnostic_msgs::msg::DiagnosticArray>(
+      "/athena_arm_interface/diagnostics", rclcpp::QoS(10).transient_local(),
+      [&msg](diagnostic_msgs::msg::DiagnosticArray::SharedPtr m) { msg = m; });
+
+  auto deadline = std::chrono::steady_clock::now() + 10s;
+  while (!msg && std::chrono::steady_clock::now() < deadline) {
+    executor_->spin_some();
+    std::this_thread::sleep_for(50ms);
+  }
+  ASSERT_NE(msg, nullptr);
+
+  std::string torque_value;
+  for (const auto& kv : msg->status[0].values) {
+    if (kv.key == "torque_enabled")
+      torque_value = kv.value;
+  }
+  EXPECT_EQ(torque_value, "true");
+
+  // Disable torque
+  auto torque_client = createTorqueClient();
+  ASSERT_TRUE(setTorque(torque_client, false));
+  std::this_thread::sleep_for(500ms);
+
+  // Wait for diagnostics to reflect torque off
+  deadline = std::chrono::steady_clock::now() + 5s;
+  while (std::chrono::steady_clock::now() < deadline) {
+    executor_->spin_some();
+    std::this_thread::sleep_for(50ms);
+    if (msg) {
+      for (const auto& kv : msg->status[0].values) {
+        if (kv.key == "torque_enabled" && kv.value == "false") {
+          goto found;
+        }
+      }
+    }
+  }
+found:
+
+  ASSERT_NE(msg, nullptr);
+  for (const auto& kv : msg->status[0].values) {
+    if (kv.key == "torque_enabled") {
+      EXPECT_EQ(kv.value, "false");
+    }
+  }
+
+  // Re-enable torque
+  ASSERT_TRUE(setTorque(torque_client, true));
+}
+
+TEST_F(HardwareInterfaceTest, Diagnostics_OnlyPublishedOnChange)
+{
+  // Wait for the initial diagnostics message (published on first state)
+  std::vector<diagnostic_msgs::msg::DiagnosticArray::SharedPtr> messages;
+  auto sub = tester_node_->create_subscription<diagnostic_msgs::msg::DiagnosticArray>(
+      "/athena_arm_interface/diagnostics", rclcpp::QoS(10).transient_local(),
+      [&messages](diagnostic_msgs::msg::DiagnosticArray::SharedPtr m) { messages.push_back(m); });
+
+  // Wait for the first message
+  auto deadline = std::chrono::steady_clock::now() + 10s;
+  while (messages.empty() && std::chrono::steady_clock::now() < deadline) {
+    executor_->spin_some();
+    std::this_thread::sleep_for(50ms);
+  }
+  ASSERT_GE(messages.size(), 1u) << "Expected at least one initial diagnostics message";
+
+  // Record count, then wait — no further messages should arrive since nothing changed
+  size_t count_after_initial = messages.size();
+  std::this_thread::sleep_for(1s);
+  for (int i = 0; i < 20; ++i) {
+    executor_->spin_some();
+    std::this_thread::sleep_for(50ms);
+  }
+  EXPECT_EQ(messages.size(), count_after_initial)
+      << "No additional diagnostics should be published when state is unchanged";
+
+  // Now trigger a state change (e-stop) and verify a new message arrives
+  auto estop_pub = createEStopPublisher();
+  std_msgs::msg::Bool estop_msg;
+  estop_msg.data = true;
+  estop_pub->publish(estop_msg);
+
+  deadline = std::chrono::steady_clock::now() + 3s;
+  while (messages.size() <= count_after_initial && std::chrono::steady_clock::now() < deadline) {
+    executor_->spin_some();
+    std::this_thread::sleep_for(50ms);
+  }
+
+  EXPECT_GT(messages.size(), count_after_initial) << "A new diagnostics message should be published on state change";
+  EXPECT_EQ(messages.back()->status[0].level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+
+  // Cleanup
+  estop_msg.data = false;
+  estop_pub->publish(estop_msg);
+  std::this_thread::sleep_for(300ms);
+}
+
+// ============================================================================
+// Goal Joint State Publishing Tests
+// ============================================================================
+
+TEST_F(HardwareInterfaceTest, GoalJointStates_Published)
+{
+  sensor_msgs::msg::JointState::SharedPtr msg;
+  auto sub = tester_node_->create_subscription<sensor_msgs::msg::JointState>(
+      "/athena_arm_interface/goal_joint_states", 10, [&msg](sensor_msgs::msg::JointState::SharedPtr m) { msg = m; });
+
+  auto deadline = std::chrono::steady_clock::now() + 10s;
+  while (!msg && std::chrono::steady_clock::now() < deadline) {
+    executor_->spin_some();
+    std::this_thread::sleep_for(50ms);
+  }
+
+  ASSERT_NE(msg, nullptr) << "No goal joint state message received";
+  EXPECT_GE(msg->name.size(), 7u) << "Should have at least 7 arm joint names";
+  EXPECT_EQ(msg->position.size(), msg->name.size());
+  EXPECT_EQ(msg->velocity.size(), msg->name.size());
+  EXPECT_EQ(msg->effort.size(), msg->name.size());
+
+  std::set<std::string> names(msg->name.begin(), msg->name.end());
+  EXPECT_TRUE(names.count("arm_joint_1")) << "Missing arm_joint_1";
+  EXPECT_TRUE(names.count("arm_joint_7")) << "Missing arm_joint_7";
+}
+
+TEST_F(HardwareInterfaceTest, GoalJointStates_ReflectsCommandedPosition)
+{
+  sensor_msgs::msg::JointState::SharedPtr goal_msg;
+  auto goal_sub = tester_node_->create_subscription<sensor_msgs::msg::JointState>(
+      "/athena_arm_interface/goal_joint_states", 10,
+      [&goal_msg](sensor_msgs::msg::JointState::SharedPtr m) { goal_msg = m; });
+
+  // Load controller and command a position
+  ASSERT_TRUE(loadAndActivateController("arm_position_controller"));
+
+  auto pub = tester_node_->create_publisher<std_msgs::msg::Float64MultiArray>("/arm_position_controller/commands", 10);
+  auto deadline = std::chrono::steady_clock::now() + 5s;
+  while (std::chrono::steady_clock::now() < deadline && pub->get_subscription_count() == 0) {
+    std::this_thread::sleep_for(50ms);
+    executor_->spin_some();
+  }
+  ASSERT_GT(pub->get_subscription_count(), 0u);
+
+  std_msgs::msg::Float64MultiArray cmd;
+  cmd.data = {0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5};
+  pub->publish(cmd);
+  std::this_thread::sleep_for(2s);
+
+  // Spin to receive latest messages
+  for (int i = 0; i < 20; ++i) {
+    executor_->spin_some();
+    std::this_thread::sleep_for(50ms);
+  }
+
+  // Check goal joint states reflect the commanded position
+  ASSERT_NE(goal_msg, nullptr) << "No goal joint state received";
+  for (size_t i = 0; i < goal_msg->name.size(); ++i) {
+    if (goal_msg->name[i].find("arm_joint") != std::string::npos) {
+      EXPECT_NEAR(goal_msg->position[i], 0.5, 0.15)
+          << "Joint " << goal_msg->name[i] << " goal position should be near target";
+    }
+  }
+}
+
+}  // namespace dynamixel_ros_control::test
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}