Skip to content

Commit 7c89c17

Browse files
RobertWilbrandtRobertWilbrandt
authored
Implement new PoseBroadcaster controller (#1311)
1 parent f96d2fc commit 7c89c17

14 files changed

+882
-0
lines changed

‎doc/controllers_index.rst‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -71,6 +71,7 @@ In the sense of ros2_control, broadcasters are still controllers using the same
7171
IMU Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst>
7272
Joint State Broadcaster <../joint_state_broadcaster/doc/userdoc.rst>
7373
Range Sensor Broadcaster <../range_sensor_broadcaster/doc/userdoc.rst>
74+
Pose Broadcaster <../pose_broadcaster/doc/userdoc.rst>
7475

7576

7677
Common Controller Parameters

‎pose_broadcaster/CMakeLists.txt‎

Lines changed: 106 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,106 @@
1+
cmake_minimum_required(VERSION 3.16)
2+
project(pose_broadcaster
3+
LANGUAGES
4+
CXX
5+
)
6+
7+
if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
8+
add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
9+
-Werror=return-type -Werror=shadow -Werror=format)
10+
endif()
11+
12+
set(THIS_PACKAGE_INCLUDE_DEPENDS
13+
controller_interface
14+
generate_parameter_library
15+
geometry_msgs
16+
hardware_interface
17+
pluginlib
18+
rclcpp
19+
rclcpp_lifecycle
20+
realtime_tools
21+
tf2_msgs
22+
)
23+
24+
find_package(ament_cmake REQUIRED)
25+
find_package(backward_ros REQUIRED)
26+
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
27+
find_package(${Dependency} REQUIRED)
28+
endforeach()
29+
30+
generate_parameter_library(pose_broadcaster_parameters
31+
src/pose_broadcaster_parameters.yaml
32+
)
33+
34+
add_library(pose_broadcaster SHARED
35+
src/pose_broadcaster.cpp
36+
)
37+
target_compile_features(pose_broadcaster PUBLIC
38+
cxx_std_17
39+
)
40+
target_include_directories(pose_broadcaster PUBLIC
41+
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
42+
$<INSTALL_INTERFACE:include/${PROJECT_NAME}>
43+
)
44+
target_link_libraries(pose_broadcaster PUBLIC
45+
pose_broadcaster_parameters
46+
)
47+
ament_target_dependencies(pose_broadcaster PUBLIC
48+
${THIS_PACKAGE_INCLUDE_DEPENDS}
49+
)
50+
51+
# Causes the visibility macros to use dllexport rather than dllimport,
52+
# which is appropriate when building the dll but not consuming it.
53+
target_compile_definitions(pose_broadcaster PRIVATE "POSE_BROADCASTER_BUILDING_DLL")
54+
55+
pluginlib_export_plugin_description_file(
56+
controller_interface pose_broadcaster.xml
57+
)
58+
59+
if(BUILD_TESTING)
60+
find_package(ament_cmake_gmock REQUIRED)
61+
find_package(controller_manager REQUIRED)
62+
find_package(hardware_interface REQUIRED)
63+
find_package(ros2_control_test_assets REQUIRED)
64+
65+
add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
66+
ament_add_gmock(test_load_pose_broadcaster
67+
test/test_load_pose_broadcaster.cpp
68+
)
69+
target_link_libraries(test_load_pose_broadcaster
70+
pose_broadcaster
71+
)
72+
ament_target_dependencies(test_load_pose_broadcaster
73+
controller_manager
74+
ros2_control_test_assets
75+
)
76+
77+
add_rostest_with_parameters_gmock(test_pose_broadcaster
78+
test/test_pose_broadcaster.cpp
79+
${CMAKE_CURRENT_SOURCE_DIR}/test/pose_broadcaster_params.yaml
80+
)
81+
target_link_libraries(test_pose_broadcaster
82+
pose_broadcaster
83+
)
84+
ament_target_dependencies(test_pose_broadcaster
85+
hardware_interface
86+
)
87+
endif()
88+
89+
install(
90+
DIRECTORY
91+
include/
92+
DESTINATION include/${PROJECT_NAME}
93+
)
94+
install(
95+
TARGETS
96+
pose_broadcaster
97+
pose_broadcaster_parameters
98+
EXPORT export_${PROJECT_NAME}
99+
RUNTIME DESTINATION bin
100+
ARCHIVE DESTINATION lib
101+
LIBRARY DESTINATION lib
102+
)
103+
104+
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
105+
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
106+
ament_package()

