Skip to content

Commit

Permalink
Implement new PoseBroadcaster controller (#1311)
Browse files Browse the repository at this point in the history
(cherry picked from commit 7c89c17)

# Conflicts:
#	doc/controllers_index.rst
  • Loading branch information
RobertWilbrandt authored and mergify[bot] committed Oct 29, 2024
1 parent 2c2a47c commit 73bc6c5
Show file tree
Hide file tree
Showing 14 changed files with 894 additions and 0 deletions.
13 changes: 13 additions & 0 deletions doc/controllers_index.rst
Original file line number Diff line number Diff line change
Expand Up @@ -70,3 +70,16 @@ 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>
<<<<<<< HEAD
=======
Pose Broadcaster <../pose_broadcaster/doc/userdoc.rst>


Common Controller Parameters
****************************

Every controller and broadcaster has a few common parameters. They are optional, but if needed they have to be set before ``onConfigure`` transition to ``inactive`` state, see `lifecycle documents <https://design.ros2.org/articles/node_lifecycle.html>`__. Once the controllers are already loaded, this transition is done using the service ``configure_controller`` of the controller_manager.

* ``update_rate``: An unsigned integer parameter representing the rate at which every controller/broadcaster runs its update cycle. When unspecified, they run at the same frequency as the controller_manager.
* ``is_async``: A boolean parameter that is needed to specify if the controller update needs to run asynchronously.
>>>>>>> 7c89c17 (Implement new PoseBroadcaster controller (#1311))
106 changes: 106 additions & 0 deletions pose_broadcaster/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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()
8 changes: 8 additions & 0 deletions pose_broadcaster/README.md
Original file line number Diff line number Diff line change
@@ -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)
27 changes: 27 additions & 0 deletions pose_broadcaster/doc/userdoc.rst
Original file line number Diff line number Diff line change
@@ -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
77 changes: 77 additions & 0 deletions pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp
Original file line number Diff line number Diff line change
@@ -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_
49 changes: 49 additions & 0 deletions pose_broadcaster/include/pose_broadcaster/visibility_control.h
Original file line number Diff line number Diff line change
@@ -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_
30 changes: 30 additions & 0 deletions pose_broadcaster/package.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,30 @@
<?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>ros2_control_test_assets</test_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
</package>
9 changes: 9 additions & 0 deletions pose_broadcaster/pose_broadcaster.xml
Original file line number Diff line number Diff line change
@@ -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>
Loading

0 comments on commit 73bc6c5

Please sign in to comment.