From a9eea4dc31d58e453f33b4baabc6e39b758d7ef8 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 25 Nov 2025 08:20:50 +0000 Subject: [PATCH 01/11] Add test for bool --- .../test/cm_topic_hardware_component_test.cpp | 23 ++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/cm_topic_hardware_component/test/cm_topic_hardware_component_test.cpp b/cm_topic_hardware_component/test/cm_topic_hardware_component_test.cpp index c15f5f0..adca7f1 100644 --- a/cm_topic_hardware_component/test/cm_topic_hardware_component_test.cpp +++ b/cm_topic_hardware_component/test/cm_topic_hardware_component_test.cpp @@ -242,6 +242,7 @@ TEST_F(TestTopicBasedSystem, topic_based_system_2dof) + @@ -267,7 +268,7 @@ TEST_F(TestTopicBasedSystem, topic_based_system_2dof) // Check interfaces EXPECT_EQ(1u, rm_->system_components_size()); - ASSERT_EQ(12u, rm_->state_interface_keys().size()); + ASSERT_EQ(13u, rm_->state_interface_keys().size()); EXPECT_TRUE(rm_->state_interface_exists("joint1/position")); EXPECT_TRUE(rm_->state_interface_exists("joint1/velocity")); EXPECT_TRUE(rm_->state_interface_exists("joint2/position")); @@ -278,6 +279,7 @@ TEST_F(TestTopicBasedSystem, topic_based_system_2dof) EXPECT_TRUE(rm_->state_interface_exists("flange_analog_IOs/analog_output1")); EXPECT_TRUE(rm_->state_interface_exists("flange_analog_IOs/analog_input1")); EXPECT_TRUE(rm_->state_interface_exists("flange_analog_IOs/analog_input2")); + EXPECT_TRUE(rm_->state_interface_exists("flange_analog_IOs/digital_input1")); EXPECT_TRUE(rm_->state_interface_exists("force_sensor/force.x")); EXPECT_TRUE(rm_->state_interface_exists("force_sensor/force.y")); EXPECT_TRUE(rm_->state_interface_exists("force_sensor/force.z")); @@ -298,6 +300,7 @@ TEST_F(TestTopicBasedSystem, topic_based_system_2dof) hardware_interface::LoanedStateInterface g_ao_s = rm_->claim_state_interface("flange_analog_IOs/analog_output1"); hardware_interface::LoanedStateInterface g_ai1_s = rm_->claim_state_interface("flange_analog_IOs/analog_input1"); hardware_interface::LoanedStateInterface g_ai2_s = rm_->claim_state_interface("flange_analog_IOs/analog_input2"); + hardware_interface::LoanedStateInterface g_di1_s = rm_->claim_state_interface("flange_analog_IOs/digital_input1"); hardware_interface::LoanedStateInterface f_x_s = rm_->claim_state_interface("force_sensor/force.x"); hardware_interface::LoanedStateInterface f_y_s = rm_->claim_state_interface("force_sensor/force.y"); hardware_interface::LoanedStateInterface f_z_s = rm_->claim_state_interface("force_sensor/force.z"); @@ -312,6 +315,7 @@ TEST_F(TestTopicBasedSystem, topic_based_system_2dof) EXPECT_TRUE(std::isnan(g_ao_s.get_optional().value())); EXPECT_TRUE(std::isnan(g_ai1_s.get_optional().value())); EXPECT_TRUE(std::isnan(g_ai2_s.get_optional().value())); + EXPECT_EQ(g_di1_s.get_optional().value(), false); EXPECT_EQ(f_x_s.get_optional().value(), 0.0); EXPECT_EQ(f_y_s.get_optional().value(), 0.0); EXPECT_EQ(f_z_s.get_optional().value(), 0.0); @@ -342,6 +346,7 @@ TEST_F(TestTopicBasedSystem, topic_based_system_2dof) EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); EXPECT_TRUE(std::isnan(j3_v_s.get_optional().value())); + EXPECT_EQ(g_di1_s.get_optional().value(), false); EXPECT_EQ(j1_p_c.get_optional().value(), 0.11); EXPECT_EQ(j2_p_c.get_optional().value(), 0.12); @@ -375,6 +380,7 @@ TEST_F(TestTopicBasedSystem, topic_based_system_2dof) EXPECT_TRUE(std::isnan(g_ao_s.get_optional().value())); EXPECT_TRUE(std::isnan(g_ai1_s.get_optional().value())); EXPECT_TRUE(std::isnan(g_ai2_s.get_optional().value())); + EXPECT_EQ(g_di1_s.get_optional().value(), false); EXPECT_EQ(f_x_s.get_optional().value(), 0.0); EXPECT_EQ(f_y_s.get_optional().value(), 0.0); EXPECT_EQ(f_z_s.get_optional().value(), 0.0); @@ -411,6 +417,7 @@ TEST_F(TestTopicBasedSystem, topic_based_system_2dof) // other states remain unchanged EXPECT_TRUE(std::isnan(g_ai1_s.get_optional().value())); EXPECT_TRUE(std::isnan(g_ai2_s.get_optional().value())); + EXPECT_EQ(g_di1_s.get_optional().value(), false); EXPECT_EQ(f_y_s.get_optional().value(), 0.0); EXPECT_EQ(f_z_s.get_optional().value(), 0.0); @@ -439,6 +446,20 @@ TEST_F(TestTopicBasedSystem, topic_based_system_2dof) EXPECT_EQ(g_ao_s.get_optional().value(), 1.4); EXPECT_TRUE(std::isnan(g_ai1_s.get_optional().value())); EXPECT_TRUE(std::isnan(g_ai2_s.get_optional().value())); + EXPECT_EQ(g_di1_s.get_optional().value(), false); EXPECT_EQ(f_y_s.get_optional().value(), 0.0); EXPECT_EQ(f_z_s.get_optional().value(), 0.0); + + // publish boolean value as double + publish({ "state_interface.flange_analog_IOs/digital_input1" }, { 1.0 }, 4u); + + wait_for_msg(std::chrono::milliseconds{ 100 }); + + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + + // was it processed and casted to bool correctly? + EXPECT_EQ(g_di1_s.get_optional().value(), true); } From 15c973dbb85ca1ae187b536b9a010376c5e824b4 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 25 Nov 2025 08:21:26 +0000 Subject: [PATCH 02/11] Apply google codestyle for includes --- .../cm_topic_hardware_component.hpp | 11 +++++------ .../src/cm_topic_hardware_component.cpp | 4 ++-- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/cm_topic_hardware_component/include/cm_topic_hardware_component/cm_topic_hardware_component.hpp b/cm_topic_hardware_component/include/cm_topic_hardware_component/cm_topic_hardware_component.hpp index 21971f4..76c1e5f 100644 --- a/cm_topic_hardware_component/include/cm_topic_hardware_component/cm_topic_hardware_component.hpp +++ b/cm_topic_hardware_component/include/cm_topic_hardware_component/cm_topic_hardware_component.hpp @@ -21,12 +21,11 @@ #include // ROS -#include -#include -#include - -#include -#include +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_component_interface_params.hpp" +#include "pal_statistics_msgs/msg/statistics_names.hpp" +#include "pal_statistics_msgs/msg/statistics_values.hpp" +#include "rclcpp/subscription.hpp" namespace cm_topic_hardware_component { diff --git a/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp b/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp index 998a205..9e15ef0 100644 --- a/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp +++ b/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp @@ -14,9 +14,9 @@ /* Author: Christoph Froehlich */ -#include +#include "cm_topic_hardware_component/cm_topic_hardware_component.hpp" -#include +#include namespace cm_topic_hardware_component { From 43b231cf536326c3707ff8833316005c178771e2 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 25 Nov 2025 08:27:03 +0000 Subject: [PATCH 03/11] Remove comment for RT publisher --- cm_topic_hardware_component/src/cm_topic_hardware_component.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp b/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp index 9e15ef0..3e67f7f 100644 --- a/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp +++ b/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp @@ -27,7 +27,7 @@ CallbackReturn CMTopicSystem::on_init(const hardware_interface::HardwareComponen { return CallbackReturn::ERROR; } - // TODO(christophfroehlich): should we use RT box here? + pal_names_subscriber_ = get_node()->create_subscription( "~/names", rclcpp::SensorDataQoS(), [this](const pal_statistics_msgs::msg::StatisticsNames::SharedPtr pal_names) { pal_statistics_names_per_topic_[pal_names->names_version] = std::move(pal_names->names); From 824a2afb1cfcc472a0b5d1a7c7d5f7847d7cf7ce Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 25 Nov 2025 08:27:53 +0000 Subject: [PATCH 04/11] Cast to other datatypes than double --- .../src/cm_topic_hardware_component.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp b/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp index 3e67f7f..85afc3d 100644 --- a/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp +++ b/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp @@ -63,8 +63,10 @@ hardware_interface::return_type CMTopicSystem::read(const rclcpp::Time& /*time*/ const auto& name = names[i].substr(prefix.length()); if (has_state(name)) { - // TODO(christophfroehlich): does not support other values than double now - set_state(name, latest_pal_values_.values.at(i)); + const auto handle = get_state_interface_handle(name); + // in case of non-double interface datatypes, try to cast from double + const auto v = handle->get_data_type().cast_from_double(latest_pal_values_.values.at(i)); + std::visit([&](auto&& concrete) { set_state(handle, concrete); }, v); } } } From bb7c235bb215cc8e5d09240ec19de4959c979043 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 25 Nov 2025 08:29:48 +0000 Subject: [PATCH 05/11] TEMP: use PR branch for ros2_control --- topic_based_hardware_interfaces.rolling.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/topic_based_hardware_interfaces.rolling.repos b/topic_based_hardware_interfaces.rolling.repos index 8b26f10..4e6fcf9 100644 --- a/topic_based_hardware_interfaces.rolling.repos +++ b/topic_based_hardware_interfaces.rolling.repos @@ -2,7 +2,7 @@ repositories: ros2_control: type: git url: https://github.com/ros-controls/ros2_control.git - version: master + version: add/cast_from_double realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git From 3fd51b1230c35a79eadbc46e1fc20630a3df9ed8 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 25 Nov 2025 11:01:25 +0000 Subject: [PATCH 06/11] Make the cast manually --- .../src/cm_topic_hardware_component.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp b/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp index 85afc3d..585cbe3 100644 --- a/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp +++ b/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp @@ -65,8 +65,19 @@ hardware_interface::return_type CMTopicSystem::read(const rclcpp::Time& /*time*/ { const auto handle = get_state_interface_handle(name); // in case of non-double interface datatypes, try to cast from double - const auto v = handle->get_data_type().cast_from_double(latest_pal_values_.values.at(i)); - std::visit([&](auto&& concrete) { set_state(handle, concrete); }, v); + const auto type = handle->get_data_type(); + switch (type) + { + case hardware_interface::HandleDataType::DOUBLE: + set_state(handle, latest_pal_values_.values.at(i)); + break; + case hardware_interface::HandleDataType::BOOL: + set_state(handle, static_cast(latest_pal_values_.values.at(i))); + break; + default: + // silently ignore unsupported datatypes + break; + } } } } From 3355ece0d507fd68491194efed5369ef311bdf87 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 25 Nov 2025 11:01:31 +0000 Subject: [PATCH 07/11] Revert "TEMP: use PR branch for ros2_control" This reverts commit bb7c235bb215cc8e5d09240ec19de4959c979043. --- topic_based_hardware_interfaces.rolling.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/topic_based_hardware_interfaces.rolling.repos b/topic_based_hardware_interfaces.rolling.repos index 4e6fcf9..8b26f10 100644 --- a/topic_based_hardware_interfaces.rolling.repos +++ b/topic_based_hardware_interfaces.rolling.repos @@ -2,7 +2,7 @@ repositories: ros2_control: type: git url: https://github.com/ros-controls/ros2_control.git - version: add/cast_from_double + version: master realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git From 824f449795dec2c2d9ba7f32e55322b67608f19b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 25 Nov 2025 11:18:55 +0000 Subject: [PATCH 08/11] TEMP: use PR branch for ros2_control --- topic_based_hardware_interfaces.rolling.repos | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/topic_based_hardware_interfaces.rolling.repos b/topic_based_hardware_interfaces.rolling.repos index 8b26f10..13a6204 100644 --- a/topic_based_hardware_interfaces.rolling.repos +++ b/topic_based_hardware_interfaces.rolling.repos @@ -1,8 +1,8 @@ repositories: ros2_control: type: git - url: https://github.com/ros-controls/ros2_control.git - version: master + url: https://github.com/pal-robotics-forks/ros2_control.git + version: add/state_and_command_interface_handle_getters realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git From 5912cc1316f0df045be2128cc840726271ef59b1 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 26 Nov 2025 20:00:04 +0000 Subject: [PATCH 09/11] Add debug message if message is skipped --- .../src/cm_topic_hardware_component.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp b/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp index 585cbe3..1e013cb 100644 --- a/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp +++ b/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp @@ -75,7 +75,9 @@ hardware_interface::return_type CMTopicSystem::read(const rclcpp::Time& /*time*/ set_state(handle, static_cast(latest_pal_values_.values.at(i))); break; default: - // silently ignore unsupported datatypes + // ignore unsupported datatypes + RCLCPP_DEBUG(get_node()->get_logger(), "Ignoring unsupported state interface datatype for interface '%s'", + name.c_str()); break; } } From 8676bb62a8f9c5c05b98720399ff39963cc19139 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 1 Dec 2025 20:31:51 +0100 Subject: [PATCH 10/11] Use upstream master --- topic_based_hardware_interfaces.rolling.repos | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/topic_based_hardware_interfaces.rolling.repos b/topic_based_hardware_interfaces.rolling.repos index 13a6204..8b26f10 100644 --- a/topic_based_hardware_interfaces.rolling.repos +++ b/topic_based_hardware_interfaces.rolling.repos @@ -1,8 +1,8 @@ repositories: ros2_control: type: git - url: https://github.com/pal-robotics-forks/ros2_control.git - version: add/state_and_command_interface_handle_getters + url: https://github.com/ros-controls/ros2_control.git + version: master realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git From f3bd146da9085f0d773a2dfa8c3661cdaf0dfba3 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 2 Dec 2025 11:43:24 +0000 Subject: [PATCH 11/11] Update upstream API --- .../src/cm_topic_hardware_component.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp b/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp index 1e013cb..d4dd744 100644 --- a/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp +++ b/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp @@ -69,10 +69,10 @@ hardware_interface::return_type CMTopicSystem::read(const rclcpp::Time& /*time*/ switch (type) { case hardware_interface::HandleDataType::DOUBLE: - set_state(handle, latest_pal_values_.values.at(i)); + set_state(handle, latest_pal_values_.values.at(i), true); break; case hardware_interface::HandleDataType::BOOL: - set_state(handle, static_cast(latest_pal_values_.values.at(i))); + set_state(handle, static_cast(latest_pal_values_.values.at(i)), true); break; default: // ignore unsupported datatypes