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.

Sign up for GitHub

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(obstacle_avoidance_planner): node refactoring #1829

Merged
merged 7 commits into from
Sep 15, 2022
Merged

refactor(obstacle_avoidance_planner): node refactoring #1829

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
42b912a
refactor
takayuki5168 Sep 9, 2022
08ee1ef
refactor
takayuki5168 Sep 9, 2022
ee77002
refactor
takayuki5168 Sep 9, 2022
b4612b9
refactor
takayuki5168 Sep 9, 2022
e2d6161
refactor
takayuki5168 Sep 9, 2022
cd078f8
fix
takayuki5168 Sep 14, 2022
7fd511b
fix
takayuki5168 Sep 15, 2022
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
6 changes: 3 additions & 3 deletions planning/obstacle_avoidance_planner/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,12 +10,12 @@ find_package(OpenCV REQUIRED)
ament_auto_add_library(obstacle_avoidance_planner SHARED
src/vehicle_model/vehicle_model_interface.cpp
src/vehicle_model/vehicle_model_bicycle_kinematics.cpp
src/utils.cpp
src/costmap_generator.cpp
src/debug_visualization.cpp
src/eb_path_optimizer.cpp
src/mpt_optimizer.cpp
src/cv_utils.cpp
src/utils/utils.cpp
src/utils/debug_utils.cpp
src/utils/cv_utils.cpp
src/node.cpp
)

Expand Down
13 changes: 5 additions & 8 deletions planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/costmap_generator.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,19 +36,17 @@ class CostmapGenerator
CVMaps getMaps(
const bool enable_avoidance, const autoware_auto_planning_msgs::msg::Path & path,
const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects,
const TrajectoryParam & traj_param, std::shared_ptr<DebugData> debug_data_ptr);
const TrajectoryParam & traj_param, DebugData & debug_data);

private:
mutable tier4_autoware_utils::StopWatch<
std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock>
stop_watch_;

cv::Mat getAreaWithObjects(
const cv::Mat & drivable_area, const cv::Mat & objects_image,
std::shared_ptr<DebugData> debug_data_ptr) const;
const cv::Mat & drivable_area, const cv::Mat & objects_image, DebugData & debug_data) const;

cv::Mat getClearanceMap(
const cv::Mat & drivable_area, std::shared_ptr<DebugData> debug_data_ptr) const;
cv::Mat getClearanceMap(const cv::Mat & drivable_area, DebugData & debug_data) const;

cv::Mat drawObstaclesOnImage(
const bool enable_avoidance,
Expand All @@ -57,10 +55,9 @@ class CostmapGenerator
const nav_msgs::msg::MapMetaData & map_info, [[maybe_unused]] const cv::Mat & drivable_area,
const cv::Mat & clearance_map, const TrajectoryParam & traj_param,
std::vector<autoware_auto_perception_msgs::msg::PredictedObject> * debug_avoiding_objects,
std::shared_ptr<DebugData> debug_data_ptr) const;
DebugData & debug_data) const;

cv::Mat getDrivableAreaInCV(
const nav_msgs::msg::OccupancyGrid & occupancy_grid,
std::shared_ptr<DebugData> debug_data_ptr) const;
const nav_msgs::msg::OccupancyGrid & occupancy_grid, DebugData & debug_data) const;
};
#endif // OBSTACLE_AVOIDANCE_PLANNER__COSTMAP_GENERATOR_HPP_
8 changes: 4 additions & 4 deletions planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -142,7 +142,7 @@ class EBPathOptimizer
boost::optional<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>>
getOptimizedTrajectory(
const autoware_auto_planning_msgs::msg::Path & path, const CandidatePoints & candidate_points,
std::shared_ptr<DebugData> debug_data_ptr);
DebugData & debug_data);

void updateConstrain(
const std::vector<geometry_msgs::msg::Point> & interpolated_points,
Expand All @@ -160,7 +160,7 @@ class EBPathOptimizer
CandidatePoints getCandidatePoints(
const geometry_msgs::msg::Pose & ego_pose,
const std::vector<autoware_auto_planning_msgs::msg::PathPoint> & path_points,
const std::unique_ptr<Trajectories> & prev_trajs, std::shared_ptr<DebugData> debug_data_ptr);
const std::unique_ptr<Trajectories> & prev_trajs, DebugData & debug_data);

CandidatePoints getDefaultCandidatePoints(
const std::vector<autoware_auto_planning_msgs::msg::PathPoint> & path_points);
Expand All @@ -171,7 +171,7 @@ class EBPathOptimizer
calculateTrajectory(
const std::vector<geometry_msgs::msg::Point> & padded_interpolated_points,
const std::vector<ConstrainRectangle> & constrain_rectangles, const int farthest_idx,
std::shared_ptr<DebugData> debug_data_ptr);
DebugData & debug_data);

public:
EBPathOptimizer(
Expand All @@ -181,7 +181,7 @@ class EBPathOptimizer
boost::optional<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>> getEBTrajectory(
const geometry_msgs::msg::Pose & ego_pose, const autoware_auto_planning_msgs::msg::Path & path,
const std::unique_ptr<Trajectories> & prev_trajs, const double current_ego_vel,
std::shared_ptr<DebugData> debug_data_ptr);
DebugData & debug_data);
};

