From f9081f9a4a5f5504526457230519fbf0b70f9d58 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Mon, 16 Dec 2024 12:35:08 +0100 Subject: [PATCH 01/32] add semantic component command interface --- .../semantic_component_command_interface.hpp | 110 ++++++++++++++++++ 1 file changed, 110 insertions(+) create mode 100644 controller_interface/include/semantic_components/semantic_component_command_interface.hpp diff --git a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp new file mode 100644 index 0000000000..e5a1ea4952 --- /dev/null +++ b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp @@ -0,0 +1,110 @@ +// Copyright (c) 2024, Sherpa Mobile Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SEMANTIC_COMPONENTS__SEMANTIC_COMPONENT_COMMAND_INTERFACE_HPP_ +#define SEMANTIC_COMPONENTS__SEMANTIC_COMPONENT_COMMAND_INTERFACE_HPP_ + +#include +#include + +#include "controller_interface/helpers.hpp" +#include "hardware_interface/loaned_command_interface.hpp" + + +namespace semantic_components +{ +template +class SemanticComponentCommandInterface +{ +public: + explicit SemanticComponentCommandInterface(const std::string & name, size_t size = 0) + { + name_ = name; + interface_names_.reserve(size); + command_interfaces_.reserve(size); + } + + ~SemanticComponentCommandInterface() = default; + + /// Assign loaned command interfaces from the hardware. + /** + * Assign loaned command interfaces on the controller start. + * + * \param[in] command_interfaces vector of command interfaces provided by the controller. + */ + bool assign_loaned_command_interfaces( + std::vector & command_interfaces) + { + return controller_interface::get_ordered_interfaces( + command_interfaces, interface_names_, "", command_interfaces_); + } + + /// Release loaned command interfaces from the hardware. + void release_interfaces() { command_interfaces_.clear(); } + + /// Definition of command interface names for the component. + /** + * The function should be used in "command_interface_configuration()" of a controller to provide + * standardized command interface names semantic component. + * + * \default Default implementation defined command interfaces as "name/NR" where NR is number + * from 0 to size of values; + * \return list of strings with command interface names for the semantic component. + */ + virtual std::vector get_command_interface_names() + { + if (interface_names_.empty()) + { + for (auto i = 0u; i < interface_names_.capacity(); ++i) + { + interface_names_.emplace_back(name_ + "/" + std::to_string(i + 1)); + } + } + return interface_names_; + } + + /// Return all values. + /** + * \return true if it gets all the values, else false + */ + bool set_values(const std::vector & values) + { + // check we have sufficient memory + if (values.values() != command_interfaces_.size()) + { + return false; + } + // set values + for (size_t i = 0; i < values.size(); ++i) + { + command_interfaces_[i].set_value(values[i]); + } + return true; + } + + /// Set values from MessageInputType + /** + * \return false by default + */ + bool set_values_from_message(const MessageInputType & /* message */) { return false; } + +protected: + std::string name_; + std::vector interface_names_; + std::vector> command_interfaces_; +}; + +} // namespace semantic_components + +#endif // SEMANTIC_COMPONENTS__SEMANTIC_COMPONENT_COMMAND_INTERFACE_HPP_ From 5585ef6f4406869c0ccefd8f2eda2e458b5844c6 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Mon, 16 Dec 2024 12:35:08 +0100 Subject: [PATCH 02/32] add led_rgb_device --- .../semantic_components/led_rgb_device.hpp | 75 +++++++++++++++++++ 1 file changed, 75 insertions(+) create mode 100644 controller_interface/include/semantic_components/led_rgb_device.hpp diff --git a/controller_interface/include/semantic_components/led_rgb_device.hpp b/controller_interface/include/semantic_components/led_rgb_device.hpp new file mode 100644 index 0000000000..ad47672202 --- /dev/null +++ b/controller_interface/include/semantic_components/led_rgb_device.hpp @@ -0,0 +1,75 @@ +// Copyright (c) 2024, Sherpa Mobile Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SEMANTIC_COMPONENTS__LED_RGB_DEVICE_HPP_ +#define SEMANTIC_COMPONENTS__LED_RGB_DEVICE_HPP_ + +#include +#include +#include + +#include "semantic_components/semantic_component_command_interface.hpp" +#include "std_msgs/msg/color_rgba.hpp" + +namespace semantic_components +{ +class LEDRgbDevice : public SemanticComponentCommandInterface +{ +public: + /** + * Constructor for a LED RGB device with interface names set based on device name. + * The constructor sets the command interface names to "/r", "/g", "/b". + */ + explicit LEDRgbDevice(const std::string & name) : SemanticComponentCommandInterface(name, 3) + { + interface_names_.emplace_back(name_ + "/" + "r"); + interface_names_.emplace_back(name_ + "/" + "g"); + interface_names_.emplace_back(name_ + "/" + "b"); + } + + /** + * Constructor for a LED RGB device with custom interface names. + * The constructor takes the three command interface names for the red, green, and blue channels. + */ + explicit LEDRgbDevice( + const std::string & interface_r, const std::string & interface_g, + const std::string & interface_b) + : SemanticComponentCommandInterface("", 3) + { + interface_names_.emplace_back(interface_r); + interface_names_.emplace_back(interface_g); + interface_names_.emplace_back(interface_b); + } + + virtual ~LEDRgbDevice() = default; + + /// Set LED states from ColorRGBA message + bool set_values_from_message(std_msgs::msg::ColorRGBA & message) + { + if ( + message.r < 0 || message.r > 1 || message.g < 0 || message.g > 1 || message.b < 0 || + message.b > 1) + { + return false; + } + command_interfaces_[0].set_value(message.r); + command_interfaces_[1].set_value(message.g); + command_interfaces_[2].set_value(message.b); + return true; + } +}; + +} // namespace semantic_components + +#endif // SEMANTIC_COMPONENTS__LED_RGB_DEVICE_HPP_ From 7d08efda27334849a49bf3bc7bb4f740a1d3ba94 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Mon, 16 Dec 2024 12:35:08 +0100 Subject: [PATCH 03/32] add tests for LED device RQ: not build yet, issue with rest of repos for build. Will rebase on previous TAG for now. --- controller_interface/CMakeLists.txt | 11 +++ .../test/test_led_rgb_device.cpp | 85 +++++++++++++++++++ .../test/test_led_rgb_device.hpp | 59 +++++++++++++ 3 files changed, 155 insertions(+) create mode 100644 controller_interface/test/test_led_rgb_device.cpp create mode 100644 controller_interface/test/test_led_rgb_device.hpp diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index 85294c68d1..672d1e02ce 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -85,6 +85,17 @@ if(BUILD_TESTING) ament_target_dependencies(test_pose_sensor geometry_msgs ) + + # Semantic component command interface tests + + ament_add_gmock(test_led_rgb_device test/test_led_rgb_device.cpp) + target_link_libraries(test_led_rgb_device + controller_interface + hardware_interface::hardware_interface + ) + ament_target_dependencies(test_led_rgb_device + std_msgs + ) endif() install( diff --git a/controller_interface/test/test_led_rgb_device.cpp b/controller_interface/test/test_led_rgb_device.cpp new file mode 100644 index 0000000000..c52b2f9a0b --- /dev/null +++ b/controller_interface/test/test_led_rgb_device.cpp @@ -0,0 +1,85 @@ +// Copyright (c) 2024, Sherpa Mobile Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_led_rgb_device.hpp" + +void LedDeviceTest::SetUp() +{ + full_cmd_interface_names_.reserve(size_); + for (const auto & interface_name : interface_names_) + { + full_cmd_interface_names_.emplace_back(device_name_ + '/' + interface_name); + } +} + +void LedDeviceTest::TearDown() { led_device_.reset(nullptr); } + +TEST_F(LedDeviceTest, validate_all) +{ + // Create device + led_device_ = std::make_unique(device_name_); + EXPECT_EQ(led_device_->name_, device_name_); + + // Validate reserved space for interface_names_ and command_interfaces_ + // As command_interfaces_ are not defined yet, use capacity() + ASSERT_EQ(led_device_->interface_names_.size(), size_); + ASSERT_EQ(led_device_->command_interfaces_.capacity(), size_); + + // Validate default interface_names_ + EXPECT_TRUE(std::equal( + led_device_->interface_names_.cbegin(), led_device_->interface_names_.cend(), + full_cmd_interface_names_.cbegin(), full_cmd_interface_names_.cend())); + + // Get interface names + std::vector interface_names = led_device_->get_command_interface_names(); + + // Assign values to position + hardware_interface::CommandInterface led_r{device_name_, interface_names_[0], &led_values_[0]}; + hardware_interface::CommandInterface led_g{device_name_, interface_names_[1], &led_values_[1]}; + hardware_interface::CommandInterface led_b{device_name_, interface_names_[2], &led_values_[2]}; + + // Create state interface vector in jumbled order + std::vector temp_command_interfaces; + temp_command_interfaces.reserve(3); + temp_command_interfaces.emplace_back(led_r); + temp_command_interfaces.emplace_back(led_g); + temp_command_interfaces.emplace_back(led_b); + + // Assign interfaces + led_device_->assign_loaned_command_interfaces(temp_command_interfaces); + EXPECT_EQ(led_device_->command_interfaces_.size(), size_); + + // Validate correct assignment + const std::vector test_led_values_cmd = {0.1, 0.2, 0.3}; + EXPECT_TRUE(led_device_->set_values(test_led_values_cmd)); + + EXPECT_TRUE(led_values_[0], test_led_values_cmd[0]); + EXPECT_TRUE(led_values_[1], test_led_values_cmd[1]); + EXPECT_TRUE(led_values_[2], test_led_values_cmd[2]); + + // Validate correct assignment from message + std_msgs::msg::ColorRGBA temp_message; + temp_message.r = test_led_values_cmd[0]; + temp_message.g = test_led_values_cmd[1]; + temp_message.b = test_led_values_cmd[2]; + EXPECT_TRUE(led_device_->set_values_from_message(temp_message)); + + EXPECT_TRUE(led_values_[0], temp_message.r); + EXPECT_TRUE(led_values_[1], temp_message.g); + EXPECT_TRUE(led_values_[2], temp_message.b); + + // Release command interfaces + led_device_->release_interfaces(); + ASSERT_EQ(led_device_->command_interfaces_.size(), 0); +} diff --git a/controller_interface/test/test_led_rgb_device.hpp b/controller_interface/test/test_led_rgb_device.hpp new file mode 100644 index 0000000000..0f6c3b2ec6 --- /dev/null +++ b/controller_interface/test/test_led_rgb_device.hpp @@ -0,0 +1,59 @@ +// Copyright (c) 2024, Sherpa Mobile Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_LED_RGB_DEVICE_HPP_ +#define TEST_LED_RGB_DEVICE_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include "semantic_components/led_rgb_device.hpp" + +class TestableLedDevice : public semantic_components::LEDRgbDevice +{ + FRIEND_TEST(TestableLedDevice, validate_all); + +public: + // Use default command interface names + explicit TestableLedDevice(const std::string & name) : PoseSensor{name} {} + + virtual ~TestableLedDevice() = default; +}; + +class LedDeviceTest : public ::testing::Test +{ +public: + void SetUp(); + void TearDown(); + +protected: + const size_t size_ = 3; + const std::string device_name_ = "test_led_device"; + + std::vector full_cmd_interface_names_; + const std::vector interface_names_ = {"r", "g", "b"}; + + std::array led_values_ = { + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}; + + std::unique_ptr led_device_; +}; + +#endif // TEST_LED_RGB_DEVICE_HPP_ From b0b64cfb11406b669269e638fcba45514523c691 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Mon, 16 Dec 2024 12:35:08 +0100 Subject: [PATCH 04/32] lint --- .../semantic_component_command_interface.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp index e5a1ea4952..c5a4c84701 100644 --- a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp +++ b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp @@ -21,7 +21,6 @@ #include "controller_interface/helpers.hpp" #include "hardware_interface/loaned_command_interface.hpp" - namespace semantic_components { template @@ -102,7 +101,8 @@ class SemanticComponentCommandInterface protected: std::string name_; std::vector interface_names_; - std::vector> command_interfaces_; + std::vector> + command_interfaces_; }; } // namespace semantic_components From 39ed94c4eeef256ed2eca0edc4ab4f2c33099921 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Mon, 16 Dec 2024 12:35:08 +0100 Subject: [PATCH 05/32] fix misc typos and fix test errors --- .../semantic_components/led_rgb_device.hpp | 11 ++++++----- .../semantic_component_command_interface.hpp | 7 ++++--- .../test/test_led_rgb_device.cpp | 19 ++++++++++--------- .../test/test_led_rgb_device.hpp | 8 ++++---- 4 files changed, 24 insertions(+), 21 deletions(-) diff --git a/controller_interface/include/semantic_components/led_rgb_device.hpp b/controller_interface/include/semantic_components/led_rgb_device.hpp index ad47672202..8784fbf171 100644 --- a/controller_interface/include/semantic_components/led_rgb_device.hpp +++ b/controller_interface/include/semantic_components/led_rgb_device.hpp @@ -24,7 +24,7 @@ namespace semantic_components { -class LEDRgbDevice : public SemanticComponentCommandInterface +class LEDRgbDevice : public SemanticComponentCommandInterface { public: /** @@ -63,10 +63,11 @@ class LEDRgbDevice : public SemanticComponentCommandInterface & values) { // check we have sufficient memory - if (values.values() != command_interfaces_.size()) + if (values.size() != command_interfaces_.size()) { return false; } // set values + bool all_set = true; for (size_t i = 0; i < values.size(); ++i) { - command_interfaces_[i].set_value(values[i]); + all_set &= command_interfaces_[i].get().set_value(values[i]); } - return true; + return all_set; } /// Set values from MessageInputType diff --git a/controller_interface/test/test_led_rgb_device.cpp b/controller_interface/test/test_led_rgb_device.cpp index c52b2f9a0b..71550fd7c3 100644 --- a/controller_interface/test/test_led_rgb_device.cpp +++ b/controller_interface/test/test_led_rgb_device.cpp @@ -64,20 +64,21 @@ TEST_F(LedDeviceTest, validate_all) const std::vector test_led_values_cmd = {0.1, 0.2, 0.3}; EXPECT_TRUE(led_device_->set_values(test_led_values_cmd)); - EXPECT_TRUE(led_values_[0], test_led_values_cmd[0]); - EXPECT_TRUE(led_values_[1], test_led_values_cmd[1]); - EXPECT_TRUE(led_values_[2], test_led_values_cmd[2]); + EXPECT_EQ(led_values_[0], test_led_values_cmd[0]); + EXPECT_EQ(led_values_[1], test_led_values_cmd[1]); + EXPECT_EQ(led_values_[2], test_led_values_cmd[2]); // Validate correct assignment from message std_msgs::msg::ColorRGBA temp_message; - temp_message.r = test_led_values_cmd[0]; - temp_message.g = test_led_values_cmd[1]; - temp_message.b = test_led_values_cmd[2]; + temp_message.r = static_cast(test_led_values_cmd[0]); + temp_message.g = static_cast(test_led_values_cmd[1]); + temp_message.b = static_cast(test_led_values_cmd[2]); EXPECT_TRUE(led_device_->set_values_from_message(temp_message)); - EXPECT_TRUE(led_values_[0], temp_message.r); - EXPECT_TRUE(led_values_[1], temp_message.g); - EXPECT_TRUE(led_values_[2], temp_message.b); + double float_tolerance = 1e-6; + EXPECT_NEAR(led_values_[0], test_led_values_cmd[0], float_tolerance); + EXPECT_NEAR(led_values_[1], test_led_values_cmd[1], float_tolerance); + EXPECT_NEAR(led_values_[2], test_led_values_cmd[2], float_tolerance); // Release command interfaces led_device_->release_interfaces(); diff --git a/controller_interface/test/test_led_rgb_device.hpp b/controller_interface/test/test_led_rgb_device.hpp index 0f6c3b2ec6..525244572f 100644 --- a/controller_interface/test/test_led_rgb_device.hpp +++ b/controller_interface/test/test_led_rgb_device.hpp @@ -27,11 +27,11 @@ class TestableLedDevice : public semantic_components::LEDRgbDevice { - FRIEND_TEST(TestableLedDevice, validate_all); + FRIEND_TEST(LedDeviceTest, validate_all); public: // Use default command interface names - explicit TestableLedDevice(const std::string & name) : PoseSensor{name} {} + explicit TestableLedDevice(const std::string & name) : LEDRgbDevice{name} {} virtual ~TestableLedDevice() = default; }; @@ -50,8 +50,8 @@ class LedDeviceTest : public ::testing::Test const std::vector interface_names_ = {"r", "g", "b"}; std::array led_values_ = { - std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()}; + {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}}; std::unique_ptr led_device_; }; From e3b99d3e990342a75a4080a24d6fd4e43aa61570 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Mon, 16 Dec 2024 13:19:36 +0100 Subject: [PATCH 06/32] refactor smr version of semantic command interface to match ros-control style --- controller_interface/CMakeLists.txt | 8 ++ ...t_semantic_component_command_interface.cpp | 127 ++++++++++++++++++ ...t_semantic_component_command_interface.hpp | 71 ++++++++++ 3 files changed, 206 insertions(+) create mode 100644 controller_interface/test/test_semantic_component_command_interface.cpp create mode 100644 controller_interface/test/test_semantic_component_command_interface.hpp diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index 672d1e02ce..5f18a447ba 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -88,6 +88,14 @@ if(BUILD_TESTING) # Semantic component command interface tests + ament_add_gmock(test_semantic_component_command_interface + test/test_semantic_component_command_interface.cpp + ) + target_link_libraries(test_semantic_component_command_interface + controller_interface + hardware_interface::hardware_interface + ) + ament_add_gmock(test_led_rgb_device test/test_led_rgb_device.cpp) target_link_libraries(test_led_rgb_device controller_interface diff --git a/controller_interface/test/test_semantic_component_command_interface.cpp b/controller_interface/test/test_semantic_component_command_interface.cpp new file mode 100644 index 0000000000..93af44a561 --- /dev/null +++ b/controller_interface/test/test_semantic_component_command_interface.cpp @@ -0,0 +1,127 @@ +// Copyright 2025 Sherpa Mobile Robotics +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Authors: Thibault Poignonec + */ + +#include "test_semantic_component_command_interface.hpp" + +#include +#include +#include +#include + +void SemanticCommandInterfaceTest::TearDown() { semantic_component_.reset(nullptr); } + +TEST_F(SemanticCommandInterfaceTest, validate_default_names) +{ + // create 'test_component' with 5 interfaces using default naming + // e.g. test_component_1, test_component_2 so on... + semantic_component_ = std::make_unique(component_name_, size_); + + // validate the component name + ASSERT_EQ(semantic_component_->name_, component_name_); + + // validate the space reserved for interface_names_ and state_interfaces_ + // Note : Using capacity() for command_interfaces_ as no such interfaces are defined yet + ASSERT_EQ(semantic_component_->interface_names_.capacity(), size_); + ASSERT_EQ(semantic_component_->command_interfaces_.capacity(), size_); + + // validate the interface_names_ + std::vector interface_names = semantic_component_->get_command_interface_names(); + ASSERT_EQ(interface_names, semantic_component_->interface_names_); + + ASSERT_EQ(interface_names.size(), size_); + ASSERT_EQ(interface_names[0], component_name_ + "/1"); + ASSERT_EQ(interface_names[1], component_name_ + "/2"); + ASSERT_EQ(interface_names[2], component_name_ + "/3"); +} + +TEST_F(SemanticCommandInterfaceTest, validate_command_interfaces) +{ + // create 'test_component' with 3 interfaces using default naming + // e.g. test_component_1, test_component_2 so on... + semantic_component_ = std::make_unique(component_name_, size_); + + // generate the interface_names_ + std::vector interface_names = semantic_component_->get_command_interface_names(); + + // validate assign_loaned_command_interfaces + // create interfaces and assign values to it + std::vector interface_values = { + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}; + hardware_interface::CommandInterface cmd_interface_1{component_name_, "1", &interface_values[0]}; + hardware_interface::CommandInterface cmd_interface_2{component_name_, "2", &interface_values[1]}; + hardware_interface::CommandInterface cmd_interface_3{component_name_, "3", &interface_values[2]}; + + // create local state interface vector + std::vector temp_command_interfaces; + temp_command_interfaces.reserve(3); + // insert the interfaces in jumbled sequence + temp_command_interfaces.emplace_back(cmd_interface_1); + temp_command_interfaces.emplace_back(cmd_interface_3); + temp_command_interfaces.emplace_back(cmd_interface_2); + + // now call the function to make them in order like interface_names + EXPECT_TRUE(semantic_component_->assign_loaned_command_interfaces(temp_command_interfaces)); + + // validate the count of command_interfaces_ + ASSERT_EQ(semantic_component_->command_interfaces_.size(), 3u); + + // Validate correct assignment + const std::vector test_cmd_values = {0.1, 0.2, 0.3}; + EXPECT_TRUE(semantic_component_->set_values(test_cmd_values)); + + EXPECT_EQ(interface_values[0], test_cmd_values[0]); + EXPECT_EQ(interface_values[1], test_cmd_values[1]); + EXPECT_EQ(interface_values[2], test_cmd_values[2]); + + // release the state_interfaces_ + semantic_component_->release_interfaces(); + + // validate the count of state_interfaces_ + ASSERT_EQ(semantic_component_->command_interfaces_.size(), 0u); + + // validate that release_interfaces() does not touch interface_names_ + ASSERT_TRUE(std::equal( + semantic_component_->interface_names_.begin(), semantic_component_->interface_names_.end(), + interface_names.begin(), interface_names.end())); +} + +TEST_F(SemanticCommandInterfaceTest, validate_custom_names) +{ + // create a component with 5 interfaces using custom naming + // as defined in the constructor + semantic_component_ = std::make_unique(size_); + + // validate the component name + ASSERT_EQ(semantic_component_->name_, semantic_component_->test_name_); + + // validate the space reserved for interface_names_ and command_interfaces_ + // Note : Using capacity() for command_interfaces_ as no such interfaces are defined yet + ASSERT_EQ(semantic_component_->interface_names_.capacity(), size_); + ASSERT_EQ(semantic_component_->command_interfaces_.capacity(), size_); + + // validate the interface_names_ + std::vector interface_names = semantic_component_->get_command_interface_names(); + ASSERT_TRUE(std::equal( + semantic_component_->interface_names_.begin(), semantic_component_->interface_names_.end(), + interface_names.begin(), interface_names.end())); + + ASSERT_EQ(interface_names.size(), size_); + ASSERT_EQ(interface_names[0], semantic_component_->test_name_ + "/i5"); + ASSERT_EQ(interface_names[1], semantic_component_->test_name_ + "/i6"); + ASSERT_EQ(interface_names[2], semantic_component_->test_name_ + "/i7"); +} diff --git a/controller_interface/test/test_semantic_component_command_interface.hpp b/controller_interface/test/test_semantic_component_command_interface.hpp new file mode 100644 index 0000000000..5df307b88c --- /dev/null +++ b/controller_interface/test/test_semantic_component_command_interface.hpp @@ -0,0 +1,71 @@ +// Copyright 2025 Sherpa Mobile Robotics +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Authors: Thibault Poignonec + */ + +#ifndef TEST_SEMANTIC_COMPONENT_COMMAND_INTERFACE_HPP_ +#define TEST_SEMANTIC_COMPONENT_COMMAND_INTERFACE_HPP_ + +#include + +#include +#include + +#include "geometry_msgs/msg/pose.hpp" +#include "semantic_components/semantic_component_command_interface.hpp" + +// implementing and friending so we can access member variables +class TestableSemanticCommandInterface +: public semantic_components::SemanticComponentCommandInterface +{ + FRIEND_TEST(SemanticCommandInterfaceTest, validate_default_names); + FRIEND_TEST(SemanticCommandInterfaceTest, validate_custom_names); + FRIEND_TEST(SemanticCommandInterfaceTest, validate_command_interfaces); + +public: + // Use generation of interface names + explicit TestableSemanticCommandInterface(const std::string & name, size_t size) + : SemanticComponentCommandInterface(name, size) + { + } + // Use custom interface names + explicit TestableSemanticCommandInterface(size_t size) + : SemanticComponentCommandInterface("TestSemanticCommandInterface", size) + { + // generate the interface_names_ + for (auto i = 0u; i < size; ++i) + { + interface_names_.emplace_back( + std::string("TestSemanticCommandInterface") + "/i" + std::to_string(i + 5)); + } + } + + virtual ~TestableSemanticCommandInterface() = default; + + std::string test_name_ = "TestSemanticCommandInterface"; +}; + +class SemanticCommandInterfaceTest : public ::testing::Test +{ +public: + void TearDown(); + +protected: + const std::string component_name_ = "test_component"; + const size_t size_ = 3; + std::unique_ptr semantic_component_; +}; + +#endif // TEST_SEMANTIC_COMPONENT_COMMAND_INTERFACE_HPP_ From 2e2798f184d60a5351a1adba51e845597e92bb29 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Mon, 16 Dec 2024 13:26:54 +0100 Subject: [PATCH 07/32] add "validate_custom_names" test case --- .../test/test_led_rgb_device.cpp | 19 +++++++++++++++++++ .../test/test_led_rgb_device.hpp | 8 ++++++++ 2 files changed, 27 insertions(+) diff --git a/controller_interface/test/test_led_rgb_device.cpp b/controller_interface/test/test_led_rgb_device.cpp index 71550fd7c3..9261fac137 100644 --- a/controller_interface/test/test_led_rgb_device.cpp +++ b/controller_interface/test/test_led_rgb_device.cpp @@ -84,3 +84,22 @@ TEST_F(LedDeviceTest, validate_all) led_device_->release_interfaces(); ASSERT_EQ(led_device_->command_interfaces_.size(), 0); } + +TEST_F(LedDeviceTest, validate_custom_names) +{ + std::string interface_name_r = "led/custom_r"; + std::string interface_name_g = "led/custom_g"; + std::string interface_name_b = "led/custom_b"; + // Create device + led_device_ = + std::make_unique(interface_name_r, interface_name_g, interface_name_b); + EXPECT_EQ(led_device_->name_, ""); + + EXPECT_EQ(led_device_->interface_names_.size(), size_); + EXPECT_EQ(led_device_->command_interfaces_.capacity(), size_); + + // Validate custom interface_names_ + EXPECT_EQ(led_device_->interface_names_[0], interface_name_r); + EXPECT_EQ(led_device_->interface_names_[1], interface_name_g); + EXPECT_EQ(led_device_->interface_names_[2], interface_name_b); +} diff --git a/controller_interface/test/test_led_rgb_device.hpp b/controller_interface/test/test_led_rgb_device.hpp index 525244572f..2e673eed8b 100644 --- a/controller_interface/test/test_led_rgb_device.hpp +++ b/controller_interface/test/test_led_rgb_device.hpp @@ -28,11 +28,19 @@ class TestableLedDevice : public semantic_components::LEDRgbDevice { FRIEND_TEST(LedDeviceTest, validate_all); + FRIEND_TEST(LedDeviceTest, validate_custom_names); public: // Use default command interface names explicit TestableLedDevice(const std::string & name) : LEDRgbDevice{name} {} + TestableLedDevice( + const std::string & interface_r, const std::string & interface_g, + const std::string & interface_b) + : LEDRgbDevice{interface_r, interface_g, interface_b} + { + } + virtual ~TestableLedDevice() = default; }; From 121da6408b272681cae9d23bca061dc96a41c911 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Fri, 20 Dec 2024 11:59:06 +0100 Subject: [PATCH 08/32] minor fixes in comments --- controller_interface/test/test_led_rgb_device.cpp | 2 +- .../test/test_semantic_component_command_interface.cpp | 4 ++-- .../test/test_semantic_component_command_interface.hpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/controller_interface/test/test_led_rgb_device.cpp b/controller_interface/test/test_led_rgb_device.cpp index 9261fac137..b3e0496abb 100644 --- a/controller_interface/test/test_led_rgb_device.cpp +++ b/controller_interface/test/test_led_rgb_device.cpp @@ -49,7 +49,7 @@ TEST_F(LedDeviceTest, validate_all) hardware_interface::CommandInterface led_g{device_name_, interface_names_[1], &led_values_[1]}; hardware_interface::CommandInterface led_b{device_name_, interface_names_[2], &led_values_[2]}; - // Create state interface vector in jumbled order + // Create command interface vector in jumbled order std::vector temp_command_interfaces; temp_command_interfaces.reserve(3); temp_command_interfaces.emplace_back(led_r); diff --git a/controller_interface/test/test_semantic_component_command_interface.cpp b/controller_interface/test/test_semantic_component_command_interface.cpp index 93af44a561..5b54a4b861 100644 --- a/controller_interface/test/test_semantic_component_command_interface.cpp +++ b/controller_interface/test/test_semantic_component_command_interface.cpp @@ -1,4 +1,4 @@ -// Copyright 2025 Sherpa Mobile Robotics +// Copyright 2024 Sherpa Mobile Robotics // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at @@ -66,7 +66,7 @@ TEST_F(SemanticCommandInterfaceTest, validate_command_interfaces) hardware_interface::CommandInterface cmd_interface_2{component_name_, "2", &interface_values[1]}; hardware_interface::CommandInterface cmd_interface_3{component_name_, "3", &interface_values[2]}; - // create local state interface vector + // create local command interface vector std::vector temp_command_interfaces; temp_command_interfaces.reserve(3); // insert the interfaces in jumbled sequence diff --git a/controller_interface/test/test_semantic_component_command_interface.hpp b/controller_interface/test/test_semantic_component_command_interface.hpp index 5df307b88c..923e246bad 100644 --- a/controller_interface/test/test_semantic_component_command_interface.hpp +++ b/controller_interface/test/test_semantic_component_command_interface.hpp @@ -1,4 +1,4 @@ -// Copyright 2025 Sherpa Mobile Robotics +// Copyright 2024 Sherpa Mobile Robotics // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at From ab32971e9805a81855c973502d73c94ede410f04 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Fri, 20 Dec 2024 12:07:18 +0100 Subject: [PATCH 09/32] Change "LEDRgbDevice" to "LedRgbDevice" --- .../include/semantic_components/led_rgb_device.hpp | 8 ++++---- controller_interface/test/test_led_rgb_device.hpp | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/controller_interface/include/semantic_components/led_rgb_device.hpp b/controller_interface/include/semantic_components/led_rgb_device.hpp index 8784fbf171..be8a78dccb 100644 --- a/controller_interface/include/semantic_components/led_rgb_device.hpp +++ b/controller_interface/include/semantic_components/led_rgb_device.hpp @@ -24,14 +24,14 @@ namespace semantic_components { -class LEDRgbDevice : public SemanticComponentCommandInterface +class LedRgbDevice : public SemanticComponentCommandInterface { public: /** * Constructor for a LED RGB device with interface names set based on device name. * The constructor sets the command interface names to "/r", "/g", "/b". */ - explicit LEDRgbDevice(const std::string & name) : SemanticComponentCommandInterface(name, 3) + explicit LedRgbDevice (const std::string & name) : SemanticComponentCommandInterface(name, 3) { interface_names_.emplace_back(name_ + "/" + "r"); interface_names_.emplace_back(name_ + "/" + "g"); @@ -42,7 +42,7 @@ class LEDRgbDevice : public SemanticComponentCommandInterface Date: Fri, 20 Dec 2024 12:08:50 +0100 Subject: [PATCH 10/32] add release notes --- doc/release_notes.rst | 2 ++ 1 file changed, 2 insertions(+) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index a187e62437..63af12f0f7 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -27,6 +27,8 @@ For details see the controller_manager section. * The ``assign_interfaces`` and ``release_interfaces`` methods are now virtual, so that the user can override them to store the interfaces into custom variable types, so that the user can have the flexibility to take the ownership of the loaned interfaces to the controller (`#1743 `_) * The new ``PoseSensor`` semantic component provides a standard interface for hardware providing cartesian poses (`#1775 `_) * The controllers now support the fallback controllers (a list of controllers that will be activated, when the spawned controllers fails by throwing an exception or returning ``return_type::ERROR`` during the ``update`` cycle) (`#1789 `_) +* A new ``SemanticComponentCommandInterface`` semantic component provides capabilities analogous to the ``SemanticComponentInterface``, but for write-only devices (`#1945 `_) +* The new semantic command interface ``LedRgbDevice`` provides standard (command) interfaces for 3-channel LED devices (`#1945 `_) controller_manager ****************** From a71c699e521de901e8feb1126e9042574e3ce7f2 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Fri, 20 Dec 2024 12:09:17 +0100 Subject: [PATCH 11/32] make CI happy --- .../include/semantic_components/led_rgb_device.hpp | 8 ++++---- controller_interface/test/test_led_rgb_device.hpp | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/controller_interface/include/semantic_components/led_rgb_device.hpp b/controller_interface/include/semantic_components/led_rgb_device.hpp index be8a78dccb..3b8bc4387a 100644 --- a/controller_interface/include/semantic_components/led_rgb_device.hpp +++ b/controller_interface/include/semantic_components/led_rgb_device.hpp @@ -24,14 +24,14 @@ namespace semantic_components { -class LedRgbDevice : public SemanticComponentCommandInterface +class LedRgbDevice : public SemanticComponentCommandInterface { public: /** * Constructor for a LED RGB device with interface names set based on device name. * The constructor sets the command interface names to "/r", "/g", "/b". */ - explicit LedRgbDevice (const std::string & name) : SemanticComponentCommandInterface(name, 3) + explicit LedRgbDevice(const std::string & name) : SemanticComponentCommandInterface(name, 3) { interface_names_.emplace_back(name_ + "/" + "r"); interface_names_.emplace_back(name_ + "/" + "g"); @@ -42,7 +42,7 @@ class LedRgbDevice : public SemanticComponentCommandInterface Date: Tue, 24 Dec 2024 21:40:34 +0100 Subject: [PATCH 12/32] Update controller_interface/include/semantic_components/led_rgb_device.hpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- .../include/semantic_components/led_rgb_device.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/controller_interface/include/semantic_components/led_rgb_device.hpp b/controller_interface/include/semantic_components/led_rgb_device.hpp index 3b8bc4387a..78e30da6ef 100644 --- a/controller_interface/include/semantic_components/led_rgb_device.hpp +++ b/controller_interface/include/semantic_components/led_rgb_device.hpp @@ -52,8 +52,6 @@ class LedRgbDevice : public SemanticComponentCommandInterface Date: Tue, 24 Dec 2024 21:40:44 +0100 Subject: [PATCH 13/32] Update controller_interface/include/semantic_components/semantic_component_command_interface.hpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- .../semantic_component_command_interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp index ab3a5be62d..ae9fc424be 100644 --- a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp +++ b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp @@ -34,7 +34,7 @@ class SemanticComponentCommandInterface command_interfaces_.reserve(size); } - ~SemanticComponentCommandInterface() = default; + virtual ~SemanticComponentCommandInterface() = default; /// Assign loaned command interfaces from the hardware. /** From db6cb8de7654e70816a75507f67320765cba6759 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec <79221188+tpoignonec@users.noreply.github.com> Date: Tue, 24 Dec 2024 21:40:56 +0100 Subject: [PATCH 14/32] Update controller_interface/include/semantic_components/semantic_component_command_interface.hpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- .../semantic_component_command_interface.hpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp index ae9fc424be..20b96d6ee8 100644 --- a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp +++ b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp @@ -27,9 +27,15 @@ template class SemanticComponentCommandInterface { public: - explicit SemanticComponentCommandInterface(const std::string & name, size_t size = 0) + SemanticComponentCommandInterface( + const std::string & name, const std::vector & interface_names) + : name_(name), interface_names_(interface_names) + { + command_interfaces_.reserve(interface_names.size()); + } + + explicit SemanticComponentCommandInterface(const std::string & name, size_t size = 0) : name_(name) { - name_ = name; interface_names_.reserve(size); command_interfaces_.reserve(size); } From 0d0bb542ab580bb0ef2f115a482c216a0592551a Mon Sep 17 00:00:00 2001 From: Thibault Poignonec <79221188+tpoignonec@users.noreply.github.com> Date: Tue, 24 Dec 2024 21:41:07 +0100 Subject: [PATCH 15/32] Update controller_interface/include/semantic_components/semantic_component_command_interface.hpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- .../semantic_component_command_interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp index 20b96d6ee8..ac4f36a889 100644 --- a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp +++ b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp @@ -92,7 +92,7 @@ class SemanticComponentCommandInterface } // set values bool all_set = true; - for (size_t i = 0; i < values.size(); ++i) + for (auto i = 0u; i < values.size(); ++i) { all_set &= command_interfaces_[i].get().set_value(values[i]); } From f8cb935ce9473ef937db18ed9dd35d7887ec4d54 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec <79221188+tpoignonec@users.noreply.github.com> Date: Tue, 24 Dec 2024 21:41:18 +0100 Subject: [PATCH 16/32] Update controller_interface/include/semantic_components/led_rgb_device.hpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- .../include/semantic_components/led_rgb_device.hpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/controller_interface/include/semantic_components/led_rgb_device.hpp b/controller_interface/include/semantic_components/led_rgb_device.hpp index 78e30da6ef..1a8c0b6c3d 100644 --- a/controller_interface/include/semantic_components/led_rgb_device.hpp +++ b/controller_interface/include/semantic_components/led_rgb_device.hpp @@ -31,11 +31,12 @@ class LedRgbDevice : public SemanticComponentCommandInterface/r", "/g", "/b". */ - explicit LedRgbDevice(const std::string & name) : SemanticComponentCommandInterface(name, 3) + explicit LedRgbDevice(const std::string & name) + : SemanticComponentCommandInterface( + name, {{name_ + "/" + "r"}, + {name_ + "/" + "g"}, + {name_ + "/" + "b"}}) { - interface_names_.emplace_back(name_ + "/" + "r"); - interface_names_.emplace_back(name_ + "/" + "g"); - interface_names_.emplace_back(name_ + "/" + "b"); } /** From ed05c4e89a0759bb8a6059a9c43854d5b70f5961 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Tue, 24 Dec 2024 21:44:22 +0100 Subject: [PATCH 17/32] run pre-commit --all --- .../include/semantic_components/led_rgb_device.hpp | 4 +--- .../semantic_component_command_interface.hpp | 3 ++- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/controller_interface/include/semantic_components/led_rgb_device.hpp b/controller_interface/include/semantic_components/led_rgb_device.hpp index 1a8c0b6c3d..f67cf6b968 100644 --- a/controller_interface/include/semantic_components/led_rgb_device.hpp +++ b/controller_interface/include/semantic_components/led_rgb_device.hpp @@ -33,9 +33,7 @@ class LedRgbDevice : public SemanticComponentCommandInterface Date: Fri, 3 Jan 2025 19:03:06 +0100 Subject: [PATCH 18/32] fix suggested changes for initialization --- .../include/semantic_components/led_rgb_device.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/include/semantic_components/led_rgb_device.hpp b/controller_interface/include/semantic_components/led_rgb_device.hpp index f67cf6b968..8bb0dcf053 100644 --- a/controller_interface/include/semantic_components/led_rgb_device.hpp +++ b/controller_interface/include/semantic_components/led_rgb_device.hpp @@ -33,7 +33,7 @@ class LedRgbDevice : public SemanticComponentCommandInterface Date: Mon, 13 Jan 2025 11:01:12 +0100 Subject: [PATCH 19/32] Update controller_interface/include/semantic_components/semantic_component_command_interface.hpp Co-authored-by: Sai Kishor Kothakota --- .../semantic_components/semantic_component_command_interface.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp index 1c856ade42..b0ad156856 100644 --- a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp +++ b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp @@ -31,6 +31,7 @@ class SemanticComponentCommandInterface const std::string & name, const std::vector & interface_names) : name_(name), interface_names_(interface_names) { + assert(interface_names.size() > 0); command_interfaces_.reserve(interface_names.size()); } From 0d94c77b86da107b7344c4f8ecaed0f16904ff44 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec <79221188+tpoignonec@users.noreply.github.com> Date: Mon, 13 Jan 2025 10:09:34 +0000 Subject: [PATCH 20/32] remove "limits" include --- .../include/semantic_components/led_rgb_device.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/controller_interface/include/semantic_components/led_rgb_device.hpp b/controller_interface/include/semantic_components/led_rgb_device.hpp index 8bb0dcf053..50c8eb16cc 100644 --- a/controller_interface/include/semantic_components/led_rgb_device.hpp +++ b/controller_interface/include/semantic_components/led_rgb_device.hpp @@ -15,7 +15,6 @@ #ifndef SEMANTIC_COMPONENTS__LED_RGB_DEVICE_HPP_ #define SEMANTIC_COMPONENTS__LED_RGB_DEVICE_HPP_ -#include #include #include From 697bdb79b3e4085e91581b92a4250bd7e12f0a26 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Mon, 13 Jan 2025 11:29:03 +0100 Subject: [PATCH 21/32] fix LedRgbDevice::set_values_from_message - mark as virtual - fix missing const ref - add method docstring --- .../include/semantic_components/led_rgb_device.hpp | 13 ++++++++++++- .../semantic_component_command_interface.hpp | 2 +- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/controller_interface/include/semantic_components/led_rgb_device.hpp b/controller_interface/include/semantic_components/led_rgb_device.hpp index 50c8eb16cc..c2b1c9f5ab 100644 --- a/controller_interface/include/semantic_components/led_rgb_device.hpp +++ b/controller_interface/include/semantic_components/led_rgb_device.hpp @@ -51,7 +51,18 @@ class LedRgbDevice : public SemanticComponentCommandInterface 1 || message.g < 0 || message.g > 1 || message.b < 0 || diff --git a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp index b0ad156856..42304375d0 100644 --- a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp +++ b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp @@ -105,7 +105,7 @@ class SemanticComponentCommandInterface /** * \return false by default */ - bool set_values_from_message(const MessageInputType & /* message */) { return false; } + virtual bool set_values_from_message(const MessageInputType & /* message */) { return false; } protected: std::string name_; From 29cb799095784c045355a1f193defc0224d696f1 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Mon, 13 Jan 2025 11:35:38 +0100 Subject: [PATCH 22/32] expand docstring for SemanticComponentCommandInterface::set_values --- .../semantic_component_command_interface.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp index 42304375d0..1c95f1a906 100644 --- a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp +++ b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp @@ -83,7 +83,8 @@ class SemanticComponentCommandInterface /// Return all values. /** - * \return true if it gets all the values, else false + * \return true if it gets all the values, else false (i.e., invalid size or if the method + * ``hardware_interface::LoanedCommandInterface::set_value`` fails). */ bool set_values(const std::vector & values) { From 69277d80e8543826f0273e3179bf2170d26b9de8 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Thu, 16 Jan 2025 21:24:01 +0100 Subject: [PATCH 23/32] make CI happy --- .../include/semantic_components/led_rgb_device.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/controller_interface/include/semantic_components/led_rgb_device.hpp b/controller_interface/include/semantic_components/led_rgb_device.hpp index c2b1c9f5ab..d053a8434b 100644 --- a/controller_interface/include/semantic_components/led_rgb_device.hpp +++ b/controller_interface/include/semantic_components/led_rgb_device.hpp @@ -54,12 +54,12 @@ class LedRgbDevice : public SemanticComponentCommandInterface Date: Thu, 16 Jan 2025 21:25:47 +0100 Subject: [PATCH 24/32] Update controller_interface/include/semantic_components/led_rgb_device.hpp Co-authored-by: Sai Kishor Kothakota --- .../include/semantic_components/led_rgb_device.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/include/semantic_components/led_rgb_device.hpp b/controller_interface/include/semantic_components/led_rgb_device.hpp index d053a8434b..421d692ad0 100644 --- a/controller_interface/include/semantic_components/led_rgb_device.hpp +++ b/controller_interface/include/semantic_components/led_rgb_device.hpp @@ -62,7 +62,7 @@ class LedRgbDevice : public SemanticComponentCommandInterface 1 || message.g < 0 || message.g > 1 || message.b < 0 || From f99afcfa0911cf773d22891e487f196e9ea16231 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Thu, 16 Jan 2025 21:38:09 +0100 Subject: [PATCH 25/32] remove virtual declaration (redundant with override keyword) --- .../include/semantic_components/led_rgb_device.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/include/semantic_components/led_rgb_device.hpp b/controller_interface/include/semantic_components/led_rgb_device.hpp index 421d692ad0..0fcfbd5eeb 100644 --- a/controller_interface/include/semantic_components/led_rgb_device.hpp +++ b/controller_interface/include/semantic_components/led_rgb_device.hpp @@ -62,7 +62,7 @@ class LedRgbDevice : public SemanticComponentCommandInterface 1 || message.g < 0 || message.g > 1 || message.b < 0 || From 609542e6fe5a89717ca77bf815c694373ba7524a Mon Sep 17 00:00:00 2001 From: Thibault Poignonec <79221188+tpoignonec@users.noreply.github.com> Date: Wed, 22 Jan 2025 16:02:19 +0100 Subject: [PATCH 26/32] declare "get_command_interface_names" as virtual Co-authored-by: Sai Kishor Kothakota --- .../semantic_component_command_interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp index 1c95f1a906..d2eb1cb4c7 100644 --- a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp +++ b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp @@ -69,7 +69,7 @@ class SemanticComponentCommandInterface * from 0 to size of values; * \return list of strings with command interface names for the semantic component. */ - virtual std::vector get_command_interface_names() + virtual const std::vector & get_command_interface_names() { if (interface_names_.empty()) { From 5df7b4358c7171e5138997e8614b0c42b459fa54 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Wed, 22 Jan 2025 16:38:00 +0100 Subject: [PATCH 27/32] make "set_values_from_message" a pure virtual method --- .../semantic_component_command_interface.hpp | 4 ++-- .../test/test_semantic_component_command_interface.hpp | 7 +++++++ 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp index d2eb1cb4c7..111b4ded5c 100644 --- a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp +++ b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp @@ -104,9 +104,9 @@ class SemanticComponentCommandInterface /// Set values from MessageInputType /** - * \return false by default + * \return True if all values were set successfully, false otherwise. */ - virtual bool set_values_from_message(const MessageInputType & /* message */) { return false; } + virtual bool set_values_from_message(const MessageInputType & /* message */) = 0; protected: std::string name_; diff --git a/controller_interface/test/test_semantic_component_command_interface.hpp b/controller_interface/test/test_semantic_component_command_interface.hpp index 923e246bad..c21817b015 100644 --- a/controller_interface/test/test_semantic_component_command_interface.hpp +++ b/controller_interface/test/test_semantic_component_command_interface.hpp @@ -54,6 +54,13 @@ class TestableSemanticCommandInterface virtual ~TestableSemanticCommandInterface() = default; + // We are not testing the implementation of the function, so we simply return false. + // Note that set_values_from_message is a pure virtual function, hence the need for this + bool set_values_from_message(const geometry_msgs::msg::Pose & /* message */) override + { + return false; + } + std::string test_name_ = "TestSemanticCommandInterface"; }; From db374fe6060bbe433f9f697f9b3ab3dada7aa027 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Wed, 22 Jan 2025 16:43:43 +0100 Subject: [PATCH 28/32] remove constructor SemanticComponentCommandInterface(name, size) --- .../semantic_components/led_rgb_device.hpp | 5 +- .../semantic_component_command_interface.hpp | 17 +----- ...t_semantic_component_command_interface.cpp | 52 +------------------ ...t_semantic_component_command_interface.hpp | 17 +----- 4 files changed, 6 insertions(+), 85 deletions(-) diff --git a/controller_interface/include/semantic_components/led_rgb_device.hpp b/controller_interface/include/semantic_components/led_rgb_device.hpp index 0fcfbd5eeb..7527f93ad5 100644 --- a/controller_interface/include/semantic_components/led_rgb_device.hpp +++ b/controller_interface/include/semantic_components/led_rgb_device.hpp @@ -43,11 +43,8 @@ class LedRgbDevice : public SemanticComponentCommandInterface & get_command_interface_names() + const std::vector & get_command_interface_names() const { - if (interface_names_.empty()) - { - for (auto i = 0u; i < interface_names_.capacity(); ++i) - { - interface_names_.emplace_back(name_ + "/" + std::to_string(i + 1)); - } - } + assert(!interface_names_.empty()); return interface_names_; } diff --git a/controller_interface/test/test_semantic_component_command_interface.cpp b/controller_interface/test/test_semantic_component_command_interface.cpp index 5b54a4b861..ce8de65c7f 100644 --- a/controller_interface/test/test_semantic_component_command_interface.cpp +++ b/controller_interface/test/test_semantic_component_command_interface.cpp @@ -24,35 +24,11 @@ void SemanticCommandInterfaceTest::TearDown() { semantic_component_.reset(nullptr); } -TEST_F(SemanticCommandInterfaceTest, validate_default_names) -{ - // create 'test_component' with 5 interfaces using default naming - // e.g. test_component_1, test_component_2 so on... - semantic_component_ = std::make_unique(component_name_, size_); - - // validate the component name - ASSERT_EQ(semantic_component_->name_, component_name_); - - // validate the space reserved for interface_names_ and state_interfaces_ - // Note : Using capacity() for command_interfaces_ as no such interfaces are defined yet - ASSERT_EQ(semantic_component_->interface_names_.capacity(), size_); - ASSERT_EQ(semantic_component_->command_interfaces_.capacity(), size_); - - // validate the interface_names_ - std::vector interface_names = semantic_component_->get_command_interface_names(); - ASSERT_EQ(interface_names, semantic_component_->interface_names_); - - ASSERT_EQ(interface_names.size(), size_); - ASSERT_EQ(interface_names[0], component_name_ + "/1"); - ASSERT_EQ(interface_names[1], component_name_ + "/2"); - ASSERT_EQ(interface_names[2], component_name_ + "/3"); -} - TEST_F(SemanticCommandInterfaceTest, validate_command_interfaces) { // create 'test_component' with 3 interfaces using default naming // e.g. test_component_1, test_component_2 so on... - semantic_component_ = std::make_unique(component_name_, size_); + semantic_component_ = std::make_unique(component_name_); // generate the interface_names_ std::vector interface_names = semantic_component_->get_command_interface_names(); @@ -99,29 +75,3 @@ TEST_F(SemanticCommandInterfaceTest, validate_command_interfaces) semantic_component_->interface_names_.begin(), semantic_component_->interface_names_.end(), interface_names.begin(), interface_names.end())); } - -TEST_F(SemanticCommandInterfaceTest, validate_custom_names) -{ - // create a component with 5 interfaces using custom naming - // as defined in the constructor - semantic_component_ = std::make_unique(size_); - - // validate the component name - ASSERT_EQ(semantic_component_->name_, semantic_component_->test_name_); - - // validate the space reserved for interface_names_ and command_interfaces_ - // Note : Using capacity() for command_interfaces_ as no such interfaces are defined yet - ASSERT_EQ(semantic_component_->interface_names_.capacity(), size_); - ASSERT_EQ(semantic_component_->command_interfaces_.capacity(), size_); - - // validate the interface_names_ - std::vector interface_names = semantic_component_->get_command_interface_names(); - ASSERT_TRUE(std::equal( - semantic_component_->interface_names_.begin(), semantic_component_->interface_names_.end(), - interface_names.begin(), interface_names.end())); - - ASSERT_EQ(interface_names.size(), size_); - ASSERT_EQ(interface_names[0], semantic_component_->test_name_ + "/i5"); - ASSERT_EQ(interface_names[1], semantic_component_->test_name_ + "/i6"); - ASSERT_EQ(interface_names[2], semantic_component_->test_name_ + "/i7"); -} diff --git a/controller_interface/test/test_semantic_component_command_interface.hpp b/controller_interface/test/test_semantic_component_command_interface.hpp index c21817b015..df1cb3a411 100644 --- a/controller_interface/test/test_semantic_component_command_interface.hpp +++ b/controller_interface/test/test_semantic_component_command_interface.hpp @@ -30,27 +30,14 @@ class TestableSemanticCommandInterface : public semantic_components::SemanticComponentCommandInterface { - FRIEND_TEST(SemanticCommandInterfaceTest, validate_default_names); FRIEND_TEST(SemanticCommandInterfaceTest, validate_custom_names); FRIEND_TEST(SemanticCommandInterfaceTest, validate_command_interfaces); public: - // Use generation of interface names - explicit TestableSemanticCommandInterface(const std::string & name, size_t size) - : SemanticComponentCommandInterface(name, size) + explicit TestableSemanticCommandInterface(const std::string & name) + : SemanticComponentCommandInterface(name, {name + "/1", name + "/2", name + "/3"}) { } - // Use custom interface names - explicit TestableSemanticCommandInterface(size_t size) - : SemanticComponentCommandInterface("TestSemanticCommandInterface", size) - { - // generate the interface_names_ - for (auto i = 0u; i < size; ++i) - { - interface_names_.emplace_back( - std::string("TestSemanticCommandInterface") + "/i" + std::to_string(i + 5)); - } - } virtual ~TestableSemanticCommandInterface() = default; From 0fee40ae47d25e24300309e99cea8523d06b7385 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec <79221188+tpoignonec@users.noreply.github.com> Date: Mon, 27 Jan 2025 08:57:18 +0100 Subject: [PATCH 29/32] Update controller_interface/include/semantic_components/semantic_component_command_interface.hpp Co-authored-by: Sai Kishor Kothakota --- .../semantic_components/semantic_component_command_interface.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp index 0e3f545d2d..287df8f53c 100644 --- a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp +++ b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp @@ -64,7 +64,6 @@ class SemanticComponentCommandInterface */ const std::vector & get_command_interface_names() const { - assert(!interface_names_.empty()); return interface_names_; } From e7b078e27e3a60aa9e5d610964df7484fe5046d0 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Mon, 27 Jan 2025 10:34:22 +0100 Subject: [PATCH 30/32] force / convention in LED constructor --- .../semantic_components/led_rgb_device.hpp | 25 ++++++++----------- .../test/test_led_rgb_device.cpp | 22 ++-------------- .../test/test_led_rgb_device.hpp | 8 ++---- 3 files changed, 15 insertions(+), 40 deletions(-) diff --git a/controller_interface/include/semantic_components/led_rgb_device.hpp b/controller_interface/include/semantic_components/led_rgb_device.hpp index 7527f93ad5..8c0d64bfa3 100644 --- a/controller_interface/include/semantic_components/led_rgb_device.hpp +++ b/controller_interface/include/semantic_components/led_rgb_device.hpp @@ -28,22 +28,19 @@ class LedRgbDevice : public SemanticComponentCommandInterface/r", "/g", "/b". - */ - explicit LedRgbDevice(const std::string & name) - : SemanticComponentCommandInterface( - name, {{name + "/" + "r"}, {name + "/" + "g"}, {name + "/" + "b"}}) - { - } - - /** - * Constructor for a LED RGB device with custom interface names. - * The constructor takes the three command interface names for the red, green, and blue channels. + * The constructor sets the command interface names to "/interface_r", + * "/interface_g", "/interface_b". + * + * \param[in] name name of the LED device, used as a prefix for the command interface names + * \param[in] interface_r name of the command interface for the red channel + * \param[in] interface_g name of the command interface for the green channel + * \param[in] interface_b name of the command interface for the blue channel */ - explicit LedRgbDevice( - const std::string & interface_r, const std::string & interface_g, + LedRgbDevice( + const std::string & name, const std::string & interface_r, const std::string & interface_g, const std::string & interface_b) - : SemanticComponentCommandInterface("", {{interface_r}, {interface_g}, {interface_b}}) + : SemanticComponentCommandInterface( + name, {{name + "/" + interface_r}, {name + "/" + interface_g}, {name + "/" + interface_b}}) { } diff --git a/controller_interface/test/test_led_rgb_device.cpp b/controller_interface/test/test_led_rgb_device.cpp index b3e0496abb..afef78b489 100644 --- a/controller_interface/test/test_led_rgb_device.cpp +++ b/controller_interface/test/test_led_rgb_device.cpp @@ -28,7 +28,8 @@ void LedDeviceTest::TearDown() { led_device_.reset(nullptr); } TEST_F(LedDeviceTest, validate_all) { // Create device - led_device_ = std::make_unique(device_name_); + led_device_ = std::make_unique( + device_name_, interface_names_[0], interface_names_[1], interface_names_[2]); EXPECT_EQ(led_device_->name_, device_name_); // Validate reserved space for interface_names_ and command_interfaces_ @@ -84,22 +85,3 @@ TEST_F(LedDeviceTest, validate_all) led_device_->release_interfaces(); ASSERT_EQ(led_device_->command_interfaces_.size(), 0); } - -TEST_F(LedDeviceTest, validate_custom_names) -{ - std::string interface_name_r = "led/custom_r"; - std::string interface_name_g = "led/custom_g"; - std::string interface_name_b = "led/custom_b"; - // Create device - led_device_ = - std::make_unique(interface_name_r, interface_name_g, interface_name_b); - EXPECT_EQ(led_device_->name_, ""); - - EXPECT_EQ(led_device_->interface_names_.size(), size_); - EXPECT_EQ(led_device_->command_interfaces_.capacity(), size_); - - // Validate custom interface_names_ - EXPECT_EQ(led_device_->interface_names_[0], interface_name_r); - EXPECT_EQ(led_device_->interface_names_[1], interface_name_g); - EXPECT_EQ(led_device_->interface_names_[2], interface_name_b); -} diff --git a/controller_interface/test/test_led_rgb_device.hpp b/controller_interface/test/test_led_rgb_device.hpp index 993239e48e..393a8323d6 100644 --- a/controller_interface/test/test_led_rgb_device.hpp +++ b/controller_interface/test/test_led_rgb_device.hpp @@ -28,16 +28,12 @@ class TestableLedDevice : public semantic_components::LedRgbDevice { FRIEND_TEST(LedDeviceTest, validate_all); - FRIEND_TEST(LedDeviceTest, validate_custom_names); public: - // Use default command interface names - explicit TestableLedDevice(const std::string & name) : LedRgbDevice{name} {} - TestableLedDevice( - const std::string & interface_r, const std::string & interface_g, + const std::string & name, const std::string & interface_r, const std::string & interface_g, const std::string & interface_b) - : LedRgbDevice{interface_r, interface_g, interface_b} + : LedRgbDevice{name, interface_r, interface_g, interface_b} { } From 19a1b9bd30fe0e16c9a9ca6cb5956bc8b5071956 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Mon, 27 Jan 2025 10:34:57 +0100 Subject: [PATCH 31/32] linting of accepted commits --- .../semantic_component_command_interface.hpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp index 287df8f53c..5569b7ba9d 100644 --- a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp +++ b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp @@ -62,10 +62,7 @@ class SemanticComponentCommandInterface * from 0 to size of values; * \return list of strings with command interface names for the semantic component. */ - const std::vector & get_command_interface_names() const - { - return interface_names_; - } + const std::vector & get_command_interface_names() const { return interface_names_; } /// Return all values. /** From c21876fab2a7408f6e1017db6ae335aa4578e7eb Mon Sep 17 00:00:00 2001 From: Thibault Poignonec <79221188+tpoignonec@users.noreply.github.com> Date: Mon, 27 Jan 2025 11:03:06 +0100 Subject: [PATCH 32/32] Update controller_interface/include/semantic_components/led_rgb_device.hpp Co-authored-by: Sai Kishor Kothakota --- .../include/semantic_components/led_rgb_device.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/include/semantic_components/led_rgb_device.hpp b/controller_interface/include/semantic_components/led_rgb_device.hpp index 8c0d64bfa3..0ecbd1e93d 100644 --- a/controller_interface/include/semantic_components/led_rgb_device.hpp +++ b/controller_interface/include/semantic_components/led_rgb_device.hpp @@ -36,7 +36,7 @@ class LedRgbDevice : public SemanticComponentCommandInterface