feat(intersection): use upstream velocity

Signed-off-by: Mamoru Sobue <[email protected]>
autowarefoundation · Sep 26, 2023 · 95f96a4 · 95f96a4
1 parent 16dc535
commit 95f96a4
Show file tree

Hide file tree

Showing 6 changed files with 11 additions and 4 deletions.
diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
@@ -34,6 +34,7 @@
           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
         keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr
+        use_upstream_velocity: false # flag to use the planned velocity profile from the upstream module
 
       occlusion:
         enable: false

diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp
@@ -103,6 +103,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     node, ns + ".collision_detection.not_prioritized.collision_end_margin_time");
   ip.collision_detection.keep_detection_vel_thr =
     getOrDeclareParameter<double>(node, ns + ".collision_detection.keep_detection_vel_thr");
+  ip.collision_detection.use_upstream_velocity =
+    getOrDeclareParameter<bool>(node, ns + ".collision_detection.use_upstream_velocity");
 
   ip.occlusion.enable = getOrDeclareParameter<bool>(node, ns + ".occlusion.enable");
   ip.occlusion.occlusion_attention_area_length =

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -1108,7 +1108,8 @@ bool IntersectionModule::checkCollision(
   const auto time_distance_array = util::calcIntersectionPassingTime(
     path, planner_data_, associative_ids_, closest_idx, time_delay,
     planner_param_.common.intersection_velocity,
-    planner_param_.collision_detection.minimum_ego_predicted_velocity);
+    planner_param_.collision_detection.minimum_ego_predicted_velocity,
+    planner_param_.collision_detection.use_upstream_velocity);
   const double passing_time = time_distance_array.back().first;
   auto target_objects = objects;
   util::cutPredictPathWithDuration(&target_objects, clock_, passing_time);

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
@@ -95,6 +95,7 @@ class IntersectionModule : public SceneModuleInterface
         double collision_end_margin_time;    //! end margin time to check collision
       } not_prioritized;
       double keep_detection_vel_thr;  //! keep detection if ego is ego.vel < keep_detection_vel_thr
+      bool use_upstream_velocity;
     } collision_detection;
     struct Occlusion
     {

diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp
@@ -1109,7 +1109,7 @@ TimeDistanceArray calcIntersectionPassingTime(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const std::shared_ptr<const PlannerData> & planner_data, const std::set<int> & associative_ids,
   const int closest_idx, const double time_delay, const double intersection_velocity,
-  const double minimum_ego_velocity)
+  const double minimum_ego_velocity, const bool use_upstream_velocity)
 {
   double dist_sum = 0.0;
   int assigned_lane_found = false;
@@ -1119,7 +1119,9 @@ TimeDistanceArray calcIntersectionPassingTime(
   PathWithLaneId reference_path;
   for (size_t i = closest_idx; i < path.points.size(); ++i) {
     auto reference_point = path.points.at(i);
-    reference_point.point.longitudinal_velocity_mps = intersection_velocity;
+    if (!use_upstream_velocity) {
+      reference_point.point.longitudinal_velocity_mps = intersection_velocity;
+    }
     reference_path.points.push_back(reference_point);
     bool has_objective_lane_id = hasLaneIds(path.points.at(i), associative_ids);
     if (assigned_lane_found && !has_objective_lane_id) {

diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp
@@ -148,7 +148,7 @@ TimeDistanceArray calcIntersectionPassingTime(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const std::shared_ptr<const PlannerData> & planner_data, const std::set<int> & associative_ids,
   const int closest_idx, const double time_delay, const double intersection_velocity,
-  const double minimum_ego_velocity);
+  const double minimum_ego_velocity, const bool use_upstream_velocity);
 
 double calcDistanceUntilIntersectionLanelet(
   const lanelet::ConstLanelet & assigned_lanelet,