diff --git a/autoware_utils/README.md b/autoware_utils/README.md index d4c14cc..bb4236a 100644 --- a/autoware_utils/README.md +++ b/autoware_utils/README.md @@ -37,13 +37,9 @@ The ROS module provides utilities for working with ROS messages and nodes: - **`diagnostics_interface.hpp`**: An interface for publishing diagnostic messages. - **`msg_covariance.hpp`**: Indices for accessing covariance matrices in ROS messages. - **`msg_operation.hpp`**: Overloaded operators for quaternion messages. -- **`parameter.hpp`**: Simplifies parameter retrieval and declaration. -- **`polling_subscriber.hpp`**: A subscriber class with different polling policies (latest, newest, all). - **`processing_time_publisher.hpp`**: Publishes processing times as diagnostic messages. - **`published_time_publisher.hpp`**: Tracks and publishes the time when messages are published. - **`self_pose_listener.hpp`**: Listens to the self-pose of the vehicle. -- **`update_param.hpp`**: Updates parameters from remote nodes. -- **`wait_for_param.hpp`**: Waits for parameters from remote nodes. - **`debug_traits.hpp`**: Traits for identifying debug message types. #### System Module @@ -95,30 +91,6 @@ int main() { ### Detailed Usage Examples -#### Update Parameters Dynamically with update_param.hpp - -```cpp -#include "autoware_utils/ros/update_param.hpp" -#include <rclcpp/rclcpp.hpp> - -int main(int argc, char * argv[]) { - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("param_node"); - - double param_value = 0.0; - std::vector<rclcpp::Parameter> params = node->get_parameters({"my_param"}); - - if (autoware_utils::update_param(params, "my_param", param_value)) { - RCLCPP_INFO(node->get_logger(), "Updated parameter value: %f", param_value); - } else { - RCLCPP_WARN(node->get_logger(), "Parameter 'my_param' not found."); - } - - rclcpp::shutdown(); - return 0; -} -``` - #### Logging Processing Times with ProcessingTimePublisher ```cpp diff --git a/autoware_utils/include/autoware_utils/ros/parameter.hpp b/autoware_utils/include/autoware_utils/ros/parameter.hpp index 2481d77..830e047 100644 --- a/autoware_utils/include/autoware_utils/ros/parameter.hpp +++ b/autoware_utils/include/autoware_utils/ros/parameter.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2025 The Autoware Contributors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,21 +15,14 @@ #ifndef AUTOWARE_UTILS__ROS__PARAMETER_HPP_ #define AUTOWARE_UTILS__ROS__PARAMETER_HPP_ -#include <rclcpp/rclcpp.hpp> +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -#include <string> +#pragma message("#include <autoware_utils/ros/parameter.hpp> is deprecated. Use #include <autoware_utils_rclcpp/parameter.hpp> instead.") +#include <autoware_utils_rclcpp/parameter.hpp> +namespace autoware_utils { using namespace autoware_utils_rclcpp; } -namespace autoware_utils -{ -template <class T> -T get_or_declare_parameter(rclcpp::Node & node, const std::string & name) -{ - if (node.has_parameter(name)) { - return node.get_parameter(name).get_value<T>(); - } - - return node.declare_parameter<T>(name); -} -} // namespace autoware_utils +// clang-format on +// NOLINTEND #endif // AUTOWARE_UTILS__ROS__PARAMETER_HPP_ diff --git a/autoware_utils/include/autoware_utils/ros/polling_subscriber.hpp b/autoware_utils/include/autoware_utils/ros/polling_subscriber.hpp index 2dbe1e9..09fa07e 100644 --- a/autoware_utils/include/autoware_utils/ros/polling_subscriber.hpp +++ b/autoware_utils/include/autoware_utils/ros/polling_subscriber.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. +// Copyright 2025 The Autoware Contributors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -11,243 +11,18 @@ // 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 AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ #define AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ -#include <rclcpp/rclcpp.hpp> - -#include <memory> -#include <stdexcept> -#include <string> -#include <vector> - -namespace autoware_utils -{ - -/** - * @brief Creates a SensorDataQoS profile with a single depth. - * @return rclcpp::SensorDataQoS The QoS profile with depth set to 1. - */ -inline rclcpp::SensorDataQoS single_depth_sensor_qos() -{ - rclcpp::SensorDataQoS qos; - qos.get_rmw_qos_profile().depth = 1; - return qos; -} - -namespace polling_policy -{ - -/** - * @brief Polling policy that keeps the latest received message. - * - * This policy retains the latest received message and provides it when requested. If a new message - * is received, it overwrites the previously stored message. - * - * @tparam MessageT The message type. - */ -template <typename MessageT> -class Latest -{ -private: - typename MessageT::ConstSharedPtr data_{nullptr}; ///< Data pointer to store the latest data - -protected: - /** - * @brief Check the QoS settings for the subscription. - * - * @param qos The QoS profile to check. - * @throws std::invalid_argument If the QoS depth is not 1. - */ - void check_qos(const rclcpp::QoS & qos) - { - if (qos.get_rmw_qos_profile().depth > 1) { - throw std::invalid_argument( - "InterProcessPollingSubscriber will be used with depth > 1, which may cause inefficient " - "serialization while updateLatestData()"); - } - } - -public: - /** - * @brief Retrieve the latest data. If no new data has been received, the previously received data - * - * @return typename MessageT::ConstSharedPtr The latest data. - */ - typename MessageT::ConstSharedPtr take_data(); -}; - -/** - * @brief Polling policy that keeps the newest received message. - * - * @tparam MessageT The message type. - */ -template <typename MessageT> -class Newest -{ -protected: - /** - * @brief Check the QoS settings for the subscription. - * - * @param qos The QoS profile to check. - * @throws std::invalid_argument If the QoS depth is not 1. - */ - void check_qos(const rclcpp::QoS & qos) - { - if (qos.get_rmw_qos_profile().depth > 1) { - throw std::invalid_argument( - "InterProcessPollingSubscriber will be used with depth > 1, which may cause inefficient " - "serialization while updateLatestData()"); - } - } - -public: - /** - * @brief Retrieve the newest data. If no new data has been received, nullptr is returned. - * - * @return typename MessageT::ConstSharedPtr The newest data. - */ - typename MessageT::ConstSharedPtr take_data(); -}; - -/** - * @brief Polling policy that keeps all received messages. - * - * @tparam MessageT The message type. - */ -template <typename MessageT> -class All -{ -protected: - /** - * @brief Check the QoS settings for the subscription. - * - * @param qos The QoS profile to check. - */ - void check_qos(const rclcpp::QoS &) {} - -public: - /** - * @brief Retrieve all data. - * - * @return std::vector<typename MessageT::ConstSharedPtr> The list of all received data. - */ - std::vector<typename MessageT::ConstSharedPtr> take_data(); -}; - -} // namespace polling_policy - -/** - * @brief Subscriber class that uses a specified polling policy. - * - * @tparam MessageT The message type. - * @tparam PollingPolicy The polling policy to use. - */ -template <typename MessageT, template <typename> class PollingPolicy = polling_policy::Latest> -class InterProcessPollingSubscriber : public PollingPolicy<MessageT> -{ - friend PollingPolicy<MessageT>; - -private: - typename rclcpp::Subscription<MessageT>::SharedPtr subscriber_; ///< Subscription object - -public: - using SharedPtr = std::shared_ptr<InterProcessPollingSubscriber<MessageT, PollingPolicy>>; - - /** - * @brief Construct a new InterProcessPollingSubscriber object. - * - * @param node The node to attach the subscriber to. - * @param topic_name The topic name to subscribe to. - * @param qos The QoS profile to use for the subscription. - */ - explicit InterProcessPollingSubscriber( - rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1}) - { - this->check_qos(qos); - - auto noexec_callback_group = - node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); - - auto noexec_subscription_options = rclcpp::SubscriptionOptions(); - noexec_subscription_options.callback_group = noexec_callback_group; - - subscriber_ = node->create_subscription<MessageT>( - topic_name, qos, - [node]([[maybe_unused]] const typename MessageT::ConstSharedPtr msg) { assert(false); }, - noexec_subscription_options); - } - - /** - * @brief Create a subscription. - * - * @param node The node to attach the subscriber to. - * @param topic_name The topic name to subscribe to. - * @param qos The QoS profile to use for the subscription. - * @return SharedPtr The created subscription. - */ - static SharedPtr create_subscription( - rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1}) - { - return std::make_shared<InterProcessPollingSubscriber<MessageT, PollingPolicy>>( - node, topic_name, qos); - } - - typename rclcpp::Subscription<MessageT>::SharedPtr subscriber() { return subscriber_; } -}; - -namespace polling_policy -{ - -template <typename MessageT> -typename MessageT::ConstSharedPtr Latest<MessageT>::take_data() -{ - auto & subscriber = - static_cast<InterProcessPollingSubscriber<MessageT, Latest> *>(this)->subscriber_; - auto new_data = std::make_shared<MessageT>(); - rclcpp::MessageInfo message_info; - const bool success = subscriber->take(*new_data, message_info); - if (success) { - data_ = new_data; - } - - return data_; -} - -template <typename MessageT> -typename MessageT::ConstSharedPtr Newest<MessageT>::take_data() -{ - auto & subscriber = - static_cast<InterProcessPollingSubscriber<MessageT, Newest> *>(this)->subscriber_; - auto new_data = std::make_shared<MessageT>(); - rclcpp::MessageInfo message_info; - const bool success = subscriber->take(*new_data, message_info); - if (success) { - return new_data; - } - return nullptr; -} - -template <typename MessageT> -std::vector<typename MessageT::ConstSharedPtr> All<MessageT>::take_data() -{ - auto & subscriber = - static_cast<InterProcessPollingSubscriber<MessageT, All> *>(this)->subscriber_; - std::vector<typename MessageT::ConstSharedPtr> data; - rclcpp::MessageInfo message_info; - for (;;) { - auto datum = std::make_shared<MessageT>(); - if (subscriber->take(*datum, message_info)) { - data.push_back(datum); - } else { - break; - } - } - return data; -} +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -} // namespace polling_policy +#pragma message("#include <autoware_utils/ros/polling_subscriber.hpp> is deprecated. Use #include <autoware_utils_rclcpp/polling_subscriber.hpp> instead.") +#include <autoware_utils_rclcpp/polling_subscriber.hpp> +namespace autoware_utils { using namespace autoware_utils_rclcpp; } -} // namespace autoware_utils +// clang-format on +// NOLINTEND #endif // AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ diff --git a/autoware_utils/include/autoware_utils/ros/update_param.hpp b/autoware_utils/include/autoware_utils/ros/update_param.hpp index e02de83..64ac1ee 100644 --- a/autoware_utils/include/autoware_utils/ros/update_param.hpp +++ b/autoware_utils/include/autoware_utils/ros/update_param.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2025 The Autoware Contributors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,29 +15,14 @@ #ifndef AUTOWARE_UTILS__ROS__UPDATE_PARAM_HPP_ #define AUTOWARE_UTILS__ROS__UPDATE_PARAM_HPP_ -#include <rclcpp/rclcpp.hpp> +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -#include <string> -#include <vector> +#pragma message("#include <autoware_utils/ros/update_param.hpp> is deprecated. Use #include <autoware_utils_rclcpp/parameter.hpp> instead.") +#include <autoware_utils_rclcpp/parameter.hpp> +namespace autoware_utils { using namespace autoware_utils_rclcpp; } -namespace autoware_utils -{ -template <class T> -bool update_param( - const std::vector<rclcpp::Parameter> & params, const std::string & name, T & value) -{ - const auto itr = std::find_if( - params.cbegin(), params.cend(), - [&name](const rclcpp::Parameter & p) { return p.get_name() == name; }); - - // Not found - if (itr == params.cend()) { - return false; - } - - value = itr->template get_value<T>(); - return true; -} -} // namespace autoware_utils +// clang-format on +// NOLINTEND #endif // AUTOWARE_UTILS__ROS__UPDATE_PARAM_HPP_ diff --git a/autoware_utils/include/autoware_utils/ros/wait_for_param.hpp b/autoware_utils/include/autoware_utils/ros/wait_for_param.hpp index 0cd1d73..a719ae9 100644 --- a/autoware_utils/include/autoware_utils/ros/wait_for_param.hpp +++ b/autoware_utils/include/autoware_utils/ros/wait_for_param.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2025 The Autoware Contributors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,38 +15,14 @@ #ifndef AUTOWARE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ #define AUTOWARE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ -#include <rclcpp/rclcpp.hpp> +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -#include <chrono> -#include <memory> -#include <string> +#pragma message("#include <autoware_utils/ros/wait_for_param.hpp> is deprecated. Use #include <autoware_utils_rclcpp/parameter.hpp> instead.") +#include <autoware_utils_rclcpp/parameter.hpp> +namespace autoware_utils { using namespace autoware_utils_rclcpp; } -namespace autoware_utils -{ -template <class T> -T wait_for_param( - rclcpp::Node * node, const std::string & remote_node_name, const std::string & param_name) -{ - std::chrono::seconds sec(1); - - auto param_client = std::make_shared<rclcpp::SyncParametersClient>(node, remote_node_name); - - while (!param_client->wait_for_service(sec)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service."); - return {}; - } - RCLCPP_INFO_THROTTLE( - node->get_logger(), *node->get_clock(), 1000 /* ms */, "waiting for node: %s, param: %s\n", - remote_node_name.c_str(), param_name.c_str()); - } - - if (param_client->has_parameter(param_name)) { - return param_client->get_parameter<T>(param_name); - } - - return {}; -} -} // namespace autoware_utils +// clang-format on +// NOLINTEND #endif // AUTOWARE_UTILS__ROS__WAIT_FOR_PARAM_HPP_ diff --git a/autoware_utils/package.xml b/autoware_utils/package.xml index c39a0c2..ca76554 100644 --- a/autoware_utils/package.xml +++ b/autoware_utils/package.xml @@ -23,6 +23,7 @@ <depend>autoware_utils_logging</depend> <depend>autoware_utils_math</depend> <depend>autoware_utils_pcl</depend> + <depend>autoware_utils_rclcpp</depend> <depend>autoware_utils_system</depend> <depend>autoware_utils_uuid</depend> <depend>autoware_utils_visualization</depend> diff --git a/autoware_utils_rclcpp/CMakeLists.txt b/autoware_utils_rclcpp/CMakeLists.txt new file mode 100644 index 0000000..3ef5c15 --- /dev/null +++ b/autoware_utils_rclcpp/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_utils_rclcpp) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +if(BUILD_TESTING) + file(GLOB_RECURSE test_files test/*.cpp) + ament_auto_add_gtest(test_${PROJECT_NAME} ${test_files}) +endif() + +ament_auto_package() diff --git a/autoware_utils_rclcpp/README.md b/autoware_utils_rclcpp/README.md new file mode 100644 index 0000000..86f5fd8 --- /dev/null +++ b/autoware_utils_rclcpp/README.md @@ -0,0 +1,38 @@ +# autoware_utils_rclcpp + +## Overview + +The **autoware_utils** library is a comprehensive toolkit designed to facilitate the development of autonomous driving applications. +This package provides essential utilities for rclcpp. +It is extensively used in the Autoware project to handle common tasks such as handling parameters, topics and services. + +## Design + +- **`parameter.hpp`**: Simplifies parameter declaration, retrieval, updating, and waiting. +- **`polling_subscriber.hpp`**: A subscriber class with different polling policies (latest, newest, all). + +## Example Code Snippets + +### Update Parameters Dynamically with update_param.hpp + +```cpp +#include <autoware_utils_rclcpp/update_param.hpp> +#include <rclcpp/rclcpp.hpp> + +int main(int argc, char * argv[]) { + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("param_node"); + + double param_value = 0.0; + std::vector<rclcpp::Parameter> params = node->get_parameters({"my_param"}); + + if (autoware_utils::update_param(params, "my_param", param_value)) { + RCLCPP_INFO(node->get_logger(), "Updated parameter value: %f", param_value); + } else { + RCLCPP_WARN(node->get_logger(), "Parameter 'my_param' not found."); + } + + rclcpp::shutdown(); + return 0; +} +``` diff --git a/autoware_utils_rclcpp/include/autoware_utils_rclcpp/parameter.hpp b/autoware_utils_rclcpp/include/autoware_utils_rclcpp/parameter.hpp new file mode 100644 index 0000000..bd61838 --- /dev/null +++ b/autoware_utils_rclcpp/include/autoware_utils_rclcpp/parameter.hpp @@ -0,0 +1,83 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 AUTOWARE_UTILS_RCLCPP__PARAMETER_HPP_ +#define AUTOWARE_UTILS_RCLCPP__PARAMETER_HPP_ + +#include <rclcpp/rclcpp.hpp> + +#include <chrono> +#include <memory> +#include <string> +#include <vector> + +namespace autoware_utils_rclcpp +{ + +template <class T> +T get_or_declare_parameter(rclcpp::Node & node, const std::string & name) +{ + if (node.has_parameter(name)) { + return node.get_parameter(name).get_value<T>(); + } + + return node.declare_parameter<T>(name); +} + +template <class T> +bool update_param( + const std::vector<rclcpp::Parameter> & params, const std::string & name, T & value) +{ + const auto itr = std::find_if( + params.cbegin(), params.cend(), + [&name](const rclcpp::Parameter & p) { return p.get_name() == name; }); + + // Not found + if (itr == params.cend()) { + return false; + } + + value = itr->template get_value<T>(); + return true; +} + +// NOTE: This function does not appear to be used. +template <class T> +[[deprecated]] T wait_for_param( + rclcpp::Node * node, const std::string & remote_node_name, const std::string & param_name) +{ + std::chrono::seconds sec(1); + + auto param_client = std::make_shared<rclcpp::SyncParametersClient>(node, remote_node_name); + + while (!param_client->wait_for_service(sec)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service."); + return {}; + } + RCLCPP_INFO_THROTTLE( + node->get_logger(), *node->get_clock(), 1000 /* ms */, "waiting for node: %s, param: %s\n", + remote_node_name.c_str(), param_name.c_str()); + } + + if (param_client->has_parameter(param_name)) { + return param_client->get_parameter<T>(param_name); + } + + return {}; +} + +} // namespace autoware_utils_rclcpp + +#endif // AUTOWARE_UTILS_RCLCPP__PARAMETER_HPP_ diff --git a/autoware_utils_rclcpp/include/autoware_utils_rclcpp/polling_subscriber.hpp b/autoware_utils_rclcpp/include/autoware_utils_rclcpp/polling_subscriber.hpp new file mode 100644 index 0000000..b15f5b8 --- /dev/null +++ b/autoware_utils_rclcpp/include/autoware_utils_rclcpp/polling_subscriber.hpp @@ -0,0 +1,253 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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 AUTOWARE_UTILS_RCLCPP__POLLING_SUBSCRIBER_HPP_ +#define AUTOWARE_UTILS_RCLCPP__POLLING_SUBSCRIBER_HPP_ + +#include <rclcpp/rclcpp.hpp> + +#include <memory> +#include <stdexcept> +#include <string> +#include <vector> + +namespace autoware_utils_rclcpp +{ + +/** + * @brief Creates a SensorDataQoS profile with a single depth. + * @return rclcpp::SensorDataQoS The QoS profile with depth set to 1. + */ +inline rclcpp::SensorDataQoS single_depth_sensor_qos() +{ + rclcpp::SensorDataQoS qos; + qos.get_rmw_qos_profile().depth = 1; + return qos; +} + +namespace polling_policy +{ + +/** + * @brief Polling policy that keeps the latest received message. + * + * This policy retains the latest received message and provides it when requested. If a new message + * is received, it overwrites the previously stored message. + * + * @tparam MessageT The message type. + */ +template <typename MessageT> +class Latest +{ +private: + typename MessageT::ConstSharedPtr data_{nullptr}; ///< Data pointer to store the latest data + +protected: + /** + * @brief Check the QoS settings for the subscription. + * + * @param qos The QoS profile to check. + * @throws std::invalid_argument If the QoS depth is not 1. + */ + void check_qos(const rclcpp::QoS & qos) + { + if (qos.get_rmw_qos_profile().depth > 1) { + throw std::invalid_argument( + "InterProcessPollingSubscriber will be used with depth > 1, which may cause inefficient " + "serialization while updateLatestData()"); + } + } + +public: + /** + * @brief Retrieve the latest data. If no new data has been received, the previously received data + * + * @return typename MessageT::ConstSharedPtr The latest data. + */ + typename MessageT::ConstSharedPtr take_data(); +}; + +/** + * @brief Polling policy that keeps the newest received message. + * + * @tparam MessageT The message type. + */ +template <typename MessageT> +class Newest +{ +protected: + /** + * @brief Check the QoS settings for the subscription. + * + * @param qos The QoS profile to check. + * @throws std::invalid_argument If the QoS depth is not 1. + */ + void check_qos(const rclcpp::QoS & qos) + { + if (qos.get_rmw_qos_profile().depth > 1) { + throw std::invalid_argument( + "InterProcessPollingSubscriber will be used with depth > 1, which may cause inefficient " + "serialization while updateLatestData()"); + } + } + +public: + /** + * @brief Retrieve the newest data. If no new data has been received, nullptr is returned. + * + * @return typename MessageT::ConstSharedPtr The newest data. + */ + typename MessageT::ConstSharedPtr take_data(); +}; + +/** + * @brief Polling policy that keeps all received messages. + * + * @tparam MessageT The message type. + */ +template <typename MessageT> +class All +{ +protected: + /** + * @brief Check the QoS settings for the subscription. + * + * @param qos The QoS profile to check. + */ + void check_qos(const rclcpp::QoS &) {} + +public: + /** + * @brief Retrieve all data. + * + * @return std::vector<typename MessageT::ConstSharedPtr> The list of all received data. + */ + std::vector<typename MessageT::ConstSharedPtr> take_data(); +}; + +} // namespace polling_policy + +/** + * @brief Subscriber class that uses a specified polling policy. + * + * @tparam MessageT The message type. + * @tparam PollingPolicy The polling policy to use. + */ +template <typename MessageT, template <typename> class PollingPolicy = polling_policy::Latest> +class InterProcessPollingSubscriber : public PollingPolicy<MessageT> +{ + friend PollingPolicy<MessageT>; + +private: + typename rclcpp::Subscription<MessageT>::SharedPtr subscriber_; ///< Subscription object + +public: + using SharedPtr = std::shared_ptr<InterProcessPollingSubscriber<MessageT, PollingPolicy>>; + + /** + * @brief Construct a new InterProcessPollingSubscriber object. + * + * @param node The node to attach the subscriber to. + * @param topic_name The topic name to subscribe to. + * @param qos The QoS profile to use for the subscription. + */ + explicit InterProcessPollingSubscriber( + rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1}) + { + this->check_qos(qos); + + auto noexec_callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + + auto noexec_subscription_options = rclcpp::SubscriptionOptions(); + noexec_subscription_options.callback_group = noexec_callback_group; + + subscriber_ = node->create_subscription<MessageT>( + topic_name, qos, + [node]([[maybe_unused]] const typename MessageT::ConstSharedPtr msg) { assert(false); }, + noexec_subscription_options); + } + + /** + * @brief Create a subscription. + * + * @param node The node to attach the subscriber to. + * @param topic_name The topic name to subscribe to. + * @param qos The QoS profile to use for the subscription. + * @return SharedPtr The created subscription. + */ + static SharedPtr create_subscription( + rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1}) + { + return std::make_shared<InterProcessPollingSubscriber<MessageT, PollingPolicy>>( + node, topic_name, qos); + } + + typename rclcpp::Subscription<MessageT>::SharedPtr subscriber() { return subscriber_; } +}; + +namespace polling_policy +{ + +template <typename MessageT> +typename MessageT::ConstSharedPtr Latest<MessageT>::take_data() +{ + auto & subscriber = + static_cast<InterProcessPollingSubscriber<MessageT, Latest> *>(this)->subscriber_; + auto new_data = std::make_shared<MessageT>(); + rclcpp::MessageInfo message_info; + const bool success = subscriber->take(*new_data, message_info); + if (success) { + data_ = new_data; + } + + return data_; +} + +template <typename MessageT> +typename MessageT::ConstSharedPtr Newest<MessageT>::take_data() +{ + auto & subscriber = + static_cast<InterProcessPollingSubscriber<MessageT, Newest> *>(this)->subscriber_; + auto new_data = std::make_shared<MessageT>(); + rclcpp::MessageInfo message_info; + const bool success = subscriber->take(*new_data, message_info); + if (success) { + return new_data; + } + return nullptr; +} + +template <typename MessageT> +std::vector<typename MessageT::ConstSharedPtr> All<MessageT>::take_data() +{ + auto & subscriber = + static_cast<InterProcessPollingSubscriber<MessageT, All> *>(this)->subscriber_; + std::vector<typename MessageT::ConstSharedPtr> data; + rclcpp::MessageInfo message_info; + for (;;) { + auto datum = std::make_shared<MessageT>(); + if (subscriber->take(*datum, message_info)) { + data.push_back(datum); + } else { + break; + } + } + return data; +} + +} // namespace polling_policy + +} // namespace autoware_utils_rclcpp + +#endif // AUTOWARE_UTILS_RCLCPP__POLLING_SUBSCRIBER_HPP_ diff --git a/autoware_utils_rclcpp/package.xml b/autoware_utils_rclcpp/package.xml new file mode 100644 index 0000000..f9fdce8 --- /dev/null +++ b/autoware_utils_rclcpp/package.xml @@ -0,0 +1,26 @@ +<?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>autoware_utils_rclcpp</name> + <version>1.1.0</version> + <description>The autoware_utils_rclcpp package</description> + <maintainer email="egon.kang@autocore.ai">Jian Kang</maintainer> + <maintainer email="ryohsuke.mitsudome@tier4.jp">Ryohsuke Mitsudome</maintainer> + <maintainer email="esteve.fernandez@tier4.jp">Esteve Fernandez</maintainer> + <maintainer email="yutaka.kondo@tier4.jp">Yutaka Kondo</maintainer> + <maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer> + <license>Apache License 2.0</license> + + <buildtool_depend>ament_cmake_auto</buildtool_depend> + <buildtool_depend>autoware_cmake</buildtool_depend> + + <depend>rclcpp</depend> + + <test_depend>ament_lint_auto</test_depend> + <test_depend>autoware_lint_common</test_depend> + <test_depend>std_msgs</test_depend> + + <export> + <build_type>ament_cmake</build_type> + </export> +</package> diff --git a/autoware_utils_rclcpp/test/cases/parameter.cpp b/autoware_utils_rclcpp/test/cases/parameter.cpp new file mode 100644 index 0000000..8387dfc --- /dev/null +++ b/autoware_utils_rclcpp/test/cases/parameter.cpp @@ -0,0 +1,56 @@ +// Copyright 2025 The Autoware Contributors +// +// 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. + +#include "autoware_utils_rclcpp/parameter.hpp" + +#include <gtest/gtest.h> + +#include <memory> +#include <string> +#include <vector> + +TEST(TestParameter, GetOrDeclare) +{ + rclcpp::NodeOptions options; + options.append_parameter_override("foo", 12345); + options.append_parameter_override("bar", "str"); + const auto node = std::make_shared<rclcpp::Node>("param_get_or_declare", options); + const auto p1 = autoware_utils_rclcpp::get_or_declare_parameter<int>(*node, "foo"); + const auto p2 = autoware_utils_rclcpp::get_or_declare_parameter<int>(*node, "foo"); + const auto p3 = autoware_utils_rclcpp::get_or_declare_parameter<std::string>(*node, "bar"); + const auto p4 = autoware_utils_rclcpp::get_or_declare_parameter<std::string>(*node, "bar"); + EXPECT_EQ(p1, 12345); + EXPECT_EQ(p2, 12345); + EXPECT_EQ(p3, "str"); + EXPECT_EQ(p4, "str"); +} + +TEST(TestParameter, Update) +{ + const std::vector<rclcpp::Parameter> params = { + rclcpp::Parameter("foo", 12345), + rclcpp::Parameter("bar", "str"), + }; + int foo = 0; + std::string bar = "default"; + std::string baz = "default"; + + EXPECT_TRUE(autoware_utils_rclcpp::update_param(params, "foo", foo)); + EXPECT_TRUE(autoware_utils_rclcpp::update_param(params, "bar", bar)); + EXPECT_EQ(foo, 12345); + EXPECT_EQ(bar, "str"); + + EXPECT_FALSE(autoware_utils_rclcpp::update_param(params, "baz", baz)); + EXPECT_EQ(baz, "default"); +} diff --git a/autoware_utils_rclcpp/test/cases/polling_subscriber.cpp b/autoware_utils_rclcpp/test/cases/polling_subscriber.cpp new file mode 100644 index 0000000..65ddbee --- /dev/null +++ b/autoware_utils_rclcpp/test/cases/polling_subscriber.cpp @@ -0,0 +1,53 @@ +// Copyright 2025 The Autoware Contributors +// +// 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. + +#include "autoware_utils_rclcpp/polling_subscriber.hpp" + +#include <std_msgs/msg/string.hpp> + +#include <gtest/gtest.h> + +#include <chrono> +#include <memory> +#include <thread> + +TEST(TestPollingSubscriber, PubSub) +{ + const auto pub_node = std::make_shared<rclcpp::Node>("pub_node"); + const auto sub_node = std::make_shared<rclcpp::Node>("sub_node"); + + const auto pub = pub_node->create_publisher<std_msgs::msg::String>("/test/text", 1); + const auto sub = autoware_utils_rclcpp::InterProcessPollingSubscriber< + std_msgs::msg::String>::create_subscription(sub_node.get(), "/test/text", 1); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(pub_node); + executor.add_node(sub_node); + + std::thread thread([&executor] { executor.spin(); }); + while (rclcpp::ok() && !executor.is_spinning()) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + std_msgs::msg::String pub_msg; + pub_msg.data = "foo-bar"; + pub->publish(pub_msg); + + const auto sub_msg = sub->take_data(); + EXPECT_NE(sub_msg, nullptr); + EXPECT_EQ(sub_msg->data, pub_msg.data); + + executor.cancel(); + thread.join(); +} diff --git a/autoware_utils_rclcpp/test/main.cpp b/autoware_utils_rclcpp/test/main.cpp new file mode 100644 index 0000000..42f409f --- /dev/null +++ b/autoware_utils_rclcpp/test/main.cpp @@ -0,0 +1,36 @@ +// Copyright 2025 The Autoware Contributors +// +// 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. + +#include <rclcpp/rclcpp.hpp> + +#include <gtest/gtest.h> + +class RclcppEnvironment : public testing::Environment +{ +public: + RclcppEnvironment(int argc, char ** argv) : argc(argc), argv(argv) {} + void SetUp() override { rclcpp::init(argc, argv); } + void TearDown() override { rclcpp::shutdown(); } + +private: + int argc; + char ** argv; +}; + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + testing::AddGlobalTestEnvironment(new RclcppEnvironment(argc, argv)); + return RUN_ALL_TESTS(); +}