Skip to content

Commit

Permalink
Add read failure publishing options (#46)
Browse files Browse the repository at this point in the history
  • Loading branch information
@lukeschmitt-tr
lukeschmitt-tr authored Jul 1, 2024
1 parent 73eccb9 commit 6d535e4
Show file tree
Hide file tree
Showing 3 changed files with 61 additions and 27 deletions.
3 changes: 3 additions & 0 deletions interbotix_ros_xseries/interbotix_xs_sdk/config/motor_configs_template.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,9 @@ joint_state_publisher:
update_rate: 100 # Optional; rate at which the motor states are sampled (and potentially published) in Hz; defaults to '100'
publish_states: true # Optional; boolean that either enables or disables the Publisher; defaults to 'true'
topic_name: joint_states # Optional; desired JointState topic name; defaults to 'joint_states'
read_failure_behavior: 0 # Optional; the behavior on joint state read failures; defaults to 0
# 0: Publishes whatever is given from the dxl_wb for all joints states (-pi)
# 1: Publishes NaN values for all joints

groups: # Optional; defines an unlimited number of joint groups in the robot; custom 'group' names can be used.
arm: [waist, shoulder, elbow, wrist_angle, wrist_rotate] # Optional; groups can contain overlapping joints but those joints will retain the operating mode of the latter group.
Expand Down
11 changes: 10 additions & 1 deletion interbotix_ros_xseries/interbotix_xs_sdk/include/interbotix_xs_sdk/xs_sdk_obj.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,12 @@ struct MotorInfo // Struc
int32_t value; // Value to write to the above register for the specified motor
};

enum ReadFailureBehavior // The behavior on joint state read failures
{
PUB_DXL_WB = 0, // Publishes whatever is given from the dxl_wb for all joints states (-pi)
PUB_NAN = 1 // Publishes NaN values for all joint states
};

// Interbotix Core Class to build any type of Dynamixel-based robot
class InterbotixRobotXS
{
Expand Down Expand Up @@ -159,7 +165,7 @@ class InterbotixRobotXS
float robot_convert_linear_position_to_radian(std::string const& name, float const& linear_position);

/// @brief Converts a specified 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]
/// @param <float> [out] - linear position [m] from a gripper finger to the center of the gripper servo horn
float robot_convert_angular_position_to_linear(std::string const& name, float const& angular_position);
Expand Down Expand Up @@ -205,6 +211,9 @@ class InterbotixRobotXS
std::unordered_map<std::string, Gripper> gripper_map; // Dictionary mapping the name of a gripper motor with information about it (as defined in the Gripper struct)
std::unordered_map<std::string, size_t> js_index_map; // Dictionary mapping the name of a joint with its position in the JointState 'name' list

// 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
/// @param <bool> [out] - True if the motor configs were successfully retrieved; False otherwise
bool robot_get_motor_configs(void);
Expand Down
74 changes: 48 additions & 26 deletions interbotix_ros_xseries/interbotix_xs_sdk/src/xs_sdk_obj.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -453,7 +453,7 @@ float InterbotixRobotXS::robot_convert_linear_position_to_radian(std::string con
}

/// @brief Converts a specified 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]
/// @param <float> [out] - linear position [m] from a gripper finger to the center of the gripper servo horn
float InterbotixRobotXS::robot_convert_angular_position_to_linear(std::string const& name, float const& angular_position)
Expand Down Expand Up @@ -618,6 +618,15 @@ bool InterbotixRobotXS::robot_get_motor_configs(void)
timer_hz = pub_configs["update_rate"].as<int>(100);
pub_states = pub_configs["publish_states"].as<bool>(true);
js_topic = pub_configs["topic_name"].as<std::string>("joint_states");
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
ROS_ERROR("[xs_sdk] 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;
}
ROS_DEBUG("[xs_sdk::robot_get_motor_configs] read_failure_behavior set to %d.", read_failure_behavior);

ROS_INFO("[xs_sdk] Loaded motor configs from '%s'.", motor_configs_file.c_str());
return true;
Expand Down Expand Up @@ -1158,17 +1167,17 @@ void InterbotixRobotXS::robot_update_joint_states(const ros::TimerEvent &e)
std::vector<int32_t> get_position(all_ptr->joint_num, 0);
joint_state_msg.name = all_ptr->joint_names;

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,
all_ptr->joint_ids.data(),
all_ptr->joint_num,
&log))
{
ROS_ERROR("[xs_sdk] Failed syncRead: %s", log);
syncread_failed = true;
read_failed = true;
}

// Gets present current of all servos
Expand All @@ -1181,7 +1190,7 @@ void InterbotixRobotXS::robot_update_joint_states(const ros::TimerEvent &e)
&log))
{
ROS_ERROR("[xs_sdk] Failed getSyncReadData for Present_Current: %s", log);
syncread_failed = true;
read_failed = true;
}

// Gets present velocity of all servos
Expand All @@ -1194,7 +1203,7 @@ void InterbotixRobotXS::robot_update_joint_states(const ros::TimerEvent &e)
&log))
{
ROS_ERROR("[xs_sdk] Failed getSyncReadData for Present_Velocity: %s", log);
syncread_failed = true;
read_failed = true;
}

// Gets present position of all servos
Expand All @@ -1207,31 +1216,15 @@ void InterbotixRobotXS::robot_update_joint_states(const ros::TimerEvent &e)
&log))
{
ROS_ERROR("[xs_sdk] 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))
{
ROS_ERROR("[xs_sdk] 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));
else effort = dxl_wb.convertValue2Current(get_current.at(index));
Expand All @@ -1258,7 +1251,8 @@ void InterbotixRobotXS::robot_update_joint_states(const ros::TimerEvent &e)
get_all_data.data(),
&log))
{
ROS_ERROR("[xs_sdk] %s", log);
ROS_ERROR("[xs_sdk] 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));
Expand Down Expand Up @@ -1287,6 +1281,34 @@ void InterbotixRobotXS::robot_update_joint_states(const ros::TimerEvent &e)
joint_state_msg.effort.push_back(0);
joint_state_msg.effort.push_back(0);
}

// 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
ROS_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<double>(joint_state_msg.position.size(), std::numeric_limits<double>::quiet_NaN());
joint_state_msg.position = std::vector<double>(nan_states);
joint_state_msg.velocity = std::vector<double>(nan_states);
joint_state_msg.effort = std::vector<double>(nan_states);
}
}

// Publish the message to the joint_states topic
joint_state_msg.header.stamp = ros::Time::now();
joint_states = joint_state_msg;
Expand Down

0 comments on commit 6d535e4

Please sign in to comment.