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

feat(map_based_prediction): use different time horizon #7157

Merged
Show file tree
Hide file tree
Changes from 1 commit
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
@@ -1,7 +1,10 @@
/**:
ros__parameters:
enable_delay_compensation: true
prediction_time_horizon: 10.0 #[s]
prediction_time_horizon:
vehicle: 15.0 #[s]
pedestrian: 10.0 #[s]
unknown: 10.0 #[s]
lateral_control_time_horizon: 5.0 #[s]
prediction_sampling_delta_time: 0.5 #[s]
min_velocity_for_map_based_prediction: 1.39 #[m/s]
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -106,6 +106,13 @@ struct PredictedRefPath
Maneuver maneuver;
};

struct PredictionTimeHorizon
{
double vehicle;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

How about add some comment in this description?
We use "vehicle" prediction in motorcycle and "pedestrian" prediction for bicycle for now and this is not obvious to users.

double pedestrian;
double unknown;
};

using LaneletsData = std::vector<LaneletData>;
using ManeuverProbability = std::unordered_map<Maneuver, float>;
using autoware_auto_mapping_msgs::msg::HADMapBin;
Expand Down Expand Up @@ -170,7 +177,7 @@ class MapBasedPredictionNode : public rclcpp::Node

// Parameters
bool enable_delay_compensation_;
double prediction_time_horizon_;
PredictionTimeHorizon prediction_time_horizon_;
double lateral_control_time_horizon_;
double prediction_time_horizon_rate_for_validate_lane_length_;
double prediction_sampling_time_interval_;
Expand Down Expand Up @@ -253,7 +260,7 @@ class MapBasedPredictionNode : public rclcpp::Node
const std::string & object_id, std::unordered_map<std::string, TrackedObject> & current_users);
std::vector<PredictedRefPath> getPredictedReferencePath(
const TrackedObject & object, const LaneletsData & current_lanelets_data,
const double object_detected_time);
const double object_detected_time, const double time_horizon);
Maneuver predictObjectManeuver(
const TrackedObject & object, const LaneletData & current_lanelet_data,
const double object_detected_time);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -80,21 +80,24 @@ using PosePath = std::vector<geometry_msgs::msg::Pose>;
class PathGenerator
{
public:
PathGenerator(
const double time_horizon, const double lateral_time_horizon,
const double sampling_time_interval, const double min_crosswalk_user_velocity);
PathGenerator(const double sampling_time_interval, const double min_crosswalk_user_velocity);

PredictedPath generatePathForNonVehicleObject(const TrackedObject & object) const;
PredictedPath generatePathForNonVehicleObject(
const TrackedObject & object, const double duration) const;

PredictedPath generatePathForLowSpeedVehicle(const TrackedObject & object) const;
PredictedPath generatePathForLowSpeedVehicle(
const TrackedObject & object, const double duration) const;

PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object) const;
PredictedPath generatePathForOffLaneVehicle(
const TrackedObject & object, const double duration) const;

PredictedPath generatePathForOnLaneVehicle(
const TrackedObject & object, const PosePath & ref_paths, const double speed_limit = 0.0) const;
const TrackedObject & object, const PosePath & ref_paths, const double duration,
const double lateral_duration, const double speed_limit = 0.0) const;

PredictedPath generatePathForCrosswalkUser(
const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const;
const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk,
const double duration) const;

PredictedPath generatePathToTargetPoint(
const TrackedObject & object, const Eigen::Vector2d & point) const;
Expand All @@ -111,22 +114,21 @@ class PathGenerator

private:
// Parameters
double time_horizon_;
double lateral_time_horizon_;
double sampling_time_interval_;
double min_crosswalk_user_velocity_;
bool use_vehicle_acceleration_;
double acceleration_exponential_half_life_;

// Member functions
PredictedPath generateStraightPath(const TrackedObject & object) const;
PredictedPath generateStraightPath(const TrackedObject & object, const double duration) const;

PredictedPath generatePolynomialPath(
const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0) const;
const TrackedObject & object, const PosePath & ref_path, const double duration,
const double lateral_duration, const double speed_limit = 0.0) const;

FrenetPath generateFrenetPath(
const FrenetPoint & current_point, const FrenetPoint & target_point,
const double max_length) const;
const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length,
const double duration, const double lateral_duration) const;
Eigen::Vector3d calcLatCoefficients(
const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) const;
Eigen::Vector2d calcLonCoefficients(
Expand All @@ -140,7 +142,8 @@ class PathGenerator
const PosePath & ref_path) const;

FrenetPoint getFrenetPoint(
const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0) const;
const TrackedObject & object, const PosePath & ref_path, const double duration,
const double speed_limit = 0.0) const;
};
} // namespace map_based_prediction

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,9 +12,23 @@
"description": "flag to enable the time delay compensation for the position of the object"
},
"prediction_time_horizon": {
"type": "number",
"default": "10.0",
"description": "predict time duration for predicted path"
"properties": {
"vehicle": {
"type": "number",
"default": "15.0",
"description": "predict time duration for predicted path of vehicle"
},
"pedestrian": {
"type": "number",
"default": "10.0",
"description": "predict time duration for predicted path of pedestrian"
},
"unknown": {
"type": "number",
"default": "10.0",
"description": "predict time duration for predicted path of unknown"
}
}
},
"lateral_control_time_horizon": {
"type": "number",
Expand Down
57 changes: 31 additions & 26 deletions perception/map_based_prediction/src/map_based_prediction_node.cpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021 Tier IV, Inc.

Check notice on line 1 in perception/map_based_prediction/src/map_based_prediction_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Lines of Code in a Single File

The lines of code increases from 1878 to 1883, improve code health by reducing it to 1000. The number of Lines of Code in a single file. More Lines of Code lowers the code health.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -766,82 +766,84 @@
google::InstallFailureSignalHandler();
}
enable_delay_compensation_ = declare_parameter<bool>("enable_delay_compensation");
prediction_time_horizon_ = declare_parameter<double>("prediction_time_horizon");
prediction_time_horizon_.vehicle = declare_parameter<double>("prediction_time_horizon.vehicle");
prediction_time_horizon_.pedestrian =
declare_parameter<double>("prediction_time_horizon.pedestrian");
prediction_time_horizon_.unknown = declare_parameter<double>("prediction_time_horizon.unknown");
lateral_control_time_horizon_ =
declare_parameter<double>("lateral_control_time_horizon"); // [s] for lateral control point
prediction_sampling_time_interval_ = declare_parameter<double>("prediction_sampling_delta_time");
min_velocity_for_map_based_prediction_ =
declare_parameter<double>("min_velocity_for_map_based_prediction");
min_crosswalk_user_velocity_ = declare_parameter<double>("min_crosswalk_user_velocity");
max_crosswalk_user_delta_yaw_threshold_for_lanelet_ =
declare_parameter<double>("max_crosswalk_user_delta_yaw_threshold_for_lanelet");
dist_threshold_for_searching_lanelet_ =
declare_parameter<double>("dist_threshold_for_searching_lanelet");
delta_yaw_threshold_for_searching_lanelet_ =
declare_parameter<double>("delta_yaw_threshold_for_searching_lanelet");
sigma_lateral_offset_ = declare_parameter<double>("sigma_lateral_offset");
sigma_yaw_angle_deg_ = declare_parameter<double>("sigma_yaw_angle_deg");
object_buffer_time_length_ = declare_parameter<double>("object_buffer_time_length");
history_time_length_ = declare_parameter<double>("history_time_length");

check_lateral_acceleration_constraints_ =
declare_parameter<bool>("check_lateral_acceleration_constraints");
max_lateral_accel_ = declare_parameter<double>("max_lateral_accel");
min_acceleration_before_curve_ = declare_parameter<double>("min_acceleration_before_curve");

{ // lane change detection
lane_change_detection_method_ = declare_parameter<std::string>("lane_change_detection.method");

// lane change detection by time_to_change_lane
dist_threshold_to_bound_ = declare_parameter<double>(
"lane_change_detection.time_to_change_lane.dist_threshold_for_lane_change_detection"); // 1m
time_threshold_to_bound_ = declare_parameter<double>(
"lane_change_detection.time_to_change_lane.time_threshold_for_lane_change_detection");
cutoff_freq_of_velocity_lpf_ = declare_parameter<double>(
"lane_change_detection.time_to_change_lane.cutoff_freq_of_velocity_for_lane_change_"
"detection");

// lane change detection by lat_diff_distance
dist_ratio_threshold_to_left_bound_ = declare_parameter<double>(
"lane_change_detection.lat_diff_distance.dist_ratio_threshold_to_left_bound");
dist_ratio_threshold_to_right_bound_ = declare_parameter<double>(
"lane_change_detection.lat_diff_distance.dist_ratio_threshold_to_right_bound");
diff_dist_threshold_to_left_bound_ = declare_parameter<double>(
"lane_change_detection.lat_diff_distance.diff_dist_threshold_to_left_bound");
diff_dist_threshold_to_right_bound_ = declare_parameter<double>(
"lane_change_detection.lat_diff_distance.diff_dist_threshold_to_right_bound");

num_continuous_state_transition_ =
declare_parameter<int>("lane_change_detection.num_continuous_state_transition");

consider_only_routable_neighbours_ =
declare_parameter<bool>("lane_change_detection.consider_only_routable_neighbours");
}
reference_path_resolution_ = declare_parameter<double>("reference_path_resolution");
/* prediction path will disabled when the estimated path length exceeds lanelet length. This
* parameter control the estimated path length = vx * th * (rate) */
prediction_time_horizon_rate_for_validate_lane_length_ =
declare_parameter<double>("prediction_time_horizon_rate_for_validate_shoulder_lane_length");
match_lost_and_appeared_crosswalk_users_ =
declare_parameter<bool>("use_crosswalk_user_history.match_lost_and_appeared_users");
remember_lost_crosswalk_users_ =
declare_parameter<bool>("use_crosswalk_user_history.remember_lost_users");
use_vehicle_acceleration_ = declare_parameter<bool>("use_vehicle_acceleration");
speed_limit_multiplier_ = declare_parameter<double>("speed_limit_multiplier");
acceleration_exponential_half_life_ =
declare_parameter<double>("acceleration_exponential_half_life");

