Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(autoware_utils_rclcpp): split package #40

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 0 additions & 28 deletions autoware_utils/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
23 changes: 8 additions & 15 deletions autoware_utils/include/autoware_utils/ros/parameter.hpp
Original file line number Diff line number Diff line change
@@ -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.
Expand All @@ -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_
243 changes: 9 additions & 234 deletions autoware_utils/include/autoware_utils/ros/polling_subscriber.hpp
Original file line number Diff line number Diff line change
@@ -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.
Expand All @@ -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_
31 changes: 8 additions & 23 deletions autoware_utils/include/autoware_utils/ros/update_param.hpp
Original file line number Diff line number Diff line change
@@ -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.
Expand All @@ -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_
Loading
Loading