add a semantic command interface to "semantic_components"

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(

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 <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>/r", "<name>/g", "<name>/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
+   */
+  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,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 <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());
+  }
+
+  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<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.
+   */
+  virtual std::vector<std::string> 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<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 false by default
+   */
+  virtual bool set_values_from_message(const MessageInputType & /* message */) { return false; }
+
+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,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<TestableLedDevice>(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<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);
+}
+
+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<TestableLedDevice>(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);
+}