debug: unify bus_watchdog bulk-read path and fix states sample

Author: michikawa07

dynamixel_handler/src/dynamixel_handler_dyn_interface_loop.cpp

@@ -623,21 +623,22 @@ tuple<double, uint8_t> DynamixelHandler::BulkReadExtra_slow(const set<id_t>& id_
     }
 
     // Group F2
-    map<id_t, vector<DynamixelAddress>> id_addrs_map_f2;
-    for ( auto id : valid_id_set ) switch ( series_[id] ) {
-        case SERIES_X: id_addrs_map_f2[id] = {AddrX::bus_watchdog}; break;
-        case SERIES_P: id_addrs_map_f2[id] = {AddrP::bus_watchdog}; break;
-        default: break; // ここに来るのはSERIES_UNKNOWNとSERIES_PROだけのはず(SERIES_PROはbus_watchdogを持っていない)
-    }
-    if ( !id_addrs_map_f2.empty() ) {
-        const auto id_data_vec_map = BulkRead_log(id_addrs_map_f2, verbose_["r_extra"], verbose_["r_extra_err"]);
-        const int N_total = id_addrs_map_f2.size();
-        const int N_suc   = id_data_vec_map.size();
-        for ( const auto& [id, data] : id_data_vec_map ) // X, P ともに1つ要素しか持たないという事前情報で簡略化して書いてる．
-            watchdog_r_[id] = id_addrs_map_f2[id][0].pulse2val(data[0], model_[id]);
-        sum_rate += N_total == 0 ? 0.0 : N_suc / (double)N_total;
-        n_rate   += 1.0;
-    }
+    map<id_t, vector<DynamixelAddress>> id_addrs_map_f2;
+    for ( auto id : valid_id_set ) switch ( series_[id] ) {
+        case SERIES_X: id_addrs_map_f2[id] = {AddrX::bus_watchdog}; break;
+        case SERIES_P: id_addrs_map_f2[id] = {AddrP::bus_watchdog}; break;
+        case SERIES_PRO: id_addrs_map_f2[id] = {AddrPro::bus_watchdog}; break; // dummy address
+        default: break; // ここに来るのはSERIES_UNKNOWNだけのはず
+    }
+    if ( !id_addrs_map_f2.empty() ) {
+        const auto id_data_vec_map = BulkRead_log(id_addrs_map_f2, verbose_["r_extra"], verbose_["r_extra_err"]);
+        const int N_total = id_addrs_map_f2.size();
+        const int N_suc   = id_data_vec_map.size();
+        for ( const auto& [id, data] : id_data_vec_map ) // 全シリーズとも1要素のみ
+            watchdog_r_[id] = id_addrs_map_f2[id][0].pulse2val(data[0], model_[id]);
+        sum_rate += N_total == 0 ? 0.0 : N_suc / (double)N_total;
+        n_rate   += 1.0;
+    }
 
     if ( n_rate == 0.0 ) return {1.0, 0}; else return {sum_rate / n_rate, n_id};
 }

dynamixel_handler_msgs/ReadMe.md

@@ -295,19 +295,19 @@ extra:                                           # DynamixelExtra型
    id_list: [1, 2, 3, 4]
    model: ["X", "X", "P", "Pro"]
    model_number: [0, 0, 0, 0]
-   protocol_type: [2, 2, 2, 0]
+   protocol_type: [2, 2, 2, 2]
    firmware_version: [0, 0, 0, 0]
    realtime_tick_s: [0.0, 0.0, 0.0, .nan]
    moving_status:
-      velocity_profile: ["none", "none", "none", ""]
+      velocity_profile: ["none", "none", "none", "none"]
       following_error: [false, false, false, false]
       profile_ongoing: [false, false, false, false]
       in_position: [false, false, false, false]
    moving: [false, false, false, false]
    return_delay_time_us: [0.0, 0.0, 0.0, 0.0]
    drive_mode:
       torque_on_by_goal_update: [false, false, false, false]
-      profile_configuration: ["velocity_based", "velocity_based", "velocity_based", ""]
+      profile_configuration: ["velocity_based", "velocity_based", "velocity_based", "velocity_based"]
       reverse_mode: [false, false, false, false]
    shadow_id: [255, 255, 255, 0]
    homing_offset_deg: [0.0, 0.0, 0.0, 0.0]
@@ -325,9 +325,9 @@ extra:                                           # DynamixelExtra型
       input_voltage_error: [false, false, false, false]
    led:
       red_percent: [0.0, 0.0, 0.0, 0.0]
-      blue_percent: [.nan, .nan, 0.0, .nan]
-      green_percent: [.nan, .nan, 0.0, .nan]
-   bus_watchdog_ms: [500.0, 500.0, 500.0, .nan]
+      blue_percent: [.nan, .nan, 0.0, 0.0]
+      green_percent: [.nan, .nan, 0.0, 0.0]
+   bus_watchdog_ms: [500.0, 500.0, 500.0, .nan]
    reboot: [false, false, false, false]
 ```