Merge branch 'humble' into mergify/bp/humble/pr-1661

.pre-commit-config.yaml

@@ -49,20 +49,20 @@ repos:
       args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"]
 
   - repo: https://github.com/psf/black
-    rev: 24.4.2
+    rev: 24.8.0
     hooks:
       - id: black
         args: ["--line-length=99"]
 
   - repo: https://github.com/pycqa/flake8
-    rev: 7.1.0
+    rev: 7.1.1
     hooks:
     - id: flake8
       args: ["--extend-ignore=E501"]
 
   # CPP hooks
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v18.1.8
+    rev: v19.1.0
     hooks:
       - id: clang-format
         args: ['-fallback-style=none', '-i']
@@ -108,7 +108,7 @@ repos:
 
   # Docs - RestructuredText hooks
   - repo: https://github.com/PyCQA/doc8
-    rev: v1.1.1
+    rev: v1.1.2
     hooks:
       - id: doc8
         args: ['--max-line-length=100', '--ignore=D001']
@@ -132,7 +132,7 @@ repos:
         exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$
 
   - repo: https://github.com/python-jsonschema/check-jsonschema
-    rev: 0.29.1
+    rev: 0.29.3
     hooks:
       - id: check-github-workflows
         args: ["--verbose"]

controller_interface/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package controller_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.43.1 (2024-09-11)
+-------------------
+
 2.43.0 (2024-08-22)
 -------------------
 

controller_interface/CMakeLists.txt

@@ -44,6 +44,7 @@ if(BUILD_TESTING)
 
   find_package(hardware_interface REQUIRED)
   find_package(sensor_msgs REQUIRED)
+  find_package(geometry_msgs REQUIRED)
 
   ament_add_gmock(test_controller_interface test/test_controller_interface.cpp)
   target_link_libraries(test_controller_interface ${PROJECT_NAME})
@@ -88,6 +89,13 @@ if(BUILD_TESTING)
     hardware_interface
     sensor_msgs
   )
+
+  ament_add_gmock(test_pose_sensor test/test_pose_sensor.cpp)
+  target_include_directories(test_pose_sensor PRIVATE include)
+  ament_target_dependencies(test_pose_sensor
+    hardware_interface
+    geometry_msgs
+  )
 endif()
 
 ament_export_dependencies(

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

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>controller_interface</name>
-  <version>2.43.0</version>
+  <version>2.43.1</version>
   <description>Description of controller_interface</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>
@@ -22,6 +22,8 @@
   <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>
     <build_type>ament_cmake</build_type>

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_

controller_manager/CHANGELOG.rst

@@ -2,6 +2,12 @@
 Changelog for package controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.43.1 (2024-09-11)
+-------------------
+* controller_manager: Add space to string literal concatenation (`#1245 <https://github.com/ros-controls/ros2_control/issues/1245>`_) (`#1747 <https://github.com/ros-controls/ros2_control/issues/1747>`_)
+* fix: the print of the information in control node was in wrong order (`#1726 <https://github.com/ros-controls/ros2_control/issues/1726>`_)
+* Contributors: Manuel Muth, roscan-tech
+
 2.43.0 (2024-08-22)
 -------------------
 * Infrom user why rt policy could not be set, infrom if is set. (backport `#1705 <https://github.com/ros-controls/ros2_control/issues/1705>`_) (`#1708 <https://github.com/ros-controls/ros2_control/issues/1708>`_)

controller_manager/doc/userdoc.rst

@@ -148,6 +148,22 @@ There are two scripts to interact with controller manager from launch files:
       --configure           Configures the given components.
 
 
+rqt_controller_manager
+----------------------
+A GUI tool to interact with the controller manager services to be able to switch the lifecycle states of the controllers as well as the hardware components.
+
+.. image:: images/rqt_controller_manager.png
+
+It can be launched independently using the following command or as rqt plugin.
+
+.. code-block:: console
+
+    ros2 run rqt_controller_manager rqt_controller_manager
+
+   * Double-click on a controller or hardware component to show the additional info.
+   * Right-click on a controller or hardware component to show a context menu with options for lifecycle management.
+
+
 Using the Controller Manager in a Process
 -----------------------------------------