Skip to content

Commit 87df6ad

Browse files
danielsanchezarandanielsanchezaran
authored
feat(autonomous_emergency_braking): add some tests to aeb (#8126)
* add initial tests Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add more tests Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * more tests Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * WIP add publishing and test subscription Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add more tests Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * fix lint cmake Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * WIP tf topic Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * Revert "WIP tf topic" This reverts commit b5ef11b. Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add path crop test Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add test for transform object Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add briefs Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * delete repeated test Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> --------- Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
1 parent f07fa8e commit 87df6ad

File tree

9 files changed

+642
-15
lines changed

9 files changed

+642
-15
lines changed

‎control/autoware_autonomous_emergency_braking/CMakeLists.txt

+5-2
Original file line numberDiff line numberDiff line change
@@ -30,8 +30,11 @@ rclcpp_components_register_node(${AEB_NODE}
3030
)
3131

3232
if(BUILD_TESTING)
33-
find_package(ament_lint_auto REQUIRED)
34-
ament_lint_auto_find_test_dependencies()
33+
ament_add_ros_isolated_gtest(test_aeb
34+
test/test.cpp)
35+
36+
target_link_libraries(test_aeb ${AEB_NODE})
37+
3538
endif()
3639

3740
ament_auto_package(

‎control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp

+185-7
Original file line numberDiff line numberDiff line change
@@ -71,6 +71,9 @@ using Vector3 = geometry_msgs::msg::Vector3;
7171
using autoware_perception_msgs::msg::PredictedObject;
7272
using autoware_perception_msgs::msg::PredictedObjects;
7373

74+
/**
75+
* @brief Struct to store object data
76+
*/
7477
struct ObjectData
7578
{
7679
rclcpp::Time stamp;
@@ -80,22 +83,44 @@ struct ObjectData
8083
double distance_to_object{0.0};
8184
};
8285

86+
/**
87+
* @brief Class to manage collision data
88+
*/
8389
class CollisionDataKeeper
8490
{
8591
public:
92+
/**
93+
* @brief Constructor for CollisionDataKeeper
94+
* @param clock Shared pointer to the clock
95+
*/
8696
explicit CollisionDataKeeper(rclcpp::Clock::SharedPtr clock) { clock_ = clock; }
8797

98+
/**
99+
* @brief Set timeout values for collision and obstacle data
100+
* @param collision_keep_time Time to keep collision data
101+
* @param previous_obstacle_keep_time Time to keep previous obstacle data
102+
*/
88103
void setTimeout(const double collision_keep_time, const double previous_obstacle_keep_time)
89104
{
90105
collision_keep_time_ = collision_keep_time;
91106
previous_obstacle_keep_time_ = previous_obstacle_keep_time;
92107
}
93108

109+
/**
110+
* @brief Get timeout values for collision and obstacle data
111+
* @return Pair of collision and obstacle data timeout values
112+
*/
94113
std::pair<double, double> getTimeout()
95114
{
96115
return {collision_keep_time_, previous_obstacle_keep_time_};
97116
}
98117

118+
/**
119+
* @brief Check if object data has expired
120+
* @param data Optional reference to the object data
121+
* @param timeout Timeout value to check against
122+
* @return True if object data has expired, false otherwise
123+
*/
99124
bool checkObjectDataExpired(std::optional<ObjectData> & data, const double timeout)
100125
{
101126
if (!data.has_value()) return true;
@@ -109,38 +134,70 @@ class CollisionDataKeeper
109134
return false;
110135
}
111136

137+
/**
138+
* @brief Check if collision data has expired
139+
* @return True if collision data has expired, false otherwise
140+
*/
112141
bool checkCollisionExpired()
113142
{
114143
return this->checkObjectDataExpired(closest_object_, collision_keep_time_);
115144
}
116145

146+
/**
147+
* @brief Check if previous object data has expired
148+
* @return True if previous object data has expired, false otherwise
149+
*/
117150
bool checkPreviousObjectDataExpired()
118151
{
119152
return this->checkObjectDataExpired(prev_closest_object_, previous_obstacle_keep_time_);
120153
}
121154

155+
/**
156+
* @brief Get the closest object data
157+
* @return Object data of the closest object
158+
*/
122159
[[nodiscard]] ObjectData get() const
123160
{
124161
return (closest_object_.has_value()) ? closest_object_.value() : ObjectData();
125162
}
126163

164+
/**
165+
* @brief Get the previous closest object data
166+
* @return Object data of the previous closest object
167+
*/
127168
[[nodiscard]] ObjectData getPreviousObjectData() const
128169
{
129170
return (prev_closest_object_.has_value()) ? prev_closest_object_.value() : ObjectData();
130171
}
131172

173+
/**
174+
* @brief Set collision data
175+
* @param data Object data to set
176+
*/
132177
void setCollisionData(const ObjectData & data)
133178
{
134179
closest_object_ = std::make_optional<ObjectData>(data);
135180
}
136181

182+
/**
183+
* @brief Set previous object data
184+
* @param data Object data to set
185+
*/
137186
void setPreviousObjectData(const ObjectData & data)
138187
{
139188
prev_closest_object_ = std::make_optional<ObjectData>(data);
140189
}
141190

191+
/**
192+
* @brief Reset the obstacle velocity history
193+
*/
142194
void resetVelocityHistory() { obstacle_velocity_history_.clear(); }
143195

196+
/**
197+
* @brief Update the velocity history with current object velocity
198+
* @param current_object_velocity Current object velocity
199+
* @param current_object_velocity_time_stamp Timestamp of the current object velocity
200+
*/
144201
void updateVelocityHistory(
145202
const double current_object_velocity, const rclcpp::Time & current_object_velocity_time_stamp)
146203
{
@@ -158,6 +215,10 @@ class CollisionDataKeeper
158215
current_object_velocity, current_object_velocity_time_stamp);
159216
}
160217

218+
/**
219+
* @brief Get the median obstacle velocity from history
220+
* @return Optional median obstacle velocity
221+
*/
161222
[[nodiscard]] std::optional<double> getMedianObstacleVelocity() const
162223
{
163224
if (obstacle_velocity_history_.empty()) return std::nullopt;
@@ -177,6 +238,13 @@ class CollisionDataKeeper
177238
return (vel1 + vel2) / 2.0;
178239
}
179240

241+
/**
242+
* @brief Calculate object speed from velocity history
243+
* @param closest_object Closest object data
244+
* @param path Ego vehicle path
245+
* @param current_ego_speed Current ego vehicle speed
246+
* @return Optional calculated object speed
247+
*/
180248
std::optional<double> calcObjectSpeedFromHistory(
181249
const ObjectData & closest_object, const Path & path, const double current_ego_speed)
182250
{
@@ -241,9 +309,16 @@ class CollisionDataKeeper
241309
rclcpp::Clock::SharedPtr clock_;
242310
};
243311

312+
/**
313+
* @brief Autonomous Emergency Braking (AEB) node
314+
*/
244315
class AEB : public rclcpp::Node
245316
{
246317
public:
318+
/**
319+
* @brief Constructor for AEB
320+
* @param node_options Options for the node
321+
*/
247322
explicit AEB(const rclcpp::NodeOptions & node_options);
248323

249324
// subscriber
@@ -267,53 +342,156 @@ class AEB : public rclcpp::Node
267342
rclcpp::TimerBase::SharedPtr timer_;
268343

269344
// callback
345+
/**
346+
* @brief Callback for point cloud messages
347+
* @param input_msg Shared pointer to the point cloud message
348+
*/
270349
void onPointCloud(const PointCloud2::ConstSharedPtr input_msg);
350+
351+
/**
352+
* @brief Callback for IMU messages
353+
* @param input_msg Shared pointer to the IMU message
354+
*/
271355
void onImu(const Imu::ConstSharedPtr input_msg);
356+
357+
/**
358+
* @brief Timer callback function
359+
*/
272360
void onTimer();
361+
362+
/**
363+
* @brief Callback for parameter updates
364+
* @param parameters Vector of updated parameters
365+
* @return Set parameters result
366+
*/
273367
rcl_interfaces::msg::SetParametersResult onParameter(
274368
const std::vector<rclcpp::Parameter> & parameters);
275369

370+
/**
371+
* @brief Fetch the latest data from subscribers
372+
* @return True if data fetch was successful, false otherwise
373+
*/
276374
bool fetchLatestData();
277375

278-
// main function
376+
/**
377+
* @brief Diagnostic check for collisions
378+
* @param stat Diagnostic status wrapper
379+
*/
279380
void onCheckCollision(DiagnosticStatusWrapper & stat);
381+
382+
/**
383+
* @brief Check for collisions
384+
* @param debug_markers Marker array for debugging
385+
* @return True if a collision is detected, false otherwise
386+
*/
280387
bool checkCollision(MarkerArray & debug_markers);
388+
389+
/**
390+
* @brief Check if there is a collision with the closest object
391+
* @param current_v Current velocity of the ego vehicle
392+
* @param closest_object Data of the closest object
393+
* @return True if a collision is detected, false otherwise
394+
*/
281395
bool hasCollision(const double current_v, const ObjectData & closest_object);
282396

397+
/**
398+
* @brief Generate the ego vehicle path
399+
* @param curr_v Current velocity of the ego vehicle
400+
* @param curr_w Current angular velocity of the ego vehicle
401+
* @return Generated ego path
402+
*/
283403
Path generateEgoPath(const double curr_v, const double curr_w);
404+
405+
/**
406+
* @brief Generate the ego vehicle path from the predicted trajectory
407+
* @param predicted_traj Predicted trajectory of the ego vehicle
408+
* @return Optional generated ego path
409+
*/
284410
std::optional<Path> generateEgoPath(const Trajectory & predicted_traj);
411+
412+
/**
413+
* @brief Generate the footprint of the path with extra width margin
414+
* @param path Ego vehicle path
415+
* @param extra_width_margin Extra width margin for the footprint
416+
* @return Vector of polygons representing the path footprint
417+
*/
285418
std::vector<Polygon2d> generatePathFootprint(const Path & path, const double extra_width_margin);
286419

420+
/**
421+
* @brief Create object data using point cloud clusters
422+
* @param ego_path Ego vehicle path
423+
* @param ego_polys Polygons representing the ego vehicle footprint
424+
* @param stamp Timestamp of the data
425+
* @param objects Vector to store the created object data
426+
* @param obstacle_points_ptr Pointer to the point cloud of obstacles
427+
*/
287428
void createObjectDataUsingPointCloudClusters(
288429
const Path & ego_path, const std::vector<Polygon2d> & ego_polys, const rclcpp::Time & stamp,
289430
std::vector<ObjectData> & objects,
290431
const pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_points_ptr);
291432

433+
/**
434+
* @brief Create object data using predicted objects
435+
* @param ego_path Ego vehicle path
436+
* @param ego_polys Polygons representing the ego vehicle footprint
437+
* @param objects Vector to store the created object data
438+
*/
292439
void createObjectDataUsingPredictedObjects(
293440
const Path & ego_path, const std::vector<Polygon2d> & ego_polys,
294441
std::vector<ObjectData> & objects);
295442

443+
/**
444+
* @brief Crop the point cloud with the ego vehicle footprint path
445+
* @param ego_polys Polygons representing the ego vehicle footprint
446+
* @param filtered_objects Pointer to the filtered point cloud of obstacles
447+
*/
296448
void cropPointCloudWithEgoFootprintPath(
297449
const std::vector<Polygon2d> & ego_polys, pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects);
298450

299-
void createObjectDataUsingPointCloudClusters(
300-
const Path & ego_path, const std::vector<Polygon2d> & ego_polys, const rclcpp::Time & stamp,
301-
std::vector<ObjectData> & objects);
302-
void cropPointCloudWithEgoFootprintPath(const std::vector<Polygon2d> & ego_polys);
303-
451+
/**
452+
* @brief Add a marker for debugging
453+
* @param current_time Current time
454+
* @param path Ego vehicle path
455+
* @param polygons Polygons representing the ego vehicle footprint
456+
* @param objects Vector of object data
457+
* @param closest_object Optional data of the closest object
458+
* @param color_r Red color component
459+
* @param color_g Green color component
460+
* @param color_b Blue color component
461+
* @param color_a Alpha (transparency) component
462+
* @param ns Namespace for the marker
463+
* @param debug_markers Marker array for debugging
464+
*/
304465
void addMarker(
305466
const rclcpp::Time & current_time, const Path & path, const std::vector<Polygon2d> & polygons,
306467
const std::vector<ObjectData> & objects, const std::optional<ObjectData> & closest_object,
307468
const double color_r, const double color_g, const double color_b, const double color_a,
308469
const std::string & ns, MarkerArray & debug_markers);
309470

471+
/**
472+
* @brief Add a collision marker for debugging
473+
* @param data Data of the collision object
474+
* @param debug_markers Marker array for debugging
475+
*/
310476
void addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers);
311477

478+
/**
479+
* @brief Add an info marker stop wall in front of the ego vehicle
480+
* @param markers Data of the closest object
481+
*/
312482
void addVirtualStopWallMarker(MarkerArray & markers);
313483

484+
/**
485+
* @brief Calculate object speed from history
486+
* @param closest_object Data of the closest object
487+
* @param path Ego vehicle path
488+
* @param current_ego_speed Current speed of the ego vehicle
489+
* @return Optional calculated object speed
490+
*/
314491
std::optional<double> calcObjectSpeedFromHistory(
315492
const ObjectData & closest_object, const Path & path, const double current_ego_speed);
316493

494+
// Member variables
317495
PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr};
318496
VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr};
319497
Vector3::SharedPtr angular_velocity_ptr_{nullptr};
@@ -330,7 +508,7 @@ class AEB : public rclcpp::Node
330508
// diag
331509
Updater updater_{this};
332510

