Implement new PoseBroadcaster controller (backport #1311) (#1327)

* Implement new PoseBroadcaster controller (#1311)

(cherry picked from commit 7c89c17)

* Fix API in tests

* Add hardware_interface_testing dependency

---------

Co-authored-by: RobertWilbrandt <[email protected]>
Co-authored-by: Christoph Froehlich <[email protected]>

doc/controllers_index.rst

@@ -70,6 +70,7 @@ In the sense of ros2_control, broadcasters are still controllers using the same
    IMU Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst>
    Joint State Broadcaster <../joint_state_broadcaster/doc/userdoc.rst>
    Range Sensor Broadcaster <../range_sensor_broadcaster/doc/userdoc.rst>
+   Pose Broadcaster <../pose_broadcaster/doc/userdoc.rst>
 
 
 Common Controller Parameters

pose_broadcaster/CMakeLists.txt

@@ -0,0 +1,106 @@
+cmake_minimum_required(VERSION 3.16)
+project(pose_broadcaster
+  LANGUAGES
+    CXX
+)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
+                      -Werror=return-type -Werror=shadow -Werror=format)
+endif()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  controller_interface
+  generate_parameter_library
+  geometry_msgs
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+  tf2_msgs
+)
+
+find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+generate_parameter_library(pose_broadcaster_parameters
+  src/pose_broadcaster_parameters.yaml
+)
+
+add_library(pose_broadcaster SHARED
+  src/pose_broadcaster.cpp
+)
+target_compile_features(pose_broadcaster PUBLIC
+    cxx_std_17
+)
+target_include_directories(pose_broadcaster PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+)
+target_link_libraries(pose_broadcaster PUBLIC
+  pose_broadcaster_parameters
+)
+ament_target_dependencies(pose_broadcaster PUBLIC
+    ${THIS_PACKAGE_INCLUDE_DEPENDS}
+)
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(pose_broadcaster PRIVATE "POSE_BROADCASTER_BUILDING_DLL")
+
+pluginlib_export_plugin_description_file(
+  controller_interface pose_broadcaster.xml
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(hardware_interface REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+  ament_add_gmock(test_load_pose_broadcaster
+    test/test_load_pose_broadcaster.cpp
+  )
+  target_link_libraries(test_load_pose_broadcaster
+    pose_broadcaster
+  )
+  ament_target_dependencies(test_load_pose_broadcaster
+    controller_manager
+    ros2_control_test_assets
+  )
+
+  add_rostest_with_parameters_gmock(test_pose_broadcaster
+    test/test_pose_broadcaster.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/pose_broadcaster_params.yaml
+  )
+  target_link_libraries(test_pose_broadcaster
+    pose_broadcaster
+  )
+  ament_target_dependencies(test_pose_broadcaster
+    hardware_interface
+  )
+endif()
+
+install(
+  DIRECTORY
+    include/
+  DESTINATION include/${PROJECT_NAME}
+)
+install(
+  TARGETS
+    pose_broadcaster
+    pose_broadcaster_parameters
+  EXPORT export_${PROJECT_NAME}
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

pose_broadcaster/README.md

@@ -0,0 +1,8 @@
+pose_broadcaster
+==========================================
+
+Controller to publish poses provided by pose sensors.
+
+Pluginlib-Library: pose_broadcaster
+
+Plugin: pose_broadcaster/PoseBroadcaster (controller_interface::ControllerInterface)

pose_broadcaster/doc/userdoc.rst

@@ -0,0 +1,27 @@
+:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/pose_broadcaster/doc/userdoc.rst
+
+.. _pose_broadcaster_userdoc:
+
+Pose Broadcaster
+--------------------------------
+Broadcaster for poses measured by a robot or a sensor.
+Poses are published as ``geometry_msgs/msg/PoseStamped`` messages and optionally as tf transforms.
+
+The controller is a wrapper around the ``PoseSensor`` semantic component (see ``controller_interface`` package).
+
+Parameters
+^^^^^^^^^^^
+This controller uses the `generate_parameter_library <https://github.com/PickNikRobotics/generate_parameter_library>`_ to handle its parameters. The parameter `definition file located in the src folder <https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/pose_broadcaster/src/pose_broadcaster_parameters.yaml>`_ contains descriptions for all the parameters used by the controller.
+
+List of parameters
+=========================
+.. generate_parameter_library_details:: ../src/pose_broadcaster_parameters.yaml
+
+
+An example parameter file
+=========================
+
+An example parameter file for this controller can be found in `the test directory <https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/pose_broadcaster/test/pose_broadcaster_params.yaml>`_:
+
+.. literalinclude:: ../test/pose_broadcaster_params.yaml
+   :language: yaml

pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp

@@ -0,0 +1,77 @@
+// 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 POSE_BROADCASTER__POSE_BROADCASTER_HPP_
+#define POSE_BROADCASTER__POSE_BROADCASTER_HPP_
+
+#include <array>
+#include <memory>
+#include <optional>
+#include <string>
+
+#include "controller_interface/controller_interface.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "pose_broadcaster/visibility_control.h"
+#include "pose_broadcaster_parameters.hpp"
+#include "rclcpp/publisher.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "realtime_tools/realtime_publisher.h"
+#include "semantic_components/pose_sensor.hpp"
+#include "tf2_msgs/msg/tf_message.hpp"
+
+namespace pose_broadcaster
+{
+
+class PoseBroadcaster : public controller_interface::ControllerInterface
+{
+public:
+  POSE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration
+  command_interface_configuration() const override;
+
+  POSE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration
+  state_interface_configuration() const override;
+
+  POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init() override;
+
+  POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  POSE_BROADCASTER_PUBLIC controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+private:
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
+
+  std::unique_ptr<semantic_components::PoseSensor> pose_sensor_;
+
+  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_publisher_;
+  std::unique_ptr<realtime_tools::RealtimePublisher<geometry_msgs::msg::PoseStamped>>
+    realtime_publisher_;
+
+  std::optional<rclcpp::Duration> tf_publish_period_;
+  rclcpp::Time tf_last_publish_time_{0, 0, RCL_CLOCK_UNINITIALIZED};
+  rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tf_publisher_;
+  std::unique_ptr<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>
+    realtime_tf_publisher_;
+};
+
+}  // namespace pose_broadcaster
+
+#endif  // POSE_BROADCASTER__POSE_BROADCASTER_HPP_

pose_broadcaster/include/pose_broadcaster/visibility_control.h

@@ -0,0 +1,49 @@
+// 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 POSE_BROADCASTER__VISIBILITY_CONTROL_H_
+#define POSE_BROADCASTER__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+#ifdef __GNUC__
+#define POSE_BROADCASTER_EXPORT __attribute__((dllexport))
+#define POSE_BROADCASTER_IMPORT __attribute__((dllimport))
+#else
+#define POSE_BROADCASTER_EXPORT __declspec(dllexport)
+#define POSE_BROADCASTER_IMPORT __declspec(dllimport)
+#endif
+#ifdef POSE_BROADCASTER_BUILDING_DLL
+#define POSE_BROADCASTER_PUBLIC POSE_BROADCASTER_EXPORT
+#else
+#define POSE_BROADCASTER_PUBLIC POSE_BROADCASTER_IMPORT
+#endif
+#define POSE_BROADCASTER_PUBLIC_TYPE POSE_BROADCASTER_PUBLIC
+#define POSE_BROADCASTER_LOCAL
+#else
+#define POSE_BROADCASTER_EXPORT __attribute__((visibility("default")))
+#define POSE_BROADCASTER_IMPORT
+#if __GNUC__ >= 4
+#define POSE_BROADCASTER_PUBLIC __attribute__((visibility("default")))
+#define POSE_BROADCASTER_LOCAL __attribute__((visibility("hidden")))
+#else
+#define POSE_BROADCASTER_PUBLIC
+#define POSE_BROADCASTER_LOCAL
+#endif
+#define POSE_BROADCASTER_PUBLIC_TYPE
+#endif
+
+#endif  // POSE_BROADCASTER__VISIBILITY_CONTROL_H_

pose_broadcaster/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>pose_broadcaster</name>
+  <version>0.0.0</version>
+  <description>Broadcaster to publish cartesian states.</description>
+  <maintainer email="[email protected]">Robert Wilbrandt</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>backward_ros</depend>
+  <depend>controller_interface</depend>
+  <depend>generate_parameter_library</depend>
+  <depend>geometry_msgs</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>realtime_tools</depend>
+  <depend>tf2_msgs</depend>
+
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>hardware_interface_testing</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

pose_broadcaster/pose_broadcaster.xml

@@ -0,0 +1,9 @@
+<library path="pose_broadcaster">
+  <class name="pose_broadcaster/PoseBroadcaster"
+         type="pose_broadcaster::PoseBroadcaster"
+         base_class_type="controller_interface::ControllerInterface">
+    <description>
+      This controller publishes a Cartesian state as a geometry_msgs/PoseStamped message and optionally as a tf transform.
+    </description>
+  </class>
+</library>