diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 7df7c4c39fff0..8277ae102b312 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -33,7 +33,10 @@ struct BehaviorPathPlannerParameters ModuleConfigParameters config_pull_out; ModuleConfigParameters config_pull_over; ModuleConfigParameters config_side_shift; - ModuleConfigParameters config_lane_change; + ModuleConfigParameters config_lane_change_left; + ModuleConfigParameters config_lane_change_right; + ModuleConfigParameters config_external_lane_change_left; + ModuleConfigParameters config_external_lane_change_right; double backward_path_length; double forward_path_length; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index 489dbfd3b5764..bbdc7c14435d7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp @@ -45,6 +45,7 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using marker_utils::CollisionCheckDebug; +using route_handler::Direction; using tier4_planning_msgs::msg::LaneChangeDebugMsg; using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; @@ -59,8 +60,7 @@ class LaneChangeModule : public SceneModuleInterface LaneChangeModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::shared_ptr & rtc_interface_left, - const std::shared_ptr & rtc_interface_right); + const std::shared_ptr & rtc_interface, Direction direction); #endif bool isExecutionRequested() const override; bool isExecutionReady() const override; @@ -75,13 +75,7 @@ class LaneChangeModule : public SceneModuleInterface void acceptVisitor( [[maybe_unused]] const std::shared_ptr & visitor) const override; -#ifndef USE_OLD_ARCHITECTURE - void updateModuleParams(const std::shared_ptr & parameters) - { - parameters_ = parameters; - } -#endif - +#ifdef USE_OLD_ARCHITECTURE void publishRTCStatus() override { rtc_interface_left_->publishCooperateStatus(clock_->now()); @@ -110,6 +104,12 @@ class LaneChangeModule : public SceneModuleInterface rtc_interface_left_->unlockCommandUpdate(); rtc_interface_right_->unlockCommandUpdate(); } +#else + void updateModuleParams(const std::shared_ptr & parameters) + { + parameters_ = parameters; + } +#endif private: std::shared_ptr parameters_; @@ -125,16 +125,21 @@ class LaneChangeModule : public SceneModuleInterface bool is_abort_path_approved_{false}; bool is_abort_approval_requested_{false}; bool is_abort_condition_satisfied_{false}; - bool is_activated_ = false; + bool is_activated_{false}; +#ifdef USE_OLD_ARCHITECTURE std::shared_ptr rtc_interface_left_; std::shared_ptr rtc_interface_right_; UUID uuid_left_; UUID uuid_right_; UUID candidate_uuid_; +#else + Direction direction_{Direction::NONE}; +#endif void resetParameters(); +#ifdef USE_OLD_ARCHITECTURE void waitApprovalLeft(const double start_distance, const double finish_distance) { rtc_interface_left_->updateCooperateStatus( @@ -190,8 +195,8 @@ class LaneChangeModule : public SceneModuleInterface rtc_interface_right_->removeCooperateStatus(uuid_right_); } } +#endif - lanelet::ConstLanelets get_original_lanes() const; PathWithLaneId getReferencePath() const; lanelet::ConstLanelets getLaneChangeLanes( const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp index 87e5d42688d06..ecb2164855bb0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp @@ -33,23 +33,24 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface public: LaneChangeModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - std::shared_ptr parameters); + std::shared_ptr parameters, Direction direction); std::shared_ptr createNewSceneModuleInstance() override { return std::make_shared( - name_, *node_, parameters_, rtc_interface_left_, rtc_interface_right_); + name_, *node_, parameters_, rtc_interface_, direction_); } void updateModuleParams(const std::vector & parameters) override; private: - std::shared_ptr rtc_interface_left_; - std::shared_ptr rtc_interface_right_; + std::shared_ptr rtc_interface_; std::shared_ptr parameters_; std::vector> registered_modules_; + + Direction direction_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/lane_change_module_data.hpp index 1d16f1bd84046..7ef20d546327b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/lane_change_module_data.hpp @@ -79,6 +79,11 @@ struct LaneChangeTargetObjectIndices std::vector target_lane{}; std::vector other_lane{}; }; + +enum class LaneChangeType { + Normal = 0, + ExternalRequest = 1, +}; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__UTIL__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp index 76b0e4b8abb83..1efc82aca1ee8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp @@ -45,6 +45,7 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using marker_utils::CollisionCheckDebug; +using route_handler::Direction; using tier4_autoware_utils::Polygon2d; PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2); @@ -162,5 +163,7 @@ LaneChangeTargetObjectIndices filterObjectIndices( double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0); +std::string getStrDirection(const std::string name, const Direction direction); + } // namespace behavior_path_planner::lane_change_utils #endif // BEHAVIOR_PATH_PLANNER__UTIL__LANE_CHANGE__UTIL_HPP_ diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 5ff95f26fdcad..be99d578e1856 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -234,10 +234,23 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "side_shift", create_publisher(path_reference_name_space + "side_shift", 1)); } - if (p.config_lane_change.enable_module) { - const std::string module_topic = "lane_change"; + if (p.config_lane_change_left.enable_module) { + const std::string module_topic = "lane_change_left"; auto manager = std::make_shared( - this, module_topic, p.config_lane_change, lane_change_param_ptr_); + this, module_topic, p.config_lane_change_left, lane_change_param_ptr_, + route_handler::Direction::LEFT); + planner_manager_->registerSceneModuleManager(manager); + path_candidate_publishers_.emplace( + module_topic, create_publisher(path_candidate_name_space + module_topic, 1)); + path_reference_publishers_.emplace( + module_topic, create_publisher(path_reference_name_space + module_topic, 1)); + } + + if (p.config_lane_change_right.enable_module) { + const std::string module_topic = "lane_change_right"; + auto manager = std::make_shared( + this, module_topic, p.config_lane_change_right, lane_change_param_ptr_, + route_handler::Direction::RIGHT); planner_manager_->registerSceneModuleManager(manager); path_candidate_publishers_.emplace( module_topic, create_publisher(path_candidate_name_space + module_topic, 1)); @@ -316,12 +329,21 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() } { - const std::string ns = "lane_change."; - p.config_lane_change.enable_module = declare_parameter(ns + "enable_module"); - p.config_lane_change.enable_simultaneous_execution = + const std::string ns = "lane_change_left."; + p.config_lane_change_left.enable_module = declare_parameter(ns + "enable_module"); + p.config_lane_change_left.enable_simultaneous_execution = + declare_parameter(ns + "enable_simultaneous_execution"); + p.config_lane_change_left.priority = declare_parameter(ns + "priority"); + p.config_lane_change_left.max_module_size = declare_parameter(ns + "max_module_size"); + } + + { + const std::string ns = "lane_change_right."; + p.config_lane_change_right.enable_module = declare_parameter(ns + "enable_module"); + p.config_lane_change_right.enable_simultaneous_execution = declare_parameter(ns + "enable_simultaneous_execution"); - p.config_lane_change.priority = declare_parameter(ns + "priority"); - p.config_lane_change.max_module_size = declare_parameter(ns + "max_module_size"); + p.config_lane_change_right.priority = declare_parameter(ns + "priority"); + p.config_lane_change_right.max_module_size = declare_parameter(ns + "max_module_size"); } { diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index a90e1cef71cf8..767369904a466 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -281,7 +281,7 @@ BehaviorModuleOutput PlannerManager::update(const std::shared_ptr & if ((*itr)->getCurrentStatus() != ModuleStatus::RUNNING) { if (itr == approved_module_ptrs_.begin()) { // update root lanelet when the lane change is done. - if (name == "lane_change") { + if (name.find("lane_change") != std::string::npos) { root_lanelet_ = updateRootLanelet(data); } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index f9510ecd7e8e5..99c0ff5ddd467 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -31,15 +31,6 @@ #include #include -std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) -{ - std::stringstream ss; - for (auto i = 0; i < 16; ++i) { - ss << std::hex << std::setfill('0') << std::setw(2) << +uuid.uuid[i]; - } - return ss.str(); -} - namespace behavior_path_planner { using autoware_auto_perception_msgs::msg::ObjectClassification; @@ -60,15 +51,10 @@ LaneChangeModule::LaneChangeModule( LaneChangeModule::LaneChangeModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::shared_ptr & rtc_interface_left, - const std::shared_ptr & rtc_interface_right) -: SceneModuleInterface{name, node}, - parameters_{parameters}, - rtc_interface_left_{rtc_interface_left}, - rtc_interface_right_{rtc_interface_right}, - uuid_left_{generateUUID()}, - uuid_right_{generateUUID()} + const std::shared_ptr & rtc_interface, Direction direction) +: SceneModuleInterface{name, node}, parameters_{parameters}, direction_{direction} { + rtc_interface_ptr_ = rtc_interface; steering_factor_interface_ptr_ = std::make_unique(&node, "lane_change"); } #endif @@ -107,6 +93,10 @@ bool LaneChangeModule::isExecutionRequested() const #endif const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); + if (lane_change_lanes.empty()) { + return false; + } + LaneChangePath selected_path; const auto [found_valid_path, found_safe_path] = getSafePath(lane_change_lanes, check_distance_, selected_path); @@ -128,6 +118,10 @@ bool LaneChangeModule::isExecutionReady() const #endif const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); + if (lane_change_lanes.empty()) { + return false; + } + LaneChangePath selected_path; const auto [found_valid_path, found_safe_path] = getSafePath(lane_change_lanes, check_distance_, selected_path); @@ -218,6 +212,7 @@ BehaviorModuleOutput LaneChangeModule::plan() void LaneChangeModule::resetPathIfAbort() { if (!is_abort_approval_requested_) { +#ifdef USE_OLD_ARCHITECTURE const auto lateral_shift = lane_change_utils::getLateralShift(*abort_path_); if (lateral_shift > 0.0) { removePreviousRTCStatusRight(); @@ -226,6 +221,9 @@ void LaneChangeModule::resetPathIfAbort() removePreviousRTCStatusLeft(); uuid_left_ = generateUUID(); } +#else + removeRTCStatus(); +#endif RCLCPP_DEBUG(getLogger(), "[abort] uuid is reset to request abort approval."); is_abort_approval_requested_ = true; is_abort_path_approved_ = false; @@ -257,6 +255,10 @@ CandidateOutput LaneChangeModule::planCandidate() const #endif const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); + if (lane_change_lanes.empty()) { + return output; + } + #ifdef USE_OLD_ARCHITECTURE [[maybe_unused]] const auto [found_valid_path, found_safe_path] = getSafePath(lane_change_lanes, check_distance_, selected_path); @@ -308,9 +310,15 @@ BehaviorModuleOutput LaneChangeModule::planWaitingApproval() const auto candidate = planCandidate(); path_candidate_ = std::make_shared(candidate.path_candidate); path_reference_ = getPreviousModuleOutput().reference_path; +#ifdef USE_OLD_ARCHITECTURE updateRTCStatus(candidate); waitApproval(); +#else + updateRTCStatus( + candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); +#endif is_abort_path_approved_ = false; + return out; } @@ -416,7 +424,11 @@ lanelet::ConstLanelets LaneChangeModule::getLaneChangeLanes( lanelet::ConstLanelets current_check_lanes = route_handler->getLaneletSequence(current_lane, current_pose, 0.0, lane_change_prepare_length); lanelet::ConstLanelet lane_change_lane; +#ifdef USE_OLD_ARCHITECTURE if (route_handler->getLaneChangeTarget(current_check_lanes, &lane_change_lane)) { +#else + if (route_handler->getLaneChangeTarget(current_check_lanes, &lane_change_lane, direction_)) { +#endif lane_change_lanes = route_handler->getLaneletSequence( lane_change_lane, current_pose, lane_change_lane_length, lane_change_lane_length); } else { @@ -667,13 +679,14 @@ std::shared_ptr LaneChangeModule::get_debug_msg_array() void LaneChangeModule::updateSteeringFactorPtr(const BehaviorModuleOutput & output) { - const auto turn_signal_info = output.turn_signal_info; const auto current_pose = getEgoPose(); const double start_distance = motion_utils::calcSignedArcLength( output.path->points, current_pose.position, status_.lane_change_path.shift_line.start.position); const double finish_distance = motion_utils::calcSignedArcLength( output.path->points, current_pose.position, status_.lane_change_path.shift_line.end.position); +#ifdef USE_OLD_ARCHITECTURE + const auto turn_signal_info = output.turn_signal_info; const uint16_t steering_factor_direction = std::invoke([this, &start_distance, &finish_distance, &turn_signal_info]() { if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { @@ -686,6 +699,17 @@ void LaneChangeModule::updateSteeringFactorPtr(const BehaviorModuleOutput & outp } return SteeringFactor::UNKNOWN; }); +#else + const auto steering_factor_direction = std::invoke([&]() { + if (direction_ == Direction::LEFT) { + return SteeringFactor::LEFT; + } + if (direction_ == Direction::RIGHT) { + return SteeringFactor::RIGHT; + } + return SteeringFactor::UNKNOWN; + }); +#endif // TODO(tkhmy) add handle status TRYING steering_factor_interface_ptr_->updateSteeringFactor( diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 79bc075de2ffe..13bb4c7db5b4c 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -23,14 +23,15 @@ namespace behavior_path_planner { - +using route_handler::Direction; LaneChangeModuleManager::LaneChangeModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - std::shared_ptr parameters) -: SceneModuleManagerInterface(node, name, config), parameters_{std::move(parameters)} + std::shared_ptr parameters, Direction direction) +: SceneModuleManagerInterface(node, name, config), + parameters_{std::move(parameters)}, + direction_{direction} { - rtc_interface_left_ = std::make_shared(node, name + "_left"); - rtc_interface_right_ = std::make_shared(node, name + "_right"); + rtc_interface_ = std::make_shared(node, name); } void LaneChangeModuleManager::updateModuleParams( @@ -40,7 +41,7 @@ void LaneChangeModuleManager::updateModuleParams( [[maybe_unused]] auto p = parameters_; - [[maybe_unused]] std::string ns = "lane_change."; + [[maybe_unused]] const std::string ns = name_ + "."; // updateParam(parameters, ns + ..., ...); std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { diff --git a/planning/behavior_path_planner/src/util/lane_change/util.cpp b/planning/behavior_path_planner/src/util/lane_change/util.cpp index 3bd7549bf3677..550185992f1a5 100644 --- a/planning/behavior_path_planner/src/util/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/util/lane_change/util.cpp @@ -1163,4 +1163,14 @@ double calcLateralBufferForFiltering(const double vehicle_width, const double la return lateral_buffer + 0.5 * vehicle_width; } +std::string getStrDirection(const std::string & name, const Direction direction) +{ + if (direction == Direction::LEFT) { + return name + "_left"; + } + if (direction == Direction::RIGHT) { + return name + "_right"; + } + return ""; +} } // namespace behavior_path_planner::lane_change_utils diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 52f8082f125e4..89e052b439f78 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -54,7 +54,7 @@ using std_msgs::msg::Header; using unique_identifier_msgs::msg::UUID; using RouteSections = std::vector; -enum class LaneChangeDirection { NONE, LEFT, RIGHT }; +enum class Direction { NONE, LEFT, RIGHT }; enum class PullOverDirection { NONE, LEFT, RIGHT }; enum class PullOutDirection { NONE, LEFT, RIGHT }; @@ -259,9 +259,19 @@ class RouteHandler const lanelet::ConstLanelet & lanelet, const double length, const lanelet::ConstLanelets & exclude_lanelets = {}) const; - int getNumLaneToPreferredLane(const lanelet::ConstLanelet & lanelet) const; + /** + * Query input lanelet to see whether it exist in the preferred lane. If it doesn't exist, return + * the number of lane-changeable lane to the preferred lane. + * @param Desired lanelet to query + * @param lane change direction + * @return number of lanes from input to the preferred lane + */ + int getNumLaneToPreferredLane( + const lanelet::ConstLanelet & lanelet, const Direction direction = Direction::NONE) const; + bool getClosestLaneletWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const; + lanelet::ConstLanelet getLaneletsFromId(const lanelet::Id id) const; lanelet::ConstLanelets getLaneletsFromIds(const lanelet::Ids & ids) const; lanelet::ConstLanelets getLaneletSequence( @@ -287,7 +297,8 @@ class RouteHandler const lanelet::ConstLanelets & lanelet_sequence, const double s_start, const double s_end, bool use_exact = true) const; bool getLaneChangeTarget( - const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const; + const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet, + const Direction direction = Direction::NONE) const; bool getRightLaneChangeTargetExceptPreferredLane( const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const; bool getLeftLaneChangeTargetExceptPreferredLane( @@ -298,8 +309,7 @@ class RouteHandler static bool getPullOutStartLane( const lanelet::ConstLanelets & lanelets, const Pose & pose, const double vehicle_width, lanelet::ConstLanelet * target_lanelet); - double getLaneChangeableDistance( - const Pose & current_pose, const LaneChangeDirection & direction) const; + double getLaneChangeableDistance(const Pose & current_pose, const Direction & direction) const; lanelet::ConstPolygon3d getIntersectionAreaById(const lanelet::Id id) const; private: diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index d2ea4c65adaa9..2fe8e3549b661 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -1106,30 +1106,33 @@ std::vector RouteHandler::getPrecedingLaneletSequence( } bool RouteHandler::getLaneChangeTarget( - const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const + const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet, + const Direction direction) const { for (const auto & lanelet : lanelets) { - const int num = getNumLaneToPreferredLane(lanelet); + const int num = getNumLaneToPreferredLane(lanelet, direction); if (num == 0) { continue; } - if (num < 0) { - if (!!routing_graph_ptr_->right(lanelet)) { - const auto right_lanelet = routing_graph_ptr_->right(lanelet); - *target_lanelet = right_lanelet.get(); - return true; + if (direction == Direction::NONE || direction == Direction::RIGHT) { + if (num < 0) { + if (!!routing_graph_ptr_->right(lanelet)) { + const auto right_lanelet = routing_graph_ptr_->right(lanelet); + *target_lanelet = right_lanelet.get(); + return true; + } } - continue; } - if (num > 0) { - if (!!routing_graph_ptr_->left(lanelet)) { - const auto left_lanelet = routing_graph_ptr_->left(lanelet); - *target_lanelet = left_lanelet.get(); - return true; + if (direction == Direction::NONE || direction == Direction::LEFT) { + if (num > 0) { + if (!!routing_graph_ptr_->left(lanelet)) { + const auto left_lanelet = routing_graph_ptr_->left(lanelet); + *target_lanelet = left_lanelet.get(); + return true; + } } - continue; } } @@ -1212,26 +1215,34 @@ lanelet::ConstLanelets RouteHandler::getClosestLaneletSequence(const Pose & pose return getLaneletSequence(lanelet); } -int RouteHandler::getNumLaneToPreferredLane(const lanelet::ConstLanelet & lanelet) const +int RouteHandler::getNumLaneToPreferredLane( + const lanelet::ConstLanelet & lanelet, const Direction direction) const { - int num = 0; if (exists(preferred_lanelets_, lanelet)) { - return num; + return 0; } - const auto & right_lanes = - lanelet::utils::query::getAllNeighborsRight(routing_graph_ptr_, lanelet); - for (const auto & right : right_lanes) { - num--; - if (exists(preferred_lanelets_, right)) { - return num; + + if ((direction == Direction::NONE) || (direction == Direction::RIGHT)) { + int num{0}; + const auto & right_lanes = + lanelet::utils::query::getAllNeighborsRight(routing_graph_ptr_, lanelet); + for (const auto & right : right_lanes) { + num--; + if (exists(preferred_lanelets_, right)) { + return num; + } } } - const auto & left_lanes = lanelet::utils::query::getAllNeighborsLeft(routing_graph_ptr_, lanelet); - num = 0; - for (const auto & left : left_lanes) { - num++; - if (exists(preferred_lanelets_, left)) { - return num; + + if ((direction == Direction::NONE) || (direction == Direction::LEFT)) { + const auto & left_lanes = + lanelet::utils::query::getAllNeighborsLeft(routing_graph_ptr_, lanelet); + int num = 0; + for (const auto & left : left_lanes) { + num++; + if (exists(preferred_lanelets_, left)) { + return num; + } } } @@ -1368,7 +1379,7 @@ lanelet::ConstLanelets RouteHandler::getLaneChangeTargetLanes(const Pose & pose) } double RouteHandler::getLaneChangeableDistance( - const Pose & current_pose, const LaneChangeDirection & direction) const + const Pose & current_pose, const Direction & direction) const { lanelet::ConstLanelet current_lane; if (!getClosestLaneletWithinRoute(current_pose, ¤t_lane)) { @@ -1382,12 +1393,12 @@ double RouteHandler::getLaneChangeableDistance( double accumulated_distance = 0; for (const auto & lane : lanelet_sequence) { lanelet::ConstLanelet target_lane; - if (direction == LaneChangeDirection::RIGHT) { + if (direction == Direction::RIGHT) { if (!getRightLaneletWithinRoute(lane, &target_lane)) { break; } } - if (direction == LaneChangeDirection::LEFT) { + if (direction == Direction::LEFT) { if (!getLeftLaneletWithinRoute(lane, &target_lane)) { break; }