diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index 45d420bafff10..9944cbb84d97c 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -43,11 +43,11 @@ function(add_testcase filepath) endfunction() if(BUILD_TESTING) - add_ros_test( + add_launch_test( test/test_ekf_localizer_launch.py TIMEOUT "30" ) - add_ros_test( + add_launch_test( test/test_ekf_localizer_mahalanobis.py TIMEOUT "30" ) diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 6fe61cd25bc6e..4beecc2625fe3 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -38,7 +38,7 @@ link_directories(${PCL_LIBRARY_DIRS}) target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES} glog::glog) if(BUILD_TESTING) - add_ros_test( + add_launch_test( test/test_ndt_scan_matcher_launch.py TIMEOUT "30" ) diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index b94eaac7ee34d..115f7e5b17464 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -46,7 +46,7 @@ rclcpp_components_register_node(lanelet2_map_visualization_node ) if(BUILD_TESTING) - add_ros_test( + add_launch_test( test/lanelet2_map_loader_launch.test.py TIMEOUT "30" ) diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index 16459bb64074b..cb65e2a3cc23c 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -86,6 +86,14 @@ The parameters `collision_detection.collision_start_margin_time` and `collision_ If collision is detected, the state transits to "STOP" immediately. On the other hand, the state does not transit to "GO" unless safe judgement continues for a certain period `collision_detection.state_transit_margin` to prevent the chattering of decisions. +Currently, the intersection module uses `motion_velocity_smoother` feature to precisely calculate ego vehicle velocity profile along the intersection lane under longitudinal/lateral constraints. If the flag `collision_detection.use_upstream_velocity` is true, the target velocity profile of the original path is used. Otherwise the target velocity is set to `common.intersection_velocity`. In the trajectory smoothing process the target velocity at/before ego trajectory points are set to ego current velocity. The smoothed trajectory is then converted to an array of (time, distance) which indicates the arrival time to each trajectory point on the path from current ego position. You can visualize this array by adding the lane id to `debug.ttc` and running + +```bash +ros2 run behavior_velocity_intersection_module ttc.py --lane_id +``` + +![ego ttc profile](./docs/ttc.gif) + #### Stop Line Automatic Generation If a stopline is associated with the intersection lane on the map, that line is used as the stopline for collision detection. Otherwise the path is interpolated at a certain intervals (=`common.path_interpolation_ds`), and the point which is `stop_line_margin` meters behind the attention area is defined as the position of the stop line for the vehicle front. diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index adbc7b3e087d6..22f68733a3bc2 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -35,24 +35,24 @@ distance_thr: 1.0 # [m] collision_detection: - state_transit_margin_time: 1.0 + state_transit_margin_time: 0.5 min_predicted_path_confidence: 0.05 minimum_ego_predicted_velocity: 1.388 # [m/s] fully_prioritized: collision_start_margin_time: 2.0 collision_end_margin_time: 0.0 partially_prioritized: - collision_start_margin_time: 2.0 + collision_start_margin_time: 3.0 collision_end_margin_time: 2.0 not_prioritized: - collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object - collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object + collision_start_margin_time: 3.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object + collision_end_margin_time: 2.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity yield_on_green_traffic_light: - distance_to_assigned_lanelet_start: 5.0 - duration: 2.0 + distance_to_assigned_lanelet_start: 10.0 + duration: 3.0 object_dist_to_stopline: 10.0 # [m] ignore_on_amber_traffic_light: object_expected_deceleration: 2.0 # [m/ss] @@ -81,7 +81,7 @@ maximum_peeking_distance: 6.0 # [m] attention_lane_crop_curvature_threshold: 0.25 attention_lane_curvature_calculation_ds: 0.5 - static_occlusion_with_traffic_light_timeout: 3.5 + static_occlusion_with_traffic_light_timeout: 0.5 debug: ttc: [0] diff --git a/planning/behavior_velocity_intersection_module/docs/ttc.gif b/planning/behavior_velocity_intersection_module/docs/ttc.gif new file mode 100644 index 0000000000000..bb797f7df59a1 Binary files /dev/null and b/planning/behavior_velocity_intersection_module/docs/ttc.gif differ diff --git a/planning/behavior_velocity_intersection_module/scripts/ttc.py b/planning/behavior_velocity_intersection_module/scripts/ttc.py index e4633981570d1..6d1593d95f055 100755 --- a/planning/behavior_velocity_intersection_module/scripts/ttc.py +++ b/planning/behavior_velocity_intersection_module/scripts/ttc.py @@ -126,8 +126,11 @@ def plot_ttc(self): self.ttc_ax.set_xlabel("ego time") self.ttc_ax.set_ylabel("ego dist") time_dist_plot = self.ttc_ax.plot(ego_ttc_time, ego_ttc_dist, label="time-dist", c="orange") - self.ttc_ax.set_xlim(min(ego_ttc_time) - 2.0, max(ego_ttc_time) + 3.0) - self.ttc_ax.set_ylim(min(ego_ttc_dist) - 2.0, max(ego_ttc_dist) + 3.0) + self.ttc_ax.set_xlim( + min(ego_ttc_time) - 2.0, + min(max(ego_ttc_time) + 3.0, self.args.max_time), + ) + # self.ttc_ax.set_ylim(min(ego_ttc_dist) - 2.0, max(ego_ttc_dist) + 3.0) for npc, color in zip(self.npc_vehicles, cycle(self.color_list)): t0, t1 = npc.collision_start_time, npc.collision_end_time d0, d1 = npc.collision_start_dist, npc.collision_end_dist @@ -137,15 +140,13 @@ def plot_ttc(self): c=color, alpha=0.2, ) - dd = [d1 - d0 for d0, d1 in zip(ego_ttc_dist, ego_ttc_dist[1:])] dt = [t1 - t0 for t0, t1 in zip(ego_ttc_time, ego_ttc_time[1:])] v = [d / t for d, t in zip(dd, dt)] self.ttc_vel_ax.yaxis.set_label_position("right") self.ttc_vel_ax.set_ylabel("ego velocity") - self.ttc_vel_ax.set_ylim(0.0, max(v) + 1.0) + # self.ttc_vel_ax.set_ylim(0.0, max(v) + 1.0) time_velocity_plot = self.ttc_vel_ax.plot(ego_ttc_time[1:], v, label="time-v", c="red") - lines = time_dist_plot + time_velocity_plot labels = [line.get_label() for line in lines] self.ttc_ax.legend(lines, labels, loc="upper left") @@ -218,6 +219,7 @@ def cleanup(self): if self.args.save: kwargs_write = {"fps": self.args.fps, "quantizer": "nq"} imageio.mimsave("./" + self.args.gif + ".gif", self.images, **kwargs_write) + rclpy.shutdown() def on_plot_timer(self): with self.lock: @@ -277,6 +279,7 @@ def on_object_ttc(self, msg): default=60, help="detect range for drawing", ) + parser.add_argument("--max_time", type=float, default=100, help="max plot limit for time") parser.add_argument("-s", "--save", action="store_true", help="flag to save gif") parser.add_argument("--gif", type=str, default="ttc", help="filename of gif file") parser.add_argument("--fps", type=float, default=5, help="fps of gif") diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index a9d0f56266181..e6e6fa0a1da9a 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -1304,14 +1305,27 @@ TimeDistanceArray calcIntersectionPassingTime( const bool use_upstream_velocity, const double minimum_upstream_velocity, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) { - double dist_sum = 0.0; + const double current_velocity = planner_data->current_velocity->twist.linear.x; + int assigned_lane_found = false; // crop intersection part of the path, and set the reference velocity to intersection_velocity // for ego's ttc PathWithLaneId reference_path; - for (size_t i = closest_idx; i < path.points.size(); ++i) { + std::optional upstream_stop_line{std::nullopt}; + for (size_t i = 0; i < path.points.size() - 1; ++i) { auto reference_point = path.points.at(i); + // assume backward velocity is current ego velocity + if (i < closest_idx) { + reference_point.point.longitudinal_velocity_mps = current_velocity; + } + if ( + i > last_intersection_stop_line_candidate_idx && + std::fabs(reference_point.point.longitudinal_velocity_mps) < + std::numeric_limits::epsilon() && + !upstream_stop_line) { + upstream_stop_line = i; + } if (!use_upstream_velocity) { reference_point.point.longitudinal_velocity_mps = intersection_velocity; } @@ -1326,28 +1340,46 @@ TimeDistanceArray calcIntersectionPassingTime( return {{0.0, 0.0}}; // has already passed the intersection. } + std::vector> original_path_xy; + for (size_t i = 0; i < reference_path.points.size(); ++i) { + const auto & p = reference_path.points.at(i).point.pose.position; + original_path_xy.emplace_back(p.x, p.y); + } + // apply smoother to reference velocity PathWithLaneId smoothed_reference_path = reference_path; - smoothPath(reference_path, smoothed_reference_path, planner_data); + if (!smoothPath(reference_path, smoothed_reference_path, planner_data)) { + smoothed_reference_path = reference_path; + } // calculate when ego is going to reach each (interpolated) points on the path TimeDistanceArray time_distance_array{}; - dist_sum = 0.0; + double dist_sum = 0.0; double passing_time = time_delay; time_distance_array.emplace_back(passing_time, dist_sum); // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so // `last_intersection_stop_line_candidate_idx` makes no sense - const auto last_intersection_stop_line_candidate_point_orig = - path.points.at(last_intersection_stop_line_candidate_idx).point.pose; - const auto last_intersection_stop_line_candidate_nearest_ind = - motion_utils::findFirstNearestIndexWithSoftConstraints( - smoothed_reference_path.points, last_intersection_stop_line_candidate_point_orig, - planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); + const auto smoothed_path_closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + smoothed_reference_path.points, path.points.at(closest_idx).point.pose, + planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); + + const std::optional upstream_stop_line_idx_opt = [&]() -> std::optional { + if (upstream_stop_line) { + const auto upstream_stop_line_point = path.points.at(upstream_stop_line.value()).point.pose; + return motion_utils::findFirstNearestIndexWithSoftConstraints( + smoothed_reference_path.points, upstream_stop_line_point, + planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); + } else { + return std::nullopt; + } + }(); + const bool has_upstream_stopline = upstream_stop_line_idx_opt.has_value(); + const size_t upstream_stopline_ind = upstream_stop_line_idx_opt.value_or(0); - for (size_t i = 1; i < smoothed_reference_path.points.size(); ++i) { - const auto & p1 = smoothed_reference_path.points.at(i - 1); - const auto & p2 = smoothed_reference_path.points.at(i); + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size() - 1; ++i) { + const auto & p1 = smoothed_reference_path.points.at(i); + const auto & p2 = smoothed_reference_path.points.at(i + 1); const double dist = tier4_autoware_utils::calcDistance2d(p1, p2); dist_sum += dist; @@ -1355,12 +1387,16 @@ TimeDistanceArray calcIntersectionPassingTime( // use average velocity between p1 and p2 const double average_velocity = (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; - const double minimum_ego_velocity_division = - (use_upstream_velocity && i > last_intersection_stop_line_candidate_nearest_ind) - ? minimum_upstream_velocity /* to avoid null division */ - : minimum_ego_velocity; - const double passing_velocity = - std::max(minimum_ego_velocity_division, average_velocity); + const double passing_velocity = [=]() { + if (use_upstream_velocity) { + if (has_upstream_stopline && i > upstream_stopline_ind) { + return minimum_upstream_velocity; + } + return std::max(average_velocity, minimum_ego_velocity); + } else { + return std::max(average_velocity, minimum_ego_velocity); + } + }(); passing_time += (dist / passing_velocity); time_distance_array.emplace_back(passing_time, dist_sum); @@ -1370,6 +1406,8 @@ TimeDistanceArray calcIntersectionPassingTime( debug_ttc_array->layout.dim.at(0).size = 5; debug_ttc_array->layout.dim.at(1).label = "values"; debug_ttc_array->layout.dim.at(1).size = time_distance_array.size(); + debug_ttc_array->data.reserve( + time_distance_array.size() * debug_ttc_array->layout.dim.at(0).size); for (unsigned i = 0; i < time_distance_array.size(); ++i) { debug_ttc_array->data.push_back(lane_id); } @@ -1379,11 +1417,13 @@ TimeDistanceArray calcIntersectionPassingTime( for (const auto & [t, d] : time_distance_array) { debug_ttc_array->data.push_back(d); } - for (const auto & p : smoothed_reference_path.points) { - debug_ttc_array->data.push_back(p.point.pose.position.x); + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { + const auto & p = smoothed_reference_path.points.at(i).point.pose.position; + debug_ttc_array->data.push_back(p.x); } - for (const auto & p : smoothed_reference_path.points) { - debug_ttc_array->data.push_back(p.point.pose.position.y); + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { + const auto & p = smoothed_reference_path.points.at(i).point.pose.position; + debug_ttc_array->data.push_back(p.y); } return time_distance_array; } diff --git a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp index 71cdd9650a598..7fe8612cc6858 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -88,15 +88,14 @@ bool smoothPath( const auto & smoother = planner_data->velocity_smoother_; auto trajectory = convertPathToTrajectoryPoints(in_path); - if (external_v_limit) { - motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( - 0, trajectory.size(), external_v_limit->max_velocity, trajectory); - } const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory); + const auto traj_steering_rate_limited = + smoother->applySteeringRateLimit(traj_lateral_acc_filtered, false); + // Resample trajectory with ego-velocity based interval distances auto traj_resampled = smoother->resampleTrajectory( - traj_lateral_acc_filtered, v0, current_pose, planner_data->ego_nearest_dist_threshold, + traj_steering_rate_limited, v0, current_pose, planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); const size_t traj_resampled_closest = motion_utils::findFirstNearestIndexWithSoftConstraints( traj_resampled, current_pose, planner_data->ego_nearest_dist_threshold, @@ -114,6 +113,10 @@ bool smoothPath( traj_smoothed.insert( traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); + if (external_v_limit) { + motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( + traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); + } out_path = convertTrajectoryPointsToPath(traj_smoothed); return true; }