Skip to content

Add cast to boolean for cm_topic component #64

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
christophfroehlich merged 12 commits into from
Dec 3, 2025
Merged

Add cast to boolean for cm_topic component #64

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
a9eea4d
Add test for bool
christophfroehlich Nov 25, 2025
15c973d
Apply google codestyle for includes
christophfroehlich Nov 25, 2025
43b231c
Remove comment for RT publisher
christophfroehlich Nov 25, 2025
824a2af
Cast to other datatypes than double
christophfroehlich Nov 25, 2025
bb7c235
TEMP: use PR branch for ros2_control
christophfroehlich Nov 25, 2025
3fd51b1
Make the cast manually
christophfroehlich Nov 25, 2025
3355ece
Revert "TEMP: use PR branch for ros2_control"
christophfroehlich Nov 25, 2025
824f449
TEMP: use PR branch for ros2_control
christophfroehlich Nov 25, 2025
5912cc1
Add debug message if message is skipped
christophfroehlich Nov 26, 2025
8676bb6
Use upstream master
christophfroehlich Dec 1, 2025
f3bd146
Update upstream API
christophfroehlich Dec 2, 2025
4d0db1e
Merge branch 'main' into cm_topic/bool
christophfroehlich Dec 2, 2025
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
11 changes: 5 additions & 6 deletions ...ic_hardware_component/include/cm_topic_hardware_component/cm_topic_hardware_component.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,12 +21,11 @@
#include <string>

// ROS
#include <hardware_interface/system_interface.hpp>
#include <hardware_interface/types/hardware_component_interface_params.hpp>
#include <rclcpp/subscription.hpp>

#include <pal_statistics_msgs/msg/statistics_names.hpp>
#include <pal_statistics_msgs/msg/statistics_values.hpp>
#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
{
Expand Down
25 changes: 20 additions & 5 deletions cm_topic_hardware_component/src/cm_topic_hardware_component.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,9 +14,9 @@

/* Author: Christoph Froehlich */

#include <string>
#include "cm_topic_hardware_component/cm_topic_hardware_component.hpp"

#include <cm_topic_hardware_component/cm_topic_hardware_component.hpp>
#include <string>

namespace cm_topic_hardware_component
{
Expand All @@ -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<pal_statistics_msgs::msg::StatisticsNames>(
"~/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);
Expand Down Expand Up @@ -63,8 +63,23 @@ 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 type = handle->get_data_type();
switch (type)
{
case hardware_interface::HandleDataType::DOUBLE:
set_state(handle, latest_pal_values_.values.at(i), true);
break;
case hardware_interface::HandleDataType::BOOL:
set_state(handle, static_cast<bool>(latest_pal_values_.values.at(i)), true);
break;
default:
// ignore unsupported datatypes
RCLCPP_DEBUG(get_node()->get_logger(), "Ignoring unsupported state interface datatype for interface '%s'",
name.c_str());
break;
}
}
}
}
Expand Down
23 changes: 22 additions & 1 deletion cm_topic_hardware_component/test/cm_topic_hardware_component_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -242,6 +242,7 @@ TEST_F(TestTopicBasedSystem, topic_based_system_2dof)
<state_interface name="analog_output1"/>
<state_interface name="analog_input1"/>
<state_interface name="analog_input2"/>
<state_interface name="digital_input1" data_type="bool"/>
</gpio>
<sensor name="force_sensor">
<state_interface name="force.x">
Expand All @@ -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"));
Expand All @@ -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"));
Expand All @@ -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");
Expand All @@ -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<bool>().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);
Expand Down Expand Up @@ -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<bool>().value(), false);

EXPECT_EQ(j1_p_c.get_optional().value(), 0.11);
EXPECT_EQ(j2_p_c.get_optional().value(), 0.12);
Expand Down Expand Up @@ -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<bool>().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);
Expand Down Expand Up @@ -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<bool>().value(), false);
EXPECT_EQ(f_y_s.get_optional().value(), 0.0);
EXPECT_EQ(f_z_s.get_optional().value(), 0.0);

Expand Down Expand Up @@ -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<bool>().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<bool>().value(), true);
}
Loading