Skip to content

Commit

Permalink
refactor(lane_change): move struct to lane change namespace (#7841)
Browse files Browse the repository at this point in the history 
* move struct to lane change namespace

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

* Revert "move struct to lane change namespace"

This reverts commit 306984a.

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

---------

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
  • Loading branch information
@zulfaqar-azmi-t4
zulfaqar-azmi-t4 authored Jul 5, 2024
1 parent 5c48933 commit ad38061
Show file tree
Hide file tree
Showing 3 changed files with 99 additions and 26 deletions.
19 changes: 17 additions & 2 deletions ...lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,10 @@ class LaneChangeBase
LaneChangeBase(
std::shared_ptr<LaneChangeParameters> parameters, LaneChangeModuleType type,
Direction direction)
: lane_change_parameters_{std::move(parameters)}, direction_{direction}, type_{type}
: lane_change_parameters_{std::move(parameters)},
common_data_ptr_{std::make_shared<lane_change::CommonData>()},
direction_{direction},
type_{type}
{
}

Expand Down Expand Up @@ -151,7 +154,18 @@ class LaneChangeBase

bool isValidPath() const { return status_.is_valid_path; }

void setData(const std::shared_ptr<const PlannerData> & data) { planner_data_ = data; }
void setData(const std::shared_ptr<const PlannerData> & data)
{
planner_data_ = data;
if (!common_data_ptr_->bpp_param_ptr) {
common_data_ptr_->bpp_param_ptr =
std::make_shared<BehaviorPathPlannerParameters>(data->parameters);
}
common_data_ptr_->self_odometry_ptr = data->self_odometry;
common_data_ptr_->route_handler_ptr = data->route_handler;
common_data_ptr_->lc_param_ptr = lane_change_parameters_;
common_data_ptr_->direction = direction_;
}

void toNormalState() { current_lane_change_state_ = LaneChangeStates::Normal; }

Expand Down Expand Up @@ -219,6 +233,7 @@ class LaneChangeBase
std::shared_ptr<LaneChangeParameters> lane_change_parameters_{};
std::shared_ptr<LaneChangePath> abort_path_{};
std::shared_ptr<const PlannerData> planner_data_{};
lane_change::CommonDataPtr common_data_ptr_{};
BehaviorModuleOutput prev_module_output_{};
std::optional<Pose> lane_change_stop_pose_{std::nullopt};

Expand Down
102 changes: 80 additions & 22 deletions ...ne_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,16 +17,28 @@
#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp"
#include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp"

#include <autoware/behavior_path_planner_common/parameters.hpp>
#include <autoware/route_handler/route_handler.hpp>
#include <interpolation/linear_interpolation.hpp>

#include <nav_msgs/msg/odometry.hpp>

#include <lanelet2_core/primitives/Lanelet.h>
#include <lanelet2_core/primitives/Polygon.h>

#include <memory>
#include <utility>
#include <vector>

namespace autoware::behavior_path_planner
namespace autoware::behavior_path_planner::lane_change
{
using geometry_msgs::msg::Pose;
using geometry_msgs::msg::Twist;
using nav_msgs::msg::Odometry;
using route_handler::Direction;
using route_handler::RouteHandler;
using utils::path_safety_checker::ExtendedPredictedObjects;

struct LateralAccelerationMap
{
std::vector<double> base_vel;
Expand Down Expand Up @@ -68,7 +80,7 @@ struct LateralAccelerationMap
}
};

struct LaneChangeCancelParameters
struct CancelParameters
{
bool enable_on_prepare_phase{true};
bool enable_on_lane_changing_phase{false};
Expand All @@ -83,7 +95,7 @@ struct LaneChangeCancelParameters
int unsafe_hysteresis_threshold{2};
};

struct LaneChangeParameters
struct Parameters
{
// trajectory generation
double backward_lane_length{200.0};
Expand All @@ -92,8 +104,8 @@ struct LaneChangeParameters
int lateral_acc_sampling_num{10};

// lane change parameters
double backward_length_buffer_for_end_of_lane;
double backward_length_buffer_for_blocking_object;
double backward_length_buffer_for_end_of_lane{0.0};
double backward_length_buffer_for_blocking_object{0.0};
double lane_changing_lateral_jerk{0.5};
double minimum_lane_changing_velocity{5.6};
double lane_change_prepare_duration{4.0};
Expand Down Expand Up @@ -143,40 +155,40 @@ struct LaneChangeParameters
utils::path_safety_checker::RSSparams rss_params_for_stuck{};

// abort
LaneChangeCancelParameters cancel{};
CancelParameters cancel{};

double finish_judge_lateral_threshold{0.2};

// debug marker
bool publish_debug_marker{false};
};

enum class LaneChangeStates {
enum class States {
Normal = 0,
Cancel,
Abort,
Stop,
};

struct LaneChangePhaseInfo
struct PhaseInfo
{
double prepare{0.0};
double lane_changing{0.0};

[[nodiscard]] double sum() const { return prepare + lane_changing; }

LaneChangePhaseInfo(const double _prepare, const double _lane_changing)
PhaseInfo(const double _prepare, const double _lane_changing)
: prepare(_prepare), lane_changing(_lane_changing)
{
}
};

struct LaneChangeInfo
struct Info
{
LaneChangePhaseInfo longitudinal_acceleration{0.0, 0.0};
LaneChangePhaseInfo velocity{0.0, 0.0};
LaneChangePhaseInfo duration{0.0, 0.0};
LaneChangePhaseInfo length{0.0, 0.0};
PhaseInfo longitudinal_acceleration{0.0, 0.0};
PhaseInfo velocity{0.0, 0.0};
PhaseInfo duration{0.0, 0.0};
PhaseInfo length{0.0, 0.0};

lanelet::ConstLanelets current_lanes{};
lanelet::ConstLanelets target_lanes{};
Expand All @@ -190,22 +202,19 @@ struct LaneChangeInfo
double terminal_lane_changing_velocity{0.0};
};

struct LaneChangeLanesFilteredObjects
struct LanesObjects
{
utils::path_safety_checker::ExtendedPredictedObjects current_lane{};
utils::path_safety_checker::ExtendedPredictedObjects target_lane{};
utils::path_safety_checker::ExtendedPredictedObjects other_lane{};
ExtendedPredictedObjects current_lane{};
ExtendedPredictedObjects target_lane{};
ExtendedPredictedObjects other_lane{};
};

enum class LaneChangeModuleType {
enum class ModuleType {
NORMAL = 0,
EXTERNAL_REQUEST,
AVOIDANCE_BY_LANE_CHANGE,
};
} // namespace autoware::behavior_path_planner

namespace autoware::behavior_path_planner::lane_change
{
struct PathSafetyStatus
{
bool is_safe{true};
Expand All @@ -218,6 +227,55 @@ struct LanesPolygon
std::optional<lanelet::BasicPolygon2d> target;
std::vector<lanelet::BasicPolygon2d> target_backward;
};

struct Lanes
{
lanelet::ConstLanelets current;
lanelet::ConstLanelets target;
std::vector<lanelet::ConstLanelets> preceding_target;
};

struct CommonData
{
std::shared_ptr<RouteHandler> route_handler_ptr;
Odometry::ConstSharedPtr self_odometry_ptr;
std::shared_ptr<BehaviorPathPlannerParameters> bpp_param_ptr;
std::shared_ptr<Parameters> lc_param_ptr;
Lanes lanes;
Direction direction;

[[nodiscard]] Pose get_ego_pose() const { return self_odometry_ptr->pose.pose; }

[[nodiscard]] Twist get_ego_twist() const { return self_odometry_ptr->twist.twist; }

[[nodiscard]] double get_ego_speed(bool use_norm = false) const
{
if (!use_norm) {
return get_ego_twist().linear.x;
}

const auto x = get_ego_twist().linear.x;
const auto y = get_ego_twist().linear.y;
return std::hypot(x, y);
}
};

using RouteHandlerPtr = std::shared_ptr<RouteHandler>;
using BppParamPtr = std::shared_ptr<BehaviorPathPlannerParameters>;
using LCParamPtr = std::shared_ptr<Parameters>;
using CommonDataPtr = std::shared_ptr<CommonData>;
using LanesPtr = std::shared_ptr<Lanes>;
} // namespace autoware::behavior_path_planner::lane_change

namespace autoware::behavior_path_planner
{
using LaneChangeModuleType = lane_change::ModuleType;
using LaneChangeParameters = lane_change::Parameters;
using LaneChangeStates = lane_change::States;
using LaneChangePhaseInfo = lane_change::PhaseInfo;
using LaneChangeInfo = lane_change::Info;
using LaneChangeLanesFilteredObjects = lane_change::LanesObjects;
using LateralAccelerationMap = lane_change::LateralAccelerationMap;
} // namespace autoware::behavior_path_planner

#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_
4 changes: 2 additions & 2 deletions ..._path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,8 +31,8 @@ struct LaneChangePath
{
PathWithLaneId path{};
ShiftedPath shifted_path{};
PathWithLaneId prev_path{};
LaneChangeInfo info{};
LaneChangeInfo info;
bool is_safe{false};
};
using LaneChangePaths = std::vector<LaneChangePath>;

Expand Down

0 comments on commit ad38061

Please sign in to comment.