@@ -66,27 +66,27 @@ double calculate_time_diff_ms(const rclcpp::Time & start, const rclcpp::Time & e
66
66
TimestampedReactionPairsVector convert_pipeline_map_to_sorted_vector (
67
67
const PipelineMap & pipelineMap)
68
68
{
69
- std::vector<std::tuple<rclcpp::Time, std::vector<ReactionPair>>> sortedVector ;
69
+ std::vector<std::tuple<rclcpp::Time, std::vector<ReactionPair>>> sorted_vector ;
70
70
71
71
for (const auto & entry : pipelineMap) {
72
- auto sortedReactions = entry.second ;
72
+ auto sorted_reactions = entry.second ;
73
73
// Sort the vector of ReactionPair based on the published stamp
74
74
std::sort (
75
- sortedReactions .begin (), sortedReactions .end (),
75
+ sorted_reactions .begin (), sorted_reactions .end (),
76
76
[](const ReactionPair & a, const ReactionPair & b) {
77
77
return rclcpp::Time (a.second .published_stamp ) < rclcpp::Time (b.second .published_stamp );
78
78
});
79
79
80
80
// Add to the vector as a tuple
81
- sortedVector. push_back (std::make_tuple (entry.first , sortedReactions ));
81
+ sorted_vector. emplace_back (std::make_tuple (entry.first , sorted_reactions ));
82
82
}
83
83
84
84
// Sort the vector of tuples by rclcpp::Time
85
- std::sort (sortedVector .begin (), sortedVector .end (), [](const auto & a, const auto & b) {
85
+ std::sort (sorted_vector .begin (), sorted_vector .end (), [](const auto & a, const auto & b) {
86
86
return std::get<0 >(a) < std::get<0 >(b);
87
87
});
88
88
89
- return sortedVector ;
89
+ return sorted_vector ;
90
90
}
91
91
92
92
unique_identifier_msgs::msg::UUID generate_uuid_msg (const std::string & input)
@@ -212,70 +212,51 @@ bool search_pointcloud_near_pose(
212
212
const pcl::PointCloud<pcl::PointXYZ> & pcl_pointcloud, const geometry_msgs::msg::Pose & pose,
213
213
const double search_radius)
214
214
{
215
- bool isAnyPointWithinRadius = std::any_of (
215
+ return std::any_of (
216
216
pcl_pointcloud.points .begin (), pcl_pointcloud.points .end (),
217
217
[pose, search_radius](const auto & point) {
218
218
return tier4_autoware_utils::calcDistance3d (pose.position , point) <= search_radius;
219
219
});
220
-
221
- if (isAnyPointWithinRadius) {
222
- return true ;
223
- }
224
- return false ;
225
220
}
226
221
227
222
bool search_predicted_objects_near_pose (
228
223
const PredictedObjects & predicted_objects, const geometry_msgs::msg::Pose & pose,
229
224
const double search_radius)
230
225
{
231
- bool isAnyObjectWithinRadius = std::any_of (
226
+ return std::any_of (
232
227
predicted_objects.objects .begin (), predicted_objects.objects .end (),
233
228
[pose, search_radius](const PredictedObject & object) {
234
229
return tier4_autoware_utils::calcDistance3d (
235
230
pose.position , object.kinematics .initial_pose_with_covariance .pose .position ) <=
236
231
search_radius;
237
232
});
238
-
239
- if (isAnyObjectWithinRadius) {
240
- return true ;
241
- }
242
- return false ;
233
+ ;
243
234
}
244
235
245
236
bool search_detected_objects_near_pose (
246
237
const DetectedObjects & detected_objects, const geometry_msgs::msg::Pose & pose,
247
238
const double search_radius)
248
239
{
249
- bool isAnyObjectWithinRadius = std::any_of (
240
+ return std::any_of (
250
241
detected_objects.objects .begin (), detected_objects.objects .end (),
251
242
[pose, search_radius](const DetectedObject & object) {
252
243
return tier4_autoware_utils::calcDistance3d (
253
244
pose.position , object.kinematics .pose_with_covariance .pose .position ) <=
254
245
search_radius;
255
246
});
256
-
257
- if (isAnyObjectWithinRadius) {
258
- return true ;
259
- }
260
- return false ;
261
247
}
262
248
263
249
bool search_tracked_objects_near_pose (
264
250
const TrackedObjects & tracked_objects, const geometry_msgs::msg::Pose & pose,
265
251
const double search_radius)
266
252
{
267
- bool isAnyObjectWithinRadius = std::any_of (
253
+ return std::any_of (
268
254
tracked_objects.objects .begin (), tracked_objects.objects .end (),
269
255
[pose, search_radius](const TrackedObject & object) {
270
256
return tier4_autoware_utils::calcDistance3d (
271
257
pose.position , object.kinematics .pose_with_covariance .pose .position ) <=
272
258
search_radius;
273
259
});
274
-
275
- if (isAnyObjectWithinRadius) {
276
- return true ;
277
- }
278
- return false ;
279
260
}
280
261
281
262
LatencyStats calculate_statistics (const std::vector<double > & latency_vec)
0 commit comments