feat(lane_change): use planning factor (#1727)

* feat(lane_change): use planning factor Signed-off-by: satoshi-ota <[email protected]> * feat(external_request_lane_change): use planning factor Signed-off-by: satoshi-ota <[email protected]> * feat(AbLC): use planning factor Signed-off-by: satoshi-ota <[email protected]> --------- Signed-off-by: satoshi-ota <[email protected]>
tier4 · Jan 15, 2025 · 1dcbb4e · 1dcbb4e
1 parent a1a65f4
commit 1dcbb4e
Show file tree

Hide file tree

Showing 7 changed files with 42 additions and 7 deletions.
diff --git a/...ior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/...ior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp
@@ -32,13 +32,15 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface(
   const std::shared_ptr<AvoidanceByLCParameters> & avoidance_by_lane_change_parameters,
   const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
   std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
-    objects_of_interest_marker_interface_ptr_map)
+    objects_of_interest_marker_interface_ptr_map,
+  const std::shared_ptr<PlanningFactorInterface> & planning_factor_interface)
 : LaneChangeInterface{
     name,
     node,
     parameters,
     rtc_interface_ptr_map,
     objects_of_interest_marker_interface_ptr_map,
+    planning_factor_interface,
     std::make_unique<AvoidanceByLaneChange>(parameters, avoidance_by_lane_change_parameters)}
 {
 }

diff --git a/...ior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp b/...ior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp
@@ -40,7 +40,8 @@ class AvoidanceByLaneChangeInterface : public LaneChangeInterface
     const std::shared_ptr<AvoidanceByLCParameters> & avoidance_by_lane_change_parameters,
     const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
     std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
-      objects_of_interest_marker_interface_ptr_map);
+      objects_of_interest_marker_interface_ptr_map,
+    const std::shared_ptr<PlanningFactorInterface> & planning_factor_interface);
 
   bool isExecutionRequested() const override;
 

diff --git a/...avior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/...avior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp
@@ -190,7 +190,7 @@ SMIPtr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance()
 {
   return std::make_unique<AvoidanceByLaneChangeInterface>(
     name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_,
-    objects_of_interest_marker_interface_ptr_map_);
+    objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_);
 }
 
 }  // namespace autoware::behavior_path_planner

diff --git a/...r_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp b/...r_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp
@@ -28,7 +28,7 @@ ExternalRequestLaneChangeRightModuleManager::createNewSceneModuleInstance()
 {
   return std::make_unique<LaneChangeInterface>(
     name_, *node_, parameters_, rtc_interface_ptr_map_,
-    objects_of_interest_marker_interface_ptr_map_,
+    objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_,
     std::make_unique<ExternalRequestLaneChange>(parameters_, direction_));
 }
 
@@ -37,7 +37,7 @@ ExternalRequestLaneChangeLeftModuleManager::createNewSceneModuleInstance()
 {
   return std::make_unique<LaneChangeInterface>(
     name_, *node_, parameters_, rtc_interface_ptr_map_,
-    objects_of_interest_marker_interface_ptr_map_,
+    objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_,
     std::make_unique<ExternalRequestLaneChange>(parameters_, direction_));
 }
 

diff --git a/...r_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/...r_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp
@@ -53,6 +53,7 @@ class LaneChangeInterface : public SceneModuleInterface
     const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
     std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
       objects_of_interest_marker_interface_ptr_map,
+    const std::shared_ptr<PlanningFactorInterface> & planning_factor_interface,
     std::unique_ptr<LaneChangeBase> && module_type);
 
   LaneChangeInterface(const LaneChangeInterface &) = delete;

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp
@@ -40,8 +40,9 @@ LaneChangeInterface::LaneChangeInterface(
   const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
   std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
     objects_of_interest_marker_interface_ptr_map,
+  const std::shared_ptr<PlanningFactorInterface> & planning_factor_interface,
   std::unique_ptr<LaneChangeBase> && module_type)
-: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map},  // NOLINT
+: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface},  // NOLINT
   parameters_{std::move(parameters)},
   module_type_{std::move(module_type)}
 {
@@ -145,6 +146,8 @@ BehaviorModuleOutput LaneChangeInterface::plan()
     }
   }
 
+  set_longitudinal_planning_factor(output.path);
+
   setVelocityFactor(output.path);
 
   return output;
@@ -179,6 +182,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
     isExecutionReady(), State::WAITING_FOR_EXECUTION);
   is_abort_path_approved_ = false;
 
+  set_longitudinal_planning_factor(out.path);
+
   setVelocityFactor(out.path);
 
   return out;
@@ -404,6 +409,20 @@ void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & o
   steering_factor_interface_.set(
     {status.lane_change_path.info.shift_line.start, status.lane_change_path.info.shift_line.end},
     {start_distance, finish_distance}, steering_factor_direction, SteeringFactor::TURNING, "");
+
+  const auto planning_factor_direction = std::invoke([&]() {
+    if (module_type_->getDirection() == Direction::LEFT) {
+      return PlanningFactor::SHIFT_LEFT;
+    }
+    if (module_type_->getDirection() == Direction::RIGHT) {
+      return PlanningFactor::SHIFT_RIGHT;
+    }
+    return PlanningFactor::UNKNOWN;
+  });
+
+  planning_factor_interface_->add(
+    start_distance, finish_distance, status.lane_change_path.info.shift_line.start,
+    status.lane_change_path.info.shift_line.end, planning_factor_direction, SafetyFactorArray{});
 }
 
 void LaneChangeInterface::updateSteeringFactorPtr(
@@ -420,5 +439,17 @@ void LaneChangeInterface::updateSteeringFactorPtr(
     {selected_path.info.shift_line.start, selected_path.info.shift_line.end},
     {output.start_distance_to_path_change, output.finish_distance_to_path_change},
     steering_factor_direction, SteeringFactor::APPROACHING, "");
+
+  const uint16_t planning_factor_direction = std::invoke([&output]() {
+    if (output.lateral_shift > 0.0) {
+      return PlanningFactor::SHIFT_LEFT;
+    }
+    return PlanningFactor::SHIFT_RIGHT;
+  });
+
+  planning_factor_interface_->add(
+    output.start_distance_to_path_change, output.finish_distance_to_path_change,
+    selected_path.info.shift_line.start, selected_path.info.shift_line.end,
+    planning_factor_direction, SafetyFactorArray{});
 }
 }  // namespace autoware::behavior_path_planner
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp
@@ -301,7 +301,7 @@ std::unique_ptr<SceneModuleInterface> LaneChangeModuleManager::createNewSceneMod
 {
   return std::make_unique<LaneChangeInterface>(
     name_, *node_, parameters_, rtc_interface_ptr_map_,
-    objects_of_interest_marker_interface_ptr_map_,
+    objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_,
     std::make_unique<NormalLaneChange>(parameters_, LaneChangeModuleType::NORMAL, direction_));
 }