Add more verbose logging on syncread failures (#15)

* Add more verbose logging on syncread failures

* Define read failure js publishing behavior

include/interbotix_xs_driver/xs_common.hpp

@@ -178,6 +178,15 @@ using MapGroup = std::unordered_map<std::string, JointGroup>;
 using MapMotor = std::unordered_map<std::string, MotorState>;
 using MapGripper = std::unordered_map<std::string, Gripper>;
 
+// Enum defining the behavior on joint state read failures
+enum ReadFailureBehavior
+{
+  // Publishes whatever is given from the dxl_wb for all joints states (-pi)
+  PUB_DXL_WB = 0,
+  // Publishes NaN values for all joint states
+  PUB_NAN = 1
+};
+
 }  // namespace interbotix_xs
 
 #endif  // INTERBOTIX_XS_DRIVER__XS_COMMON_HPP_

include/interbotix_xs_driver/xs_driver.hpp

@@ -53,8 +53,8 @@ class InterbotixDriverXS
 {
 public:
   /// @brief Constructor for the InterbotixDriverXS
-  /// @param filepath_motor_configs absolute filepah to the motor configuration file
-  /// @param filepath_mode_configs absolute filepah to the modes configuration file
+  /// @param filepath_motor_configs absolute filepath to the motor configuration file
+  /// @param filepath_mode_configs absolute filepath to the modes configuration file
   /// @param write_eeprom_on_startup whether or not to write configs to EEPROM on startup,
   ///   defaults to `true`
   /// @param logging_level the logging level of the xs_logging utility; defaults to "INFO"
@@ -252,7 +252,7 @@ class InterbotixDriverXS
 
   /// @brief Convert angular position into the linear distance from one gripper finger to the
   ///   center of the gripper servo horn
-  /// @param name name of the gripper sevo to command
+  /// @param name name of the gripper servo to command
   /// @param angular_position desired gripper angular position [rad]
   /// @returns linear position [m] from a gripper finger to the center of the gripper servo horn
   float convert_angular_position_to_linear(
@@ -394,6 +394,9 @@ class InterbotixDriverXS
   // Mutex for updating / getting joint states
   std::mutex _mutex_js;
 
+  // The behavior on joint state read failures
+  ReadFailureBehavior read_failure_behavior{ReadFailureBehavior::PUB_DXL_WB};
+
   /// @brief Loads a robot-specific 'motor_configs' yaml file and populates class variables with
   ///   its contents
   bool retrieve_motor_configs(

src/xs_driver.cpp

@@ -979,6 +979,24 @@ bool InterbotixDriverXS::retrieve_motor_configs(
     group_map.insert({name, group});
   }
 
+  YAML::Node pub_configs = motor_configs["joint_state_publisher"];
+  read_failure_behavior = static_cast<ReadFailureBehavior>(
+    pub_configs["read_failure_behavior"].as<int>(ReadFailureBehavior::PUB_DXL_WB));
+
+  // Do input validation on read failure behavior parameter
+  if (
+    read_failure_behavior < ReadFailureBehavior::PUB_DXL_WB ||
+    read_failure_behavior > ReadFailureBehavior::PUB_NAN)
+  {
+    // If out of range or invalid, default to PUB_DXL_WB/0
+    XSLOG_ERROR(
+      "Invalid option %d provided to joint_state_publisher.read_failure_behavior. "
+      "Will default to option 0.",
+      read_failure_behavior);
+    read_failure_behavior = ReadFailureBehavior::PUB_DXL_WB;
+  }
+  XSLOG_DEBUG("read_failure_behavior set to %d.", read_failure_behavior);
+
   XSLOG_INFO(
     "Loaded motor configs from '%s'.",
     filepath_motor_configs.c_str());
@@ -1288,15 +1306,17 @@ void InterbotixDriverXS::read_joint_states()
   std::vector<int32_t> get_velocity(all_ptr->joint_num, 0);
   std::vector<int32_t> get_position(all_ptr->joint_num, 0);
 
+  bool read_failed = false;
   if (dxl_wb.getProtocolVersion() == 2.0f) {
-    // Checks if data can be sent properly
+    // Execute sync read from all pinged DYNAMIXELs
     if (!dxl_wb.syncRead(
         SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT,
         all_ptr->joint_ids.data(),
         all_ptr->joint_num,
         &log))
     {
-      XSLOG_ERROR("%s", log);
+      XSLOG_ERROR("Failed syncRead: %s", log);
+      read_failed = true;
     }
 
     // Gets present current of all servos
@@ -1309,7 +1329,8 @@ void InterbotixDriverXS::read_joint_states()
         get_current.data(),
         &log))
     {
-      XSLOG_ERROR("%s", log);
+      XSLOG_ERROR("Failed getSyncReadData for Present_Current: %s", log);
+      read_failed = true;
     }
 
     // Gets present velocity of all servos
@@ -1322,7 +1343,8 @@ void InterbotixDriverXS::read_joint_states()
         get_velocity.data(),
         &log))
     {
-      XSLOG_ERROR("%s", log);
+      XSLOG_ERROR("Failed getSyncReadData for Present_Velocity: %s", log);
+      read_failed = true;
     }
 
     // Gets present position of all servos