#endif // OBSTACLE_AVOIDANCE_PLANNER__EB_PATH_OPTIMIZER_HPP_
50 changes: 0 additions & 50 deletions planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/marker_helper.hpp
View file
Open in desktop

This file was deleted.

30 changes: 13 additions & 17 deletions planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -202,15 +202,15 @@ class MPTOptimizer
std::vector<ReferencePoint> getReferencePoints(
const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & points,
const std::unique_ptr<Trajectories> & prev_mpt_points, const bool enable_avoidance,
const CVMaps & maps, std::shared_ptr<DebugData> debug_data_ptr) const;
const CVMaps & maps, DebugData & debug_data) const;

void calcPlanningFromEgo(std::vector<ReferencePoint> & ref_points) const;

/*
std::vector<ReferencePoint> convertToReferencePoints(
const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & points,
const std::unique_ptr<Trajectories> & prev_mpt_points, const bool enable_avoidance,
const CVMaps & maps, std::shared_ptr<DebugData> debug_data_ptr) const;
const CVMaps & maps, DebugData & debug_data) const;
*/

std::vector<ReferencePoint> getFixedReferencePoints(
Expand All @@ -219,12 +219,11 @@ class MPTOptimizer

void calcBounds(
std::vector<ReferencePoint> & ref_points, const bool enable_avoidance, const CVMaps & maps,
const std::unique_ptr<Trajectories> & prev_trajs,
std::shared_ptr<DebugData> debug_data_ptr) const;
const std::unique_ptr<Trajectories> & prev_trajs, DebugData & debug_data) const;

void calcVehicleBounds(
std::vector<ReferencePoint> & ref_points, const CVMaps & maps,
std::shared_ptr<DebugData> debug_data_ptr, const bool enable_avoidance) const;
std::vector<ReferencePoint> & ref_points, const CVMaps & maps, DebugData & debug_data,
const bool enable_avoidance) const;

// void calcFixState(
// std::vector<ReferencePoint> & ref_points,
Expand All @@ -246,13 +245,12 @@ class MPTOptimizer
* Uex Qex = diag([Q,Q,...]), R1ex = diag([R,R,...])
*/
MPTMatrix generateMPTMatrix(
const std::vector<ReferencePoint> & reference_points,
std::shared_ptr<DebugData> debug_data_ptr) const;
const std::vector<ReferencePoint> & reference_points, DebugData & debug_data) const;

ValueMatrix generateValueMatrix(
const std::vector<ReferencePoint> & reference_points,
const std::vector<autoware_auto_planning_msgs::msg::PathPoint> & path_points,
std::shared_ptr<DebugData> debug_data_ptr) const;
DebugData & debug_data) const;

void addSteerWeightR(
std::vector<Eigen::Triplet<double>> & Rex_triplet_vec,
Expand All @@ -261,19 +259,19 @@ class MPTOptimizer
boost::optional<Eigen::VectorXd> executeOptimization(
const std::unique_ptr<Trajectories> & prev_trajs, const bool enable_avoidance,
const MPTMatrix & mpt_mat, const ValueMatrix & obj_mat,
const std::vector<ReferencePoint> & ref_points, std::shared_ptr<DebugData> debug_data_ptr);
const std::vector<ReferencePoint> & ref_points, DebugData & debug_data);

std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> getMPTPoints(
std::vector<ReferencePoint> & fixed_ref_points,
std::vector<ReferencePoint> & non_fixed_ref_points, const Eigen::VectorXd & Uex,
const MPTMatrix & mpt_matrix, std::shared_ptr<DebugData> debug_data_ptr);
const MPTMatrix & mpt_matrix, DebugData & debug_data);

std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> getMPTFixedPoints(
const std::vector<ReferencePoint> & ref_points) const;

BoundsCandidates getBoundsCandidates(
const bool enable_avoidance, const geometry_msgs::msg::Pose & avoiding_point,
const CVMaps & maps, std::shared_ptr<DebugData> debug_data_ptr) const;
const CVMaps & maps, DebugData & debug_data) const;

CollisionType getCollisionType(
const CVMaps & maps, const bool enable_avoidance,
Expand All @@ -286,13 +284,11 @@ class MPTOptimizer

ObjectiveMatrix getObjectiveMatrix(
const MPTMatrix & mpt_mat, const ValueMatrix & obj_mat,
[[maybe_unused]] const std::vector<ReferencePoint> & ref_points,
std::shared_ptr<DebugData> debug_data_ptr) const;
[[maybe_unused]] const std::vector<ReferencePoint> & ref_points, DebugData & debug_data) const;

ConstraintMatrix getConstraintMatrix(
const bool enable_avoidance, const MPTMatrix & mpt_mat,
const std::vector<ReferencePoint> & ref_points,
std::shared_ptr<DebugData> debug_data_ptr) const;
const std::vector<ReferencePoint> & ref_points, DebugData & debug_data) const;

public:
MPTOptimizer(
Expand All @@ -305,7 +301,7 @@ class MPTOptimizer
const std::vector<autoware_auto_planning_msgs::msg::PathPoint> & path_points,
const std::unique_ptr<Trajectories> & prev_trajs, const CVMaps & maps,
const geometry_msgs::msg::Pose & current_ego_pose, const double current_ego_vel,
std::shared_ptr<DebugData> debug_data_ptr);
DebugData & debug_data);
};

#endif // OBSTACLE_AVOIDANCE_PLANNER__MPT_OPTIMIZER_HPP_
Loading