add a broadcaster for range sensor (#725)

range_sensor_broadcaster/CMakeLists.txt

@@ -0,0 +1,106 @@
+cmake_minimum_required(VERSION 3.8)
+project(range_sensor_broadcaster)
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  controller_interface
+  generate_parameter_library
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+  sensor_msgs
+)
+
+# find dependencies
+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(range_sensor_broadcaster_parameters
+  src/range_sensor_broadcaster_parameters.yaml
+)
+
+add_library(
+  range_sensor_broadcaster SHARED
+  src/range_sensor_broadcaster.cpp
+)
+target_include_directories(range_sensor_broadcaster
+  PRIVATE $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  PRIVATE $<INSTALL_INTERFACE:include>
+)
+ament_target_dependencies(range_sensor_broadcaster ${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(range_sensor_broadcaster PRIVATE "RANGE_SENSOR_BROADCASTER_BUILDING_DLL")
+target_link_libraries(range_sensor_broadcaster
+  range_sensor_broadcaster_parameters
+)
+
+pluginlib_export_plugin_description_file(
+  controller_interface range_sensor_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_rostest_with_parameters_gmock(test_load_range_sensor_broadcaster
+  test/test_load_range_sensor_broadcaster.cpp
+  ${CMAKE_CURRENT_SOURCE_DIR}/test/range_sensor_broadcaster_params.yaml)
+  target_include_directories(test_load_range_sensor_broadcaster PRIVATE include)
+  target_link_libraries(test_load_range_sensor_broadcaster
+    range_sensor_broadcaster
+  )
+  ament_target_dependencies(test_load_range_sensor_broadcaster
+    controller_manager
+    hardware_interface
+    ros2_control_test_assets
+  )
+
+  add_rostest_with_parameters_gmock(test_range_sensor_broadcaster
+    test/test_range_sensor_broadcaster.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/range_sensor_broadcaster_params.yaml)
+  target_include_directories(test_range_sensor_broadcaster PRIVATE include)
+  target_link_libraries(test_range_sensor_broadcaster
+    range_sensor_broadcaster
+  )
+  ament_target_dependencies(test_range_sensor_broadcaster
+    hardware_interface
+  )
+endif()
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(
+  TARGETS
+    range_sensor_broadcaster
+    range_sensor_broadcaster_parameters
+  EXPORT export_range_sensor_broadcaster
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  INCLUDES DESTINATION include
+)
+
+ament_export_targets(export_range_sensor_broadcaster HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+ament_package()

range_sensor_broadcaster/README.md

@@ -0,0 +1,8 @@
+range_sensor_broadcaster
+==========================================
+
+Controller to publish readings of Range sensors.
+
+Pluginlib-Library: range_sensor_broadcaster
+
+Plugin: range_sensor_broadcaster/RangeSensorBroadcaster (controller_interface::ControllerInterface)

range_sensor_broadcaster/doc/userdoc.rst

@@ -0,0 +1,15 @@
+:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/range_sensor_broadcaster/doc/userdoc.rst
+
+.. _range_sensor_broadcaster_userdoc:
+
+Range Sensor Broadcaster
+--------------------------------
+Broadcaster of messages from Range sensor.
+The published message type is ``sensor_msgs/msg/Range``.
+
+The controller is a wrapper around ``RangeSensor`` semantic component (see ``controller_interface`` package).
+
+Parameters
+^^^^^^^^^^^
+
+.. generate_parameter_library_details:: ../src/range_sensor_broadcaster_parameters.yaml

range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp

@@ -0,0 +1,77 @@
+// Copyright 2021 PAL Robotics SL.
+//
+// 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.
+
+/*
+ * Authors: Subhas Das, Denis Stogl, Victor Lopez
+ */
+
+#ifndef RANGE_SENSOR_BROADCASTER__RANGE_SENSOR_BROADCASTER_HPP_
+#define RANGE_SENSOR_BROADCASTER__RANGE_SENSOR_BROADCASTER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "controller_interface/controller_interface.hpp"
+#include "range_sensor_broadcaster/visibility_control.h"
+#include "range_sensor_broadcaster_parameters.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "realtime_tools/realtime_publisher.h"
+#include "semantic_components/range_sensor.hpp"
+#include "sensor_msgs/msg/range.hpp"
+
+namespace range_sensor_broadcaster
+{
+class RangeSensorBroadcaster : public controller_interface::ControllerInterface
+{
+public:
+  RANGE_SENSOR_BROADCASTER_PUBLIC
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  RANGE_SENSOR_BROADCASTER_PUBLIC
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  RANGE_SENSOR_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init() override;
+
+  RANGE_SENSOR_BROADCASTER_PUBLIC
+  controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  RANGE_SENSOR_BROADCASTER_PUBLIC
+  controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  RANGE_SENSOR_BROADCASTER_PUBLIC
+  controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  RANGE_SENSOR_BROADCASTER_PUBLIC
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+protected:
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
+
+  std::unique_ptr<semantic_components::RangeSensor> range_sensor_;
+
+  using StatePublisher = realtime_tools::RealtimePublisher<sensor_msgs::msg::Range>;
+  rclcpp::Publisher<sensor_msgs::msg::Range>::SharedPtr sensor_state_publisher_;
+  std::unique_ptr<StatePublisher> realtime_publisher_;
+};
+
+}  // namespace range_sensor_broadcaster
+
+#endif  // RANGE_SENSOR_BROADCASTER__RANGE_SENSOR_BROADCASTER_HPP_

range_sensor_broadcaster/include/range_sensor_broadcaster/visibility_control.h

@@ -0,0 +1,53 @@
+// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// 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.
+
+/*
+ * Author: Subhas Das, Denis Stogl
+ */
+
+#ifndef RANGE_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_
+#define RANGE_SENSOR_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 RANGE_SENSOR_BROADCASTER_EXPORT __attribute__((dllexport))
+#define RANGE_SENSOR_BROADCASTER_IMPORT __attribute__((dllimport))
+#else
+#define RANGE_SENSOR_BROADCASTER_EXPORT __declspec(dllexport)
+#define RANGE_SENSOR_BROADCASTER_IMPORT __declspec(dllimport)
+#endif
+#ifdef RANGE_SENSOR_BROADCASTER_BUILDING_DLL
+#define RANGE_SENSOR_BROADCASTER_PUBLIC RANGE_SENSOR_BROADCASTER_EXPORT
+#else
+#define RANGE_SENSOR_BROADCASTER_PUBLIC RANGE_SENSOR_BROADCASTER_IMPORT
+#endif
+#define RANGE_SENSOR_BROADCASTER_PUBLIC_TYPE RANGE_SENSOR_BROADCASTER_PUBLIC
+#define RANGE_SENSOR_BROADCASTER_LOCAL
+#else
+#define RANGE_SENSOR_BROADCASTER_EXPORT __attribute__((visibility("default")))
+#define RANGE_SENSOR_BROADCASTER_IMPORT
+#if __GNUC__ >= 4
+#define RANGE_SENSOR_BROADCASTER_PUBLIC __attribute__((visibility("default")))
+#define RANGE_SENSOR_BROADCASTER_LOCAL __attribute__((visibility("hidden")))
+#else
+#define RANGE_SENSOR_BROADCASTER_PUBLIC
+#define RANGE_SENSOR_BROADCASTER_LOCAL
+#endif
+#define RANGE_SENSOR_BROADCASTER_PUBLIC_TYPE
+#endif
+
+#endif  // RANGE_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_

range_sensor_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>range_sensor_broadcaster</name>
+  <version>3.14.0</version>
+  <description>Controller to publish readings of Range sensors.</description>
+  <maintainer email="[email protected]">Bence Magyar</maintainer>
+  <author email="[email protected]">Florent Chretien</author>
+  <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>hardware_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>realtime_tools</depend>
+  <depend>sensor_msgs</depend>
+
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>hardware_interface</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

range_sensor_broadcaster/range_sensor_broadcaster.xml

@@ -0,0 +1,8 @@
+<library path="range_sensor_broadcaster">
+  <class name="range_sensor_broadcaster/RangeSensorBroadcaster"
+         type="range_sensor_broadcaster::RangeSensorBroadcaster" base_class_type="controller_interface::ControllerInterface">
+  <description>
+	  This controller publishes the readings of an Range sensor as sensor_msgs/Range message.
+  </description>
+  </class>
+</library>