Skip to content

Commit cc37685

Browse files
christophfroehlichchristophfroehlich
authored
Add cast to boolean for cm_topic component (#64)
1 parent a151ab3 commit cc37685

File tree

3 files changed

+47
-12
lines changed

3 files changed

+47
-12
lines changed

‎cm_topic_hardware_component/include/cm_topic_hardware_component/cm_topic_hardware_component.hpp‎

Lines changed: 5 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -21,12 +21,11 @@
2121
#include <string>
2222

2323
// ROS
24-
#include <hardware_interface/system_interface.hpp>
25-
#include <hardware_interface/types/hardware_component_interface_params.hpp>
26-
#include <rclcpp/subscription.hpp>
27-
28-
#include <pal_statistics_msgs/msg/statistics_names.hpp>
29-
#include <pal_statistics_msgs/msg/statistics_values.hpp>
24+
#include "hardware_interface/system_interface.hpp"
25+
#include "hardware_interface/types/hardware_component_interface_params.hpp"
26+
#include "pal_statistics_msgs/msg/statistics_names.hpp"
27+
#include "pal_statistics_msgs/msg/statistics_values.hpp"
28+
#include "rclcpp/subscription.hpp"
3029

3130
namespace cm_topic_hardware_component
3231
{

‎cm_topic_hardware_component/src/cm_topic_hardware_component.cpp‎

Lines changed: 20 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -14,9 +14,9 @@
1414

1515
/* Author: Christoph Froehlich */
1616

17-
#include <string>
17+
#include "cm_topic_hardware_component/cm_topic_hardware_component.hpp"
1818

19-
#include <cm_topic_hardware_component/cm_topic_hardware_component.hpp>
19+
#include <string>
2020

2121
namespace cm_topic_hardware_component
2222
{
@@ -27,7 +27,7 @@ CallbackReturn CMTopicSystem::on_init(const hardware_interface::HardwareComponen
2727
{
2828
return CallbackReturn::ERROR;
2929
}
30-
// TODO(christophfroehlich): should we use RT box here?
30+
3131
pal_names_subscriber_ = get_node()->create_subscription<pal_statistics_msgs::msg::StatisticsNames>(
3232
"~/names", rclcpp::SensorDataQoS(), [this](const pal_statistics_msgs::msg::StatisticsNames::SharedPtr pal_names) {
3333
pal_statistics_names_per_topic_[pal_names->names_version] = std::move(pal_names->names);
@@ -63,8 +63,23 @@ hardware_interface::return_type CMTopicSystem::read(const rclcpp::Time& /*time*/
6363
const auto& name = names[i].substr(prefix.length());
6464
if (has_state(name))
6565
{
66-
// TODO(christophfroehlich): does not support other values than double now
67-
set_state(name, latest_pal_values_.values.at(i));
66+
const auto handle = get_state_interface_handle(name);
67+
// in case of non-double interface datatypes, try to cast from double
68+
const auto type = handle->get_data_type();
69+
switch (type)
70+
{
71+
case hardware_interface::HandleDataType::DOUBLE:
72+
set_state(handle, latest_pal_values_.values.at(i), true);
73+
break;
74+
case hardware_interface::HandleDataType::BOOL:
75+
set_state(handle, static_cast<bool>(latest_pal_values_.values.at(i)), true);
76+
break;
77+
default:
78+
// ignore unsupported datatypes
79+
RCLCPP_DEBUG(get_node()->get_logger(), "Ignoring unsupported state interface datatype for interface '%s'",
80+
name.c_str());
81+
break;
82+
}
6883
}
6984
}
7085
}

‎cm_topic_hardware_component/test/cm_topic_hardware_component_test.cpp‎

Lines changed: 22 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -242,6 +242,7 @@ TEST_F(TestTopicBasedSystem, topic_based_system_2dof)
242242
<state_interface name="analog_output1"/>
243243
<state_interface name="analog_input1"/>
244244
<state_interface name="analog_input2"/>
245+
<state_interface name="digital_input1" data_type="bool"/>
245246
</gpio>
246247
<sensor name="force_sensor">
247248
<state_interface name="force.x">
@@ -267,7 +268,7 @@ TEST_F(TestTopicBasedSystem, topic_based_system_2dof)
267268

268269
// Check interfaces
269270
EXPECT_EQ(1u, rm_->system_components_size());
270-
ASSERT_EQ(12u, rm_->state_interface_keys().size());
271+
ASSERT_EQ(13u, rm_->state_interface_keys().size());
271272
EXPECT_TRUE(rm_->state_interface_exists("joint1/position"));
272273
EXPECT_TRUE(rm_->state_interface_exists("joint1/velocity"));
273274
EXPECT_TRUE(rm_->state_interface_exists("joint2/position"));
@@ -278,6 +279,7 @@ TEST_F(TestTopicBasedSystem, topic_based_system_2dof)
278279
EXPECT_TRUE(rm_->state_interface_exists("flange_analog_IOs/analog_output1"));
279280
EXPECT_TRUE(rm_->state_interface_exists("flange_analog_IOs/analog_input1"));
280281
EXPECT_TRUE(rm_->state_interface_exists("flange_analog_IOs/analog_input2"));
282+
EXPECT_TRUE(rm_->state_interface_exists("flange_analog_IOs/digital_input1"));
281283
EXPECT_TRUE(rm_->state_interface_exists("force_sensor/force.x"));
282284
EXPECT_TRUE(rm_->state_interface_exists("force_sensor/force.y"));
283285
EXPECT_TRUE(rm_->state_interface_exists("force_sensor/force.z"));
@@ -298,6 +300,7 @@ TEST_F(TestTopicBasedSystem, topic_based_system_2dof)
298300
hardware_interface::LoanedStateInterface g_ao_s = rm_->claim_state_interface("flange_analog_IOs/analog_output1");
299301
hardware_interface::LoanedStateInterface g_ai1_s = rm_->claim_state_interface("flange_analog_IOs/analog_input1");
300302
hardware_interface::LoanedStateInterface g_ai2_s = rm_->claim_state_interface("flange_analog_IOs/analog_input2");
303+
hardware_interface::LoanedStateInterface g_di1_s = rm_->claim_state_interface("flange_analog_IOs/digital_input1");
301304
hardware_interface::LoanedStateInterface f_x_s = rm_->claim_state_interface("force_sensor/force.x");
302305
hardware_interface::LoanedStateInterface f_y_s = rm_->claim_state_interface("force_sensor/force.y");
303306
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)
312315
EXPECT_TRUE(std::isnan(g_ao_s.get_optional().value()));
313316
EXPECT_TRUE(std::isnan(g_ai1_s.get_optional().value()));
314317
EXPECT_TRUE(std::isnan(g_ai2_s.get_optional().value()));
318+
EXPECT_EQ(g_di1_s.get_optional<bool>().value(), false);
315319
EXPECT_EQ(f_x_s.get_optional().value(), 0.0);
316320
EXPECT_EQ(f_y_s.get_optional().value(), 0.0);
317321
EXPECT_EQ(f_z_s.get_optional().value(), 0.0);
@@ -342,6 +346,7 @@ TEST_F(TestTopicBasedSystem, topic_based_system_2dof)
342346
EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value()));
343347
EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value()));
344348
EXPECT_TRUE(std::isnan(j3_v_s.get_optional().value()));
349+
EXPECT_EQ(g_di1_s.get_optional<bool>().value(), false);
345350

