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 avoidance2 #117

Closed
wants to merge 8 commits into from
Closed

Refactor/obstacle avoidance2 #117

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
cee3ca6
refactor
takayuki5168 Sep 9, 2022
4acc3b1
refactor
takayuki5168 Sep 9, 2022
a34ebf4
refactor
takayuki5168 Sep 9, 2022
db543b7
refactor
takayuki5168 Sep 9, 2022
e11e665
refactor
takayuki5168 Sep 9, 2022
3618b50
po
takayuki5168 Sep 9, 2022
0322d9a
po
takayuki5168 Sep 9, 2022
2923e36
po
takayuki5168 Sep 9, 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
4 changes: 2 additions & 2 deletions ..._driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,8 +77,8 @@
# replanning & trimming trajectory param outside algorithm
replan:
max_path_shape_change_dist: 0.3 # threshold of path shape change from behavior path [m]
max_ego_moving_dist_for_replan: 3.0 # threshold of ego's moving distance for replan [m]
max_delta_time_sec_for_replan: 1.0 # threshold of delta time for replan [second]
max_ego_moving_dist: 3.0 # threshold of ego's moving distance for replan [m]
max_delta_time_sec: 1.0 # threshold of delta time for replan [second]

# advanced parameters to improve performance as much as possible
advanced:
Expand Down
7 changes: 4 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,13 @@ 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/replan_checker.cpp
src/utils/utils.cpp
src/utils/debug_utils.cpp
src/utils/cv_utils.cpp
src/node.cpp
)

Expand Down
4 changes: 2 additions & 2 deletions planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,8 +77,8 @@
# replanning & trimming trajectory param outside algorithm
replan:
max_path_shape_change_dist: 0.3 # threshold of path shape change from behavior path [m]
max_ego_moving_dist_for_replan: 3.0 # threshold of ego's moving distance for replan [m]
max_delta_time_sec_for_replan: 1.0 # threshold of delta time for replan [second]
max_ego_moving_dist: 3.0 # threshold of ego's moving distance for replan [m]
max_delta_time_sec: 1.0 # threshold of delta time for replan [second]

# advanced parameters to improve performance as much as possible
advanced:
Expand Down
34 changes: 24 additions & 10 deletions planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include "rclcpp/rclcpp.hpp"

#include "autoware_auto_perception_msgs/msg/predicted_object.hpp"
#include "autoware_auto_planning_msgs/msg/path.hpp"
#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
#include "nav_msgs/msg/map_meta_data.hpp"

Expand All @@ -38,6 +39,11 @@ using SequentialBounds = std::vector<Bounds>;
using BoundsCandidates = std::vector<Bounds>;
using SequentialBoundsCandidates = std::vector<BoundsCandidates>;

// message type
using autoware_auto_perception_msgs::msg::PredictedObject;
using autoware_auto_planning_msgs::msg::Path;
using autoware_auto_planning_msgs::msg::TrajectoryPoint;

struct CVMaps
{
cv::Mat drivable_area;
Expand Down Expand Up @@ -148,8 +154,8 @@ struct DebugData
std::vector<geometry_msgs::msg::Pose> fixed_points;
std::vector<geometry_msgs::msg::Pose> non_fixed_points;
std::vector<ConstrainRectangle> constrain_rectangles;
std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> avoiding_traj_points;
std::vector<autoware_auto_perception_msgs::msg::PredictedObject> avoiding_objects;
std::vector<TrajectoryPoint> avoiding_traj_points;
std::vector<PredictedObject> avoiding_objects;

cv::Mat clearance_map;
cv::Mat only_object_clearance_map;
Expand All @@ -161,21 +167,21 @@ struct DebugData
std::vector<geometry_msgs::msg::Pose> mpt_ref_poses;
std::vector<double> lateral_errors;

std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> eb_traj;
std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> mpt_fixed_traj;
std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> mpt_ref_traj;
std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> mpt_traj;
std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> extended_fixed_traj;
std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> extended_non_fixed_traj;
std::vector<TrajectoryPoint> eb_traj;
std::vector<TrajectoryPoint> mpt_fixed_traj;
std::vector<TrajectoryPoint> mpt_ref_traj;
std::vector<TrajectoryPoint> mpt_traj;
std::vector<TrajectoryPoint> extended_fixed_traj;
std::vector<TrajectoryPoint> extended_non_fixed_traj;

SequentialBoundsCandidates sequential_bounds_candidates;
};

struct Trajectories
{
std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> smoothed_trajectory;
std::vector<TrajectoryPoint> smoothed_trajectory;
std::vector<ReferencePoint> mpt_ref_points;
std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> model_predictive_trajectory;
std::vector<TrajectoryPoint> model_predictive_trajectory;
};

struct TrajectoryParam
Expand Down Expand Up @@ -260,4 +266,12 @@ struct MPTParam
double max_plan_from_ego_length;
};

struct PlannerData
{
Path path;
geometry_msgs::msg::Pose ego_pose;
double ego_vel;
std::vector<PredictedObject> objects;
};

#endif // OBSTACLE_AVOIDANCE_PLANNER__COMMON_STRUCTS_HPP_
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_
17 changes: 11 additions & 6 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 @@ -33,6 +33,11 @@
#include <utility>
#include <vector>

using autoware_auto_planning_msgs::msg::Path;
using autoware_auto_planning_msgs::msg::PathPoint;
using autoware_auto_planning_msgs::msg::Trajectory;
using autoware_auto_planning_msgs::msg::TrajectoryPoint;

class EBPathOptimizer
{
private:
Expand Down Expand Up @@ -142,7 +147,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 @@ -155,12 +160,12 @@ class EBPathOptimizer
std::vector<geometry_msgs::msg::Pose> getFixedPoints(
const geometry_msgs::msg::Pose & ego_pose,
const std::vector<autoware_auto_planning_msgs::msg::PathPoint> & path_points,
const std::unique_ptr<Trajectories> & prev_optimized_points);
const std::shared_ptr<std::vector<TrajectoryPoint>> prev_traj);

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::shared_ptr<std::vector<TrajectoryPoint>> prev_traj, DebugData & debug_data);

CandidatePoints getDefaultCandidatePoints(
const std::vector<autoware_auto_planning_msgs::msg::PathPoint> & path_points);
Expand All @@ -171,7 +176,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 @@ -180,8 +185,8 @@ 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);
const std::shared_ptr<std::vector<TrajectoryPoint>> prev_traj, const double current_ego_vel,
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.

Loading