added status msgs

dynamixel_ros_control/CMakeLists.txt

@@ -20,16 +20,20 @@ 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)
 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

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

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.

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>

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

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();