Skip to content
Open
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 21 additions & 0 deletions joint_state_broadcaster/src/joint_state_broadcaster.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -251,11 +251,20 @@ bool JointStateBroadcaster::init_joint_data()
name_if_value_mapping_[prefix_name] = {};
}
// add interface name

std::string interface_name = si->get_interface_name();
if (map_interface_to_joint_state_.count(interface_name) > 0)
{
interface_name = map_interface_to_joint_state_[interface_name];
}
else
{
RCLCPP_WARN(
get_node()->get_logger(),
"Interface '%s' of joint '%s' is not mapped to any joint state field. The default value %f "
"will be used.",
interface_name.c_str(), prefix_name.c_str(), kUninitializedValue);
}
name_if_value_mapping_[prefix_name][interface_name] = kUninitializedValue;

// filter state interfaces that have at least one of the joint_states fields,
Expand All @@ -274,7 +283,19 @@ bool JointStateBroadcaster::init_joint_data()
}
}
}
else
{
// Moreover, when there is not even the default interface (position, velocity, effort),
// then it should also print that it is not present and NaN's will be obtained in the
// respective field.
Comment on lines +288 to +290
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
// Moreover, when there is not even the default interface (position, velocity, effort),
// then it should also print that it is not present and NaN's will be obtained in the
// respective field.
// If default interfaces (pos/vel/eff) are missing, log a warning and return NaN in
// the fields.

Currently it's too wordy than necessary.

RCLCPP_WARN(
get_node()->get_logger(),
"Interface '%s' of joint '%s' is not present in JointState message fields. NaN's will be "
"filled in the respective field.",
interface_name.c_str(), prefix_name.c_str());
}
}

std::reverse(joint_names_.begin(), joint_names_.end());
if (is_model_loaded_ && params_.use_urdf_to_filter && params_.joints.empty())
{
Expand Down