add a semantic command interface to "semantic_components" (#1945)

controller_interface/CMakeLists.txt

@@ -87,12 +87,30 @@ if(BUILD_TESTING)
   ament_target_dependencies(test_pose_sensor
     geometry_msgs
   )
-
   ament_add_gmock(test_gps_sensor test/test_gps_sensor.cpp)
   target_link_libraries(test_gps_sensor
     controller_interface
     hardware_interface::hardware_interface
   )
+
+  # 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(

controller_interface/include/semantic_components/led_rgb_device.hpp

@@ -0,0 +1,77 @@
+// 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 <string>
+#include <vector>
+
+#include "semantic_components/semantic_component_command_interface.hpp"
+#include "std_msgs/msg/color_rgba.hpp"
+
+namespace semantic_components
+{
+class LedRgbDevice : public SemanticComponentCommandInterface<std_msgs::msg::ColorRGBA>
+{
+public:
+  /**
+   * Constructor for a LED RGB device with interface names set based on device name.
+   * The constructor sets the command interface names to "<name>/interface_r",
+   * "<name>/interface_g", "<name>/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 & name, const std::string & interface_r, const std::string & interface_g,
+    const std::string & interface_b)
+  : SemanticComponentCommandInterface(
+      name, {{name + "/" + interface_r}, {name + "/" + interface_g}, {name + "/" + 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
+   */
+  bool set_values_from_message(const std_msgs::msg::ColorRGBA & message) override
+  {
+    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_

controller_interface/include/semantic_components/semantic_component_command_interface.hpp

@@ -0,0 +1,103 @@
+// 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 <string>
+#include <vector>
+
+#include "controller_interface/helpers.hpp"
+#include "hardware_interface/loaned_command_interface.hpp"
+
+namespace semantic_components
+{
+template <typename MessageInputType>
+class SemanticComponentCommandInterface
+{
+public:
+  SemanticComponentCommandInterface(
+    const std::string & name, const std::vector<std::string> & interface_names)
+  : name_(name), interface_names_(interface_names)
+  {
+    assert(interface_names.size() > 0);
+    command_interfaces_.reserve(interface_names.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<hardware_interface::LoanedCommandInterface> & 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.
+   */
+  const std::vector<std::string> & get_command_interface_names() const { 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<double> & 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 True if all values were set successfully, false otherwise.
+   */
+  virtual bool set_values_from_message(const MessageInputType & /* message */) = 0;
+
+protected:
+  std::string name_;
+  std::vector<std::string> interface_names_;
+  std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    command_interfaces_;
+};
+
+}  // namespace semantic_components
+
+#endif  // SEMANTIC_COMPONENTS__SEMANTIC_COMPONENT_COMMAND_INTERFACE_HPP_

controller_interface/test/test_led_rgb_device.cpp

@@ -0,0 +1,87 @@
+// 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<TestableLedDevice>(
+    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_
+  // 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<std::string> 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<hardware_interface::LoanedCommandInterface> 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<double> 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<float>(test_led_values_cmd[0]);
+  temp_message.g = static_cast<float>(test_led_values_cmd[1]);
+  temp_message.b = static_cast<float>(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);
+}

controller_interface/test/test_led_rgb_device.hpp

@@ -0,0 +1,63 @@
+// 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 <gmock/gmock.h>
+
+#include <array>
+#include <limits>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "semantic_components/led_rgb_device.hpp"
+
+class TestableLedDevice : public semantic_components::LedRgbDevice
+{
+  FRIEND_TEST(LedDeviceTest, validate_all);
+
+public:
+  TestableLedDevice(
+    const std::string & name, const std::string & interface_r, const std::string & interface_g,
+    const std::string & interface_b)
+  : LedRgbDevice{name, 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<std::string> full_cmd_interface_names_;
+  const std::vector<std::string> interface_names_ = {"r", "g", "b"};
+
+  std::array<double, 3> led_values_ = {
+    {std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN(),
+     std::numeric_limits<double>::quiet_NaN()}};
+
+  std::unique_ptr<TestableLedDevice> led_device_;
+};
+
+#endif  // TEST_LED_RGB_DEVICE_HPP_