diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp index 3080ba1045b41..37bee1a178f90 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -32,13 +32,15 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( const std::shared_ptr & avoidance_by_lane_change_parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface) : LaneChangeInterface{ name, node, parameters, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, + planning_factor_interface, std::make_unique(parameters, avoidance_by_lane_change_parameters)} { } diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp index c7fbba34b1adb..6e524f3c91341 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp +++ b/planning/behavior_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 & avoidance_by_lane_change_parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface); bool isExecutionRequested() const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index c54774a575332..2bf8a60676d72 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -190,7 +190,7 @@ SMIPtr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() { return std::make_unique( 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/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp index 45c997f364633..0b438d1a1758e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp @@ -28,7 +28,7 @@ ExternalRequestLaneChangeRightModuleManager::createNewSceneModuleInstance() { return std::make_unique( 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(parameters_, direction_)); } @@ -37,7 +37,7 @@ ExternalRequestLaneChangeLeftModuleManager::createNewSceneModuleInstance() { return std::make_unique( 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(parameters_, direction_)); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp index 9d6fbd65acfd8..5bfb9cd8e6af3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp @@ -52,6 +52,7 @@ class LaneChangeInterface : public SceneModuleInterface const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface, std::unique_ptr && 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 index d65e51997eb32..8309a1ab63672 100644 --- 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> & rtc_interface_ptr_map, std::unordered_map> & objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface, std::unique_ptr && 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)}, prev_approved_path_{std::make_unique()} @@ -150,6 +151,8 @@ BehaviorModuleOutput LaneChangeInterface::plan() } } + set_longitudinal_planning_factor(output.path); + setVelocityFactor(output.path); return output; @@ -188,6 +191,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; @@ -436,6 +441,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( @@ -452,5 +471,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 index 1bb41069140e7..8a66f598f61eb 100644 --- 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 @@ -284,7 +284,7 @@ std::unique_ptr LaneChangeModuleManager::createNewSceneMod { return std::make_unique( 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(parameters_, LaneChangeModuleType::NORMAL, direction_)); }