refactor smr version of semantic command interface to match ros-contr…

…ol style

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

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 <limits>
+#include <memory>
+#include <string>
+#include <vector>
+
+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<TestableSemanticCommandInterface>(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<std::string> 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<TestableSemanticCommandInterface>(component_name_, size_);
+
+  // generate the interface_names_
+  std::vector<std::string> interface_names = semantic_component_->get_command_interface_names();
+
+  // validate assign_loaned_command_interfaces
+  // create interfaces and assign values to it
+  std::vector<double> interface_values = {
+    std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN(),
+    std::numeric_limits<double>::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<hardware_interface::LoanedCommandInterface> 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<double> 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<TestableSemanticCommandInterface>(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<std::string> 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");
+}

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 <gmock/gmock.h>
+
+#include <memory>
+#include <string>
+
+#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<geometry_msgs::msg::Pose>
+{
+  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<geometry_msgs::msg::Pose>(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<TestableSemanticCommandInterface> semantic_component_;
+};
+
+#endif  // TEST_SEMANTIC_COMPONENT_COMMAND_INTERFACE_HPP_