use_crosswalk_signal_ = declare_parameter<bool>("crosswalk_with_signal.use_crosswalk_signal");
threshold_velocity_assumed_as_stopping_ =
declare_parameter<double>("crosswalk_with_signal.threshold_velocity_assumed_as_stopping");
distance_set_for_no_intention_to_walk_ = declare_parameter<std::vector<double>>(
"crosswalk_with_signal.distance_set_for_no_intention_to_walk");
timeout_set_for_no_intention_to_walk_ = declare_parameter<std::vector<double>>(
"crosswalk_with_signal.timeout_set_for_no_intention_to_walk");

path_generator_ = std::make_shared<PathGenerator>(
prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_,
min_crosswalk_user_velocity_);
prediction_sampling_time_interval_, min_crosswalk_user_velocity_);

Check warning on line 846 in perception/map_based_prediction/src/map_based_prediction_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Large Method

MapBasedPredictionNode::MapBasedPredictionNode increases from 95 to 97 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.

path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_);
path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_);
Expand Down Expand Up @@ -1050,141 +1052,143 @@

// For off lane obstacles
if (current_lanelets.empty()) {
PredictedPath predicted_path =
path_generator_->generatePathForOffLaneVehicle(transformed_object);
PredictedPath predicted_path = path_generator_->generatePathForOffLaneVehicle(
transformed_object, prediction_time_horizon_.vehicle);
predicted_path.confidence = 1.0;
if (predicted_path.path.empty()) break;

auto predicted_object_vehicle = convertToPredictedObject(transformed_object);
predicted_object_vehicle.kinematics.predicted_paths.push_back(predicted_path);
output.objects.push_back(predicted_object_vehicle);
break;
}