346351
EXPECT_EQ(j1_p_c.get_optional().value(), 0.11);
347352
EXPECT_EQ(j2_p_c.get_optional().value(), 0.12);
@@ -375,6 +380,7 @@ TEST_F(TestTopicBasedSystem, topic_based_system_2dof)
375380
EXPECT_TRUE(std::isnan(g_ao_s.get_optional().value()));
376381
EXPECT_TRUE(std::isnan(g_ai1_s.get_optional().value()));
377382
EXPECT_TRUE(std::isnan(g_ai2_s.get_optional().value()));
383+
EXPECT_EQ(g_di1_s.get_optional<bool>().value(), false);
378384
EXPECT_EQ(f_x_s.get_optional().value(), 0.0);
379385
EXPECT_EQ(f_y_s.get_optional().value(), 0.0);
380386
EXPECT_EQ(f_z_s.get_optional().value(), 0.0);
@@ -411,6 +417,7 @@ TEST_F(TestTopicBasedSystem, topic_based_system_2dof)
411417
// other states remain unchanged
412418
EXPECT_TRUE(std::isnan(g_ai1_s.get_optional().value()));
413419
EXPECT_TRUE(std::isnan(g_ai2_s.get_optional().value()));
420+
EXPECT_EQ(g_di1_s.get_optional<bool>().value(), false);
414421
EXPECT_EQ(f_y_s.get_optional().value(), 0.0);
415422
EXPECT_EQ(f_z_s.get_optional().value(), 0.0);
416423

@@ -439,6 +446,20 @@ TEST_F(TestTopicBasedSystem, topic_based_system_2dof)
439446
EXPECT_EQ(g_ao_s.get_optional().value(), 1.4);
440447
EXPECT_TRUE(std::isnan(g_ai1_s.get_optional().value()));
441448
EXPECT_TRUE(std::isnan(g_ai2_s.get_optional().value()));
449+
EXPECT_EQ(g_di1_s.get_optional<bool>().value(), false);
442450
EXPECT_EQ(f_y_s.get_optional().value(), 0.0);
443451
EXPECT_EQ(f_z_s.get_optional().value(), 0.0);
452+
453+
// publish boolean value as double
454+
publish({ "state_interface.flange_analog_IOs/digital_input1" }, { 1.0 }, 4u);
455+
456+
wait_for_msg(std::chrono::milliseconds{ 100 });
457+
458+
ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result);
459+
ASSERT_EQ(ret, hardware_interface::return_type::OK);
460+
ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result);
461+
ASSERT_EQ(ret, hardware_interface::return_type::OK);
462+
463+
// was it processed and casted to bool correctly?
464+
EXPECT_EQ(g_di1_s.get_optional<bool>().value(), true);
444465
}

0 commit comments

Comments
 (0)