added status msgs

[Joschi3]

Added diagnostic messages for status output and a real-time publisher publishing the target values.

[Copilot]

Pull request overview

This PR adds diagnostic message publishing and real-time goal joint state publishing to the Dynamixel hardware interface. The changes enable monitoring of the hardware interface's status (e-stop state, torque state, error counts, and per-joint hardware errors) and tracking commanded joint positions in real-time.

Changes:

• Added a DiagnosticState struct for efficient change detection and diagnostic message generation
• Integrated real-time publishers for diagnostics (with transient_local QoS) and goal joint states into the hardware interface's read cycle
• Added comprehensive tests for diagnostic publishing behavior and goal joint state accuracy

Reviewed changes

Copilot reviewed 7 out of 7 changed files in this pull request and generated 8 comments.

Show a summary per file

File	Description
dynamixel_ros_control/include/dynamixel_ros_control/diagnostic_state.hpp	New header defining the DiagnosticState struct for lightweight status snapshots and message conversion
dynamixel_ros_control/src/diagnostic_state.cpp	Implementation of diagnostic message generation and hardware error string conversion
dynamixel_ros_control/include/dynamixel_ros_control/dynamixel_hardware_interface.hpp	Added realtime publisher members and diagnostic state tracking to the hardware interface
dynamixel_ros_control/src/dynamixel_hardware_interface.cpp	Integrated diagnostic and goal joint state publishing into the read cycle with change detection
dynamixel_ros_control/test/test_hw_diagnostics.cpp	Comprehensive test suite covering diagnostic publishing behavior, state changes, and goal joint state accuracy
dynamixel_ros_control/CMakeLists.txt	Added new dependencies (diagnostic_msgs, sensor_msgs, realtime_tools) and source files to the build
dynamixel_ros_control/package.xml	Moved realtime_tools from test dependency to runtime dependency and added diagnostic_msgs and sensor_msgs

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[Copilot]

The test expects goal positions to be "near 0.5" with a tolerance of 0.15. This is a relatively large tolerance (30% of the target value). While this may be appropriate for this specific hardware/controller combination, consider documenting why this tolerance is needed, or if the tolerance could be tightened to catch potential control issues more reliably.

Suggested change

	if (goal_msg->name[i].find("arm_joint") != std::string::npos) {
	if (goal_msg->name[i].find("arm_joint") != std::string::npos) {
	// NOTE: The tolerance of 0.15 (30% of the 0.5 target) is intentionally conservative.
	// It accounts for potential delays between commanding the controller, propagation
	// through the ROS 2 control pipeline, and the publication of goal joint states
	// on real hardware. This helps keep the test robust to timing jitter and transient
	// effects while still verifying that the commanded position is being tracked.
	// If the controller / hardware combination proves more precise and stable over time,
	// this tolerance can be reduced to make the test more sensitive to control regressions.

[Copilot]

The comment states "no heap allocations" but the struct contains std::map<std::string, int32_t> joint_hw_errors which performs heap allocations when entries are added or when strings are stored. This comment is misleading. Consider updating the comment to be more accurate, such as "Lightweight snapshot for cheap change detection using value comparison" without claiming no heap allocations.

Suggested change

	/// @brief Lightweight snapshot of diagnostic values for cheap change detection (no heap allocations).
	/// @brief Lightweight snapshot of diagnostic values for cheap change detection using value comparison.

[Copilot]

The test assumes the first status in the array msg->status[0] is the relevant one without verifying which status entry it is. In diagnostic message arrays, there can be multiple status entries for different components. Consider checking the status.name field to ensure you're examining the correct diagnostic status entry for the hardware interface, or document why index 0 is always the correct entry.

[Copilot]

If error_status contains error flags that are not recognized by this function (e.g., new error types added to the hardware), the function will return an empty string for a non-OK status. Consider adding a fallback case that returns something like "Unknown error" or includes the raw error status value to help with debugging when new error types are encountered.

Suggested change

	}
	}
	if (result.empty()) {
	return "Unknown error (status=" + std::to_string(error_status) + ")";
	}

[Copilot]

The test verifies that diagnostics are only published on change by counting messages. However, the QoS is set to transient_local, which means late-joining subscribers might receive the last published message immediately. The test creates a subscription and then waits for the initial message, but if the hardware interface had already published a diagnostic message before the subscription was created, the transient_local QoS would deliver it. This is the intended behavior and the test accounts for it correctly by waiting for at least one message. However, consider adding a comment explaining that the transient_local QoS is intentional and how it affects the test expectations.

Suggested change

	std::vector<diagnostic_msgs::msg::DiagnosticArray::SharedPtr> messages;
	std::vector<diagnostic_msgs::msg::DiagnosticArray::SharedPtr> messages;
	// Use transient_local QoS so late-joining subscribers receive the last latched
	// diagnostics message immediately. The test logic deliberately waits for at least
	// one initial message before asserting that no additional diagnostics are published
	// while the hardware state remains unchanged.

[Copilot]

The use of goto here is non-idiomatic and makes the control flow harder to understand. Consider using a boolean flag variable to break out of the loop instead. For example: bool torque_updated = false; and check for it after the loop, or refactor to use a helper function that can return early.

Suggested change

	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:
	bool torque_disabled_reported = false;
	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") {
	torque_disabled_reported = true;
	break;
	}
	}
	if (torque_disabled_reported) {
	break;
	}
	}
	}

[Copilot]

The comment "Remove trailing ', '" states that 2 characters should be removed, but the erased length should account for the actual trailing string which is ", " (comma and space). While the code is correct (2 characters), the string concatenation logic always adds ", " after each error type. However, if only one error is present, the trailing ", " is correctly removed. Consider using a more robust string building approach, such as using a vector to collect error strings and then joining them with ", " to avoid the need for trailing removal logic.

[Copilot]

The test expects at least 7 arm joint names but doesn't verify that the test setup actually has 7 arm joints configured. If the hardware configuration changes to have fewer joints, this test would fail with an unclear error. Consider checking the actual joint count from the fixture or making the expectation more specific by checking for each expected joint name individually.

Suggested change

EXPECT_GE(msg->name.size(), 7u) << "Should have at least 7 arm joint names";