// For too-slow vehicle
const double abs_obj_speed = std::hypot(
transformed_object.kinematics.twist_with_covariance.twist.linear.x,
transformed_object.kinematics.twist_with_covariance.twist.linear.y);
if (std::fabs(abs_obj_speed) < min_velocity_for_map_based_prediction_) {
PredictedPath predicted_path =
path_generator_->generatePathForLowSpeedVehicle(transformed_object);
PredictedPath predicted_path = path_generator_->generatePathForLowSpeedVehicle(
transformed_object, prediction_time_horizon_.vehicle);
predicted_path.confidence = 1.0;
if (predicted_path.path.empty()) break;

auto predicted_slow_object = convertToPredictedObject(transformed_object);
predicted_slow_object.kinematics.predicted_paths.push_back(predicted_path);
output.objects.push_back(predicted_slow_object);
break;
}

// Get Predicted Reference Path for Each Maneuver and current lanelets
// return: <probability, paths>
const auto ref_paths =
getPredictedReferencePath(transformed_object, current_lanelets, objects_detected_time);
const auto ref_paths = getPredictedReferencePath(
transformed_object, current_lanelets, objects_detected_time,
prediction_time_horizon_.vehicle);

// 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);
PredictedPath predicted_path = path_generator_->generatePathForLowSpeedVehicle(
transformed_object, prediction_time_horizon_.vehicle);
predicted_path.confidence = 1.0;
if (predicted_path.path.empty()) break;

