Skip to content

Commit 7112a5b

Browse files
authored
fix(AEB): check empty path at low speed (#6346)
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent eef501e commit 7112a5b

File tree

3 files changed

+29
-25
lines changed

3 files changed

+29
-25
lines changed

control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,7 @@
4141

4242
#include <memory>
4343
#include <mutex>
44+
#include <optional>
4445
#include <string>
4546
#include <vector>
4647

@@ -140,10 +141,9 @@ class AEB : public rclcpp::Node
140141
bool hasCollision(
141142
const double current_v, const Path & ego_path, const std::vector<ObjectData> & objects);
142143

143-
void generateEgoPath(
144-
const double curr_v, const double curr_w, Path & path, std::vector<Polygon2d> & polygons);
145-
void generateEgoPath(
146-
const Trajectory & predicted_traj, Path & path, std::vector<Polygon2d> & polygons);
144+
Path generateEgoPath(const double curr_v, const double curr_w, std::vector<Polygon2d> & polygons);
145+
std::optional<Path> generateEgoPath(
146+
const Trajectory & predicted_traj, std::vector<Polygon2d> & polygons);
147147
void createObjectData(
148148
const Path & ego_path, const std::vector<Polygon2d> & ego_polys, const rclcpp::Time & stamp,
149149
std::vector<ObjectData> & objects);

control/autonomous_emergency_braking/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
<description>Autonomous Emergency Braking package as a ROS 2 node</description>
77
<maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
88
<maintainer email="tomoya.kimura@tier4.jp">Tomoya Kimura</maintainer>
9+
<maintainer email="mamoru.sobue@tier4.jp">Mamoru Sobue</maintainer>
910

1011
<license>Apache License 2.0</license>
1112

control/autonomous_emergency_braking/src/node.cpp

+24-21
Original file line numberDiff line numberDiff line change
@@ -309,16 +309,14 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
309309
// step3. create ego path based on sensor data
310310
bool has_collision_ego = false;
311311
if (use_imu_path_) {
312-
Path ego_path;
313312
std::vector<Polygon2d> ego_polys;
314313
const double current_w = angular_velocity_ptr_->z;
315314
constexpr double color_r = 0.0 / 256.0;
316315
constexpr double color_g = 148.0 / 256.0;
317316
constexpr double color_b = 205.0 / 256.0;
318317
constexpr double color_a = 0.999;
319318
const auto current_time = get_clock()->now();
320-
generateEgoPath(current_v, current_w, ego_path, ego_polys);
321-
319+
const auto ego_path = generateEgoPath(current_v, current_w, ego_polys);
322320
std::vector<ObjectData> objects;
323321
createObjectData(ego_path, ego_polys, current_time, objects);
324322
has_collision_ego = hasCollision(current_v, ego_path, objects);
@@ -332,23 +330,25 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
332330
// step4. transform predicted trajectory from control module
333331
bool has_collision_predicted = false;
334332
if (use_predicted_trajectory_) {
335-
Path predicted_path;
336333
std::vector<Polygon2d> predicted_polys;
337334
const auto predicted_traj_ptr = predicted_traj_ptr_;
338335
constexpr double color_r = 0.0;
339336
constexpr double color_g = 100.0 / 256.0;
340337
constexpr double color_b = 0.0;
341338
constexpr double color_a = 0.999;
342339
const auto current_time = predicted_traj_ptr->header.stamp;
343-
generateEgoPath(*predicted_traj_ptr, predicted_path, predicted_polys);
344-
std::vector<ObjectData> objects;
345-
createObjectData(predicted_path, predicted_polys, current_time, objects);
346-
has_collision_predicted = hasCollision(current_v, predicted_path, objects);
347-
348-
std::string ns = "predicted";
349-
addMarker(
350-
current_time, predicted_path, predicted_polys, objects, color_r, color_g, color_b, color_a,
351-
ns, debug_markers);
340+
const auto predicted_path_opt = generateEgoPath(*predicted_traj_ptr, predicted_polys);
341+
if (predicted_path_opt) {
342+
const auto & predicted_path = predicted_path_opt.value();
343+
std::vector<ObjectData> objects;
344+
createObjectData(predicted_path, predicted_polys, current_time, objects);
345+
has_collision_predicted = hasCollision(current_v, predicted_path, objects);
346+
347+
std::string ns = "predicted";
348+
addMarker(
349+
current_time, predicted_path, predicted_polys, objects, color_r, color_g, color_b, color_a,
350+
ns, debug_markers);
351+
}
352352
}
353353

354354
return has_collision_ego || has_collision_predicted;
@@ -384,9 +384,10 @@ bool AEB::hasCollision(
384384
return false;
385385
}
386386

387-
void AEB::generateEgoPath(
388-
const double curr_v, const double curr_w, Path & path, std::vector<Polygon2d> & polygons)
387+
Path AEB::generateEgoPath(
388+
const double curr_v, const double curr_w, std::vector<Polygon2d> & polygons)
389389
{
390+
Path path;
390391
double curr_x = 0.0;
391392
double curr_y = 0.0;
392393
double curr_yaw = 0.0;
@@ -397,7 +398,7 @@ void AEB::generateEgoPath(
397398

398399
if (curr_v < 0.1) {
399400
// if current velocity is too small, assume it stops at the same point
400-
return;
401+
return path;
401402
}
402403

403404
constexpr double epsilon = 1e-6;
@@ -435,14 +436,14 @@ void AEB::generateEgoPath(
435436
for (size_t i = 0; i < path.size() - 1; ++i) {
436437
polygons.at(i) = createPolygon(path.at(i), path.at(i + 1), vehicle_info_, expand_width_);
437438
}
439+
return path;
438440
}
439441

440-
void AEB::generateEgoPath(
441-
const Trajectory & predicted_traj, Path & path,
442-
std::vector<tier4_autoware_utils::Polygon2d> & polygons)
442+
std::optional<Path> AEB::generateEgoPath(
443+
const Trajectory & predicted_traj, std::vector<tier4_autoware_utils::Polygon2d> & polygons)
443444
{
444445
if (predicted_traj.points.empty()) {
445-
return;
446+
return std::nullopt;
446447
}
447448

448449
geometry_msgs::msg::TransformStamped transform_stamped{};
@@ -452,10 +453,11 @@ void AEB::generateEgoPath(
452453
rclcpp::Duration::from_seconds(0.5));
453454
} catch (tf2::TransformException & ex) {
454455
RCLCPP_ERROR_STREAM(get_logger(), "[AEB] Failed to look up transform from base_link to map");
455-
return;
456+
return std::nullopt;
456457
}
457458

458459
// create path
460+
Path path;
459461
path.resize(predicted_traj.points.size());
460462
for (size_t i = 0; i < predicted_traj.points.size(); ++i) {
461463
geometry_msgs::msg::Pose map_pose;
@@ -471,6 +473,7 @@ void AEB::generateEgoPath(
471473
for (size_t i = 0; i < path.size() - 1; ++i) {
472474
polygons.at(i) = createPolygon(path.at(i), path.at(i + 1), vehicle_info_, expand_width_);
473475
}
476+
return path;
474477
}
475478

476479
void AEB::createObjectData(

0 commit comments

Comments
 (0)