333-
// member variables
511+
// Member variables
334512
bool publish_debug_pointcloud_;
335513
bool publish_debug_markers_;
336514
bool use_predicted_trajectory_;

‎control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml

-1
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,6 @@
33
<arg name="input_pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
44
<arg name="input_velocity" default="/vehicle/status/velocity_status"/>
55
<arg name="input_imu" default="/sensing/imu/imu_data"/>
6-
<arg name="input_odometry" default="/localization/kinematic_state"/>
76
<arg name="input_predicted_trajectory" default="/control/trajectory_follower/lateral/predicted_trajectory"/>
87
<arg name="input_objects" default="/perception/object_recognition/objects"/>
98

‎control/autoware_autonomous_emergency_braking/package.xml

+5-3
Original file line numberDiff line numberDiff line change
@@ -14,12 +14,17 @@
1414

1515
<buildtool_depend>ament_cmake</buildtool_depend>
1616
<buildtool_depend>autoware_cmake</buildtool_depend>
17+
<test_depend>ament_cmake_ros</test_depend>
18+
<test_depend>ament_lint_auto</test_depend>
19+
<test_depend>autoware_lint_common</test_depend>
20+
<test_depend>autoware_test_utils</test_depend>
1721

1822
<depend>autoware_control_msgs</depend>
1923
<depend>autoware_motion_utils</depend>
2024
<depend>autoware_planning_msgs</depend>
2125
<depend>autoware_pointcloud_preprocessor</depend>
2226
<depend>autoware_system_msgs</depend>
27+
<depend>autoware_test_utils</depend>
2328
<depend>autoware_universe_utils</depend>
2429
<depend>autoware_vehicle_info_utils</depend>
2530
<depend>autoware_vehicle_msgs</depend>
@@ -38,9 +43,6 @@
3843
<depend>tf2_ros</depend>
3944
<depend>visualization_msgs</depend>
4045

41-
<test_depend>ament_lint_auto</test_depend>
42-
<test_depend>autoware_lint_common</test_depend>
43-
4446
<export>
4547
<build_type>ament_cmake</build_type>
4648
</export>

‎control/autoware_autonomous_emergency_braking/src/node.cpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -507,7 +507,8 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
507507
};
508508

509509
// Data of filtered point cloud
510-
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects(new PointCloud);
510+
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects =
511+
pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
511512
// evaluate if there is a collision for both paths
512513
const bool has_collision =
513514
has_collision_ego(filtered_objects) || has_collision_predicted(filtered_objects);

0 commit comments

Comments
 (0)