Add PoseSensor semantic component (#1775)

(cherry picked from commit 8cdded3)

# Conflicts:
#	doc/release_notes.rst

controller_interface/CMakeLists.txt

@@ -32,6 +32,7 @@ target_compile_definitions(controller_interface PRIVATE "CONTROLLER_INTERFACE_BU
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gmock REQUIRED)
+  find_package(geometry_msgs REQUIRED)
   find_package(sensor_msgs REQUIRED)
 
   ament_add_gmock(test_controller_interface test/test_controller_interface.cpp)
@@ -70,6 +71,15 @@ if(BUILD_TESTING)
   ament_target_dependencies(test_imu_sensor
     sensor_msgs
   )
+
+  ament_add_gmock(test_pose_sensor test/test_pose_sensor.cpp)
+  target_link_libraries(test_pose_sensor
+    controller_interface
+    hardware_interface::hardware_interface
+  )
+  ament_target_dependencies(test_pose_sensor
+    geometry_msgs
+  )
 endif()
 
 install(

controller_interface/include/semantic_components/pose_sensor.hpp

@@ -0,0 +1,110 @@
+// Copyright 2024 FZI Forschungszentrum Informatik
+//
+// 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__POSE_SENSOR_HPP_
+#define SEMANTIC_COMPONENTS__POSE_SENSOR_HPP_
+
+#include <array>
+#include <limits>
+#include <string>
+
+#include "geometry_msgs/msg/pose.hpp"
+#include "semantic_components/semantic_component_interface.hpp"
+
+namespace semantic_components
+{
+
+class PoseSensor : public SemanticComponentInterface<geometry_msgs::msg::Pose>
+{
+public:
+  /// Constructor for a standard pose sensor with interface names set based on sensor name.
+  explicit PoseSensor(const std::string & name) : SemanticComponentInterface{name, 7}
+  {
+    // Use standard interface names
+    interface_names_.emplace_back(name_ + '/' + "position.x");
+    interface_names_.emplace_back(name_ + '/' + "position.y");
+    interface_names_.emplace_back(name_ + '/' + "position.z");
+    interface_names_.emplace_back(name_ + '/' + "orientation.x");
+    interface_names_.emplace_back(name_ + '/' + "orientation.y");
+    interface_names_.emplace_back(name_ + '/' + "orientation.z");
+    interface_names_.emplace_back(name_ + '/' + "orientation.w");
+
+    // Set all sensor values to default value NaN
+    std::fill(position_.begin(), position_.end(), std::numeric_limits<double>::quiet_NaN());
+    std::fill(orientation_.begin(), orientation_.end(), std::numeric_limits<double>::quiet_NaN());
+  }
+
+  virtual ~PoseSensor() = default;
+
+  /// Update and return position.
+  /*!
+   * Update and return current pose position from state interfaces.
+   *
+   * \return Array of position coordinates.
+   */
+  std::array<double, 3> get_position()
+  {
+    for (size_t i = 0; i < 3; ++i)
+    {
+      position_[i] = state_interfaces_[i].get().get_value();
+    }
+
+    return position_;
+  }
+
+  /// Update and return orientation
+  /*!
+   * Update and return current pose orientation from state interfaces.
+   *
+   * \return Array of orientation coordinates in xyzw convention.
+   */
+  std::array<double, 4> get_orientation()
+  {
+    for (size_t i = 3; i < 7; ++i)
+    {
+      orientation_[i - 3] = state_interfaces_[i].get().get_value();
+    }
+
+    return orientation_;
+  }
+
+  /// Fill pose message with current values.
+  /**
+   * Fill a pose message with current position and orientation from the state interfaces.
+   */
+  bool get_values_as_message(geometry_msgs::msg::Pose & message)
+  {
+    // Update state from state interfaces
+    get_position();
+    get_orientation();
+
+    // Set message values from current state
+    message.position.x = position_[0];
+    message.position.y = position_[1];
+    message.position.z = position_[2];
+    message.orientation.x = orientation_[0];
+    message.orientation.y = orientation_[1];
+    message.orientation.z = orientation_[2];
+    message.orientation.w = orientation_[3];
+
+    return true;
+  }
+
+protected:
+  std::array<double, 3> position_;
+  std::array<double, 4> orientation_;
+};
+
+}  // namespace semantic_components
+
+#endif  // SEMANTIC_COMPONENTS__POSE_SENSOR_HPP_

controller_interface/package.xml

@@ -18,6 +18,7 @@
   <build_export_depend>rclcpp_lifecycle</build_export_depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>geometry_msgs</test_depend>
   <test_depend>sensor_msgs</test_depend>
 
   <export>

controller_interface/test/test_pose_sensor.cpp

@@ -0,0 +1,98 @@
+// Copyright (c) 2024, FZI Forschungszentrum Informatik
+//
+// 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_pose_sensor.hpp"
+
+void PoseSensorTest::SetUp()
+{
+  full_interface_names_.reserve(size_);
+  for (const auto & interface_name : interface_names_)
+  {
+    full_interface_names_.emplace_back(sensor_name_ + '/' + interface_name);
+  }
+}
+
+void PoseSensorTest::TearDown() { pose_sensor_.reset(nullptr); }
+
+TEST_F(PoseSensorTest, validate_all)
+{
+  // Create sensor
+  pose_sensor_ = std::make_unique<TestablePoseSensor>(sensor_name_);
+  EXPECT_EQ(pose_sensor_->name_, sensor_name_);
+
+  // Validate reserved space for interface_names_ and state_interfaces_
+  // As state_interfaces_ are not defined yet, use capacity()
+  ASSERT_EQ(pose_sensor_->interface_names_.size(), size_);
+  ASSERT_EQ(pose_sensor_->state_interfaces_.capacity(), size_);
+
+  // Validate default interface_names_
+  EXPECT_TRUE(std::equal(
+    pose_sensor_->interface_names_.cbegin(), pose_sensor_->interface_names_.cend(),
+    full_interface_names_.cbegin(), full_interface_names_.cend()));
+
+  // Get interface names
+  std::vector<std::string> interface_names = pose_sensor_->get_state_interface_names();
+
+  // Assign values to position
+  hardware_interface::StateInterface position_x{
+    sensor_name_, interface_names_[0], &position_values_[0]};
+  hardware_interface::StateInterface position_y{
+    sensor_name_, interface_names_[1], &position_values_[1]};
+  hardware_interface::StateInterface position_z{
+    sensor_name_, interface_names_[2], &position_values_[2]};
+
+  // Assign values to orientation
+  hardware_interface::StateInterface orientation_x{
+    sensor_name_, interface_names_[3], &orientation_values_[0]};
+  hardware_interface::StateInterface orientation_y{
+    sensor_name_, interface_names_[4], &orientation_values_[1]};
+  hardware_interface::StateInterface orientation_z{
+    sensor_name_, interface_names_[5], &orientation_values_[2]};
+  hardware_interface::StateInterface orientation_w{
+    sensor_name_, interface_names_[6], &orientation_values_[3]};
+
+  // Create state interface vector in jumbled order
+  std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
+  temp_state_interfaces.reserve(7);
+
+  temp_state_interfaces.emplace_back(position_z);
+  temp_state_interfaces.emplace_back(orientation_y);
+  temp_state_interfaces.emplace_back(orientation_x);
+  temp_state_interfaces.emplace_back(position_x);
+  temp_state_interfaces.emplace_back(orientation_w);
+  temp_state_interfaces.emplace_back(position_y);
+  temp_state_interfaces.emplace_back(orientation_z);
+
+  // Assign interfaces
+  pose_sensor_->assign_loaned_state_interfaces(temp_state_interfaces);
+  EXPECT_EQ(pose_sensor_->state_interfaces_.size(), size_);
+
+  // Validate correct position and orientation
+  EXPECT_EQ(pose_sensor_->get_position(), position_values_);
+  EXPECT_EQ(pose_sensor_->get_orientation(), orientation_values_);
+
+  // Validate generated message
+  geometry_msgs::msg::Pose temp_message;
+  ASSERT_TRUE(pose_sensor_->get_values_as_message(temp_message));
+  EXPECT_EQ(temp_message.position.x, position_values_[0]);
+  EXPECT_EQ(temp_message.position.y, position_values_[1]);
+  EXPECT_EQ(temp_message.position.z, position_values_[2]);
+  EXPECT_EQ(temp_message.orientation.x, orientation_values_[0]);
+  EXPECT_EQ(temp_message.orientation.y, orientation_values_[1]);
+  EXPECT_EQ(temp_message.orientation.z, orientation_values_[2]);
+  EXPECT_EQ(temp_message.orientation.w, orientation_values_[3]);
+
+  // Release state interfaces
+  pose_sensor_->release_interfaces();
+  ASSERT_EQ(pose_sensor_->state_interfaces_.size(), 0);
+}

controller_interface/test/test_pose_sensor.hpp

@@ -0,0 +1,59 @@
+// Copyright (c) 2024, FZI Forschungszentrum Informatik
+//
+// 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_POSE_SENSOR_HPP_
+#define TEST_POSE_SENSOR_HPP_
+
+#include <gmock/gmock.h>
+
+#include <array>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "semantic_components/pose_sensor.hpp"
+
+class TestablePoseSensor : public semantic_components::PoseSensor
+{
+  FRIEND_TEST(PoseSensorTest, validate_all);
+
+public:
+  // Use default interface names
+  explicit TestablePoseSensor(const std::string & name) : PoseSensor{name} {}
+
+  virtual ~TestablePoseSensor() = default;
+};
+
+class PoseSensorTest : public ::testing::Test
+{
+public:
+  void SetUp();
+  void TearDown();
+
+protected:
+  const size_t size_ = 7;
+  const std::string sensor_name_ = "test_pose_sensor";
+
+  std::vector<std::string> full_interface_names_;
+  const std::vector<std::string> interface_names_ = {
+    "position.x",    "position.y",    "position.z",   "orientation.x",
+    "orientation.y", "orientation.z", "orientation.w"};
+
+  std::array<double, 3> position_values_ = {{1.1, 2.2, 3.3}};
+  std::array<double, 4> orientation_values_ = {{4.4, 5.5, 6.6, 7.7}};
+
+  std::unique_ptr<TestablePoseSensor> pose_sensor_;
+};
+
+#endif  // TEST_POSE_SENSOR_HPP_