auto predicted_object_out_of_lane = convertToPredictedObject(transformed_object);
predicted_object_out_of_lane.kinematics.predicted_paths.push_back(predicted_path);
output.objects.push_back(predicted_object_out_of_lane);
break;
}

// 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);

// Fix object angle if its orientation unreliable (e.g. far object by radar sensor)
// This prevent bending predicted path
TrackedObject yaw_fixed_transformed_object = transformed_object;
if (
transformed_object.kinematics.orientation_availability ==
autoware_auto_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE) {
replaceObjectYawWithLaneletsYaw(current_lanelets, yaw_fixed_transformed_object);
}
// Generate Predicted Path
std::vector<PredictedPath> predicted_paths;
double min_avg_curvature = std::numeric_limits<double>::max();
PredictedPath path_with_smallest_avg_curvature;

for (const auto & ref_path : ref_paths) {
PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle(
yaw_fixed_transformed_object, ref_path.path, ref_path.speed_limit);
yaw_fixed_transformed_object, ref_path.path, prediction_time_horizon_.vehicle,
lateral_control_time_horizon_, ref_path.speed_limit);
if (predicted_path.path.empty()) continue;

if (!check_lateral_acceleration_constraints_) {
predicted_path.confidence = ref_path.probability;
predicted_paths.push_back(predicted_path);
continue;
}

// Check lat. acceleration constraints
const auto trajectory_with_const_velocity =
toTrajectoryPoints(predicted_path, abs_obj_speed);

if (isLateralAccelerationConstraintSatisfied(
trajectory_with_const_velocity, prediction_sampling_time_interval_)) {
predicted_path.confidence = ref_path.probability;
predicted_paths.push_back(predicted_path);
continue;
}

// Calculate curvature assuming the trajectory points interval is constant
// In case all paths are deleted, a copy of the straightest path is kept

