diff --git a/include/interbotix_xs_driver/xs_common.hpp b/include/interbotix_xs_driver/xs_common.hpp index 39a0f98..4a888d2 100644 --- a/include/interbotix_xs_driver/xs_common.hpp +++ b/include/interbotix_xs_driver/xs_common.hpp @@ -178,6 +178,15 @@ using MapGroup = std::unordered_map; using MapMotor = std::unordered_map; using MapGripper = std::unordered_map; +// 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_ diff --git a/include/interbotix_xs_driver/xs_driver.hpp b/include/interbotix_xs_driver/xs_driver.hpp index 790a4ea..69fd0be 100644 --- a/include/interbotix_xs_driver/xs_driver.hpp +++ b/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( diff --git a/src/xs_driver.cpp b/src/xs_driver.cpp index 5824bfd..90eb136 100644 --- a/src/xs_driver.cpp +++ b/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( + pub_configs["read_failure_behavior"].as(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,8 +1306,8 @@ void InterbotixDriverXS::read_joint_states() std::vector get_velocity(all_ptr->joint_num, 0); std::vector get_position(all_ptr->joint_num, 0); + bool read_failed = false; if (dxl_wb.getProtocolVersion() == 2.0f) { - bool syncread_failed = false; // Execute sync read from all pinged DYNAMIXELs if (!dxl_wb.syncRead( SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, @@ -1298,7 +1316,7 @@ void InterbotixDriverXS::read_joint_states() &log)) { XSLOG_ERROR("Failed syncRead: %s", log); - syncread_failed = true; + read_failed = true; } // Gets present current of all servos @@ -1312,7 +1330,7 @@ void InterbotixDriverXS::read_joint_states() &log)) { XSLOG_ERROR("Failed getSyncReadData for Present_Current: %s", log); - syncread_failed = true; + read_failed = true; } // Gets present velocity of all servos @@ -1326,7 +1344,7 @@ void InterbotixDriverXS::read_joint_states() &log)) { XSLOG_ERROR("Failed getSyncReadData for Present_Velocity: %s", log); - syncread_failed = true; + read_failed = true; } // Gets present position of all servos @@ -1340,27 +1358,14 @@ void InterbotixDriverXS::read_joint_states() &log)) { XSLOG_ERROR("Failed getSyncReadData for Present_Position: %s", log); - syncread_failed = true; - } - - // If syncread 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 (syncread_failed) { - 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)) { - XSLOG_ERROR("Failed to read from DYNAMIXEL ID: %d", id); - } - } + 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)); @@ -1388,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)); @@ -1405,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( + robot_positions.size(), + std::numeric_limits::quiet_NaN()); + robot_positions = std::vector(nan_states); + robot_velocities = std::vector(nan_states); + robot_efforts = std::vector(nan_states); + } + } } std::vector InterbotixDriverXS::get_all_joint_names()