Skip to content

Commit 009d98c

Browse files
authored
feat(autoware_utils_rclcpp): split package (#40)
* feat(autoware_utils_rclcpp): split package Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * update test Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * update readme Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> --------- Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
1 parent 219bf84 commit 009d98c

File tree

14 files changed

+591
-332
lines changed

14 files changed

+591
-332
lines changed

autoware_utils/README.md

-28
Original file line numberDiff line numberDiff line change
@@ -37,13 +37,9 @@ The ROS module provides utilities for working with ROS messages and nodes:
3737
- **`diagnostics_interface.hpp`**: An interface for publishing diagnostic messages.
3838
- **`msg_covariance.hpp`**: Indices for accessing covariance matrices in ROS messages.
3939
- **`msg_operation.hpp`**: Overloaded operators for quaternion messages.
40-
- **`parameter.hpp`**: Simplifies parameter retrieval and declaration.
41-
- **`polling_subscriber.hpp`**: A subscriber class with different polling policies (latest, newest, all).
4240
- **`processing_time_publisher.hpp`**: Publishes processing times as diagnostic messages.
4341
- **`published_time_publisher.hpp`**: Tracks and publishes the time when messages are published.
4442
- **`self_pose_listener.hpp`**: Listens to the self-pose of the vehicle.
45-
- **`update_param.hpp`**: Updates parameters from remote nodes.
46-
- **`wait_for_param.hpp`**: Waits for parameters from remote nodes.
4743
- **`debug_traits.hpp`**: Traits for identifying debug message types.
4844

4945
#### System Module
@@ -95,30 +91,6 @@ int main() {
9591
9692
### Detailed Usage Examples
9793
98-
#### Update Parameters Dynamically with update_param.hpp
99-
100-
```cpp
101-
#include "autoware_utils/ros/update_param.hpp"
102-
#include <rclcpp/rclcpp.hpp>
103-
104-
int main(int argc, char * argv[]) {
105-
rclcpp::init(argc, argv);
106-
auto node = rclcpp::Node::make_shared("param_node");
107-
108-
double param_value = 0.0;
109-
std::vector<rclcpp::Parameter> params = node->get_parameters({"my_param"});
110-
111-
if (autoware_utils::update_param(params, "my_param", param_value)) {
112-
RCLCPP_INFO(node->get_logger(), "Updated parameter value: %f", param_value);
113-
} else {
114-
RCLCPP_WARN(node->get_logger(), "Parameter 'my_param' not found.");
115-
}
116-
117-
rclcpp::shutdown();
118-
return 0;
119-
}
120-
```
121-
12294
#### Logging Processing Times with ProcessingTimePublisher
12395
12496
```cpp
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2023 Tier IV, Inc.
1+
// Copyright 2025 The Autoware Contributors
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -15,21 +15,14 @@
1515
#ifndef AUTOWARE_UTILS__ROS__PARAMETER_HPP_
1616
#define AUTOWARE_UTILS__ROS__PARAMETER_HPP_
1717

18-
#include <rclcpp/rclcpp.hpp>
18+
// NOLINTBEGIN(build/namespaces, whitespace/line_length)
19+
// clang-format off
1920

20-
#include <string>
21+
#pragma message("#include <autoware_utils/ros/parameter.hpp> is deprecated. Use #include <autoware_utils_rclcpp/parameter.hpp> instead.")
22+
#include <autoware_utils_rclcpp/parameter.hpp>
23+
namespace autoware_utils { using namespace autoware_utils_rclcpp; }
2124

22-
namespace autoware_utils
23-
{
24-
template <class T>
25-
T get_or_declare_parameter(rclcpp::Node & node, const std::string & name)
26-
{
27-
if (node.has_parameter(name)) {
28-
return node.get_parameter(name).get_value<T>();
29-
}
30-
31-
return node.declare_parameter<T>(name);
32-
}
33-
} // namespace autoware_utils
25+
// clang-format on
26+
// NOLINTEND
3427

3528
#endif // AUTOWARE_UTILS__ROS__PARAMETER_HPP_
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2024 TIER IV, Inc.
1+
// Copyright 2025 The Autoware Contributors
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -11,243 +11,18 @@
1111
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
14+
1415
#ifndef AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_
1516
#define AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_
1617

17-
#include <rclcpp/rclcpp.hpp>
18-
19-
#include <memory>
20-
#include <stdexcept>
21-
#include <string>
22-
#include <vector>
23-
24-
namespace autoware_utils
25-
{
26-
27-
/**
28-
* @brief Creates a SensorDataQoS profile with a single depth.
29-
* @return rclcpp::SensorDataQoS The QoS profile with depth set to 1.
30-
*/
31-
inline rclcpp::SensorDataQoS single_depth_sensor_qos()
32-
{
33-
rclcpp::SensorDataQoS qos;
34-
qos.get_rmw_qos_profile().depth = 1;
35-
return qos;
36-
}
37-
38-
namespace polling_policy
39-
{
40-
41-
/**
42-
* @brief Polling policy that keeps the latest received message.
43-
*
44-
* This policy retains the latest received message and provides it when requested. If a new message
45-
* is received, it overwrites the previously stored message.
46-
*
47-
* @tparam MessageT The message type.
48-
*/
49-
template <typename MessageT>
50-
class Latest
51-
{
52-
private:
53-
typename MessageT::ConstSharedPtr data_{nullptr}; ///< Data pointer to store the latest data
54-
55-
protected:
56-
/**
57-
* @brief Check the QoS settings for the subscription.
58-
*
59-
* @param qos The QoS profile to check.
60-
* @throws std::invalid_argument If the QoS depth is not 1.
61-
*/
62-
void check_qos(const rclcpp::QoS & qos)
63-
{
64-
if (qos.get_rmw_qos_profile().depth > 1) {
65-
throw std::invalid_argument(
66-
"InterProcessPollingSubscriber will be used with depth > 1, which may cause inefficient "
67-
"serialization while updateLatestData()");
68-
}
69-
}
70-
71-
public:
72-
/**
73-
* @brief Retrieve the latest data. If no new data has been received, the previously received data
74-
*
75-
* @return typename MessageT::ConstSharedPtr The latest data.
76-
*/
77-
typename MessageT::ConstSharedPtr take_data();
78-
};
79-
80-
/**
81-
* @brief Polling policy that keeps the newest received message.
82-
*
83-
* @tparam MessageT The message type.
84-
*/
85-
template <typename MessageT>
86-
class Newest
87-
{
88-
protected:
89-
/**
90-
* @brief Check the QoS settings for the subscription.
91-
*
92-
* @param qos The QoS profile to check.
93-
* @throws std::invalid_argument If the QoS depth is not 1.
94-
*/
95-
void check_qos(const rclcpp::QoS & qos)
96-
{
97-
if (qos.get_rmw_qos_profile().depth > 1) {
98-
throw std::invalid_argument(
99-
"InterProcessPollingSubscriber will be used with depth > 1, which may cause inefficient "
100-
"serialization while updateLatestData()");
101-
}
102-
}
103-
104-
public:
105-
/**
106-
* @brief Retrieve the newest data. If no new data has been received, nullptr is returned.
107-
*
108-
* @return typename MessageT::ConstSharedPtr The newest data.
109-
*/
110-
typename MessageT::ConstSharedPtr take_data();
111-
};
112-
113-
/**
114-
* @brief Polling policy that keeps all received messages.
115-
*
116-
* @tparam MessageT The message type.
117-
*/
118-
template <typename MessageT>
119-
class All
120-
{
121-
protected:
122-
/**
123-
* @brief Check the QoS settings for the subscription.
124-
*
125-
* @param qos The QoS profile to check.
126-
*/
127-
void check_qos(const rclcpp::QoS &) {}
128-
129-
public:
130-
/**
131-
* @brief Retrieve all data.
132-
*
133-
* @return std::vector<typename MessageT::ConstSharedPtr> The list of all received data.
134-
*/
135-
std::vector<typename MessageT::ConstSharedPtr> take_data();
136-
};
137-
138-
} // namespace polling_policy
139-
140-
/**
141-
* @brief Subscriber class that uses a specified polling policy.
142-
*
143-
* @tparam MessageT The message type.
144-
* @tparam PollingPolicy The polling policy to use.
145-
*/
146-
template <typename MessageT, template <typename> class PollingPolicy = polling_policy::Latest>
147-
class InterProcessPollingSubscriber : public PollingPolicy<MessageT>
148-
{
149-
friend PollingPolicy<MessageT>;
150-
151-
private:
152-
typename rclcpp::Subscription<MessageT>::SharedPtr subscriber_; ///< Subscription object
153-
154-
public:
155-
using SharedPtr = std::shared_ptr<InterProcessPollingSubscriber<MessageT, PollingPolicy>>;
156-
157-
/**
158-
* @brief Construct a new InterProcessPollingSubscriber object.
159-
*
160-
* @param node The node to attach the subscriber to.
161-
* @param topic_name The topic name to subscribe to.
162-
* @param qos The QoS profile to use for the subscription.
163-
*/
164-
explicit InterProcessPollingSubscriber(
165-
rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1})
166-
{
167-
this->check_qos(qos);
168-
169-
auto noexec_callback_group =
170-
node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
171-
172-
auto noexec_subscription_options = rclcpp::SubscriptionOptions();
173-
noexec_subscription_options.callback_group = noexec_callback_group;
174-
175-
subscriber_ = node->create_subscription<MessageT>(
176-
topic_name, qos,
177-
[node]([[maybe_unused]] const typename MessageT::ConstSharedPtr msg) { assert(false); },
178-
noexec_subscription_options);
179-
}
180-
181-
/**
182-
* @brief Create a subscription.
183-
*
184-
* @param node The node to attach the subscriber to.
185-
* @param topic_name The topic name to subscribe to.
186-
* @param qos The QoS profile to use for the subscription.
187-
* @return SharedPtr The created subscription.
188-
*/
189-
static SharedPtr create_subscription(
190-
rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1})
191-
{
192-
return std::make_shared<InterProcessPollingSubscriber<MessageT, PollingPolicy>>(
193-
node, topic_name, qos);
194-
}
195-
196-
typename rclcpp::Subscription<MessageT>::SharedPtr subscriber() { return subscriber_; }
197-
};
198-
199-
namespace polling_policy
200-
{
201-
202-
template <typename MessageT>
203-
typename MessageT::ConstSharedPtr Latest<MessageT>::take_data()
204-
{
205-
auto & subscriber =
206-
static_cast<InterProcessPollingSubscriber<MessageT, Latest> *>(this)->subscriber_;
207-
auto new_data = std::make_shared<MessageT>();
208-
rclcpp::MessageInfo message_info;
209-
const bool success = subscriber->take(*new_data, message_info);
210-
if (success) {
211-
data_ = new_data;
212-
}
213-
214-
return data_;
215-
}
216-
217-
template <typename MessageT>
218-
typename MessageT::ConstSharedPtr Newest<MessageT>::take_data()
219-
{
220-
auto & subscriber =
221-
static_cast<InterProcessPollingSubscriber<MessageT, Newest> *>(this)->subscriber_;
222-
auto new_data = std::make_shared<MessageT>();
223-
rclcpp::MessageInfo message_info;
224-
const bool success = subscriber->take(*new_data, message_info);
225-
if (success) {
226-
return new_data;
227-
}
228-
return nullptr;
229-
}
230-
231-
template <typename MessageT>
232-
std::vector<typename MessageT::ConstSharedPtr> All<MessageT>::take_data()
233-
{
234-
auto & subscriber =
235-
static_cast<InterProcessPollingSubscriber<MessageT, All> *>(this)->subscriber_;
236-
std::vector<typename MessageT::ConstSharedPtr> data;
237-
rclcpp::MessageInfo message_info;
238-
for (;;) {
239-
auto datum = std::make_shared<MessageT>();
240-
if (subscriber->take(*datum, message_info)) {
241-
data.push_back(datum);
242-
} else {
243-
break;
244-
}
245-
}
246-
return data;
247-
}
18+
// NOLINTBEGIN(build/namespaces, whitespace/line_length)
19+
// clang-format off
24820

249-
} // namespace polling_policy
21+
#pragma message("#include <autoware_utils/ros/polling_subscriber.hpp> is deprecated. Use #include <autoware_utils_rclcpp/polling_subscriber.hpp> instead.")
22+
#include <autoware_utils_rclcpp/polling_subscriber.hpp>
23+
namespace autoware_utils { using namespace autoware_utils_rclcpp; }
25024

251-
} // namespace autoware_utils
25+
// clang-format on
26+
// NOLINTEND
25227

25328
#endif // AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2021 Tier IV, Inc.
1+
// Copyright 2025 The Autoware Contributors
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -15,29 +15,14 @@
1515
#ifndef AUTOWARE_UTILS__ROS__UPDATE_PARAM_HPP_
1616
#define AUTOWARE_UTILS__ROS__UPDATE_PARAM_HPP_
1717

18-
#include <rclcpp/rclcpp.hpp>
18+
// NOLINTBEGIN(build/namespaces, whitespace/line_length)
19+
// clang-format off
1920

20-
#include <string>
21-
#include <vector>
21+
#pragma message("#include <autoware_utils/ros/update_param.hpp> is deprecated. Use #include <autoware_utils_rclcpp/parameter.hpp> instead.")
22+
#include <autoware_utils_rclcpp/parameter.hpp>
23+
namespace autoware_utils { using namespace autoware_utils_rclcpp; }
2224

23-
namespace autoware_utils
24-
{
25-
template <class T>
26-
bool update_param(
27-
const std::vector<rclcpp::Parameter> & params, const std::string & name, T & value)
28-
{
29-
const auto itr = std::find_if(
30-
params.cbegin(), params.cend(),
31-
[&name](const rclcpp::Parameter & p) { return p.get_name() == name; });
32-
33-
// Not found
34-
if (itr == params.cend()) {
35-
return false;
36-
}
37-
38-
value = itr->template get_value<T>();
39-
return true;
40-
}
41-
} // namespace autoware_utils
25+
// clang-format on
26+
// NOLINTEND
4227

4328
#endif // AUTOWARE_UTILS__ROS__UPDATE_PARAM_HPP_

0 commit comments

Comments
 (0)