From 4a1364b3e8bdb35ddc76db19bedde45413330c21 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 30 Jun 2025 23:50:41 +0200 Subject: [PATCH 1/3] Handle cases of state interfaces with different data types --- .../src/joint_state_broadcaster.cpp | 25 ++++++++++++++++--- 1 file changed, 22 insertions(+), 3 deletions(-) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index b88971a3a6..1230866114 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -414,10 +414,29 @@ controller_interface::return_type JointStateBroadcaster::update( for (auto i = 0u; i < state_interfaces_.size(); ++i) { // no retries, just try to get the latest value once - const auto & opt = state_interfaces_[i].get_optional(0); - if (opt.has_value()) + if (state_interfaces_[i].get_data_type() == hardware_interface::HandleDataType::DOUBLE) { - *mapped_values_[i] = opt.value(); + const auto & opt = state_interfaces_[i].get_optional(0); + if (opt.has_value()) + { + *mapped_values_[i] = opt.value(); + } + } + else if (state_interfaces_[i].get_data_type() == hardware_interface::HandleDataType::BOOL) + { + const auto & opt = state_interfaces_[i].get_optional(0); + if (opt.has_value()) + { + *mapped_values_[i] = static_cast(opt.value()); + } + } + else + { + RCLCPP_DEBUG( + get_node()->get_logger(), + "Unsupported data type for state interface '%s'. " + "Only double and bool are supported.", + state_interfaces_[i].get_name().c_str()); } } From de8fa489f3181116611a3e67a5f106641c9fcf8d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 1 Jul 2025 23:22:14 +0200 Subject: [PATCH 2/3] Revert "Handle cases of state interfaces with different data types" This reverts commit 4a1364b3e8bdb35ddc76db19bedde45413330c21. --- .../src/joint_state_broadcaster.cpp | 25 +++---------------- 1 file changed, 3 insertions(+), 22 deletions(-) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 1230866114..b88971a3a6 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -414,29 +414,10 @@ controller_interface::return_type JointStateBroadcaster::update( for (auto i = 0u; i < state_interfaces_.size(); ++i) { // no retries, just try to get the latest value once - if (state_interfaces_[i].get_data_type() == hardware_interface::HandleDataType::DOUBLE) + const auto & opt = state_interfaces_[i].get_optional(0); + if (opt.has_value()) { - const auto & opt = state_interfaces_[i].get_optional(0); - if (opt.has_value()) - { - *mapped_values_[i] = opt.value(); - } - } - else if (state_interfaces_[i].get_data_type() == hardware_interface::HandleDataType::BOOL) - { - const auto & opt = state_interfaces_[i].get_optional(0); - if (opt.has_value()) - { - *mapped_values_[i] = static_cast(opt.value()); - } - } - else - { - RCLCPP_DEBUG( - get_node()->get_logger(), - "Unsupported data type for state interface '%s'. " - "Only double and bool are supported.", - state_interfaces_[i].get_name().c_str()); + *mapped_values_[i] = opt.value(); } } From 8a7fb3afbebc3beae7adcadc50f271d1a0fbf380 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 1 Jul 2025 23:58:38 +0200 Subject: [PATCH 3/3] cleanup complex non-castable interfaces on activate --- .../src/joint_state_broadcaster.cpp | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index b88971a3a6..491911a7b6 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -201,6 +201,29 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_activate( return CallbackReturn::ERROR; } + // Erase the interfaces that are not castable to double + std::vector state_interfaces_to_keep; + std::for_each( + state_interfaces_.begin(), state_interfaces_.end(), + [&state_interfaces_to_keep](hardware_interface::LoanedStateInterface & si) + { + if (si.is_castable_to_double()) + { + state_interfaces_to_keep.push_back(std::move(si)); + } + else + { + RCLCPP_ERROR( + rclcpp::get_logger("JointStateBroadcaster"), + "State interface '%s' is not castable to double. " + "It will not be used in JointStateBroadcaster.", + si.get_name().c_str()); + } + }); + + state_interfaces_.clear(); + state_interfaces_ = std::move(state_interfaces_to_keep); + init_auxiliary_data(); init_joint_state_msg(); init_dynamic_joint_state_msg();