‎pose_broadcaster/README.md‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
pose_broadcaster
2+
==========================================
3+
4+
Controller to publish poses provided by pose sensors.
5+
6+
Pluginlib-Library: pose_broadcaster
7+
8+
Plugin: pose_broadcaster/PoseBroadcaster (controller_interface::ControllerInterface)

‎pose_broadcaster/doc/userdoc.rst‎

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/pose_broadcaster/doc/userdoc.rst
2+
3+
.. _pose_broadcaster_userdoc:
4+
5+
Pose Broadcaster
6+
--------------------------------
7+
Broadcaster for poses measured by a robot or a sensor.
8+
Poses are published as ``geometry_msgs/msg/PoseStamped`` messages and optionally as tf transforms.
9+
10+
The controller is a wrapper around the ``PoseSensor`` semantic component (see ``controller_interface`` package).
11+
12+
Parameters
13+
^^^^^^^^^^^
14+
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.
15+
16+
List of parameters
17+
=========================
18+
.. generate_parameter_library_details:: ../src/pose_broadcaster_parameters.yaml
19+
20+
21+
An example parameter file
22+
=========================
23+
24+
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>`_:
25+
26+
.. literalinclude:: ../test/pose_broadcaster_params.yaml
27+
:language: yaml

‎pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp‎

Lines changed: 77 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,77 @@
1+
// Copyright 2024 FZI Forschungszentrum Informatik
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
#ifndef POSE_BROADCASTER__POSE_BROADCASTER_HPP_
15+
#define POSE_BROADCASTER__POSE_BROADCASTER_HPP_
16+
17+
#include <array>
18+
#include <memory>
19+
#include <optional>
20+
#include <string>
21+
22+
#include "controller_interface/controller_interface.hpp"
23+
#include "geometry_msgs/msg/pose_stamped.hpp"
24+
#include "pose_broadcaster/visibility_control.h"
25+
#include "pose_broadcaster_parameters.hpp"
26+
#include "rclcpp/publisher.hpp"
27+
#include "rclcpp_lifecycle/state.hpp"
28+
#include "realtime_tools/realtime_publisher.h"
29+
#include "semantic_components/pose_sensor.hpp"
30+
#include "tf2_msgs/msg/tf_message.hpp"
31+
32+
namespace pose_broadcaster
33+
{
34+
35+
class PoseBroadcaster : public controller_interface::ControllerInterface
36+
{
37+
public:
38+
POSE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration
39+
command_interface_configuration() const override;
40+
41+
POSE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration
42+
state_interface_configuration() const override;
43+
44+
POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init() override;
45+
46+
POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_configure(
47+
const rclcpp_lifecycle::State & previous_state) override;
48+
49+
POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_activate(
50+
const rclcpp_lifecycle::State & previous_state) override;
51+
52+
POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_deactivate(
53+
const rclcpp_lifecycle::State & previous_state) override;
54+
55+
POSE_BROADCASTER_PUBLIC controller_interface::return_type update(
56+
const rclcpp::Time & time, const rclcpp::Duration & period) override;
57+
58+
private:
59+
std::shared_ptr<ParamListener> param_listener_;
60+
Params params_;
61+
62+
std::unique_ptr<semantic_components::PoseSensor> pose_sensor_;
63+
64+
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_publisher_;
65+
std::unique_ptr<realtime_tools::RealtimePublisher<geometry_msgs::msg::PoseStamped>>
66+
realtime_publisher_;
67+
68+
std::optional<rclcpp::Duration> tf_publish_period_;
69+
rclcpp::Time tf_last_publish_time_{0, 0, RCL_CLOCK_UNINITIALIZED};
70+
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tf_publisher_;
71+
std::unique_ptr<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>
72+
realtime_tf_publisher_;
73+
};
74+
75+
} // namespace pose_broadcaster
76+
77+
#endif // POSE_BROADCASTER__POSE_BROADCASTER_HPP_

‎pose_broadcaster/include/pose_broadcaster/visibility_control.h‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
// Copyright 2024 FZI Forschungszentrum Informatik
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef POSE_BROADCASTER__VISIBILITY_CONTROL_H_
16+
#define POSE_BROADCASTER__VISIBILITY_CONTROL_H_
17+
18+
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
19+
// https://gcc.gnu.org/wiki/Visibility
20+
21+
#if defined _WIN32 || defined __CYGWIN__
22+
#ifdef __GNUC__
23+
#define POSE_BROADCASTER_EXPORT __attribute__((dllexport))
24+
#define POSE_BROADCASTER_IMPORT __attribute__((dllimport))
25+
#else
26+
#define POSE_BROADCASTER_EXPORT __declspec(dllexport)
27+
#define POSE_BROADCASTER_IMPORT __declspec(dllimport)
28+
#endif
29+
#ifdef POSE_BROADCASTER_BUILDING_DLL
30+
#define POSE_BROADCASTER_PUBLIC POSE_BROADCASTER_EXPORT
31+
#else
32+
#define POSE_BROADCASTER_PUBLIC POSE_BROADCASTER_IMPORT
33+
#endif
34+
#define POSE_BROADCASTER_PUBLIC_TYPE POSE_BROADCASTER_PUBLIC
35+
#define POSE_BROADCASTER_LOCAL
36+
#else
37+
#define POSE_BROADCASTER_EXPORT __attribute__((visibility("default")))
38+
#define POSE_BROADCASTER_IMPORT
39+
#if __GNUC__ >= 4
40+
#define POSE_BROADCASTER_PUBLIC __attribute__((visibility("default")))
41+
#define POSE_BROADCASTER_LOCAL __attribute__((visibility("hidden")))
42+
#else
43+
#define POSE_BROADCASTER_PUBLIC
44+
#define POSE_BROADCASTER_LOCAL
45+
#endif
46+
#define POSE_BROADCASTER_PUBLIC_TYPE
47+
#endif
48+
49+
#endif // POSE_BROADCASTER__VISIBILITY_CONTROL_H_

‎pose_broadcaster/package.xml‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>pose_broadcaster</name>
5+
<version>0.0.0</version>
6+
<description>Broadcaster to publish cartesian states.</description>
7+
<maintainer email="[email protected]">Robert Wilbrandt</maintainer>
8+
9+
<license>Apache License 2.0</license>
10+
11+
<buildtool_depend>ament_cmake</buildtool_depend>
12+
13+
<depend>backward_ros</depend>
14+
<depend>controller_interface</depend>
15+
<depend>generate_parameter_library</depend>
16+
<depend>geometry_msgs</depend>
17+
<depend>pluginlib</depend>
18+
<depend>rclcpp</depend>
19+
<depend>rclcpp_lifecycle</depend>
20+
<depend>realtime_tools</depend>
21+
<depend>tf2_msgs</depend>
22+
23+
<test_depend>ament_cmake_gmock</test_depend>
24+
<test_depend>controller_manager</test_depend>
25+
<test_depend>ros2_control_test_assets</test_depend>
26+
27+
<export>
28+
<build_type>ament_cmake</build_type>
29+
</export>
30+
</package>

‎pose_broadcaster/pose_broadcaster.xml‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
<library path="pose_broadcaster">
2+
<class name="pose_broadcaster/PoseBroadcaster"
3+
type="pose_broadcaster::PoseBroadcaster"
4+
base_class_type="controller_interface::ControllerInterface">
5+
<description>
6+
This controller publishes a Cartesian state as a geometry_msgs/PoseStamped message and optionally as a tf transform.
7+
</description>
8+
</class>
9+
</library>

0 commit comments

Comments
 (0)