From 4b6a2014c9e536091d27d323028b579d8f8c6c80 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Tue, 18 Jun 2024 09:58:59 +0900 Subject: [PATCH] add parameters and reuse predicted obj speed Signed-off-by: Daniel Sanchez --- .../autonomous_emergency_braking.param.yaml | 2 + .../node.hpp | 9 + .../utils.hpp | 22 +++ .../src/node.cpp | 163 +++++++++--------- 4 files changed, 117 insertions(+), 79 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 4233f8b6bebcb..344d076197170 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -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 diff --git a/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp index d9627d1037ccb..bc96684d6e744 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp @@ -181,6 +181,13 @@ class CollisionDataKeeper std::optional 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(); @@ -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_; diff --git a/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/utils.hpp b/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/utils.hpp index 11703469c71cf..a1cfde1116e2f 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/utils.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/utils.hpp @@ -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) { diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index b4eae5f2c7aa1..8351daa5cce0c 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -127,6 +127,8 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) publish_debug_pointcloud_ = declare_parameter("publish_debug_pointcloud"); use_predicted_trajectory_ = declare_parameter("use_predicted_trajectory"); use_imu_path_ = declare_parameter("use_imu_path"); + use_pointcloud_data_ = declare_parameter("use_pointcloud_data"); + use_predicted_object_data_ = declare_parameter("use_predicted_object_data"); use_object_velocity_calculation_ = declare_parameter("use_object_velocity_calculation"); path_footprint_extra_margin_ = declare_parameter("path_footprint_extra_margin"); detection_range_min_height_ = declare_parameter("detection_range_min_height"); @@ -175,6 +177,8 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( updateParam(parameters, "publish_debug_pointcloud", publish_debug_pointcloud_); updateParam(parameters, "use_predicted_trajectory", use_predicted_trajectory_); updateParam(parameters, "use_imu_path", use_imu_path_); + updateParam(parameters, "use_pointcloud_data", use_pointcloud_data_); + updateParam(parameters, "use_predicted_object_data", use_predicted_object_data_); updateParam( parameters, "use_object_velocity_calculation", use_object_velocity_calculation_); updateParam(parameters, "path_footprint_extra_margin", path_footprint_extra_margin_); @@ -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(); @@ -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"); @@ -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::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::Ptr filtered_objects) { // Check which points of the cropped point cloud are on the ego path, and get the closest one - std::vector 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 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 { - 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_) @@ -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()) @@ -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 @@ -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 @@ -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 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(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) { @@ -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); }