Skip to content

Commit

Permalink
add parameters and reuse predicted obj speed
Browse files Browse the repository at this point in the history 
Signed-off-by: Daniel Sanchez <[email protected]>
  • Loading branch information
@danielsanchezaran
danielsanchezaran committed Jun 18, 2024
1 parent 7ade4b3 commit 4b6a201
Show file tree
Hide file tree
Showing 4 changed files with 117 additions and 79 deletions.
2 changes: 2 additions & 0 deletions control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@
# Ego path calculation
use_predicted_trajectory: true
use_imu_path: false
use_pointcloud_data: true
use_predicted_object_data: true
use_object_velocity_calculation: true
min_generated_path_length: 0.5
imu_prediction_time_horizon: 1.5
Expand Down
9 changes: 9 additions & 0 deletions ...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 @@ -181,6 +181,13 @@ class CollisionDataKeeper
std::optional<double> calcObjectSpeedFromHistory(
const ObjectData & closest_object, const Path & path, const double current_ego_speed)
{
// in case the object comes from predicted objects info, we reuse the speed.
if (closest_object.velocity > 0.0) {
this->setPreviousObjectData(closest_object);
this->updateVelocityHistory(closest_object.velocity, closest_object.stamp);
return this->getMedianObstacleVelocity();
}

if (this->checkPreviousObjectDataExpired()) {
this->setPreviousObjectData(closest_object);
this->resetVelocityHistory();
Expand Down Expand Up @@ -331,6 +338,8 @@ class AEB : public rclcpp::Node
bool publish_debug_pointcloud_;
bool use_predicted_trajectory_;
bool use_imu_path_;
bool use_pointcloud_data_;
bool use_predicted_object_data_;
bool use_object_velocity_calculation_;
double path_footprint_extra_margin_;
double detection_range_min_height_;
Expand Down
22 changes: 22 additions & 0 deletions ...ware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,28 @@ using geometry_msgs::msg::TransformStamped;
using tier4_autoware_utils::Point2d;
using tier4_autoware_utils::Polygon2d;

inline PredictedObject transformObjectFrame(
const PredictedObject & input, geometry_msgs::msg::TransformStamped transform_stamped)
{
PredictedObject output = input;
const auto & linear_twist = input.kinematics.initial_twist_with_covariance.twist.linear;
const auto & angular_twist = input.kinematics.initial_twist_with_covariance.twist.angular;
const auto & pose = input.kinematics.initial_pose_with_covariance.pose;

geometry_msgs::msg::Pose t_pose;
Vector3 t_linear_twist;
Vector3 t_angular_twist;

tf2::doTransform(pose, t_pose, transform_stamped);
tf2::doTransform(linear_twist, t_linear_twist, transform_stamped);
tf2::doTransform(angular_twist, t_angular_twist, transform_stamped);

output.kinematics.initial_pose_with_covariance.pose = t_pose;
output.kinematics.initial_twist_with_covariance.twist.linear = t_linear_twist;
output.kinematics.initial_twist_with_covariance.twist.angular = t_angular_twist;
return output;
}

inline Polygon2d convertPolygonObjectToGeometryPolygon(
const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape)
{
Expand Down
163 changes: 84 additions & 79 deletions control/autoware_autonomous_emergency_braking/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -127,6 +127,8 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
publish_debug_pointcloud_ = declare_parameter<bool>("publish_debug_pointcloud");
use_predicted_trajectory_ = declare_parameter<bool>("use_predicted_trajectory");
use_imu_path_ = declare_parameter<bool>("use_imu_path");
use_pointcloud_data_ = declare_parameter<bool>("use_pointcloud_data");
use_predicted_object_data_ = declare_parameter<bool>("use_predicted_object_data");
use_object_velocity_calculation_ = declare_parameter<bool>("use_object_velocity_calculation");
path_footprint_extra_margin_ = declare_parameter<double>("path_footprint_extra_margin");
detection_range_min_height_ = declare_parameter<double>("detection_range_min_height");
Expand Down Expand Up @@ -175,6 +177,8 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter(
updateParam<bool>(parameters, "publish_debug_pointcloud", publish_debug_pointcloud_);
updateParam<bool>(parameters, "use_predicted_trajectory", use_predicted_trajectory_);
updateParam<bool>(parameters, "use_imu_path", use_imu_path_);
updateParam<bool>(parameters, "use_pointcloud_data", use_pointcloud_data_);
updateParam<bool>(parameters, "use_predicted_object_data", use_predicted_object_data_);
updateParam<bool>(
parameters, "use_object_velocity_calculation", use_object_velocity_calculation_);
updateParam<double>(parameters, "path_footprint_extra_margin", path_footprint_extra_margin_);
Expand Down Expand Up @@ -301,13 +305,31 @@ bool AEB::fetchLatestData()
return missing("ego velocity");
}

const auto pointcloud_ptr = sub_point_cloud_.takeData();
if (!pointcloud_ptr) {
return missing("object pointcloud message");
if (use_pointcloud_data_) {
const auto pointcloud_ptr = sub_point_cloud_.takeData();
if (!pointcloud_ptr) {
return missing("object pointcloud message");
}

onPointCloud(pointcloud_ptr);
if (!obstacle_ros_pointcloud_ptr_) {
return missing("object pointcloud");
}
} else {
obstacle_ros_pointcloud_ptr_.reset();
}
onPointCloud(pointcloud_ptr);
if (!obstacle_ros_pointcloud_ptr_) {
return missing("object pointcloud");

if (use_predicted_object_data_) {
predicted_objects_ptr_ = predicted_objects_sub_.takeData();
if (!predicted_objects_ptr_) {
return missing("predicted objects");
}
} else {
predicted_objects_ptr_.reset();
}

if (!obstacle_ros_pointcloud_ptr_ && !predicted_objects_ptr_) {
return missing("object detection method (pointcloud or predicted objects)");
}

const auto imu_ptr = sub_imu_.takeData();
Expand All @@ -327,11 +349,6 @@ bool AEB::fetchLatestData()
return missing("control predicted trajectory");
}

predicted_objects_ptr_ = predicted_objects_sub_.takeData();
if (!predicted_objects_ptr_) {
return missing("predicted objects");
}

autoware_state_ = sub_autoware_state_.takeData();
if (!autoware_state_) {
return missing("autoware_state");
Expand Down Expand Up @@ -385,33 +402,37 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
return false;
}

auto check_pointcloud_collision = [&](
const auto & path, const colorTuple & debug_colors,
const std::string & debug_ns,
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects) {
// Crop out Pointcloud using an extra wide ego path
const auto expanded_ego_polys =
generatePathFootprint(path, expand_width_ + path_footprint_extra_margin_);
cropPointCloudWithEgoFootprintPath(expanded_ego_polys, filtered_objects);

auto check_collision = [&](
const auto & path, const colorTuple & debug_colors,
const std::string & debug_ns,
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects) {
// Check which points of the cropped point cloud are on the ego path, and get the closest one
std::vector<ObjectData> objects_from_point_clusters;
const auto ego_polys = generatePathFootprint(path, expand_width_);
// const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp;
// createObjectDataUsingPointCloudClusters(
// path, ego_polys, current_time, objects_from_point_clusters, filtered_objects);

createObjectDataUsingPredictedObjects(path, ego_polys, objects_from_point_clusters);
auto objects = std::invoke([&]() {
std::vector<ObjectData> objects;
// Crop out Pointcloud using an extra wide ego path
if (use_pointcloud_data_) {
const auto expanded_ego_polys =
generatePathFootprint(path, expand_width_ + path_footprint_extra_margin_);
cropPointCloudWithEgoFootprintPath(expanded_ego_polys, filtered_objects);
const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp;
createObjectDataUsingPointCloudClusters(
path, ego_polys, current_time, objects, filtered_objects);
}
if (use_predicted_object_data_) {
createObjectDataUsingPredictedObjects(path, ego_polys, objects);
}
return objects;
});

// Get only the closest object and calculate its speed
const auto closest_object_point = std::invoke([&]() -> std::optional<ObjectData> {
const auto closest_object_point_itr = std::min_element(
objects_from_point_clusters.begin(), objects_from_point_clusters.end(),
[](const auto & o1, const auto & o2) {
const auto closest_object_point_itr =
std::min_element(objects.begin(), objects.end(), [](const auto & o1, const auto & o2) {
return o1.distance_to_object < o2.distance_to_object;
});

if (closest_object_point_itr == objects_from_point_clusters.end()) {
if (closest_object_point_itr == objects.end()) {
return std::nullopt;
}
const auto closest_object_speed = (use_object_velocity_calculation_)
Expand All @@ -429,8 +450,8 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
{
const auto [color_r, color_g, color_b, color_a] = debug_colors;
addMarker(
this->get_clock()->now(), path, ego_polys, objects_from_point_clusters,
closest_object_point, color_r, color_g, color_b, color_a, debug_ns, debug_markers);
this->get_clock()->now(), path, ego_polys, objects, closest_object_point, color_r, color_g,
color_b, color_a, debug_ns, debug_markers);
}
// check collision using rss distance
return (closest_object_point.has_value())
Expand All @@ -446,7 +467,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
const std::string ns = "ego";
const auto ego_path = generateEgoPath(current_v, current_w);

return check_pointcloud_collision(ego_path, debug_color, ns, filtered_objects);
return check_collision(ego_path, debug_color, ns, filtered_objects);
};

// step4. make function to check collision with predicted trajectory from control module
Expand All @@ -461,7 +482,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
const std::string ns = "predicted";
const auto & predicted_path = predicted_path_opt.value();

return check_pointcloud_collision(predicted_path, debug_color, ns, filtered_objects);
return check_collision(predicted_path, debug_color, ns, filtered_objects);
};

// Data of filtered point cloud
Expand Down Expand Up @@ -604,60 +625,44 @@ void AEB::createObjectDataUsingPredictedObjects(
return tier4_autoware_utils::createPoint(p.x, p.y, p.z);
}();

auto get_object_velocity = [&](const PredictedObject & predicted_object, const auto & obj_pose) {
const double obj_vel_norm = std::hypot(
predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x,
predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y);

const auto obj_yaw = tf2::getYaw(obj_pose.orientation);
const auto obj_idx = motion_utils::findNearestIndex(ego_path, obj_pose.position);
const auto path_yaw = (current_ego_speed > 0.0)
? tf2::getYaw(ego_path.at(obj_idx).orientation)
: tf2::getYaw(ego_path.at(obj_idx).orientation) + M_PI;
return obj_vel_norm * std::cos(obj_yaw - path_yaw);
};
auto get_object_tangent_velocity =
[&](const PredictedObject & predicted_object, const auto & obj_pose) {
const double obj_vel_norm = std::hypot(
predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x,
predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y);

// auto convert_polygon_object_to_geometry_polygon =
// [](
// const geometry_msgs::msg::Pose & current_pose,
// const autoware_perception_msgs::msg::Shape & obj_shape) {
// Polygon2d object_polygon;
// tf2::Transform tf_map2obj;
// fromMsg(current_pose, tf_map2obj);
// const auto obj_points = obj_shape.footprint.points;
// object_polygon.outer().reserve(obj_points.size() + 1);
// for (const auto & obj_point : obj_points) {
// tf2::Vector3 obj(obj_point.x, obj_point.y, obj_point.z);
// tf2::Vector3 tf_obj = tf_map2obj * obj;
// object_polygon.outer().emplace_back(tf_obj.x(), tf_obj.y());
// }
// object_polygon.outer().push_back(object_polygon.outer().front());
// object_polygon = tier4_autoware_utils::isClockwise(object_polygon)
// ? object_polygon
// : tier4_autoware_utils::inverseClockwise(object_polygon);
// bg::correct(object_polygon);
// return object_polygon;
// };
const auto obj_yaw = tf2::getYaw(obj_pose.orientation);
const auto obj_idx = motion_utils::findNearestIndex(ego_path, obj_pose.position);
const auto path_yaw = (current_ego_speed > 0.0)
? tf2::getYaw(ego_path.at(obj_idx).orientation)
: tf2::getYaw(ego_path.at(obj_idx).orientation) + M_PI;
return obj_vel_norm * std::cos(obj_yaw - path_yaw);
};

geometry_msgs::msg::TransformStamped transform_stamped{};
try {
transform_stamped = tf_buffer_.lookupTransform(
"base_link", predicted_objects_ptr_->header.frame_id, 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");
return;
}

// Check which objects collide with the ego footprints
std::for_each(objects.begin(), objects.end(), [&](const auto & predicted_object) {
const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose;
const auto obj_poly = convertObjToPolygon(predicted_object);
const double obj_velocity = get_object_velocity(predicted_object, obj_pose);
// get objects in base_link frame
const auto t_predicted_object =
utils::transformObjectFrame(predicted_object, transform_stamped);
const auto & obj_pose = t_predicted_object.kinematics.initial_pose_with_covariance.pose;
const auto obj_poly = convertObjToPolygon(t_predicted_object);
const double obj_tangent_velocity = get_object_tangent_velocity(t_predicted_object, obj_pose);

for (const auto & ego_poly : ego_polys) {
// check collision with 2d polygon
std::vector<Point2d> collision_points_bg;
bg::intersection(ego_poly, obj_poly, collision_points_bg);

const bool inter_sec = bg::intersects(obj_poly, ego_poly);
std::cerr << "BEFORE Intersects! " << static_cast<int>(inter_sec) << "\n";

std::cerr << "obj_poly.outer().size() " << obj_poly.outer().size() << "\n";
std::cerr << "ego_poly.outer().size() " << ego_poly.outer().size() << "\n";

if (collision_points_bg.empty()) continue;
std::cerr << "Intersects! YEEEEEEEES\n";

// Create an object for each intersection point
for (const auto & collision_point : collision_points_bg) {
Expand All @@ -677,7 +682,7 @@ void AEB::createObjectDataUsingPredictedObjects(
ObjectData obj;
obj.stamp = stamp;
obj.position = obj_position;
obj.velocity = (obj_velocity > 0.0) ? obj_velocity : 0.0;
obj.velocity = (obj_tangent_velocity > 0.0) ? obj_tangent_velocity : 0.0;
obj.distance_to_object = std::abs(dist_ego_to_object);
object_data_vector.push_back(obj);
}
Expand Down

0 comments on commit 4b6a201

Please sign in to comment.