Skip to content

Commit de8fa48

Browse files
committed
Revert "Handle cases of state interfaces with different data types"
This reverts commit 4a1364b.
1 parent 4a1364b commit de8fa48

File tree

1 file changed

+3
-22
lines changed

1 file changed

+3
-22
lines changed

joint_state_broadcaster/src/joint_state_broadcaster.cpp

Lines changed: 3 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -414,29 +414,10 @@ controller_interface::return_type JointStateBroadcaster::update(
414414
for (auto i = 0u; i < state_interfaces_.size(); ++i)
415415
{
416416
// no retries, just try to get the latest value once
417-
if (state_interfaces_[i].get_data_type() == hardware_interface::HandleDataType::DOUBLE)
417+
const auto & opt = state_interfaces_[i].get_optional(0);
418+
if (opt.has_value())
418419
{
419-
const auto & opt = state_interfaces_[i].get_optional(0);
420-
if (opt.has_value())
421-
{
422-
*mapped_values_[i] = opt.value();
423-
}
424-
}
425-
else if (state_interfaces_[i].get_data_type() == hardware_interface::HandleDataType::BOOL)
426-
{
427-
const auto & opt = state_interfaces_[i].get_optional<bool>(0);
428-
if (opt.has_value())
429-
{
430-
*mapped_values_[i] = static_cast<double>(opt.value());
431-
}
432-
}
433-
else
434-
{
435-
RCLCPP_DEBUG(
436-
get_node()->get_logger(),
437-
"Unsupported data type for state interface '%s'. "
438-
"Only double and bool are supported.",
439-
state_interfaces_[i].get_name().c_str());
420+
*mapped_values_[i] = opt.value();
440421
}
441422
}
442423

0 commit comments

Comments
 (0)