Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(AEB): check empty path at low speed #6346

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@

#include <memory>
#include <mutex>
#include <optional>
#include <string>
#include <vector>

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

void generateEgoPath(
const double curr_v, const double curr_w, Path & path, std::vector<Polygon2d> & polygons);
void generateEgoPath(
const Trajectory & predicted_traj, Path & path, std::vector<Polygon2d> & polygons);
Path generateEgoPath(const double curr_v, const double curr_w, std::vector<Polygon2d> & polygons);
std::optional<Path> generateEgoPath(
const Trajectory & predicted_traj, std::vector<Polygon2d> & polygons);
void createObjectData(
const Path & ego_path, const std::vector<Polygon2d> & ego_polys, const rclcpp::Time & stamp,
std::vector<ObjectData> & objects);
Expand Down
1 change: 1 addition & 0 deletions control/autonomous_emergency_braking/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
<description>Autonomous Emergency Braking package as a ROS 2 node</description>
<maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
<maintainer email="tomoya.kimura@tier4.jp">Tomoya Kimura</maintainer>
<maintainer email="mamoru.sobue@tier4.jp">Mamoru Sobue</maintainer>

<license>Apache License 2.0</license>

Expand Down
45 changes: 24 additions & 21 deletions control/autonomous_emergency_braking/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -309,16 +309,14 @@
// step3. create ego path based on sensor data
bool has_collision_ego = false;
if (use_imu_path_) {
Path ego_path;
std::vector<Polygon2d> ego_polys;
const double current_w = angular_velocity_ptr_->z;
constexpr double color_r = 0.0 / 256.0;
constexpr double color_g = 148.0 / 256.0;
constexpr double color_b = 205.0 / 256.0;
constexpr double color_a = 0.999;
const auto current_time = get_clock()->now();
generateEgoPath(current_v, current_w, ego_path, ego_polys);

const auto ego_path = generateEgoPath(current_v, current_w, ego_polys);

Check warning on line 319 in control/autonomous_emergency_braking/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

control/autonomous_emergency_braking/src/node.cpp#L319

Added line #L319 was not covered by tests
std::vector<ObjectData> objects;
createObjectData(ego_path, ego_polys, current_time, objects);
has_collision_ego = hasCollision(current_v, ego_path, objects);
Expand All @@ -332,23 +330,25 @@
// step4. transform predicted trajectory from control module
bool has_collision_predicted = false;
if (use_predicted_trajectory_) {
Path predicted_path;
std::vector<Polygon2d> predicted_polys;
const auto predicted_traj_ptr = predicted_traj_ptr_;
constexpr double color_r = 0.0;
constexpr double color_g = 100.0 / 256.0;
constexpr double color_b = 0.0;
constexpr double color_a = 0.999;
const auto current_time = predicted_traj_ptr->header.stamp;
generateEgoPath(*predicted_traj_ptr, predicted_path, predicted_polys);
std::vector<ObjectData> objects;
createObjectData(predicted_path, predicted_polys, current_time, objects);
has_collision_predicted = hasCollision(current_v, predicted_path, objects);

std::string ns = "predicted";
addMarker(
current_time, predicted_path, predicted_polys, objects, color_r, color_g, color_b, color_a,
ns, debug_markers);
const auto predicted_path_opt = generateEgoPath(*predicted_traj_ptr, predicted_polys);
if (predicted_path_opt) {

Check warning on line 341 in control/autonomous_emergency_braking/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

control/autonomous_emergency_braking/src/node.cpp#L340-L341

Added lines #L340 - L341 were not covered by tests
const auto & predicted_path = predicted_path_opt.value();
std::vector<ObjectData> objects;
createObjectData(predicted_path, predicted_polys, current_time, objects);
has_collision_predicted = hasCollision(current_v, predicted_path, objects);

Check warning on line 345 in control/autonomous_emergency_braking/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

control/autonomous_emergency_braking/src/node.cpp#L344-L345

Added lines #L344 - L345 were not covered by tests

std::string ns = "predicted";
addMarker(

Check warning on line 348 in control/autonomous_emergency_braking/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

control/autonomous_emergency_braking/src/node.cpp#L347-L348

Added lines #L347 - L348 were not covered by tests
current_time, predicted_path, predicted_polys, objects, color_r, color_g, color_b, color_a,
ns, debug_markers);
}

Check warning on line 351 in control/autonomous_emergency_braking/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

control/autonomous_emergency_braking/src/node.cpp#L351

Added line #L351 was not covered by tests
}

return has_collision_ego || has_collision_predicted;
Expand Down Expand Up @@ -384,9 +384,10 @@
return false;
}

void AEB::generateEgoPath(
const double curr_v, const double curr_w, Path & path, std::vector<Polygon2d> & polygons)
Path AEB::generateEgoPath(

Check warning on line 387 in control/autonomous_emergency_braking/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

control/autonomous_emergency_braking/src/node.cpp#L387

Added line #L387 was not covered by tests
const double curr_v, const double curr_w, std::vector<Polygon2d> & polygons)
{
Path path;
double curr_x = 0.0;
double curr_y = 0.0;
double curr_yaw = 0.0;
Expand All @@ -397,7 +398,7 @@

if (curr_v < 0.1) {
// if current velocity is too small, assume it stops at the same point
return;
return path;
}

constexpr double epsilon = 1e-6;
Expand Down Expand Up @@ -435,14 +436,14 @@
for (size_t i = 0; i < path.size() - 1; ++i) {
polygons.at(i) = createPolygon(path.at(i), path.at(i + 1), vehicle_info_, expand_width_);
}
return path;
}

void AEB::generateEgoPath(
const Trajectory & predicted_traj, Path & path,
std::vector<tier4_autoware_utils::Polygon2d> & polygons)
std::optional<Path> AEB::generateEgoPath(

Check warning on line 442 in control/autonomous_emergency_braking/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

control/autonomous_emergency_braking/src/node.cpp#L442

Added line #L442 was not covered by tests
const Trajectory & predicted_traj, std::vector<tier4_autoware_utils::Polygon2d> & polygons)
{
if (predicted_traj.points.empty()) {
return;
return std::nullopt;

Check warning on line 446 in control/autonomous_emergency_braking/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

control/autonomous_emergency_braking/src/node.cpp#L446

Added line #L446 was not covered by tests
}

geometry_msgs::msg::TransformStamped transform_stamped{};
Expand All @@ -452,10 +453,11 @@
rclcpp::Duration::from_seconds(0.5));
} catch (tf2::TransformException & ex) {
RCLCPP_ERROR_STREAM(get_logger(), "[AEB] Failed to look up transform from base_link to map");
return;
return std::nullopt;
}

// create path
Path path;
path.resize(predicted_traj.points.size());
for (size_t i = 0; i < predicted_traj.points.size(); ++i) {
geometry_msgs::msg::Pose map_pose;
Expand All @@ -471,6 +473,7 @@
for (size_t i = 0; i < path.size() - 1; ++i) {
polygons.at(i) = createPolygon(path.at(i), path.at(i + 1), vehicle_info_, expand_width_);
}
return path;
}

void AEB::createObjectData(
Expand Down
Loading