diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index 60b213cd57..1855180c89 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -86,6 +86,25 @@ if(BUILD_TESTING) ament_target_dependencies(test_pose_sensor geometry_msgs ) + + # 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 + hardware_interface::hardware_interface + ) + ament_target_dependencies(test_led_rgb_device + std_msgs + ) endif() install( 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..c2b1c9f5ab --- /dev/null +++ b/controller_interface/include/semantic_components/led_rgb_device.hpp @@ -0,0 +1,83 @@ +// 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 "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, {{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. + */ + 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); + } + + /// Set LED states from ColorRGBA message + + /** + * Set the values of the LED RGB device from a ColorRGBA message. + * + * \details Sets the values of the red, green, and blue channels from the message. + * If any of the values are out of the range [0, 1], the function fails and returns false. + * + * \param[in] message ColorRGBA message + * + * \return true if all values were set, false otherwise + */ + virtual bool set_values_from_message(const 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; + } + bool all_set = true; + all_set &= command_interfaces_[0].get().set_value(message.r); + all_set &= command_interfaces_[1].get().set_value(message.g); + all_set &= command_interfaces_[2].get().set_value(message.b); + return all_set; + } +}; + +} // namespace semantic_components + +#endif // SEMANTIC_COMPONENTS__LED_RGB_DEVICE_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..1c95f1a906 --- /dev/null +++ b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp @@ -0,0 +1,120 @@ +// 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: + 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()); + } + + explicit SemanticComponentCommandInterface(const std::string & name, size_t size = 0) + : name_(name) + { + interface_names_.reserve(size); + command_interfaces_.reserve(size); + } + + virtual ~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 (i.e., invalid size or if the method + * ``hardware_interface::LoanedCommandInterface::set_value`` fails). + */ + bool set_values(const std::vector & values) + { + // check we have sufficient memory + if (values.size() != command_interfaces_.size()) + { + return false; + } + // set values + bool all_set = true; + for (auto i = 0u; i < values.size(); ++i) + { + all_set &= command_interfaces_[i].get().set_value(values[i]); + } + return all_set; + } + + /// Set values from MessageInputType + /** + * \return false by default + */ + virtual 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_ 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..b3e0496abb --- /dev/null +++ b/controller_interface/test/test_led_rgb_device.cpp @@ -0,0 +1,105 @@ +// 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 command 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_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 = 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)); + + 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(); + 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 new file mode 100644 index 0000000000..993239e48e --- /dev/null +++ b/controller_interface/test/test_led_rgb_device.hpp @@ -0,0 +1,67 @@ +// 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(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; +}; + +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_ 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..5b54a4b861 --- /dev/null +++ b/controller_interface/test/test_semantic_component_command_interface.cpp @@ -0,0 +1,127 @@ +// 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 +// +// 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 command 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..923e246bad --- /dev/null +++ b/controller_interface/test/test_semantic_component_command_interface.hpp @@ -0,0 +1,71 @@ +// 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 +// +// 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_ diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 58930ec6c4..3dec704f23 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 ******************