Skip to content

Commit

Permalink
feat(autonomous_emergency_braking): add timekeeper to AEB (#8706)
Browse files Browse the repository at this point in the history 
* add timekeeper to AEB

Signed-off-by: Daniel Sanchez <[email protected]>

* add more info to output

Signed-off-by: Daniel Sanchez <[email protected]>

---------

Signed-off-by: Daniel Sanchez <[email protected]>
  • Loading branch information
@danielsanchezaran
danielsanchezaran authored Sep 2, 2024
1 parent 978ab43 commit 980890f
Show file tree
Hide file tree
Showing 2 changed files with 45 additions and 16 deletions.
7 changes: 6 additions & 1 deletion ...oware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
#ifndef AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_
#define AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_

#include "autoware/universe_utils/system/time_keeper.hpp"

#include <autoware/motion_utils/trajectory/trajectory.hpp>
#include <autoware/universe_utils/geometry/geometry.hpp>
#include <autoware/universe_utils/ros/polling_subscriber.hpp>
Expand Down Expand Up @@ -334,9 +336,11 @@ class AEB : public rclcpp::Node
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_obstacle_pointcloud_;
rclcpp::Publisher<MarkerArray>::SharedPtr debug_marker_publisher_;
rclcpp::Publisher<MarkerArray>::SharedPtr info_marker_publisher_;

rclcpp::Publisher<autoware::universe_utils::ProcessingTimeDetail>::SharedPtr
debug_processing_time_detail_pub_;
// timer
rclcpp::TimerBase::SharedPtr timer_;
mutable std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper_{nullptr};

// callback
/**
Expand Down Expand Up @@ -508,6 +512,7 @@ class AEB : public rclcpp::Node
// Member variables
bool publish_debug_pointcloud_;
bool publish_debug_markers_;
bool publish_debug_time_;
bool use_predicted_trajectory_;
bool use_imu_path_;
bool use_pointcloud_data_;
Expand Down
54 changes: 39 additions & 15 deletions control/autoware_autonomous_emergency_braking/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -181,6 +181,12 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
const double aeb_hz = declare_parameter<double>("aeb_hz");
const auto period_ns = rclcpp::Rate(aeb_hz).period();
timer_ = rclcpp::create_timer(this, this->get_clock(), period_ns, std::bind(&AEB::onTimer, this));

debug_processing_time_detail_pub_ =
create_publisher<autoware::universe_utils::ProcessingTimeDetail>(
"~/debug/processing_time_detail_ms", 1);
time_keeper_ =
std::make_shared<autoware::universe_utils::TimeKeeper>(debug_processing_time_detail_pub_);
}

rcl_interfaces::msg::SetParametersResult AEB::onParameter(
Expand Down Expand Up @@ -259,6 +265,7 @@ void AEB::onImu(const Imu::ConstSharedPtr input_msg)

void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
PointCloud::Ptr pointcloud_ptr(new PointCloud);
pcl::fromROSMsg(*input_msg, *pointcloud_ptr);

Expand Down Expand Up @@ -401,7 +408,6 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat)
addCollisionMarker(data.value(), debug_markers);
}
}

addVirtualStopWallMarker(info_markers);
} else {
const std::string error_msg = "[AEB]: No Collision";
Expand All @@ -416,6 +422,7 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat)

bool AEB::checkCollision(MarkerArray & debug_markers)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
using colorTuple = std::tuple<double, double, double, double>;

// step1. check data
Expand Down Expand Up @@ -495,6 +502,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
};

// step3. make function to check collision with ego path created with sensor data
time_keeper_->start_track("has_collision_imu_path");
const auto has_collision_ego = [&](pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects) -> bool {
if (!use_imu_path_ || !angular_velocity_ptr_) return false;
const double current_w = angular_velocity_ptr_->z;
Expand All @@ -504,8 +512,10 @@ bool AEB::checkCollision(MarkerArray & debug_markers)

return check_collision(ego_path, debug_color, ns, filtered_objects);
};
time_keeper_->end_track("has_collision_imu_path");

// step4. make function to check collision with predicted trajectory from control module
time_keeper_->start_track("has_collision_mpc_path");
const auto has_collision_predicted =
[&](pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects) -> bool {
if (!use_predicted_trajectory_ || !predicted_traj_ptr_) return false;
Expand All @@ -519,6 +529,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers)

return check_collision(predicted_path, debug_color, ns, filtered_objects);
};
time_keeper_->end_track("has_collision_mpc_path");

// Data of filtered point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects =
Expand All @@ -538,10 +549,10 @@ bool AEB::checkCollision(MarkerArray & debug_markers)

bool AEB::hasCollision(const double current_v, const ObjectData & closest_object)
{
const double & obj_v = closest_object.velocity;
const double & t = t_response_;

autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
const double rss_dist = std::invoke([&]() {
const double & obj_v = closest_object.velocity;
const double & t = t_response_;
const double pre_braking_covered_distance = std::abs(current_v) * t;
const double braking_distance = (current_v * current_v) / (2 * std::fabs(a_ego_min_));
const double ego_stopping_distance = pre_braking_covered_distance + braking_distance;
Expand All @@ -551,19 +562,19 @@ bool AEB::hasCollision(const double current_v, const ObjectData & closest_object
return ego_stopping_distance + obj_braking_distance + longitudinal_offset_;
});

if (closest_object.distance_to_object < rss_dist) {
// collision happens
ObjectData collision_data = closest_object;
collision_data.rss = rss_dist;
collision_data.distance_to_object = closest_object.distance_to_object;
collision_data_keeper_.setCollisionData(collision_data);
return true;
}
return false;
if (closest_object.distance_to_object > rss_dist) return false;

// collision happens
ObjectData collision_data = closest_object;
collision_data.rss = rss_dist;
collision_data.distance_to_object = closest_object.distance_to_object;
collision_data_keeper_.setCollisionData(collision_data);
return true;
}

Path AEB::generateEgoPath(const double curr_v, const double curr_w)
{
autoware::universe_utils::ScopedTimeTrack st(std::string(__func__) + "(IMU)", *time_keeper_);
Path path;
double curr_x = 0.0;
double curr_y = 0.0;
Expand Down Expand Up @@ -612,21 +623,26 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w)

std::optional<Path> AEB::generateEgoPath(const Trajectory & predicted_traj)
{
autoware::universe_utils::ScopedTimeTrack st(std::string(__func__) + "(MPC)", *time_keeper_);
if (predicted_traj.points.empty()) {
return std::nullopt;
}

time_keeper_->start_track("lookUpTransform");
geometry_msgs::msg::TransformStamped transform_stamped{};
try {
transform_stamped = tf_buffer_.lookupTransform(
"base_link", predicted_traj.header.frame_id, predicted_traj.header.stamp,
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");
RCLCPP_ERROR_STREAM(
get_logger(),
"[AEB] Failed to look up transform from base_link to " + predicted_traj.header.frame_id);
return std::nullopt;
}
time_keeper_->end_track("lookUpTransform");

// create path
time_keeper_->start_track("createPath");
Path path;
path.reserve(predicted_traj.points.size());
for (size_t i = 0; i < predicted_traj.points.size(); ++i) {
Expand All @@ -638,12 +654,14 @@ std::optional<Path> AEB::generateEgoPath(const Trajectory & predicted_traj)
break;
}
}
time_keeper_->end_track("createPath");
return path;
}

std::vector<Polygon2d> AEB::generatePathFootprint(
const Path & path, const double extra_width_margin)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
std::vector<Polygon2d> polygons;
for (size_t i = 0; i < path.size() - 1; ++i) {
polygons.push_back(
Expand All @@ -656,6 +674,7 @@ void AEB::createObjectDataUsingPredictedObjects(
const Path & ego_path, const std::vector<Polygon2d> & ego_polys,
std::vector<ObjectData> & object_data_vector)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
if (predicted_objects_ptr_->objects.empty()) return;

const double current_ego_speed = current_velocity_ptr_->longitudinal_velocity;
Expand Down Expand Up @@ -743,6 +762,7 @@ void AEB::createObjectDataUsingPointCloudClusters(
const Path & ego_path, const std::vector<Polygon2d> & ego_polys, const rclcpp::Time & stamp,
std::vector<ObjectData> & objects, const pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_points_ptr)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
// check if the predicted path has valid number of points
if (ego_path.size() < 2 || ego_polys.empty() || obstacle_points_ptr->empty()) {
return;
Expand Down Expand Up @@ -827,6 +847,7 @@ void AEB::createObjectDataUsingPointCloudClusters(
void AEB::cropPointCloudWithEgoFootprintPath(
const std::vector<Polygon2d> & ego_polys, pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
PointCloud::Ptr full_points_ptr(new PointCloud);
pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *full_points_ptr);
// Create a Point cloud with the points of the ego footprint
Expand Down Expand Up @@ -860,6 +881,7 @@ void AEB::addMarker(
const double color_r, const double color_g, const double color_b, const double color_a,
const std::string & ns, MarkerArray & debug_markers)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
auto path_marker = autoware::universe_utils::createDefaultMarker(
"base_link", current_time, ns + "_path", 0L, Marker::LINE_STRIP,
autoware::universe_utils::createMarkerScale(0.2, 0.2, 0.2),
Expand Down Expand Up @@ -922,6 +944,7 @@ void AEB::addMarker(

void AEB::addVirtualStopWallMarker(MarkerArray & markers)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
const auto ego_map_pose = std::invoke([this]() -> std::optional<geometry_msgs::msg::Pose> {
geometry_msgs::msg::TransformStamped tf_current_pose;
geometry_msgs::msg::Pose p;
Expand Down Expand Up @@ -952,6 +975,7 @@ void AEB::addVirtualStopWallMarker(MarkerArray & markers)

void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
auto point_marker = autoware::universe_utils::createDefaultMarker(
"base_link", data.stamp, "collision_point", 0, Marker::SPHERE,
autoware::universe_utils::createMarkerScale(0.3, 0.3, 0.3),
Expand Down

0 comments on commit 980890f

Please sign in to comment.