@@ -1335,14 +1357,15 @@ void InterbotixDriverXS::read_joint_states()
         get_position.data(),
         &log))
     {
-      XSLOG_ERROR("%s", log);
+      XSLOG_ERROR("Failed getSyncReadData for Present_Position: %s", log);
+      read_failed = true;
     }
 
     uint8_t index = 0;
     for (auto const & id : all_ptr->joint_ids) {
-      float position = 0;
-      float velocity = 0;
-      float effort = 0;
+      float position = 0.0;
+      float velocity = 0.0;
+      float effort = 0.0;
 
       if (strcmp(dxl_wb.getModelName(id), "XL-320") == 0) {
         effort = dxl_wb.convertValue2Load(get_current.at(index));
@@ -1370,7 +1393,8 @@ void InterbotixDriverXS::read_joint_states()
           get_all_data.data(),
           &log))
       {
-        XSLOG_ERROR("%s", log);
+        XSLOG_ERROR("Failed readRegister for joint states: %s", log);
+        read_failed = true;
       }
 
       int16_t effort_raw = DXL_MAKEWORD(get_all_data.at(4), get_all_data.at(5));
@@ -1387,6 +1411,31 @@ void InterbotixDriverXS::read_joint_states()
       robot_positions.push_back(position);
     }
   }
+
+  // If read failed, check to see what motors we can actually read from. This will provide
+  // some additional troubleshooting information on what motors may have been disconnected or are
+  // unresponsive
+  if (read_failed) {
+    // Note that this process is slow and dramatically reduces the joint state pub frequency
+    // However, we are in a failure state so performance does not matter
+    for (const auto id : all_ptr->joint_ids) {
+      int32_t value = 0;
+      // Try to read from an item available on all motor models
+      if (!dxl_wb.itemRead(id, "ID", &value)) {
+        // Log the motor IDs we can't read from
+        XSLOG_ERROR("[xs_sdk] Failed to read from DYNAMIXEL ID: %d", id);
+      }
+    }
+    // If read failed and SDK is configured to publish NaNs, fill the joint state message with NaNs
+    if (read_failure_behavior == ReadFailureBehavior::PUB_NAN) {
+      const auto nan_states = std::vector<float>(
+        robot_positions.size(),
+        std::numeric_limits<float>::quiet_NaN());
+      robot_positions = std::vector<float>(nan_states);
+      robot_velocities = std::vector<float>(nan_states);
+      robot_efforts = std::vector<float>(nan_states);
+    }
+  }
 }
 
 std::vector<std::string> InterbotixDriverXS::get_all_joint_names()