constexpr double curvature_calculation_distance = 2.0;
constexpr double points_interval = 1.0;
const size_t idx_dist = static_cast<size_t>(
std::max(static_cast<int>((curvature_calculation_distance) / points_interval), 1));
const auto curvature_v =
calcTrajectoryCurvatureFrom3Points(trajectory_with_const_velocity, idx_dist);
if (curvature_v.empty()) {
continue;
}
const auto curvature_avg =
std::accumulate(curvature_v.begin(), curvature_v.end(), 0.0) / curvature_v.size();
if (curvature_avg < min_avg_curvature) {
min_avg_curvature = curvature_avg;
path_with_smallest_avg_curvature = predicted_path;
path_with_smallest_avg_curvature.confidence = ref_path.probability;
}
}

if (predicted_paths.empty()) predicted_paths.push_back(path_with_smallest_avg_curvature);
// 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);

auto predicted_object = convertToPredictedObject(transformed_object);

for (auto & predicted_path : predicted_paths) {
predicted_path.confidence = predicted_path.confidence / sum_confidence;
if (predicted_object.kinematics.predicted_paths.size() >= 100) break;
predicted_object.kinematics.predicted_paths.push_back(predicted_path);
}
output.objects.push_back(predicted_object);
break;
}
default: {
auto predicted_unknown_object = convertToPredictedObject(transformed_object);
PredictedPath predicted_path =
path_generator_->generatePathForNonVehicleObject(transformed_object);
PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject(
transformed_object, prediction_time_horizon_.unknown);

Check warning on line 1191 in perception/map_based_prediction/src/map_based_prediction_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

MapBasedPredictionNode::objectsCallback already has high cyclomatic complexity, and now it increases in Lines of Code from 215 to 217. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
predicted_path.confidence = 1.0;

predicted_unknown_object.kinematics.predicted_paths.push_back(predicted_path);
Expand Down Expand Up @@ -1337,112 +1341,113 @@
{
auto predicted_object = convertToPredictedObject(object);
{
PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject(object);
PredictedPath predicted_path =
path_generator_->generatePathForNonVehicleObject(object, prediction_time_horizon_.pedestrian);
predicted_path.confidence = 1.0;

predicted_object.kinematics.predicted_paths.push_back(predicted_path);
}

boost::optional<lanelet::ConstLanelet> crossing_crosswalk{boost::none};
for (const auto & crosswalk : crosswalks_) {
if (withinLanelet(object, crosswalk)) {
crossing_crosswalk = crosswalk;
break;
}
}

// If the object is in the crosswalk, generate path to the crosswalk edge
if (crossing_crosswalk) {
const auto edge_points = getCrosswalkEdgePoints(crossing_crosswalk.get());

if (hasPotentialToReach(
object, edge_points.front_center_point, edge_points.front_right_point,
edge_points.front_left_point, std::numeric_limits<double>::max(),
min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) {
PredictedPath predicted_path =
path_generator_->generatePathToTargetPoint(object, edge_points.front_center_point);
predicted_path.confidence = 1.0;
predicted_object.kinematics.predicted_paths.push_back(predicted_path);
}

if (hasPotentialToReach(
object, edge_points.back_center_point, edge_points.back_right_point,
edge_points.back_left_point, std::numeric_limits<double>::max(),
min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) {
PredictedPath predicted_path =
path_generator_->generatePathToTargetPoint(object, edge_points.back_center_point);
predicted_path.confidence = 1.0;
predicted_object.kinematics.predicted_paths.push_back(predicted_path);
}

// If the object is not crossing the crosswalk, in the road lanelets, try to find the closest
// crosswalk and generate path to the crosswalk edge
} else if (withinRoadLanelet(object, lanelet_map_ptr_)) {
lanelet::ConstLanelet closest_crosswalk{};
const auto & obj_pose = object.kinematics.pose_with_covariance.pose;
const auto found_closest_crosswalk =
lanelet::utils::query::getClosestLanelet(crosswalks_, obj_pose, &closest_crosswalk);

if (found_closest_crosswalk) {
const auto edge_points = getCrosswalkEdgePoints(closest_crosswalk);

if (hasPotentialToReach(
object, edge_points.front_center_point, edge_points.front_right_point,
edge_points.front_left_point, prediction_time_horizon_ * 2.0,
edge_points.front_left_point, prediction_time_horizon_.pedestrian * 2.0,
min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) {
PredictedPath predicted_path =
path_generator_->generatePathToTargetPoint(object, edge_points.front_center_point);
predicted_path.confidence = 1.0;
predicted_object.kinematics.predicted_paths.push_back(predicted_path);
}

if (hasPotentialToReach(
object, edge_points.back_center_point, edge_points.back_right_point,
edge_points.back_left_point, prediction_time_horizon_ * 2.0,
edge_points.back_left_point, prediction_time_horizon_.pedestrian * 2.0,
min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) {
PredictedPath predicted_path =
path_generator_->generatePathToTargetPoint(object, edge_points.back_center_point);
predicted_path.confidence = 1.0;
predicted_object.kinematics.predicted_paths.push_back(predicted_path);
}
}
}

// try to find the edge points for all crosswalks and generate path to the crosswalk edge
for (const auto & crosswalk : crosswalks_) {
const auto crosswalk_signal_id_opt = getTrafficSignalId(crosswalk);
if (crosswalk_signal_id_opt.has_value() && use_crosswalk_signal_) {
if (!calcIntentionToCrossWithTrafficSignal(
object, crosswalk, crosswalk_signal_id_opt.value())) {
continue;
}
}

const auto edge_points = getCrosswalkEdgePoints(crosswalk);

const auto reachable_first = hasPotentialToReach(
object, edge_points.front_center_point, edge_points.front_right_point,
edge_points.front_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_,
max_crosswalk_user_delta_yaw_threshold_for_lanelet_);
edge_points.front_left_point, prediction_time_horizon_.pedestrian,
min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_);
const auto reachable_second = hasPotentialToReach(
object, edge_points.back_center_point, edge_points.back_right_point,
edge_points.back_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_,
max_crosswalk_user_delta_yaw_threshold_for_lanelet_);
edge_points.back_left_point, prediction_time_horizon_.pedestrian,
min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_);

if (!reachable_first && !reachable_second) {
continue;
}

const auto reachable_crosswalk = isReachableCrosswalkEdgePoints(
object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_,
object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_.pedestrian,
min_crosswalk_user_velocity_);

if (!reachable_crosswalk) {
continue;
}

PredictedPath predicted_path =
path_generator_->generatePathForCrosswalkUser(object, reachable_crosswalk.get());
PredictedPath predicted_path = path_generator_->generatePathForCrosswalkUser(
object, reachable_crosswalk.get(), prediction_time_horizon_.pedestrian);

Check warning on line 1450 in perception/map_based_prediction/src/map_based_prediction_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser already has high cyclomatic complexity, and now it increases in Lines of Code from 106 to 107. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
predicted_path.confidence = 1.0;

if (predicted_path.path.empty()) {
Expand Down Expand Up @@ -1740,7 +1745,7 @@

std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
const TrackedObject & object, const LaneletsData & current_lanelets_data,
const double object_detected_time)
const double object_detected_time, const double time_horizon)
{
const double obj_vel = std::hypot(
object.kinematics.twist_with_covariance.twist.linear.x,
Expand All @@ -1752,7 +1757,7 @@
object.kinematics.acceleration_with_covariance.accel.linear.x,
object.kinematics.acceleration_with_covariance.accel.linear.y)
: 0.0;
const double t_h = prediction_time_horizon_;
const double t_h = time_horizon;
const double λ = std::log(2) / acceleration_exponential_half_life_;

auto get_search_distance_with_decaying_acc = [&]() -> double {
Expand Down
Loading
Loading