From 48c0f8187d24fbcb28c96be25f3fdeaa9b184082 Mon Sep 17 00:00:00 2001 From: flochre <39066045+flochre@users.noreply.github.com> Date: Thu, 7 Sep 2023 06:17:30 +0600 Subject: [PATCH] add a broadcaster for range sensor (#725) (cherry picked from commit 5b86e3c848a0f1498978e24fe5b1b48566c2d14e) --- range_sensor_broadcaster/CMakeLists.txt | 106 ++++++++ range_sensor_broadcaster/README.md | 8 + range_sensor_broadcaster/doc/userdoc.rst | 15 ++ .../range_sensor_broadcaster.hpp | 77 ++++++ .../visibility_control.h | 53 ++++ range_sensor_broadcaster/package.xml | 31 +++ .../range_sensor_broadcaster.xml | 8 + .../src/range_sensor_broadcaster.cpp | 138 ++++++++++ .../range_sensor_broadcaster_parameters.yaml | 16 ++ .../test/range_sensor_broadcaster_params.yaml | 12 + .../test_load_range_sensor_broadcaster.cpp | 52 ++++ .../test/test_range_sensor_broadcaster.cpp | 252 ++++++++++++++++++ .../test/test_range_sensor_broadcaster.hpp | 59 ++++ ros2_controllers/package.xml | 1 + 14 files changed, 828 insertions(+) create mode 100644 range_sensor_broadcaster/CMakeLists.txt create mode 100644 range_sensor_broadcaster/README.md create mode 100644 range_sensor_broadcaster/doc/userdoc.rst create mode 100644 range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp create mode 100644 range_sensor_broadcaster/include/range_sensor_broadcaster/visibility_control.h create mode 100644 range_sensor_broadcaster/package.xml create mode 100644 range_sensor_broadcaster/range_sensor_broadcaster.xml create mode 100644 range_sensor_broadcaster/src/range_sensor_broadcaster.cpp create mode 100644 range_sensor_broadcaster/src/range_sensor_broadcaster_parameters.yaml create mode 100644 range_sensor_broadcaster/test/range_sensor_broadcaster_params.yaml create mode 100644 range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp create mode 100644 range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp create mode 100644 range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp diff --git a/range_sensor_broadcaster/CMakeLists.txt b/range_sensor_broadcaster/CMakeLists.txt new file mode 100644 index 0000000000..00da395db5 --- /dev/null +++ b/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 $ + PRIVATE $ +) +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() diff --git a/range_sensor_broadcaster/README.md b/range_sensor_broadcaster/README.md new file mode 100644 index 0000000000..33195d272b --- /dev/null +++ b/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) diff --git a/range_sensor_broadcaster/doc/userdoc.rst b/range_sensor_broadcaster/doc/userdoc.rst new file mode 100644 index 0000000000..e35bec67ad --- /dev/null +++ b/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 diff --git a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp new file mode 100644 index 0000000000..b2e5fbfac0 --- /dev/null +++ b/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 +#include +#include + +#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 param_listener_; + Params params_; + + std::unique_ptr range_sensor_; + + using StatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr sensor_state_publisher_; + std::unique_ptr realtime_publisher_; +}; + +} // namespace range_sensor_broadcaster + +#endif // RANGE_SENSOR_BROADCASTER__RANGE_SENSOR_BROADCASTER_HPP_ diff --git a/range_sensor_broadcaster/include/range_sensor_broadcaster/visibility_control.h b/range_sensor_broadcaster/include/range_sensor_broadcaster/visibility_control.h new file mode 100644 index 0000000000..0a9a9f53a8 --- /dev/null +++ b/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_ diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml new file mode 100644 index 0000000000..11175d9f03 --- /dev/null +++ b/range_sensor_broadcaster/package.xml @@ -0,0 +1,31 @@ + + + + range_sensor_broadcaster + 3.14.0 + Controller to publish readings of Range sensors. + Bence Magyar + Florent Chretien + Apache License 2.0 + + ament_cmake + + backward_ros + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + sensor_msgs + + ament_cmake_gmock + controller_manager + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/range_sensor_broadcaster/range_sensor_broadcaster.xml b/range_sensor_broadcaster/range_sensor_broadcaster.xml new file mode 100644 index 0000000000..fd82c7ae25 --- /dev/null +++ b/range_sensor_broadcaster/range_sensor_broadcaster.xml @@ -0,0 +1,8 @@ + + + + This controller publishes the readings of an Range sensor as sensor_msgs/Range message. + + + diff --git a/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp b/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp new file mode 100644 index 0000000000..b821da8c13 --- /dev/null +++ b/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp @@ -0,0 +1,138 @@ +// 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 + */ + +#include "range_sensor_broadcaster/range_sensor_broadcaster.hpp" + +#include +#include + +namespace range_sensor_broadcaster +{ +controller_interface::CallbackReturn RangeSensorBroadcaster::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn RangeSensorBroadcaster::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + if (params_.sensor_name.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'sensor_name' parameter has to be specified."); + return CallbackReturn::ERROR; + } + + if (params_.frame_id.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter has to be provided."); + return CallbackReturn::ERROR; + } + + range_sensor_ = std::make_unique( + semantic_components::RangeSensor(params_.sensor_name)); + try + { + // register ft sensor data publisher + sensor_state_publisher_ = + get_node()->create_publisher("~/range", rclcpp::SystemDefaultsQoS()); + realtime_publisher_ = std::make_unique(sensor_state_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return CallbackReturn::ERROR; + } + + realtime_publisher_->lock(); + realtime_publisher_->msg_.header.frame_id = params_.frame_id; + realtime_publisher_->msg_.radiation_type = params_.radiation_type; + realtime_publisher_->msg_.field_of_view = params_.field_of_view; + realtime_publisher_->msg_.min_range = params_.min_range; + realtime_publisher_->msg_.max_range = params_.max_range; + realtime_publisher_->msg_.variance = params_.variance; + realtime_publisher_->unlock(); + + RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); + return CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +RangeSensorBroadcaster::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::NONE; + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration RangeSensorBroadcaster::state_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names = range_sensor_->get_state_interface_names(); + return state_interfaces_config; +} + +controller_interface::CallbackReturn RangeSensorBroadcaster::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + range_sensor_->assign_loaned_state_interfaces(state_interfaces_); + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn RangeSensorBroadcaster::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + range_sensor_->release_interfaces(); + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type RangeSensorBroadcaster::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + if (realtime_publisher_ && realtime_publisher_->trylock()) + { + realtime_publisher_->msg_.header.stamp = time; + range_sensor_->get_values_as_message(realtime_publisher_->msg_); + realtime_publisher_->unlockAndPublish(); + } + + return controller_interface::return_type::OK; +} + +} // namespace range_sensor_broadcaster + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + range_sensor_broadcaster::RangeSensorBroadcaster, controller_interface::ControllerInterface) diff --git a/range_sensor_broadcaster/src/range_sensor_broadcaster_parameters.yaml b/range_sensor_broadcaster/src/range_sensor_broadcaster_parameters.yaml new file mode 100644 index 0000000000..57d39d20c4 --- /dev/null +++ b/range_sensor_broadcaster/src/range_sensor_broadcaster_parameters.yaml @@ -0,0 +1,16 @@ +range_sensor_broadcaster: + sensor_name: { + type: string, + default_value: "", + description: "Name of the sensor used as prefix for interfaces if there are no individual interface names defined.", + } + frame_id: { + type: string, + default_value: "", + description: "Sensor's frame_id in which values are published.", + } + radiation_type: {type: int, default_value: 0, description: "The type of radiation used by the sensor / 0 = Ultrason / 1 = Infrared",} + field_of_view: {type: double, default_value: 0.52, description: "The size of the arc that the distance reading is valid for [rad]",} + min_range: {type: double, default_value: 0.52, description: "Minimum range value [m]",} + max_range: {type: double, default_value: 4.0, description: "Maximum range value [m]",} + variance: {type: double, default_value: 0.0, description: "Variance of the range value",} diff --git a/range_sensor_broadcaster/test/range_sensor_broadcaster_params.yaml b/range_sensor_broadcaster/test/range_sensor_broadcaster_params.yaml new file mode 100644 index 0000000000..1e59a7f27b --- /dev/null +++ b/range_sensor_broadcaster/test/range_sensor_broadcaster_params.yaml @@ -0,0 +1,12 @@ +test_range_sensor_broadcaster: + ros__parameters: + # Setting mendatory parameters + sensor_name: "range_sensor" + frame_id: "range_sensor_frame" + + # Setting parameters with changed default value to check those are used + radiation_type: 1 + field_of_view: 0.1 + min_range: 0.10 + max_range: 7.0 + variance: 1.0 diff --git a/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp new file mode 100644 index 0000000000..5c400bef91 --- /dev/null +++ b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp @@ -0,0 +1,52 @@ +// 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, Florent Chretien + */ + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadRangeSensorBroadcaster, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NE( + cm.load_controller( + "test_range_sensor_broadcaster", "range_sensor_broadcaster/RangeSensorBroadcaster"), + nullptr); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp new file mode 100644 index 0000000000..855f7e037b --- /dev/null +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -0,0 +1,252 @@ +// Copyright 2023 flochre +// +// 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: flochre + */ + +#include + +#include "test_range_sensor_broadcaster.hpp" + +#include "hardware_interface/loaned_state_interface.hpp" + +void RangeSensorBroadcasterTest::SetUp() +{ + // initialize controller + range_broadcaster_ = std::make_unique(); +} + +void RangeSensorBroadcasterTest::TearDown() { range_broadcaster_.reset(nullptr); } + +controller_interface::return_type RangeSensorBroadcasterTest::init_broadcaster( + std::string broadcaster_name) +{ + controller_interface::return_type result = controller_interface::return_type::ERROR; + result = range_broadcaster_->init(broadcaster_name); + + if (controller_interface::return_type::OK == result) + { + std::vector state_interfaces; + state_interfaces.emplace_back(range_); + + range_broadcaster_->assign_interfaces({}, std::move(state_interfaces)); + } + + return result; +} + +controller_interface::CallbackReturn RangeSensorBroadcasterTest::configure_broadcaster( + std::vector & parameters) +{ + // Configure the broadcaster + for (auto parameter : parameters) + { + range_broadcaster_->get_node()->set_parameter(parameter); + } + + return range_broadcaster_->on_configure(rclcpp_lifecycle::State()); +} + +void RangeSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Range & range_msg) +{ + // create a new subscriber + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const sensor_msgs::msg::Range::SharedPtr) {}; + auto subscription = test_subscription_node.create_subscription( + "/test_range_sensor_broadcaster/range", 10, subs_callback); + + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + range_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + + // take message from subscription + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(range_msg, msg_info)); +} + +TEST_F(RangeSensorBroadcasterTest, Initialize_RangeBroadcaster_Exception) +{ + ASSERT_THROW(init_broadcaster(""), std::exception); +} + +TEST_F(RangeSensorBroadcasterTest, Initialize_RangeBroadcaster_Success) +{ + ASSERT_EQ( + init_broadcaster("test_range_sensor_broadcaster"), controller_interface::return_type::OK); +} + +TEST_F(RangeSensorBroadcasterTest, Configure_RangeBroadcaster_Error_1) +{ + // First Test without frame_id ERROR Expected + init_broadcaster("test_range_sensor_broadcaster"); + + std::vector parameters; + // explicitly give an empty sensor name to generate an error + parameters.emplace_back(rclcpp::Parameter("sensor_name", "")); + ASSERT_EQ(configure_broadcaster(parameters), controller_interface::CallbackReturn::ERROR); +} + +TEST_F(RangeSensorBroadcasterTest, Configure_RangeBroadcaster_Error_2) +{ + // Second Test without sensor_name ERROR Expected + init_broadcaster("test_range_sensor_broadcaster"); + + std::vector parameters; + // explicitly give an empty frame_id to generate an error + parameters.emplace_back(rclcpp::Parameter("frame_id", "")); + ASSERT_EQ(configure_broadcaster(parameters), controller_interface::CallbackReturn::ERROR); +} + +TEST_F(RangeSensorBroadcasterTest, Configure_RangeBroadcaster_Success) +{ + // Third Test without sensor_name SUCCESS Expected + init_broadcaster("test_range_sensor_broadcaster"); + + ASSERT_EQ( + range_broadcaster_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(RangeSensorBroadcasterTest, Activate_RangeBroadcaster_Success) +{ + init_broadcaster("test_range_sensor_broadcaster"); + + range_broadcaster_->on_configure(rclcpp_lifecycle::State()); + + ASSERT_EQ( + range_broadcaster_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(RangeSensorBroadcasterTest, Update_RangeBroadcaster_Success) +{ + init_broadcaster("test_range_sensor_broadcaster"); + + range_broadcaster_->on_configure(rclcpp_lifecycle::State()); + ASSERT_EQ( + range_broadcaster_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + auto result = range_broadcaster_->update( + range_broadcaster_->get_node()->get_clock()->now(), rclcpp::Duration::from_seconds(0.01)); + + ASSERT_EQ(result, controller_interface::return_type::OK); +} + +TEST_F(RangeSensorBroadcasterTest, Publish_RangeBroadcaster_Success) +{ + init_broadcaster("test_range_sensor_broadcaster"); + + range_broadcaster_->on_configure(rclcpp_lifecycle::State()); + range_broadcaster_->on_activate(rclcpp_lifecycle::State()); + + sensor_msgs::msg::Range range_msg; + subscribe_and_get_message(range_msg); + + EXPECT_EQ(range_msg.header.frame_id, frame_id_); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_EQ(range_msg.radiation_type, radiation_type_); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); +} + +TEST_F(RangeSensorBroadcasterTest, Publish_Bandaries_RangeBroadcaster_Success) +{ + init_broadcaster("test_range_sensor_broadcaster"); + + range_broadcaster_->on_configure(rclcpp_lifecycle::State()); + range_broadcaster_->on_activate(rclcpp_lifecycle::State()); + + sensor_msgs::msg::Range range_msg; + + sensor_range_ = 0.10; + subscribe_and_get_message(range_msg); + + EXPECT_EQ(range_msg.header.frame_id, frame_id_); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_EQ(range_msg.radiation_type, radiation_type_); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); + + sensor_range_ = 4.0; + subscribe_and_get_message(range_msg); + + EXPECT_EQ(range_msg.header.frame_id, frame_id_); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_EQ(range_msg.radiation_type, radiation_type_); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); +} + +TEST_F(RangeSensorBroadcasterTest, Publish_OutOfBandaries_RangeBroadcaster_Success) +{ + init_broadcaster("test_range_sensor_broadcaster"); + + range_broadcaster_->on_configure(rclcpp_lifecycle::State()); + range_broadcaster_->on_activate(rclcpp_lifecycle::State()); + + sensor_msgs::msg::Range range_msg; + + sensor_range_ = 0.0; + subscribe_and_get_message(range_msg); + + EXPECT_EQ(range_msg.header.frame_id, frame_id_); + // Even out of boundaries you will get the out_of_range range value + EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_EQ(range_msg.radiation_type, radiation_type_); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); + + sensor_range_ = 6.0; + subscribe_and_get_message(range_msg); + + EXPECT_EQ(range_msg.header.frame_id, frame_id_); + // Even out of boundaries you will get the out_of_range range value + EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_EQ(range_msg.radiation_type, radiation_type_); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp new file mode 100644 index 0000000000..10696d071f --- /dev/null +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp @@ -0,0 +1,59 @@ +// Copyright 2023 flochre +// +// 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: flochre + */ + +#ifndef TEST_RANGE_SENSOR_BROADCASTER_HPP_ +#define TEST_RANGE_SENSOR_BROADCASTER_HPP_ + +#include +#include +#include + +#include "gmock/gmock.h" + +#include "range_sensor_broadcaster/range_sensor_broadcaster.hpp" + +class RangeSensorBroadcasterTest : public ::testing::Test +{ +public: + void SetUp(); + void TearDown(); + +protected: + // for the sake of the test + // defining the parameter names same as in test/range_sensor_broadcaster_params.yaml + const std::string sensor_name_ = "range_sensor"; + const std::string frame_id_ = "range_sensor_frame"; + + const double field_of_view_ = 0.1; + const int radiation_type_ = 1; + const double min_range_ = 0.1; + const double max_range_ = 7.0; + const double variance_ = 1.0; + + double sensor_range_ = 3.1; + hardware_interface::StateInterface range_{sensor_name_, "range", &sensor_range_}; + + std::unique_ptr range_broadcaster_; + + controller_interface::return_type init_broadcaster(std::string broadcaster_name); + controller_interface::CallbackReturn configure_broadcaster( + std::vector & parameters); + void subscribe_and_get_message(sensor_msgs::msg::Range & range_msg); +}; + +#endif // TEST_RANGE_SENSOR_BROADCASTER_HPP_ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 96e5e75de2..be6d147a11 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -21,6 +21,7 @@ joint_state_broadcaster joint_trajectory_controller position_controllers + range_sensor_broadcaster steering_controllers_library tricycle_controller tricycle_steering_controller