Skip to content

Commit 4508d23

Browse files
committed
fix(AEB): check empty path at low speed
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent 02bc6d8 commit 4508d23

File tree

2 files changed

+40
-32
lines changed
  • control/autonomous_emergency_braking

2 files changed

+40
-32
lines changed

control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp

+5-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,10 @@ 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+
std::optional<Path> generateEgoPath(
145+
const double curr_v, const double curr_w, std::vector<Polygon2d> & polygons);
146+
std::optional<Path> generateEgoPath(
147+
const Trajectory & predicted_traj, std::vector<Polygon2d> & polygons);
147148
void createObjectData(
148149
const Path & ego_path, const std::vector<Polygon2d> & ego_polys, const rclcpp::Time & stamp,
149150
std::vector<ObjectData> & objects);

control/autonomous_emergency_braking/src/node.cpp

+35-28
Original file line numberDiff line numberDiff line change
@@ -317,38 +317,42 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
317317
constexpr double color_b = 205.0 / 256.0;
318318
constexpr double color_a = 0.999;
319319
const auto current_time = get_clock()->now();
320-
generateEgoPath(current_v, current_w, ego_path, ego_polys);
321-
322-
std::vector<ObjectData> objects;
323-
createObjectData(ego_path, ego_polys, current_time, objects);
324-
has_collision_ego = hasCollision(current_v, ego_path, objects);
325-
326-
std::string ns = "ego";
327-
addMarker(
328-
current_time, ego_path, ego_polys, objects, color_r, color_g, color_b, color_a, ns,
329-
debug_markers);
320+
const auto ego_path_opt = generateEgoPath(current_v, current_w, ego_polys);
321+
if (ego_path_opt) {
322+
const auto & ego_path = ego_path_opt.value();
323+
std::vector<ObjectData> objects;
324+
createObjectData(ego_path, ego_polys, current_time, objects);
325+
has_collision_ego = hasCollision(current_v, ego_path, objects);
326+
327+
std::string ns = "ego";
328+
addMarker(
329+
current_time, ego_path, ego_polys, objects, color_r, color_g, color_b, color_a, ns,
330+
debug_markers);
331+
}
330332
}
331333

332334
// step4. transform predicted trajectory from control module
333335
bool has_collision_predicted = false;
334336
if (use_predicted_trajectory_) {
335-
Path predicted_path;
336337
std::vector<Polygon2d> predicted_polys;
337338
const auto predicted_traj_ptr = predicted_traj_ptr_;
338339
constexpr double color_r = 0.0;
339340
constexpr double color_g = 100.0 / 256.0;
340341
constexpr double color_b = 0.0;
341342
constexpr double color_a = 0.999;
342343
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);
344+
const auto predicted_path_opt = generateEgoPath(*predicted_traj_ptr, predicted_polys);
345+
if (predicted_path_opt) {
346+
const auto & predicted_path = predicted_path_opt.value();
347+
std::vector<ObjectData> objects;
348+
createObjectData(predicted_path, predicted_polys, current_time, objects);
349+
has_collision_predicted = hasCollision(current_v, predicted_path, objects);
350+
351+
std::string ns = "predicted";
352+
addMarker(
353+
current_time, predicted_path, predicted_polys, objects, color_r, color_g, color_b, color_a,
354+
ns, debug_markers);
355+
}
352356
}
353357

354358
return has_collision_ego || has_collision_predicted;
@@ -384,9 +388,10 @@ bool AEB::hasCollision(
384388
return false;
385389
}
386390

387-
void AEB::generateEgoPath(
388-
const double curr_v, const double curr_w, Path & path, std::vector<Polygon2d> & polygons)
391+
std::optional<Path> AEB::generateEgoPath(
392+
const double curr_v, const double curr_w, std::vector<Polygon2d> & polygons)
389393
{
394+
Path path;
390395
double curr_x = 0.0;
391396
double curr_y = 0.0;
392397
double curr_yaw = 0.0;
@@ -397,7 +402,7 @@ void AEB::generateEgoPath(
397402

398403
if (curr_v < 0.1) {
399404
// if current velocity is too small, assume it stops at the same point
400-
return;
405+
return std::nullopt;
401406
}
402407

403408
constexpr double epsilon = 1e-6;
@@ -435,24 +440,25 @@ void AEB::generateEgoPath(
435440
for (size_t i = 0; i < path.size() - 1; ++i) {
436441
polygons.at(i) = createPolygon(path.at(i), path.at(i + 1), vehicle_info_, expand_width_);
437442
}
443+
return path;
438444
}
439445

440-
void AEB::generateEgoPath(
441-
const Trajectory & predicted_traj, Path & path,
442-
std::vector<tier4_autoware_utils::Polygon2d> & polygons)
446+
std::optional<Path> AEB::generateEgoPath(
447+
const Trajectory & predicted_traj, std::vector<tier4_autoware_utils::Polygon2d> & polygons)
443448
{
444449
if (predicted_traj.points.empty()) {
445-
return;
450+
return std::nullopt;
446451
}
447452

453+
Path path;
448454
geometry_msgs::msg::TransformStamped transform_stamped{};
449455
try {
450456
transform_stamped = tf_buffer_.lookupTransform(
451457
"base_link", predicted_traj.header.frame_id, predicted_traj.header.stamp,
452458
rclcpp::Duration::from_seconds(0.5));
453459
} catch (tf2::TransformException & ex) {
454460
RCLCPP_ERROR_STREAM(get_logger(), "[AEB] Failed to look up transform from base_link to map");
455-
return;
461+
return std::nullopt;
456462
}
457463

458464
// create path
@@ -471,6 +477,7 @@ void AEB::generateEgoPath(
471477
for (size_t i = 0; i < path.size() - 1; ++i) {
472478
polygons.at(i) = createPolygon(path.at(i), path.at(i + 1), vehicle_info_, expand_width_);
473479
}
480+
return path;
474481
}
475482

476483
void AEB::createObjectData(

0 commit comments

Comments
 (0)