Diagnostic support

[Noa-Thouard-Nexxis]

I've recently added support for the control method offered by this dynamixel_hardware_interface + ros2 control for my 4 legged 4-dof robot. The process was relatively smooth however the interface seems to be missing diagnostic state information required for safe operation.

The current dxl_state i.e.

header:
  stamp:
    sec:
    nanosec:
  frame_id:
comm_state:
id:
torque_state:
dxl_hw_state:

Lacks present temperature & hardware error status fields.

As far as I can tell the only way to expose these are via the service: dynamixel_hardware_interface/get_dxl_data which uses item names found in the .model (https://github.com/ROBOTIS-GIT/dynamixel_hardware_interface/blob/main/param/dxl_model/ph54_200_s500.model).

From my usage of dynamixel_hardware_interface/get_dxl_data via command line so far. I've found that the service itself to be very slow... and the return type uint32 isn't great for many of the fields.

First attempt with 5 second timeout - FAILS with timeout output

$ ros2 service call /dynamixel_hardware_interface/port1/get_dxl_data dynamixel_interfaces/srv/GetDataFromDxl "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: ''}, id: 11, item_name: 'Present Temperature', timeout_sec: 5.0}"
requester: making request: dynamixel_interfaces.srv.GetDataFromDxl_Request(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), id=11, item_name='Present Temperature', timeout_sec=5.0)
response:
dynamixel_interfaces.srv.GetDataFromDxl_Response(item_data=0, result=False)

Second attempt with 10 second timeout - SUCCEEDS

$ ros2 service call /dynamixel_hardware_interface/port1/get_dxl_data dynamixel_interfaces/srv/GetDataFromDxl "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: ''}, id: 11, item_name: 'Present Temperature', timeout_sec: 10.0}"
requester: making request: dynamixel_interfaces.srv.GetDataFromDxl_Request(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), id=11, item_name='Present Temperature', timeout_sec=10.0)
response:
dynamixel_interfaces.srv.GetDataFromDxl_Response(item_data=42, result=True)

This demonstrates:

• A simple temperature read requires >5 seconds but <10 seconds to complete
• The same call succeeds or fails purely based on timeout value

And this is the result of a working field! Many of the others listed in the [control table] simply don't. For instance Hardware Error Status.

Please let me know if I'm missing something and/or if there's another method for getting these states.

[Noa-Thouard-Nexxis]

Update:

I've created a servo diagnostics ros2 control plugin some of it's features include

• It enables torque control of individual motors via dynamixel_interfaces::srv::SetDataToDxl
• Periodically gets and exposes temperature via dynamixel_interfaces::srv::GetDataFromDxl

This is how I retrieve the temperature via dynamixel_interfaces::srv::GetDataFromDxl for each of my servos (19 servos across 4 different ports):

void JointStateDiagnostics::request_data_from_dxl_interface(Servo& servo, const std::string& item_name,
                                                            double timeout_sec) {
    if (servo.get_data_from_dxl_client_->service_is_ready() &&
        !(*servo.get_data_from_dxl_request_pending.readFromRT())) {
        const auto& name = servo.diagnostic.name;
        auto request = std::make_shared<GetDataFromDxl::Request>();
        request->header = common::makeHeader(name);
        request->id = servo.diagnostic.id;
        request->item_name = item_name;
        request->timeout_sec = timeout_sec;
        servo.get_data_from_dxl_client_->async_send_request(
            request, [this, name](rclcpp::Client<GetDataFromDxl>::SharedFuture future) {
                try {
                    auto response = future.get();
                    auto& servo = servo_states_[name];
                    servo.get_data_from_dxl_request_pending.writeFromNonRT(false);
                    if (response->result) {
                        servo.rt_present_temperature_.writeFromNonRT(response->item_data);
                    } else {
                        RCLCPP_WARN(this->get_node()->get_logger(), "Failed to get dxl temp data for servo: %s",
                                    name.c_str());
                    }
                } catch (const std::exception& e) {
                    servo_states_[name].get_data_from_dxl_request_pending.writeFromNonRT(false);
                    RCLCPP_ERROR(this->get_node()->get_logger(), "Exception getting temp for servo %s: %s",
                                 name.c_str(), e.what());
                }
            });
        servo.get_data_from_dxl_request_pending.writeFromNonRT(true);
    }
}

It works however, I believe it's throttling the dynamixel_hardware_interface - with this called every time a request isn't pending, I noticed issues/throttling with the joint state broadcaster and other ros2 control plugins to use the state/command interfaces. In the console I see periodic timeouts from dynamixel_hardware_interface even with it set to 20seconds. It does not seem capable of consistently reporting control_table data which i suspected but I'm not sure there any other way to access it?

I've also noticed that the information in "dynamixel_hardware_interface/dxl_state" doesn't seem to reflect the real state of the motors. In the below example all servos on this port are torqued however torque is reported as false... I then switched the torque on/off multiple times, it never changed from false.

`header:
  stamp:
    sec: 1769049919
    nanosec: 452492323
  frame_id: ''
comm_state: 0
id:
- 11
- 12
- 13
- 14
- 41
- 42
- 43
- 44
torque_state:
- false
- false
- false
- false
- false
- false
- false
- false
dxl_hw_state:
- 0
- 0
- 0
- 0
- 0
- 0
- 0
- 0

[Woojin-Crive]

Hi @Noa-Thouard-Nexxis ,

Sorry for the torque_state, the code has some bug for handling torque_state.
It should be fixed.

You can add 'Hardware Error Status' for state_interface in gpio tag to get a dxl_hw_state informations.

[Noa-Thouard-Nexxis]

Hey,

Okay thanks for the info! I'll double check that I've pulled the latest for the bug fix.
That covers 'Hardware Error Status' and 'Torque Reading' which is great.

In terms of more slightly obscure control table elements I require, like temperature, etc. Is my only option to use the dynamixel_interfaces::srv::GetDataFromDxl service? Ideally I wouldn't want to use a method that introduces any type of blocking/delay to the read/writing of the joint states.

Additionally, this more of a comment. But I find it a bit strange that the reboot/torqued state is only done across an entire port rather than on at a individual motor level. To me it makes more sense to create a variation of reboot_dxl_srv_name & dynamixel_hardware_interface/set_dxl_torque where you can include a specific motor id. In my experience errors/requirements for a reboot/torque are often triggered on a single servo, not across every servo on a port. I've gotten round this by using dynamixel_hardware_interface/set_dxl_data to target the torque state on a single leg, however I don't believe the same can be done to reboot.

[Noa-Thouard-Nexxis]

Unfortunately I've stumbled onto another bug with the current interface...dynamixel_hardware_interface/set_dxl_torque isn't able to set torque ON. Calling the service allows you to switch the torque all motors on a port OFF but not ON. Tried via my plugin and command line, doesn't work.

I was able to overcome this by using dynamixel_hardware_interface/set_dxl_data to target each individual motor, setting torque ON/OFF which works. But the dynamixel_hardware_interface/set_dxl_torque doesn't work, atleast for me.