diff --git a/perception/map_based_prediction/Readme.md b/perception/map_based_prediction/README.md similarity index 81% rename from perception/map_based_prediction/Readme.md rename to perception/map_based_prediction/README.md index 0694f973735b0..fa3307168fc0f 100644 --- a/perception/map_based_prediction/Readme.md +++ b/perception/map_based_prediction/README.md @@ -6,6 +6,8 @@ ## Inner-workings / Algorithms +### Path prediction for road users + 1. Get lanelet path The first step is to get the lanelet of the current position of the car. After that, we obtain several trajectories based on the map. @@ -30,6 +32,29 @@ 4. Drawing predicted trajectories From the current position and reference trajectories that we get in the step1, we create predicted trajectories by using Quintic polynomial. Note that, since this algorithm consider lateral and longitudinal motions separately, it sometimes generates dynamically-infeasible trajectories when the vehicle travels at a low speed. To deal with this problem, we only make straight line predictions when the vehicle speed is lower than a certain value (which is given as a parameter). +### Path prediction for crosswalk users + +This module treats **Pedestrians** and **Bicycles** as objects using the crosswalk, and outputs prediction path based on map and estimated object's velocity, assuming the object has intention to cross the crosswalk, if the objects satisfies at least one of the following conditions: + +- move toward the crosswalk +- stop near the crosswalk + +
+ +
+ +If there are a reachable crosswalk entry points within the `prediction_time_horizon` and the objects satisfies above condition, this module outputs additional predicted path to cross the opposite side via the crosswalk entry point. + +
+ +
+ +If the target object is inside the road or crosswalk, this module outputs one or two additional prediction path(s) to reach exit point of the crosswalk. The number of prediction paths are depend on whether object is moving or not. If the object is moving, this module outputs one prediction path toward an exit point that existed in the direction of object's movement. One the other hand, if the object has stopped, it is impossible to infer which exit points the object want to go, so this module outputs two prediction paths toward both side exit point. + +
+ +
+ ## Inputs / Outputs ### Input diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml index ef67fa7b00aed..ec28cbc0c1986 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -3,7 +3,7 @@ enable_delay_compensation: true prediction_time_horizon: 10.0 #[s] prediction_sampling_delta_time: 0.5 #[s] - min_velocity_for_map_based_prediction: 1.0 #[m/s] + min_velocity_for_map_based_prediction: 1.39 #[m/s] dist_threshold_for_searching_lanelet: 3.0 #[m] delta_yaw_threshold_for_searching_lanelet: 0.785 #[rad] sigma_lateral_offset: 0.5 #[m] diff --git a/perception/map_based_prediction/images/exception.svg b/perception/map_based_prediction/images/exception.svg new file mode 100644 index 0000000000000..89e7206e92786 --- /dev/null +++ b/perception/map_based_prediction/images/exception.svg @@ -0,0 +1,4 @@ + + + +
road
(lanelet)
road...
road
(lanelet)
road...
crosswalk
(lanelet)
crosswalk...
road
(lanelet)
road...
Pedestrian(move)
Pedes...
target object
(append additional prediction path)
target object...
original naive prediction path
original naive prediction path
additional map based prediction path
additional map based prediction pa...Text is not SVG - cannot display \ No newline at end of file diff --git a/perception/map_based_prediction/images/inside_road.svg b/perception/map_based_prediction/images/inside_road.svg new file mode 100644 index 0000000000000..61d2605ef3414 --- /dev/null +++ b/perception/map_based_prediction/images/inside_road.svg @@ -0,0 +1,4 @@ + + + +
road
(lanelet)
road...
road
(lanelet)
road...
Pedestrian(move)
Pedes...
target object
(append additional prediction path)
target object...
Pedestrian(move)
Pedes...
original naive prediction path
original naive prediction path
additional map based prediction path
additional map based prediction pa...
Pedestrian(stop)
Pedes...
Pedestrian(stop)
Pedes...
output predicted paths toward　both exit points
output predicted paths towar...
output predicted path toward exit point in the direction of movement
output predicted path toward exit po...
crosswalk entry/exit point
crosswalk entry/exit pointText is not SVG - cannot display \ No newline at end of file diff --git a/perception/map_based_prediction/images/outside_road.svg b/perception/map_based_prediction/images/outside_road.svg new file mode 100644 index 0000000000000..b15749bcd04ee --- /dev/null +++ b/perception/map_based_prediction/images/outside_road.svg @@ -0,0 +1,4 @@ + + + +
road
(lanelet)
road...
road
(lanelet)
road...
crosswalk
(lanelet)
crosswalk...
Pedestrian(move)
Pedes...
Pedestrian(stop)
Pedes...
target object
(append additional prediction path)
target object...
Pedestrian(move)
Pedes...
original naive prediction path
original naive prediction path
additional map based prediction path
additional map based prediction pa...
crosswalk entry/exit point
crosswalk entry/exit pointText is not SVG - cannot display \ No newline at end of file diff --git a/perception/map_based_prediction/images/target_objects.svg b/perception/map_based_prediction/images/target_objects.svg new file mode 100644 index 0000000000000..1288e2cf43b8e --- /dev/null +++ b/perception/map_based_prediction/images/target_objects.svg @@ -0,0 +1,4 @@ + + + +
road
(lanelet)
road...
road
(lanelet)
road...
crosswalk
(lanelet)
crosswalk...
Pedestrian(move)
Pedes...
Pedestrian(stop)
Pedes...
Pedestrian(move)
Pedes...
Pedestrian(stop)
Pedes...
Pedestrian(move)
Pedes...
Pedestrian(stop)
Pedes...
target object
(append additional prediction path)
target object...
non-target object
non-target object
moving direction
moving direction
  • far from crosswalk
  • stop
