Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(lane_change): move for lane change module to interface #3536

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 0 additions & 2 deletions planning/behavior_path_planner/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,7 @@ set(common_src
src/scene_module/goal_planner/goal_planner_module.cpp
src/scene_module/side_shift/side_shift_module.cpp
src/scene_module/lane_change/interface.cpp
src/scene_module/lane_change/lane_change_module.cpp
src/scene_module/lane_change/normal.cpp
src/scene_module/lane_change/bt_normal.cpp
src/scene_module/lane_change/external_request.cpp
src/turn_signal_decider.cpp
src/utils/utils.cpp
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,6 @@
#include "behavior_path_planner/behavior_tree_manager.hpp"
#include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp"
#include "behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp"
#include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp"
#include "behavior_path_planner/scene_module/lane_following/lane_following_module.hpp"
#include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp"
#include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp"
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -53,15 +53,19 @@ class LaneChangeBase
{
public:
LaneChangeBase(
const std::shared_ptr<LaneChangeParameters> & parameters, LaneChangeModuleType type,
std::shared_ptr<LaneChangeParameters> parameters, LaneChangeModuleType type,
Direction direction)
: parameters_{parameters}, direction_{direction}, type_{type}
: parameters_{std::move(parameters)}, direction_{direction}, type_{type}
{
prev_module_reference_path_ = std::make_shared<PathWithLaneId>();
prev_module_path_ = std::make_shared<PathWithLaneId>();
prev_drivable_lanes_ = std::make_shared<std::vector<DrivableLanes>>();
}

LaneChangeBase(const LaneChangeBase &) = delete;
LaneChangeBase(LaneChangeBase &&) = delete;
LaneChangeBase & operator=(const LaneChangeBase &) = delete;
LaneChangeBase & operator=(LaneChangeBase &&) = delete;
virtual ~LaneChangeBase() = default;

virtual void updateLaneChangeStatus() = 0;
Expand Down

This file was deleted.

Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@
#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_HPP_
#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_HPP_

#include "behavior_path_planner/scene_module/lane_change/bt_normal.hpp"
#include "behavior_path_planner/scene_module/lane_change/normal.hpp"

#include <memory>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -142,6 +142,26 @@ class LaneChangeBTInterface : public LaneChangeInterface
bool is_activated_{false};
};

class LaneChangeBTModule : public LaneChangeBTInterface
{
public:
LaneChangeBTModule(
const std::string & name, rclcpp::Node & node,
const std::shared_ptr<LaneChangeParameters> & parameters);

protected:
void updateRTCStatus(const double start_distance, const double finish_distance) override
{
const auto direction = std::invoke([&]() -> std::string {
const auto dir = module_type_->getDirection();
return (dir == Direction::LEFT) ? "left" : "right";
});

rtc_interface_ptr_map_.at(direction)->updateCooperateStatus(
uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now());
}
};

class ExternalRequestLaneChangeLeftBTModule : public LaneChangeBTInterface
{
public:
Expand Down

This file was deleted.

Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,10 @@ class NormalLaneChange : public LaneChangeBase
const std::shared_ptr<LaneChangeParameters> & parameters, LaneChangeModuleType type,
Direction direction);

NormalLaneChange(const NormalLaneChange &) = delete;
NormalLaneChange(NormalLaneChange &&) = delete;
NormalLaneChange & operator=(const NormalLaneChange &) = delete;
NormalLaneChange & operator=(NormalLaneChange &&) = delete;
~NormalLaneChange() override = default;

void updateLaneChangeStatus() override;
Expand Down Expand Up @@ -86,5 +90,33 @@ class NormalLaneChange : public LaneChangeBase

bool isValidPath(const PathWithLaneId & path) const override;
};

class NormalLaneChangeBT : public NormalLaneChange
{
public:
NormalLaneChangeBT(
const std::shared_ptr<LaneChangeParameters> & parameters, LaneChangeModuleType type,
Direction direction);

NormalLaneChangeBT(const NormalLaneChangeBT &) = delete;
NormalLaneChangeBT(NormalLaneChangeBT &&) = delete;
NormalLaneChangeBT & operator=(const NormalLaneChangeBT &) = delete;
NormalLaneChangeBT & operator=(NormalLaneChangeBT &&) = delete;
~NormalLaneChangeBT() override = default;

PathWithLaneId getReferencePath() const override;

protected:
lanelet::ConstLanelets getCurrentLanes() const override;

int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override;

PathWithLaneId getPrepareSegment(
const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current,
const double backward_path_length, const double prepare_length,
const double prepare_velocity) const override;

std::vector<DrivableLanes> getDrivableLanes() const override;
};
} // namespace behavior_path_planner
#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -390,14 +390,14 @@ class SceneModuleInterface
for (const auto & rtc_type : rtc_types) {
const auto snake_case_name = utils::convertToSnakeCase(name);
const auto rtc_interface_name =
rtc_type == "" ? snake_case_name : snake_case_name + "_" + rtc_type;
rtc_type.empty() ? snake_case_name : (snake_case_name + "_" + rtc_type);
rtc_interface_ptr_map.emplace(
rtc_type, std::make_shared<RTCInterface>(&node, rtc_interface_name));
}
return rtc_interface_ptr_map;
}

void updateRTCStatus(const double start_distance, const double finish_distance)
virtual void updateRTCStatus(const double start_distance, const double finish_distance)
{
for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) {
if (itr->second) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -172,7 +172,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
bt_manager_->registerSceneModule(ext_request_lane_change_left_module);

auto lane_change_module =
std::make_shared<LaneChangeModule>("LaneChange", *this, lane_change_param_ptr_);
std::make_shared<LaneChangeBTModule>("LaneChange", *this, lane_change_param_ptr_);
path_candidate_publishers_.emplace(
"LaneChange", create_publisher<Path>(path_candidate_name_space + "lane_change", 1));
bt_manager_->registerSceneModule(lane_change_module);
Expand Down
Loading