far from crosswalkstop
  • close to crosswalk
  • stop
close to crosswalkstop
  • within road
  • stop
within roadstop
  • within crosswalk
  • stop
within crosswalkstop
  • close to crosswalk
  • move away from crosswalk
close to crosswalkmove away from cros...
  • close to crosswalk
  • move toward crosswalk
close to crosswalkmove toward cr...
conditions for target objects are follows:
  • move toward the crosswalk
  • stop near the crosswalk
conditions for target objects are fo...Text is not SVG - cannot display \ No newline at end of file diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index d700f6484a8af..3740f47a4fd91 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -18,9 +18,11 @@ #include "map_based_prediction/path_generator.hpp" #include +#include #include #include #include +#include #include #include @@ -86,6 +88,7 @@ using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_perception_msgs::msg::TrackedObject; using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; using autoware_auto_perception_msgs::msg::TrackedObjects; +using tier4_autoware_utils::StopWatch; class MapBasedPredictionNode : public rclcpp::Node { @@ -113,6 +116,9 @@ class MapBasedPredictionNode : public rclcpp::Node // Path Generator std::shared_ptr path_generator_; + // Crosswalk Entry Points + lanelet::ConstLanelets crosswalks_; + // Parameters bool enable_delay_compensation_; double prediction_time_horizon_; @@ -131,6 +137,9 @@ class MapBasedPredictionNode : public rclcpp::Node double diff_dist_threshold_to_right_bound_; double reference_path_resolution_; + // Stop watch + StopWatch stop_watch_; + // Member Functions void mapCallback(const HADMapBin::ConstSharedPtr msg); void objectsCallback(const TrackedObjects::ConstSharedPtr in_objects); @@ -140,6 +149,8 @@ class MapBasedPredictionNode : public rclcpp::Node PredictedObject convertToPredictedObject(const TrackedObject & tracked_object); + PredictedObject getPredictedObjectAsCrosswalkUser(const TrackedObject & object); + void removeOldObjectsHistory(const double current_time); LaneletsData getCurrentLanelets(const TrackedObject & object); diff --git a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp index 14922078095fb..4ed752f410478 100644 --- a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -24,6 +24,8 @@ #include #include +#include +#include #include namespace map_based_prediction @@ -47,13 +49,16 @@ struct FrenetPoint float d_acc; }; +using EntryPoint = std::pair; using FrenetPath = std::vector; using PosePath = std::vector; class PathGenerator { public: - PathGenerator(const double time_horizon, const double sampling_time_interval); + PathGenerator( + const double time_horizon, const double sampling_time_interval, + const double min_velocity_for_map_based_prediction); PredictedPath generatePathForNonVehicleObject(const TrackedObject & object); @@ -64,10 +69,17 @@ class PathGenerator PredictedPath generatePathForOnLaneVehicle( const TrackedObject & object, const PosePath & ref_paths); + PredictedPath generatePathForCrosswalkUser( + const TrackedObject & object, const EntryPoint & reachable_crosswalk) const; + + PredictedPath generatePathToTargetPoint( + const TrackedObject & object, const Eigen::Vector2d & point) const; + private: // Parameters double time_horizon_; double sampling_time_interval_; + double min_velocity_for_map_based_prediction_; // Member functions PredictedPath generateStraightPath(const TrackedObject & object) const; diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index d3b61e13eba59..e1cb743ea4d70 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -17,6 +17,9 @@ #include #include +#include +#include + #include #ifdef ROS_DISTRO_GALACTIC @@ -30,6 +33,8 @@ #include #include +namespace map_based_prediction +{ namespace { std::string toHexString(const unique_identifier_msgs::msg::UUID & id) @@ -50,10 +55,197 @@ lanelet::ConstLanelets getLanelets(const map_based_prediction::LaneletsData & da return lanelets; } -} // namespace -namespace map_based_prediction +EntryPoint getCrosswalkEntryPoint(const lanelet::ConstLanelet & crosswalk) +{ + const auto & r_p_front = crosswalk.rightBound().front(); + const auto & l_p_front = crosswalk.leftBound().front(); + const Eigen::Vector2d front_entry_point( + (r_p_front.x() + l_p_front.x()) / 2.0, (r_p_front.y() + l_p_front.y()) / 2.0); + + const auto & r_p_back = crosswalk.rightBound().back(); + const auto & l_p_back = crosswalk.leftBound().back(); + const Eigen::Vector2d back_entry_point( + (r_p_back.x() + l_p_back.x()) / 2.0, (r_p_back.y() + l_p_back.y()) / 2.0); + + return std::make_pair(front_entry_point, back_entry_point); +} + +bool withinLanelet(const TrackedObject & object, const lanelet::ConstLanelet & lanelet) { + using Point = boost::geometry::model::d2::point_xy; + + const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; + const Point p_object{obj_pos.x, obj_pos.y}; + + auto polygon = lanelet.polygon2d().basicPolygon(); + boost::geometry::correct(polygon); + + return boost::geometry::within(p_object, polygon); +} + +bool withinRoadLanelet(const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + using Point = boost::geometry::model::d2::point_xy; + + const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; + const Point p_object{obj_pos.x, obj_pos.y}; + + lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y); + // nearest lanelet + constexpr double search_radius = 10.0; // [m] + const auto surrounding_lanelets = + lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, search_radius); + + for (const auto & lanelet : surrounding_lanelets) { + if (lanelet.second.hasAttribute(lanelet::AttributeName::Subtype)) { + lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype); + if ( + attr.value() == lanelet::AttributeValueString::Crosswalk || + attr.value() == lanelet::AttributeValueString::Walkway) { + continue; + } + } + + if (withinLanelet(object, lanelet.second)) { + return true; + } + } + + return false; +} + +boost::optional isReachableEntryPoint( + const TrackedObject & object, const EntryPoint & entry_point, + const lanelet::LaneletMapPtr & lanelet_map_ptr, const double time_horizon, + const double min_object_vel) +{ + using Point = boost::geometry::model::d2::point_xy; + using Line = boost::geometry::model::linestring; + + const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; + const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; + const auto yaw = tier4_autoware_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; + + const auto & p1 = entry_point.first; + const auto & p2 = entry_point.second; + + auto ret = std::make_pair(p1, p2); + auto distance_pedestrian_to_p1 = std::hypot(p1.x() - obj_pos.x, p1.y() - obj_pos.y); + auto distance_pedestrian_to_p2 = std::hypot(p2.x() - obj_pos.x, p2.y() - obj_pos.y); + + if (distance_pedestrian_to_p2 < distance_pedestrian_to_p1) { + std::swap(ret.first, ret.second); + std::swap(distance_pedestrian_to_p1, distance_pedestrian_to_p2); + } + + constexpr double stop_velocity_th = 0.14; // [m/s] + const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); + const auto is_stop_object = estimated_velocity < stop_velocity_th; + const auto velocity = std::max(min_object_vel, estimated_velocity); + + lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y); + // nearest lanelet + const auto surrounding_lanelets = lanelet::geometry::findNearest( + lanelet_map_ptr->laneletLayer, search_point, time_horizon * velocity); + + bool first_intersect_load = false; + bool second_intersect_load = false; + std::vector intersects_first; + std::vector intersects_second; + for (const auto & lanelet : surrounding_lanelets) { + if (withinLanelet(object, lanelet.second)) { + return {}; + } + + lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype); + if (attr.value() != "road") { + continue; + } + + { + const Line object_to_entry_point{{obj_pos.x, obj_pos.y}, {ret.first.x(), ret.first.y()}}; + std::vector tmp_intersects; + boost::geometry::intersection( + object_to_entry_point, lanelet.second.polygon2d().basicPolygon(), tmp_intersects); + for (const auto & p : tmp_intersects) { + intersects_first.push_back(p); + } + } + + { + const Line object_to_entry_point{{obj_pos.x, obj_pos.y}, {ret.second.x(), ret.second.y()}}; + std::vector tmp_intersects; + boost::geometry::intersection( + object_to_entry_point, lanelet.second.polygon2d().basicPolygon(), tmp_intersects); + for (const auto & p : tmp_intersects) { + intersects_second.push_back(p); + } + } + } + + if (1 < intersects_first.size()) { + first_intersect_load = true; + } + + if (1 < intersects_second.size()) { + second_intersect_load = true; + } + + if (first_intersect_load && second_intersect_load) { + return {}; + } + + if (first_intersect_load && !second_intersect_load) { + std::swap(ret.first, ret.second); + } + + const Eigen::Vector2d pedestrian_to_crosswalk( + (ret.first.x() + ret.second.x()) / 2.0 - obj_pos.x, + (ret.first.y() + ret.second.y()) / 2.0 - obj_pos.y); + const Eigen::Vector2d pedestrian_heading_direction( + obj_vel.x * std::cos(yaw), obj_vel.x * std::sin(yaw)); + const auto reachable = + std::min(distance_pedestrian_to_p1, distance_pedestrian_to_p2) < velocity * time_horizon; + const auto heading_for_crosswalk = + pedestrian_to_crosswalk.dot(pedestrian_heading_direction) > 0.0; + + if ((reachable && heading_for_crosswalk) || (reachable && is_stop_object)) { + return ret; + } + + return {}; +} + +bool hasPotentialToReach( + const TrackedObject & object, const Eigen::Vector2d & point, const double time_horizon, + const double min_object_vel) +{ + const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; + const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; + const auto yaw = tier4_autoware_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; + + constexpr double stop_velocity_th = 0.14; // [m/s] + const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); + const auto is_stop_object = estimated_velocity < stop_velocity_th; + const auto velocity = std::max(min_object_vel, estimated_velocity); + + const Eigen::Vector2d pedestrian_to_crosswalk(point.x() - obj_pos.x, point.y() - obj_pos.y); + const Eigen::Vector2d pedestrian_heading_direction( + obj_vel.x * std::cos(yaw), obj_vel.x * std::sin(yaw)); + const auto heading_for_crosswalk = + pedestrian_to_crosswalk.dot(pedestrian_heading_direction) > 0.0; + + const auto reachable = pedestrian_to_crosswalk.norm() < velocity * time_horizon; + + if (reachable && (heading_for_crosswalk || is_stop_object)) { + return true; + } + + return false; +} +} // namespace + MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_options) : Node("map_based_prediction", node_options), debug_accumulated_time_(0.0) { @@ -79,8 +271,9 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ declare_parameter("diff_dist_threshold_to_right_bound", -0.29); reference_path_resolution_ = declare_parameter("reference_path_resolution", 0.5); - path_generator_ = - std::make_shared(prediction_time_horizon_, prediction_sampling_time_interval_); + path_generator_ = std::make_shared( + prediction_time_horizon_, prediction_sampling_time_interval_, + min_velocity_for_map_based_prediction_); sub_objects_ = this->create_subscription( "/perception/object_recognition/tracking/objects", 1, @@ -124,6 +317,12 @@ void MapBasedPredictionNode::mapCallback(const HADMapBin::ConstSharedPtr msg) lanelet::utils::conversion::fromBinMsg( *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); RCLCPP_INFO(get_logger(), "[Map Based Prediction]: Map is loaded"); + + const auto all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); + const auto crosswalks = lanelet::utils::query::crosswalkLanelets(all_lanelets); + const auto walkways = lanelet::utils::query::walkwayLanelets(all_lanelets); + crosswalks_.insert(crosswalks_.end(), crosswalks.begin(), crosswalks.end()); + crosswalks_.insert(crosswalks_.end(), walkways.begin(), walkways.end()); } void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects) @@ -175,125 +374,235 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt transformed_object.kinematics.pose_with_covariance.pose = pose_in_map.pose; } - // For non-vehicle object - if ( - transformed_object.classification.front().label != ObjectClassification::CAR && - transformed_object.classification.front().label != ObjectClassification::BUS && - transformed_object.classification.front().label != ObjectClassification::TRAILER && - transformed_object.classification.front().label != ObjectClassification::MOTORCYCLE && - transformed_object.classification.front().label != ObjectClassification::TRUCK) { - PredictedPath predicted_path = - path_generator_->generatePathForNonVehicleObject(transformed_object); - predicted_path.confidence = 1.0; + const auto & label = transformed_object.classification.front().label; - auto predicted_object = convertToPredictedObject(transformed_object); - predicted_object.kinematics.predicted_paths.push_back(predicted_path); + // For crosswalk user + if (label == ObjectClassification::PEDESTRIAN || label == ObjectClassification::BICYCLE) { + const auto predicted_object = getPredictedObjectAsCrosswalkUser(transformed_object); output.objects.push_back(predicted_object); - continue; - } + // For road user + } else if ( + label == ObjectClassification::CAR || label == ObjectClassification::BUS || + label == ObjectClassification::TRAILER || label == ObjectClassification::MOTORCYCLE || + label == ObjectClassification::TRUCK) { + // Get Closest Lanelet + const auto current_lanelets = getCurrentLanelets(transformed_object); + + // Update Objects History + updateObjectsHistory(output.header, transformed_object, current_lanelets); + + // For off lane obstacles + if (current_lanelets.empty()) { + PredictedPath predicted_path = + path_generator_->generatePathForOffLaneVehicle(transformed_object); + predicted_path.confidence = 1.0; + if (predicted_path.path.empty()) { + continue; + } - // Get Closest Lanelet - const auto current_lanelets = getCurrentLanelets(transformed_object); + auto predicted_object = convertToPredictedObject(transformed_object); + predicted_object.kinematics.predicted_paths.push_back(predicted_path); + output.objects.push_back(predicted_object); + continue; + } - // Update Objects History - updateObjectsHistory(output.header, transformed_object, current_lanelets); + // For too-slow vehicle + if ( + std::fabs(transformed_object.kinematics.twist_with_covariance.twist.linear.x) < + min_velocity_for_map_based_prediction_) { + PredictedPath predicted_path = + path_generator_->generatePathForLowSpeedVehicle(transformed_object); + predicted_path.confidence = 1.0; + if (predicted_path.path.empty()) { + continue; + } - // For off lane obstacles - if (current_lanelets.empty()) { - PredictedPath predicted_path = - path_generator_->generatePathForOffLaneVehicle(transformed_object); - predicted_path.confidence = 1.0; - if (predicted_path.path.empty()) { + auto predicted_object = convertToPredictedObject(transformed_object); + predicted_object.kinematics.predicted_paths.push_back(predicted_path); + output.objects.push_back(predicted_object); continue; } - auto predicted_object = convertToPredictedObject(transformed_object); - predicted_object.kinematics.predicted_paths.push_back(predicted_path); - output.objects.push_back(predicted_object); - continue; - } + // Get Predicted Reference Path for Each Maneuver and current lanelets + // return: + const auto ref_paths = + getPredictedReferencePath(transformed_object, current_lanelets, objects_detected_time); + + // If predicted reference path is empty, assume this object is out of the lane + if (ref_paths.empty()) { + PredictedPath predicted_path = + path_generator_->generatePathForLowSpeedVehicle(transformed_object); + predicted_path.confidence = 1.0; + if (predicted_path.path.empty()) { + continue; + } - // For too-slow vehicle - if ( - std::fabs(transformed_object.kinematics.twist_with_covariance.twist.linear.x) < - min_velocity_for_map_based_prediction_) { - PredictedPath predicted_path = - path_generator_->generatePathForLowSpeedVehicle(transformed_object); - predicted_path.confidence = 1.0; - if (predicted_path.path.empty()) { + auto predicted_object = convertToPredictedObject(transformed_object); + predicted_object.kinematics.predicted_paths.push_back(predicted_path); + output.objects.push_back(predicted_object); continue; } + // Get Debug Marker for On Lane Vehicles + const auto max_prob_path = std::max_element( + ref_paths.begin(), ref_paths.end(), + [](const PredictedRefPath & a, const PredictedRefPath & b) { + return a.probability < b.probability; + }); + const auto debug_marker = + getDebugMarker(object, max_prob_path->maneuver, debug_markers.markers.size()); + debug_markers.markers.push_back(debug_marker); + + // Generate Predicted Path + std::vector predicted_paths; + for (const auto & ref_path : ref_paths) { + PredictedPath predicted_path = + path_generator_->generatePathForOnLaneVehicle(transformed_object, ref_path.path); + predicted_path.confidence = ref_path.probability; + + predicted_paths.push_back(predicted_path); + } + + // Normalize Path Confidence and output the predicted object + { + float sum_confidence = 0.0; + for (const auto & predicted_path : predicted_paths) { + sum_confidence += predicted_path.confidence; + } + const float min_sum_confidence_value = 1e-3; + sum_confidence = std::max(sum_confidence, min_sum_confidence_value); + + for (auto & predicted_path : predicted_paths) { + predicted_path.confidence = predicted_path.confidence / sum_confidence; + } + + auto predicted_object = convertToPredictedObject(transformed_object); + for (const auto & predicted_path : predicted_paths) { + predicted_object.kinematics.predicted_paths.push_back(predicted_path); + } + output.objects.push_back(predicted_object); + } + // For unknown object + } else { auto predicted_object = convertToPredictedObject(transformed_object); + PredictedPath predicted_path = + path_generator_->generatePathForNonVehicleObject(transformed_object); + predicted_path.confidence = 1.0; + predicted_object.kinematics.predicted_paths.push_back(predicted_path); output.objects.push_back(predicted_object); - continue; } + } + + // Publish Results + pub_objects_->publish(output); + pub_debug_markers_->publish(debug_markers); +} + +PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( + const TrackedObject & object) +{ + auto predicted_object = convertToPredictedObject(object); + { + PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject(object); + predicted_path.confidence = 1.0; + + predicted_object.kinematics.predicted_paths.push_back(predicted_path); + } + + boost::optional crossing_crosswalk{boost::none}; + for (const auto & crosswalk : crosswalks_) { + if (withinLanelet(object, crosswalk)) { + crossing_crosswalk = crosswalk; + break; + } + } - // Get Predicted Reference Path for Each Maneuver and current lanelets - // return: - const auto ref_paths = - getPredictedReferencePath(transformed_object, current_lanelets, objects_detected_time); + if (crossing_crosswalk) { + const auto entry_point = getCrosswalkEntryPoint(crossing_crosswalk.get()); - // If predicted reference path is empty, assume this object is out of the lane - if (ref_paths.empty()) { + if (hasPotentialToReach( + object, entry_point.first, prediction_time_horizon_, + min_velocity_for_map_based_prediction_)) { PredictedPath predicted_path = - path_generator_->generatePathForLowSpeedVehicle(transformed_object); + path_generator_->generatePathToTargetPoint(object, entry_point.first); predicted_path.confidence = 1.0; - if (predicted_path.path.empty()) { - continue; - } + predicted_object.kinematics.predicted_paths.push_back(predicted_path); + } - auto predicted_object = convertToPredictedObject(transformed_object); + if (hasPotentialToReach( + object, entry_point.second, prediction_time_horizon_, + min_velocity_for_map_based_prediction_)) { + PredictedPath predicted_path = + path_generator_->generatePathToTargetPoint(object, entry_point.second); + predicted_path.confidence = 1.0; predicted_object.kinematics.predicted_paths.push_back(predicted_path); - output.objects.push_back(predicted_object); - continue; } - // Get Debug Marker for On Lane Vehicles - const auto max_prob_path = std::max_element( - ref_paths.begin(), ref_paths.end(), - [](const PredictedRefPath & a, const PredictedRefPath & b) { - return a.probability < b.probability; - }); - const auto debug_marker = - getDebugMarker(object, max_prob_path->maneuver, debug_markers.markers.size()); - debug_markers.markers.push_back(debug_marker); + } else if (withinRoadLanelet(object, lanelet_map_ptr_)) { + for (const auto & crosswalk : crosswalks_) { + const auto entry_point = getCrosswalkEntryPoint(crosswalk); - // Generate Predicted Path - std::vector predicted_paths; - for (const auto & ref_path : ref_paths) { - PredictedPath predicted_path = - path_generator_->generatePathForOnLaneVehicle(transformed_object, ref_path.path); - predicted_path.confidence = ref_path.probability; + if (hasPotentialToReach( + object, entry_point.first, prediction_time_horizon_, + min_velocity_for_map_based_prediction_)) { + PredictedPath predicted_path = + path_generator_->generatePathToTargetPoint(object, entry_point.first); + predicted_path.confidence = 1.0; + predicted_object.kinematics.predicted_paths.push_back(predicted_path); + } - predicted_paths.push_back(predicted_path); + if (hasPotentialToReach( + object, entry_point.second, prediction_time_horizon_, + min_velocity_for_map_based_prediction_)) { + PredictedPath predicted_path = + path_generator_->generatePathToTargetPoint(object, entry_point.second); + predicted_path.confidence = 1.0; + predicted_object.kinematics.predicted_paths.push_back(predicted_path); + } } - // Normalize Path Confidence and output the predicted object - { - float sum_confidence = 0.0; - for (const auto & predicted_path : predicted_paths) { - sum_confidence += predicted_path.confidence; + } else { + for (const auto & crosswalk : crosswalks_) { + const auto entry_point = getCrosswalkEntryPoint(crosswalk); + + const auto reachable_first = hasPotentialToReach( + object, entry_point.first, prediction_time_horizon_, + min_velocity_for_map_based_prediction_); + const auto reachable_second = hasPotentialToReach( + object, entry_point.second, prediction_time_horizon_, + min_velocity_for_map_based_prediction_); + + if (!reachable_first && !reachable_second) { + continue; } - const float min_sum_confidence_value = 1e-3; - sum_confidence = std::max(sum_confidence, min_sum_confidence_value); - for (auto & predicted_path : predicted_paths) { - predicted_path.confidence = predicted_path.confidence / sum_confidence; + const auto reachable_crosswalk = isReachableEntryPoint( + object, entry_point, lanelet_map_ptr_, prediction_time_horizon_, + min_velocity_for_map_based_prediction_); + + if (!reachable_crosswalk) { + continue; } - auto predicted_object = convertToPredictedObject(transformed_object); - for (const auto & predicted_path : predicted_paths) { - predicted_object.kinematics.predicted_paths.push_back(predicted_path); + PredictedPath predicted_path = + path_generator_->generatePathForCrosswalkUser(object, reachable_crosswalk.get()); + predicted_path.confidence = 1.0; + + if (predicted_path.path.empty()) { + continue; } - output.objects.push_back(predicted_object); + + predicted_object.kinematics.predicted_paths.push_back(predicted_path); } } - // Publish Results - pub_objects_->publish(output); - pub_debug_markers_->publish(debug_markers); + const auto n_path = predicted_object.kinematics.predicted_paths.size(); + for (auto & predicted_path : predicted_object.kinematics.predicted_paths) { + predicted_path.confidence = 1.0 / n_path; + } + + return predicted_object; } double MapBasedPredictionNode::getObjectYaw(const TrackedObject & object) diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 9f28f113b194b..4b143e9cd8a29 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -21,8 +21,12 @@ namespace map_based_prediction { -PathGenerator::PathGenerator(const double time_horizon, const double sampling_time_interval) -: time_horizon_(time_horizon), sampling_time_interval_(sampling_time_interval) +PathGenerator::PathGenerator( + const double time_horizon, const double sampling_time_interval, + const double min_velocity_for_map_based_prediction) +: time_horizon_(time_horizon), + sampling_time_interval_(sampling_time_interval), + min_velocity_for_map_based_prediction_(min_velocity_for_map_based_prediction) { } @@ -31,6 +35,90 @@ PredictedPath PathGenerator::generatePathForNonVehicleObject(const TrackedObject return generateStraightPath(object); } +PredictedPath PathGenerator::generatePathToTargetPoint( + const TrackedObject & object, const Eigen::Vector2d & point) const +{ + PredictedPath predicted_path{}; + const double ep = 0.001; + + const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; + const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; + + const Eigen::Vector2d pedestrian_to_entry_point(point.x() - obj_pos.x, point.y() - obj_pos.y); + const auto velocity = + std::max(std::hypot(obj_vel.x, obj_vel.y), min_velocity_for_map_based_prediction_); + const auto arrival_time = pedestrian_to_entry_point.norm() / velocity; + + for (double dt = 0.0; dt < arrival_time + ep; dt += sampling_time_interval_) { + geometry_msgs::msg::Pose world_frame_pose; + world_frame_pose.position.x = + obj_pos.x + velocity * pedestrian_to_entry_point.normalized().x() * dt; + world_frame_pose.position.y = + obj_pos.y + velocity * pedestrian_to_entry_point.normalized().y() * dt; + world_frame_pose.position.z = obj_pos.z; + world_frame_pose.orientation = object.kinematics.pose_with_covariance.pose.orientation; + predicted_path.path.push_back(world_frame_pose); + if (predicted_path.path.size() >= predicted_path.path.max_size()) { + break; + } + } + + predicted_path.confidence = 1.0; + predicted_path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); + + return predicted_path; +} + +PredictedPath PathGenerator::generatePathForCrosswalkUser( + const TrackedObject & object, const EntryPoint & reachable_crosswalk) const +{ + PredictedPath predicted_path{}; + const double ep = 0.001; + + const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; + const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; + + const Eigen::Vector2d pedestrian_to_entry_point( + reachable_crosswalk.first.x() - obj_pos.x, reachable_crosswalk.first.y() - obj_pos.y); + const Eigen::Vector2d entry_to_exit_point( + reachable_crosswalk.second.x() - reachable_crosswalk.first.x(), + reachable_crosswalk.second.y() - reachable_crosswalk.first.y()); + const auto velocity = + std::max(std::hypot(obj_vel.x, obj_vel.y), min_velocity_for_map_based_prediction_); + const auto arrival_time = pedestrian_to_entry_point.norm() / velocity; + + for (double dt = 0.0; dt < time_horizon_ + ep; dt += sampling_time_interval_) { + geometry_msgs::msg::Pose world_frame_pose; + if (dt < arrival_time) { + world_frame_pose.position.x = + obj_pos.x + velocity * pedestrian_to_entry_point.normalized().x() * dt; + world_frame_pose.position.y = + obj_pos.y + velocity * pedestrian_to_entry_point.normalized().y() * dt; + world_frame_pose.position.z = obj_pos.z; + world_frame_pose.orientation = object.kinematics.pose_with_covariance.pose.orientation; + predicted_path.path.push_back(world_frame_pose); + } else { + world_frame_pose.position.x = + reachable_crosswalk.first.x() + + velocity * entry_to_exit_point.normalized().x() * (dt - arrival_time); + world_frame_pose.position.y = + reachable_crosswalk.first.y() + + velocity * entry_to_exit_point.normalized().y() * (dt - arrival_time); + world_frame_pose.position.z = obj_pos.z; + world_frame_pose.orientation = object.kinematics.pose_with_covariance.pose.orientation; + predicted_path.path.push_back(world_frame_pose); + } + if (predicted_path.path.size() >= predicted_path.path.max_size()) { + break; + } + } + + predicted_path.confidence = 1.0; + predicted_path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); + + return predicted_path; +} + PredictedPath PathGenerator::generatePathForLowSpeedVehicle(const TrackedObject & object) const { PredictedPath path;