From 7f0138c356c742b6e15e571e7a4683caa55969ac Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Mon, 12 Dec 2022 22:16:54 +0900 Subject: [PATCH] feat(behavior_path_planner, obstacle_avoidance_planner): add new drivable area (#2472) * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update obstacle avoidance planner Signed-off-by: yutaka * update Signed-off-by: yutaka * clean code Signed-off-by: yutaka * uddate Signed-off-by: yutaka * clean code Signed-off-by: yutaka * remove resample Signed-off-by: yutaka * update Signed-off-by: yutaka * add orientation Signed-off-by: yutaka * change color Signed-off-by: yutaka * update Signed-off-by: yutaka * remove drivable area Signed-off-by: yutaka * add flag Signed-off-by: yutaka * update Signed-off-by: yutaka * update color Signed-off-by: yutaka * fix some codes Signed-off-by: yutaka * change to makerker array Signed-off-by: yutaka * change avoidance utils Signed-off-by: yutaka Signed-off-by: yutaka --- common/motion_utils/src/resample/resample.cpp | 6 +- .../src/drivable_area/display.cpp | 3 +- .../avoidance/avoidance.param.yaml | 3 + .../config/avoidance/avoidance.param.yaml | 3 + .../behavior_path_planner_node.hpp | 8 + .../behavior_path_planner/data_manager.hpp | 1 + .../avoidance/avoidance_module.hpp | 2 +- .../avoidance/avoidance_module_data.hpp | 3 + .../avoidance/avoidance_utils.hpp | 28 ++ .../behavior_path_planner/utilities.hpp | 14 +- .../src/behavior_path_planner_node.cpp | 40 ++ .../src/path_utilities.cpp | 3 +- .../avoidance/avoidance_module.cpp | 16 +- .../avoidance/avoidance_utils.cpp | 154 ++++++ .../lane_change/lane_change_module.cpp | 11 +- .../lane_following/lane_following_module.cpp | 3 +- .../scene_module/pull_out/pull_out_module.cpp | 10 +- .../scene_module/pull_out/shift_pull_out.cpp | 5 +- .../pull_over/pull_over_module.cpp | 13 +- .../side_shift/side_shift_module.cpp | 3 +- .../behavior_path_planner/src/utilities.cpp | 328 ++++++------ .../behavior_velocity_planner/src/node.cpp | 6 +- .../src/scene_module/run_out/utils.cpp | 3 +- .../obstacle_avoidance_planner/CMakeLists.txt | 2 - .../common_structs.hpp | 12 +- .../costmap_generator.hpp | 63 --- .../mpt_optimizer.hpp | 31 +- .../obstacle_avoidance_planner/node.hpp | 7 +- .../utils/cv_utils.hpp | 121 ----- .../utils/debug_utils.hpp | 3 - .../utils/utils.hpp | 8 + .../src/costmap_generator.cpp | 337 ------------- .../src/mpt_optimizer.cpp | 403 ++++----------- .../obstacle_avoidance_planner/src/node.cpp | 62 +-- .../src/utils/cv_utils.cpp | 470 ------------------ .../src/utils/debug_utils.cpp | 56 +-- .../src/utils/utils.cpp | 105 ++++ planning/route_handler/src/route_handler.cpp | 3 +- .../collision_free_optimizer_node.hpp | 2 - .../src/collision_free_optimizer_node.cpp | 10 +- .../src/static_centerline_optimizer_node.cpp | 3 +- .../static_centerline_optimizer/src/utils.cpp | 5 +- 42 files changed, 715 insertions(+), 1654 deletions(-) delete mode 100644 planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/costmap_generator.hpp delete mode 100644 planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/cv_utils.hpp delete mode 100644 planning/obstacle_avoidance_planner/src/costmap_generator.cpp delete mode 100644 planning/obstacle_avoidance_planner/src/utils/cv_utils.cpp diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index c49addeec1a0..5417da86f535 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -260,7 +260,8 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( autoware_auto_planning_msgs::msg::PathWithLaneId resampled_path; resampled_path.header = input_path.header; - resampled_path.drivable_area = input_path.drivable_area; + resampled_path.left_bound = input_path.left_bound; + resampled_path.right_bound = input_path.right_bound; resampled_path.points.resize(interpolated_pose.size()); for (size_t i = 0; i < resampled_path.points.size(); ++i) { autoware_auto_planning_msgs::msg::PathPoint path_point; @@ -402,7 +403,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath( autoware_auto_planning_msgs::msg::Path resampled_path; resampled_path.header = input_path.header; - resampled_path.drivable_area = input_path.drivable_area; + resampled_path.left_bound = input_path.left_bound; + resampled_path.right_bound = resampled_path.right_bound; resampled_path.points.resize(interpolated_pose.size()); for (size_t i = 0; i < resampled_path.points.size(); ++i) { autoware_auto_planning_msgs::msg::PathPoint path_point; diff --git a/common/tier4_planning_rviz_plugin/src/drivable_area/display.cpp b/common/tier4_planning_rviz_plugin/src/drivable_area/display.cpp index 26071d8eddf5..97a232fd9a54 100644 --- a/common/tier4_planning_rviz_plugin/src/drivable_area/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/drivable_area/display.cpp @@ -308,9 +308,8 @@ bool validateFloats(const nav_msgs::msg::OccupancyGrid & msg) } void AutowareDrivableAreaDisplay::processMessage( - autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg) + [[maybe_unused]] autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg) { - current_map_ = msg->drivable_area; loaded_ = true; // updated via signal in case ros spinner is in a different thread Q_EMIT mapUpdated(); diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index 153b879cd96b..b4e992ab5691 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -50,6 +50,9 @@ min_avoidance_speed_for_acc_prevention: 3.0 # [m/s] max_avoidance_acceleration: 0.5 # [m/ss] + # bound clipping for objects + enable_bound_clipping: false + # for debug publish_debug_marker: false print_debug_info: false diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 43be10146c7a..5e2e4673166d 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -50,6 +50,9 @@ min_avoidance_speed_for_acc_prevention: 3.0 # [m/s] max_avoidance_acceleration: 0.5 # [m/s2] + # bound clipping for objects + enable_bound_clipping: false + # for debug publish_debug_marker: false print_debug_info: false diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 366ecc095d0b..84c36bd093d0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include @@ -80,6 +81,7 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; using tier4_planning_msgs::msg::PathChangeModule; using tier4_planning_msgs::msg::Scenario; +using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; class BehaviorPathPlannerNode : public rclcpp::Node @@ -99,6 +101,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr path_candidate_publisher_; rclcpp::Publisher::SharedPtr turn_signal_publisher_; rclcpp::Publisher::SharedPtr hazard_signal_publisher_; + rclcpp::Publisher::SharedPtr bound_publisher_; rclcpp::TimerBase::SharedPtr timer_; std::shared_ptr planner_data_; @@ -184,6 +187,11 @@ class BehaviorPathPlannerNode : public rclcpp::Node */ void publish_steering_factor(const TurnIndicatorsCommand & turn_signal); + /** + * @brief publish left and right bound + */ + void publish_bounds(const PathWithLaneId & path); + /** * @brief publish debug messages */ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 79039e606252..0543c9319f47 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index ddd20f09aa5d..a4783a445b17 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -275,7 +275,7 @@ class AvoidanceModule : public SceneModuleInterface // -- path generation -- ShiftedPath generateAvoidancePath(PathShifter & shifter) const; - void generateExtendedDrivableArea(ShiftedPath * shifted_path) const; + void generateExtendedDrivableArea(PathWithLaneId & path) const; // -- velocity planning -- std::shared_ptr ego_velocity_starting_avoidance_ptr_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp index 91018cb61974..25b29352973c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp @@ -179,6 +179,9 @@ struct AvoidanceParameters double drivable_area_right_bound_offset; double drivable_area_left_bound_offset; + // clip left and right bounds for objects + bool enable_bound_clipping{false}; + // debug bool publish_debug_marker = false; bool print_debug_info = false; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp index 530238eb0ae6..ce04323a775d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp @@ -25,6 +25,14 @@ namespace behavior_path_planner { using behavior_path_planner::PlannerData; + +struct PolygonPoint +{ + geometry_msgs::msg::Point point; + double lat_dist_to_bound; + double lon_dist; +}; + bool isOnRight(const ObjectData & obj); double calcShiftLength( @@ -73,6 +81,26 @@ std::vector getUuidStr(const ObjectDataArray & objs); Polygon2d createEnvelopePolygon( const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer); + +void getEdgePoints( + const Polygon2d & object_polygon, const double threshold, std::vector & edge_points); + +void getEdgePoints( + const std::vector & bound, const std::vector & edge_points, + const double lat_dist_to_path, std::vector & edge_points_data, + size_t & start_segment_idx, size_t & end_segment_idx); + +void sortPolygonPoints( + const std::vector & points, std::vector & sorted_points); + +std::vector updateBoundary( + const std::vector & original_bound, const std::vector & points, + const size_t start_segment_idx, const size_t end_segment_idx); + +void generateDrivableArea( + PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, + const std::shared_ptr planner_data, const ObjectDataArray & objects, + const bool enable_bound_clipping); } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 2da8d1ab2f15..a6978c8e27d1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -66,7 +66,6 @@ using geometry_msgs::msg::PoseArray; using geometry_msgs::msg::PoseStamped; using geometry_msgs::msg::Twist; using geometry_msgs::msg::Vector3; -using nav_msgs::msg::OccupancyGrid; using route_handler::RouteHandler; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; @@ -293,16 +292,9 @@ size_t getOverlappedLaneletId(const std::vector & lanes); std::vector cutOverlappedLanes( PathWithLaneId & path, const std::vector & lanes); -void occupancyGridToImage(const OccupancyGrid & occupancy_grid, cv::Mat * cv_image); - -void imageToOccupancyGrid(const cv::Mat & cv_image, OccupancyGrid * occupancy_grid); - -cv::Point toCVPoint( - const Point & geom_point, const double width_m, const double height_m, const double resolution); - -OccupancyGrid generateDrivableArea( - const PathWithLaneId & path, const std::vector & lanes, const double resolution, - const double vehicle_length, const std::shared_ptr planner_data); +void generateDrivableArea( + PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, + const std::shared_ptr planner_data); lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets( const std::shared_ptr & planner_data); 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 39089e9b8e0a..b536689ed7b1 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -75,6 +75,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod create_publisher("~/drivable_area_boundary", 1); } + bound_publisher_ = create_publisher("~/debug/bound", 1); + // subscriber velocity_subscriber_ = create_subscription( "~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onVelocity, this, _1), @@ -335,6 +337,8 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() p.drivable_area_right_bound_offset = dp("drivable_area_right_bound_offset", 0.0); p.drivable_area_left_bound_offset = dp("drivable_area_left_bound_offset", 0.0); + p.enable_bound_clipping = dp("enable_bound_clipping", false); + p.avoidance_execution_lateral_threshold = dp("avoidance_execution_lateral_threshold", 0.499); return p; @@ -625,6 +629,9 @@ void BehaviorPathPlannerNode::run() planner_data_->prev_output_path = path; mutex_pd_.unlock(); + // publish drivable bounds + publish_bounds(*path); + const size_t target_idx = findEgoIndex(path->points); util::clipPathLength(*path, target_idx, planner_data_->parameters); @@ -700,6 +707,39 @@ void BehaviorPathPlannerNode::publish_steering_factor(const TurnIndicatorsComman steering_factor_interface_ptr_->publishSteeringFactor(get_clock()->now()); } +void BehaviorPathPlannerNode::publish_bounds(const PathWithLaneId & path) +{ + constexpr double scale_x = 0.1; + constexpr double scale_y = 0.1; + constexpr double scale_z = 0.1; + constexpr double color_r = 0.0 / 256.0; + constexpr double color_g = 148.0 / 256.0; + constexpr double color_b = 205.0 / 256.0; + constexpr double color_a = 0.999; + + const auto current_time = path.header.stamp; + auto left_marker = tier4_autoware_utils::createDefaultMarker( + "map", current_time, "left_bound", 0L, Marker::LINE_STRIP, + tier4_autoware_utils::createMarkerScale(scale_x, scale_y, scale_z), + tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + for (const auto lb : path.left_bound) { + left_marker.points.push_back(lb); + } + + auto right_marker = tier4_autoware_utils::createDefaultMarker( + "map", current_time, "right_bound", 0L, Marker::LINE_STRIP, + tier4_autoware_utils::createMarkerScale(scale_x, scale_y, scale_z), + tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + for (const auto rb : path.right_bound) { + right_marker.points.push_back(rb); + } + + MarkerArray msg; + msg.markers.push_back(left_marker); + msg.markers.push_back(right_marker); + bound_publisher_->publish(msg); +} + void BehaviorPathPlannerNode::publishSceneModuleDebugMsg() { { diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp index 110df193abfd..20d9cfc0fc3b 100644 --- a/planning/behavior_path_planner/src/path_utilities.cpp +++ b/planning/behavior_path_planner/src/path_utilities.cpp @@ -123,7 +123,8 @@ Path toPath(const PathWithLaneId & input) { Path output{}; output.header = input.header; - output.drivable_area = input.drivable_area; + output.left_bound = input.left_bound; + output.right_bound = input.right_bound; output.points.resize(input.points.size()); for (size_t i = 0; i < input.points.size(); ++i) { output.points.at(i) = input.points.at(i).point; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index cd248af314fc..77e6180f3eb8 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -1902,7 +1902,7 @@ double AvoidanceModule::getLeftShiftBound() const // TODO(murooka) freespace during turning in intersection where there is no neighbour lanes // NOTE: Assume that there is no situation where there is an object in the middle lane of more than // two lanes since which way to avoid is not obvious -void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) const +void AvoidanceModule::generateExtendedDrivableArea(PathWithLaneId & path) const { const auto has_same_lane = [](const lanelet::ConstLanelets lanes, const lanelet::ConstLanelet & lane) { @@ -2007,7 +2007,7 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c drivable_lanes.push_back(current_drivable_lanes); } - const auto shorten_lanes = util::cutOverlappedLanes(shifted_path->path, drivable_lanes); + const auto shorten_lanes = util::cutOverlappedLanes(path, drivable_lanes); const auto extended_lanes = util::expandLanelets( shorten_lanes, parameters_->drivable_area_left_bound_offset, @@ -2015,9 +2015,9 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c { const auto & p = planner_data_->parameters; - shifted_path->path.drivable_area = util::generateDrivableArea( - shifted_path->path, extended_lanes, p.drivable_area_resolution, p.vehicle_length, - planner_data_); + generateDrivableArea( + path, drivable_lanes, p.vehicle_length, planner_data_, avoidance_data_.target_objects, + parameters_->enable_bound_clipping); } } @@ -2261,9 +2261,6 @@ BehaviorModuleOutput AvoidanceModule::plan() auto avoidance_path = generateAvoidancePath(path_shifter_); debug_data_.output_shift = avoidance_path.shift_length; - // Drivable area generation. - generateExtendedDrivableArea(&avoidance_path); - // modify max speed to prevent acceleration in avoidance maneuver. modifyPathVelocityToPreventAccelerationOnAvoidance(avoidance_path); @@ -2293,6 +2290,9 @@ BehaviorModuleOutput AvoidanceModule::plan() const size_t ego_idx = findEgoIndex(output.path->points); util::clipPathLength(*output.path, ego_idx, planner_data_->parameters); + // Drivable area generation. + generateExtendedDrivableArea(*output.path); + DEBUG_PRINT("exit plan(): set prev output (back().lat = %f)", prev_output_.shift_length.back()); updateRegisteredRTCStatus(avoidance_path.path); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp index 31f3ce7fa32d..ffe2d0a4b83a 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp @@ -342,4 +342,158 @@ Polygon2d createEnvelopePolygon( return offset_polygons.front(); } +void getEdgePoints( + const Polygon2d & object_polygon, const double threshold, std::vector & edge_points) +{ + if (object_polygon.outer().size() < 2) { + return; + } + + const size_t num_points = object_polygon.outer().size(); + for (size_t i = 0; i < num_points - 1; ++i) { + const auto & curr_p = object_polygon.outer().at(i); + const auto & next_p = object_polygon.outer().at(i + 1); + const auto & prev_p = + i == 0 ? object_polygon.outer().at(num_points - 2) : object_polygon.outer().at(i - 1); + const Eigen::Vector2d current_to_next(next_p.x() - curr_p.x(), next_p.y() - curr_p.y()); + const Eigen::Vector2d current_to_prev(prev_p.x() - curr_p.x(), prev_p.y() - curr_p.y()); + const double inner_val = current_to_next.dot(current_to_prev); + if (std::fabs(inner_val) > threshold) { + continue; + } + + const auto edge_point = tier4_autoware_utils::createPoint(curr_p.x(), curr_p.y(), 0.0); + edge_points.push_back(edge_point); + } +} + +void sortPolygonPoints( + const std::vector & points, std::vector & sorted_points) +{ + sorted_points = points; + if (points.size() <= 2) { + // sort data based on longitudinal distance to the boundary + std::sort( + sorted_points.begin(), sorted_points.end(), + [](const PolygonPoint & a, const PolygonPoint & b) { return a.lon_dist < b.lon_dist; }); + return; + } + + // sort data based on lateral distance to the boundary + std::sort( + sorted_points.begin(), sorted_points.end(), [](const PolygonPoint & a, const PolygonPoint & b) { + return std::fabs(a.lat_dist_to_bound) > std::fabs(b.lat_dist_to_bound); + }); + PolygonPoint first_point; + PolygonPoint second_point; + if (sorted_points.at(0).lon_dist < sorted_points.at(1).lon_dist) { + first_point = sorted_points.at(0); + second_point = sorted_points.at(1); + } else { + first_point = sorted_points.at(1); + second_point = sorted_points.at(0); + } + + for (size_t i = 2; i < sorted_points.size(); ++i) { + const auto & next_point = sorted_points.at(i); + if (next_point.lon_dist < first_point.lon_dist) { + sorted_points = {next_point, first_point, second_point}; + return; + } else if (second_point.lon_dist < next_point.lon_dist) { + sorted_points = {first_point, second_point, next_point}; + return; + } + } + + sorted_points = {first_point, second_point}; +} + +void getEdgePoints( + const std::vector & bound, const std::vector & edge_points, + const double lat_dist_to_path, std::vector & edge_points_data, + size_t & start_segment_idx, size_t & end_segment_idx) +{ + for (const auto & edge_point : edge_points) { + const size_t segment_idx = motion_utils::findNearestSegmentIndex(bound, edge_point); + start_segment_idx = std::min(start_segment_idx, segment_idx); + end_segment_idx = std::max(end_segment_idx, segment_idx); + + PolygonPoint edge_point_data; + edge_point_data.point = edge_point; + edge_point_data.lat_dist_to_bound = motion_utils::calcLateralOffset(bound, edge_point); + edge_point_data.lon_dist = motion_utils::calcSignedArcLength(bound, 0, edge_point); + if (lat_dist_to_path >= 0.0 && edge_point_data.lat_dist_to_bound > 0.0) { + continue; + } else if (lat_dist_to_path < 0.0 && edge_point_data.lat_dist_to_bound < 0.0) { + continue; + } + + edge_points_data.push_back(edge_point_data); + } +} + +std::vector updateBoundary( + const std::vector & original_bound, const std::vector & points, + const size_t start_segment_idx, const size_t end_segment_idx) +{ + if (start_segment_idx >= end_segment_idx) { + return original_bound; + } + + std::vector updated_bound; + std::copy( + original_bound.begin(), original_bound.begin() + start_segment_idx + 1, + std::back_inserter(updated_bound)); + for (size_t i = 0; i < points.size(); ++i) { + updated_bound.push_back(points.at(i).point); + } + std::copy( + original_bound.begin() + end_segment_idx + 1, original_bound.end(), + std::back_inserter(updated_bound)); + return updated_bound; +} + +void generateDrivableArea( + PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, + const std::shared_ptr planner_data, const ObjectDataArray & objects, + const bool enable_bound_clipping) +{ + util::generateDrivableArea(path, lanes, vehicle_length, planner_data); + if (objects.empty() || !enable_bound_clipping) { + return; + } + + path.left_bound = motion_utils::resamplePointVector(path.left_bound, 1.0, true); + path.right_bound = motion_utils::resamplePointVector(path.right_bound, 1.0, true); + + for (const auto & object : objects) { + const auto & obj_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto & obj_poly = object.envelope_poly; + constexpr double threshold = 0.01; + + // get edge points + std::vector edge_points; + getEdgePoints(obj_poly, threshold, edge_points); + + // get boundary + const double lat_dist_to_path = motion_utils::calcLateralOffset(path.points, obj_pose.position); + auto & bound = lat_dist_to_path < 0.0 ? path.right_bound : path.left_bound; + + size_t start_segment_idx = (bound.size() == 1 ? 0 : (bound.size() - 2)); + size_t end_segment_idx = 0; + + // get edge points data + std::vector edge_points_data; + getEdgePoints( + bound, edge_points, lat_dist_to_path, edge_points_data, start_segment_idx, end_segment_idx); + + // sort points + std::vector sorted_points; + sortPolygonPoints(edge_points_data, sorted_points); + + // update boundary + bound = updateBoundary(bound, sorted_points, start_segment_idx, end_segment_idx); + } +} + } // namespace behavior_path_planner 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 375704ac8a0b..dc9a19e4da7d 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 @@ -276,10 +276,8 @@ PathWithLaneId LaneChangeModule::getReferencePath() const const auto expanded_lanes = util::expandLanelets( shorten_lanes, parameters_->drivable_area_left_bound_offset, parameters_->drivable_area_right_bound_offset); - - reference_path.drivable_area = util::generateDrivableArea( - reference_path, expanded_lanes, common_parameters.drivable_area_resolution, - common_parameters.vehicle_length, planner_data_); + util::generateDrivableArea( + reference_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); return reference_path; } @@ -621,10 +619,7 @@ void LaneChangeModule::generateExtendedDrivableArea(PathWithLaneId & path) const auto expanded_lanes = util::expandLanelets( shorten_lanes, parameters_->drivable_area_left_bound_offset, parameters_->drivable_area_right_bound_offset); - - const double & resolution = common_parameters.drivable_area_resolution; - path.drivable_area = util::generateDrivableArea( - path, expanded_lanes, resolution, common_parameters.vehicle_length, planner_data_); + util::generateDrivableArea(path, expanded_lanes, common_parameters.vehicle_length, planner_data_); } void LaneChangeModule::updateOutputTurnSignal(BehaviorModuleOutput & output) diff --git a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp index 3ad35afb276a..6d958cfc9810 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp @@ -142,8 +142,7 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const shorten_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); - reference_path.drivable_area = util::generateDrivableArea( - reference_path, expanded_lanes, p.drivable_area_resolution, p.vehicle_length, planner_data_); + util::generateDrivableArea(reference_path, expanded_lanes, p.vehicle_length, planner_data_); return reference_path; } diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 532d3173c315..5ebbf2e41547 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -189,9 +189,8 @@ BehaviorModuleOutput PullOutModule::plan() const auto expanded_lanes = util::expandLanelets( shorten_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); - path.drivable_area = util::generateDrivableArea( - path, expanded_lanes, planner_data_->parameters.drivable_area_resolution, - planner_data_->parameters.vehicle_length, planner_data_); + util::generateDrivableArea( + path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_); output.path = std::make_shared(path); output.turn_signal_info = calcTurnSignalInfo(); @@ -287,9 +286,8 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() parameters_.drivable_area_right_bound_offset); auto candidate_path = status_.back_finished ? getCurrentPath() : status_.backward_path; - candidate_path.drivable_area = util::generateDrivableArea( - candidate_path, expanded_lanes, planner_data_->parameters.drivable_area_resolution, - planner_data_->parameters.vehicle_length, planner_data_); + util::generateDrivableArea( + candidate_path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_); auto stop_path = candidate_path; for (auto & p : stop_path.points) { p.point.longitudinal_velocity_mps = 0.0; diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp index ab8e77ffe138..b72558260ed7 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp @@ -91,10 +91,9 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) } // Generate drivable area - const double resolution = common_parameters.drivable_area_resolution; const auto shorten_lanes = util::cutOverlappedLanes(shift_path, drivable_lanes); - shift_path.drivable_area = util::generateDrivableArea( - shift_path, shorten_lanes, resolution, common_parameters.vehicle_length, planner_data_); + util::generateDrivableArea( + shift_path, shorten_lanes, common_parameters.vehicle_length, planner_data_); shift_path.header = planner_data_->route_handler->getRouteHeader(); diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index faea572f97db..29fcf8e12233 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -442,8 +442,7 @@ BehaviorModuleOutput PullOverModule::plan() const auto lane = util::expandLanelets( shorten_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); - path.drivable_area = util::generateDrivableArea( - path, lane, p.drivable_area_resolution, p.vehicle_length, planner_data_); + util::generateDrivableArea(path, lane, p.vehicle_length, planner_data_); } BehaviorModuleOutput output; @@ -615,10 +614,8 @@ PathWithLaneId PullOverModule::getReferencePath() const const auto lanes = util::expandLanelets( shorten_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); - - reference_path.drivable_area = util::generateDrivableArea( - reference_path, lanes, common_parameters.drivable_area_resolution, - common_parameters.vehicle_length, planner_data_); + util::generateDrivableArea( + reference_path, lanes, common_parameters.vehicle_length, planner_data_); return reference_path; } @@ -666,9 +663,7 @@ PathWithLaneId PullOverModule::generateStopPath() const shorten_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); - stop_path.drivable_area = util::generateDrivableArea( - stop_path, lanes, common_parameters.drivable_area_resolution, common_parameters.vehicle_length, - planner_data_); + util::generateDrivableArea(stop_path, lanes, common_parameters.vehicle_length, planner_data_); return stop_path; } diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index fb87fadc476b..8f07df3f5d57 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -421,8 +421,7 @@ void SideShiftModule::adjustDrivableArea(ShiftedPath * path) const { const auto & p = planner_data_->parameters; - path->path.drivable_area = util::generateDrivableArea( - path->path, expanded_lanes, p.drivable_area_resolution, p.vehicle_length, planner_data_); + util::generateDrivableArea(path->path, expanded_lanes, p.vehicle_length, planner_data_); } } diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 6683c987a9c8..b9289248465c 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -963,7 +963,8 @@ PathWithLaneId removeOverlappingPoints(const PathWithLaneId & input_path) filtered_path.points.push_back(pt); } } - filtered_path.drivable_area = input_path.drivable_area; + filtered_path.left_bound = input_path.left_bound; + filtered_path.right_bound = input_path.right_bound; return filtered_path; } @@ -1086,7 +1087,8 @@ bool setGoal( output_ptr->points.back().lane_ids = input.points.back().lane_ids; } - output_ptr->drivable_area = input.drivable_area; + output_ptr->left_bound = input.left_bound; + output_ptr->right_bound = input.right_bound; return true; } catch (std::out_of_range & ex) { RCLCPP_ERROR_STREAM( @@ -1342,148 +1344,170 @@ std::vector cutOverlappedLanes( return shorten_lanes; } -// input lanes must be in sequence -// NOTE: lanes in the path argument is used to calculate the size of the drivable area to cover -// designated forward and backward length by getPathScope function. -// lanes argument is used to determine (= draw) the drivable area. -// This is because lanes argument has multiple parallel lanes which makes hard to calculate -// the size of the drivable area -OccupancyGrid generateDrivableArea( - const PathWithLaneId & path, const std::vector & lanes, const double resolution, - const double vehicle_length, const std::shared_ptr planner_data) +size_t findNearestSegmentIndex( + const std::vector & points, const geometry_msgs::msg::Pose & pose, + const double dist_threshold, const double yaw_threshold) { - const auto transformed_lanes = util::transformToLanelets(lanes); - const auto & params = planner_data->parameters; - const auto route_handler = planner_data->route_handler; - const auto current_pose = planner_data->self_pose; - - // calculate min/max x and y from lanes in path argument (not from lanes argument) - const auto path_scope = drivable_area_utils::getPathScope( - path, route_handler, current_pose->pose, params.drivable_lane_forward_length, - params.drivable_lane_backward_length, params.drivable_lane_margin, - params.ego_nearest_dist_threshold, params.ego_nearest_yaw_threshold); - - const double min_x = - drivable_area_utils::quantize(path_scope.at(0) - params.drivable_area_margin, resolution); - const double min_y = - drivable_area_utils::quantize(path_scope.at(1) - params.drivable_area_margin, resolution); - const double max_x = - drivable_area_utils::quantize(path_scope.at(2) + params.drivable_area_margin, resolution); - const double max_y = - drivable_area_utils::quantize(path_scope.at(3) + params.drivable_area_margin, resolution); - - const double width = max_x - min_x; - const double height = max_y - min_y; - - lanelet::ConstLanelets drivable_lanes; - { // add lanes which covers initial and final footprints - // 1. add preceding lanes before current pose - const auto lanes_before_current_pose = route_handler->getLanesBeforePose( - current_pose->pose, params.drivable_lane_backward_length + params.drivable_lane_margin); - drivable_lanes.insert( - drivable_lanes.end(), lanes_before_current_pose.begin(), lanes_before_current_pose.end()); - - // 2. add lanes - drivable_lanes.insert(drivable_lanes.end(), transformed_lanes.begin(), transformed_lanes.end()); - - // 3. add succeeding lanes after goal - if (containsGoal(transformed_lanes, route_handler->getGoalLaneId())) { - const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length); - drivable_lanes.insert(drivable_lanes.end(), lanes_after_goal.begin(), lanes_after_goal.end()); - } - } - - OccupancyGrid occupancy_grid; - PoseStamped grid_origin; + const auto nearest_idx = + motion_utils::findNearestSegmentIndex(points, pose, dist_threshold, yaw_threshold); + return nearest_idx ? nearest_idx.get() + : motion_utils::findNearestSegmentIndex(points, pose.position); +} - // calculate grid origin - { - grid_origin.header = current_pose->header; +geometry_msgs::msg::Pose calcLongitudinalOffsetStartPose( + const std::vector & points, const geometry_msgs::msg::Pose & pose, + const size_t nearest_segment_idx, const double offset) +{ + const double offset_length = + motion_utils::calcLongitudinalOffsetToSegment(points, nearest_segment_idx, pose.position); + const auto offset_pose = + motion_utils::calcLongitudinalOffsetPose(points, nearest_segment_idx, offset_length + offset); - grid_origin.pose.position.x = min_x; - grid_origin.pose.position.y = min_y; - grid_origin.pose.position.z = current_pose->pose.position.z; - } + return offset_pose ? offset_pose.get() : points.at(nearest_segment_idx); +} - // header - { - occupancy_grid.header.stamp = current_pose->header.stamp; - occupancy_grid.header.frame_id = "map"; - } +geometry_msgs::msg::Pose calcLongitudinalOffsetGoalPose( + const std::vector & points, const geometry_msgs::msg::Pose & pose, + const size_t nearest_segment_idx, const double offset) +{ + const double offset_length = + motion_utils::calcLongitudinalOffsetToSegment(points, nearest_segment_idx, pose.position); + const auto offset_pose = + motion_utils::calcLongitudinalOffsetPose(points, nearest_segment_idx, offset_length + offset); - // info - { - const int width_cell = std::round(width / resolution); - const int height_cell = std::round(height / resolution); + return offset_pose ? offset_pose.get() : points.at(nearest_segment_idx + 1); +} - occupancy_grid.info.map_load_time = occupancy_grid.header.stamp; - occupancy_grid.info.resolution = resolution; - occupancy_grid.info.width = width_cell; - occupancy_grid.info.height = height_cell; - occupancy_grid.info.origin = grid_origin.pose; - } +void generateDrivableArea( + PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, + const std::shared_ptr planner_data) +{ + std::vector left_bound; + std::vector right_bound; - // occupancy_grid.data = image; - { - constexpr uint8_t free_space = 0; - constexpr uint8_t occupied_space = 100; - // get transform - tf2::Stamped tf_grid2map, tf_map2grid; - tf2::fromMsg(grid_origin, tf_grid2map); - tf_map2grid.setData(tf_grid2map.inverse()); - const auto geom_tf_map2grid = tf2::toMsg(tf_map2grid); - - // convert lane polygons into cv type - cv::Mat cv_image( - occupancy_grid.info.width, occupancy_grid.info.height, CV_8UC1, cv::Scalar(occupied_space)); - for (const auto & lane : drivable_lanes) { - lanelet::BasicPolygon2d lane_poly = lane.polygon2d().basicPolygon(); - - if (lane.hasAttribute("intersection_area")) { - const std::string area_id = lane.attributeOr("intersection_area", "none"); - const auto intersection_area = - route_handler->getIntersectionAreaById(atoi(area_id.c_str())); - const auto poly = lanelet::utils::to2D(intersection_area).basicPolygon(); - std::vector lane_polys{}; - if (boost::geometry::intersection(poly, lane_poly, lane_polys)) { - lane_poly = lane_polys.front(); - } + // extract data + const auto transformed_lanes = util::transformToLanelets(lanes); + const auto current_pose = planner_data->self_pose; + const auto route_handler = planner_data->route_handler; + const auto & param = planner_data->parameters; + const double dist_threshold = std::numeric_limits::max(); + const double yaw_threshold = param.ego_nearest_yaw_threshold; + constexpr double overlap_threshold = 0.01; + + auto addLeftBoundPoints = [&left_bound](const lanelet::ConstLanelet & lane) { + for (const auto & lp : lane.leftBound()) { + geometry_msgs::msg::Pose current_pose; + current_pose.position = lanelet::utils::conversion::toGeomMsgPt(lp); + if (left_bound.empty()) { + left_bound.push_back(current_pose); + } else if ( + tier4_autoware_utils::calcDistance2d(current_pose, left_bound.back()) > overlap_threshold) { + left_bound.push_back(current_pose); } + } + }; - // create drivable area using opencv - std::vector> cv_polygons; - std::vector cv_polygon; - cv_polygon.reserve(lane_poly.size()); - for (const auto & p : lane_poly) { - const double z = lane.polygon3d().basicPolygon().at(0).z(); - Point geom_pt = tier4_autoware_utils::createPoint(p.x(), p.y(), z); - Point transformed_geom_pt; - tf2::doTransform(geom_pt, transformed_geom_pt, geom_tf_map2grid); - cv_polygon.push_back(toCVPoint(transformed_geom_pt, width, height, resolution)); - } - if (!cv_polygon.empty()) { - cv_polygons.push_back(cv_polygon); - // fill in drivable area and copy to occupancy grid - cv::fillPoly(cv_image, cv_polygons, cv::Scalar(free_space)); + auto addRightBoundPoints = [&right_bound](const lanelet::ConstLanelet & lane) { + for (const auto & rp : lane.rightBound()) { + geometry_msgs::msg::Pose current_pose; + current_pose.position = lanelet::utils::conversion::toGeomMsgPt(rp); + if (right_bound.empty()) { + right_bound.push_back(current_pose); + } else if ( + tier4_autoware_utils::calcDistance2d(current_pose, right_bound.back()) > + overlap_threshold) { + right_bound.push_back(current_pose); } } + }; - // Closing - // NOTE: Because of the discretization error, there may be some discontinuity between two - // successive lanelets in the drivable area. This issue is dealt with by the erode/dilate - // process. - constexpr int num_iter = 1; - cv::Mat cv_erode, cv_dilate; - cv::erode(cv_image, cv_erode, cv::Mat(), cv::Point(-1, -1), num_iter); - cv::dilate(cv_erode, cv_dilate, cv::Mat(), cv::Point(-1, -1), num_iter); - - // const auto & cv_image_reshaped = cv_dilate.reshape(1, 1); - imageToOccupancyGrid(cv_dilate, &occupancy_grid); - occupancy_grid.data[0] = 0; - // cv_image_reshaped.copyTo(occupancy_grid.data); + // Insert Position + for (const auto & lane : lanes) { + addLeftBoundPoints(lane.left_lane); + addRightBoundPoints(lane.right_lane); + } + + // Insert points after goal + if (containsGoal(transformed_lanes, route_handler->getGoalLaneId())) { + const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length); + for (const auto & lane : lanes_after_goal) { + addLeftBoundPoints(lane); + addRightBoundPoints(lane); + } + } + + // Give Orientation + motion_utils::insertOrientation(left_bound, true); + motion_utils::insertOrientation(right_bound, true); + + // Get Closest segment for the start point + constexpr double front_length = 0.5; + const auto front_pose = path.points.empty() ? current_pose->pose : path.points.front().point.pose; + const size_t front_left_start_idx = + findNearestSegmentIndex(left_bound, front_pose, dist_threshold, yaw_threshold); + const size_t front_right_start_idx = + findNearestSegmentIndex(right_bound, front_pose, dist_threshold, yaw_threshold); + const auto left_start_pose = + calcLongitudinalOffsetStartPose(left_bound, front_pose, front_left_start_idx, -front_length); + const auto right_start_pose = + calcLongitudinalOffsetStartPose(right_bound, front_pose, front_right_start_idx, -front_length); + const size_t left_start_idx = + findNearestSegmentIndex(left_bound, left_start_pose, dist_threshold, yaw_threshold); + const size_t right_start_idx = + findNearestSegmentIndex(right_bound, right_start_pose, dist_threshold, yaw_threshold); + + // Get Closest segment for the goal point + const auto goal_pose = path.points.empty() ? current_pose->pose : path.points.back().point.pose; + const size_t goal_left_start_idx = + findNearestSegmentIndex(left_bound, goal_pose, dist_threshold, yaw_threshold); + const size_t goal_right_start_idx = + findNearestSegmentIndex(right_bound, goal_pose, dist_threshold, yaw_threshold); + const auto left_goal_pose = + calcLongitudinalOffsetGoalPose(left_bound, goal_pose, goal_left_start_idx, vehicle_length); + const auto right_goal_pose = + calcLongitudinalOffsetGoalPose(right_bound, goal_pose, goal_right_start_idx, vehicle_length); + const size_t left_goal_idx = + findNearestSegmentIndex(left_bound, left_goal_pose, dist_threshold, yaw_threshold); + const size_t right_goal_idx = + findNearestSegmentIndex(right_bound, right_goal_pose, dist_threshold, yaw_threshold); + + // Store Data + path.left_bound.clear(); + path.right_bound.clear(); + path.left_bound.reserve(left_goal_idx - left_start_idx); + path.right_bound.reserve(right_goal_idx - right_start_idx); + + // Insert a start point + path.left_bound.push_back(left_start_pose.position); + path.right_bound.push_back(right_start_pose.position); + + // Insert middle points + for (size_t i = left_start_idx + 1; i <= left_goal_idx; ++i) { + const auto & next_point = left_bound.at(i).position; + const double dist = tier4_autoware_utils::calcDistance2d(path.left_bound.back(), next_point); + if (dist > overlap_threshold) { + path.left_bound.push_back(next_point); + } + } + for (size_t i = right_start_idx + 1; i <= right_goal_idx; ++i) { + const auto & next_point = right_bound.at(i).position; + const double dist = tier4_autoware_utils::calcDistance2d(path.right_bound.back(), next_point); + if (dist > overlap_threshold) { + path.right_bound.push_back(next_point); + } + } + + // Insert a goal point + if ( + tier4_autoware_utils::calcDistance2d(path.left_bound.back(), left_goal_pose.position) > + overlap_threshold) { + path.left_bound.push_back(left_goal_pose.position); + } + if ( + tier4_autoware_utils::calcDistance2d(path.right_bound.back(), right_goal_pose.position) > + overlap_threshold) { + path.right_bound.push_back(right_goal_pose.position); } - - return occupancy_grid; } double getDistanceToEndOfLane(const Pose & current_pose, const lanelet::ConstLanelets & lanelets) @@ -1642,7 +1666,8 @@ Path convertToPathFromPathWithLaneId(const PathWithLaneId & path_with_lane_id) { Path path; path.header = path_with_lane_id.header; - path.drivable_area = path_with_lane_id.drivable_area; + path.left_bound = path_with_lane_id.left_bound; + path.right_bound = path_with_lane_id.right_bound; path.points.reserve(path_with_lane_id.points.size()); for (const auto & pt_with_lane_id : path_with_lane_id.points) { path.points.push_back(pt_with_lane_id.point); @@ -1838,42 +1863,6 @@ std::vector getTargetLaneletPolygons( return polygons; } -void occupancyGridToImage(const OccupancyGrid & occupancy_grid, cv::Mat * cv_image) -{ - const int width = cv_image->cols; - const int height = cv_image->rows; - for (int x = width - 1; x >= 0; x--) { - for (int y = height - 1; y >= 0; y--) { - const int idx = (height - 1 - y) + (width - 1 - x) * height; - const unsigned char intensity = occupancy_grid.data.at(idx); - cv_image->at(y, x) = intensity; - } - } -} - -void imageToOccupancyGrid(const cv::Mat & cv_image, OccupancyGrid * occupancy_grid) -{ - const int width = cv_image.cols; - const int height = cv_image.rows; - occupancy_grid->data.clear(); - occupancy_grid->data.resize(width * height); - for (int x = width - 1; x >= 0; x--) { - for (int y = height - 1; y >= 0; y--) { - const int idx = (height - 1 - y) + (width - 1 - x) * height; - const unsigned char intensity = cv_image.at(y, x); - occupancy_grid->data.at(idx) = intensity; - } - } -} - -cv::Point toCVPoint( - const Point & geom_point, const double width_m, const double height_m, const double resolution) -{ - return { - static_cast((height_m - geom_point.y) / resolution), - static_cast((width_m - geom_point.x) / resolution)}; -} - // TODO(Horibe) There is a similar function in route_handler. std::shared_ptr generateCenterLinePath( const std::shared_ptr & planner_data) @@ -1908,8 +1897,7 @@ std::shared_ptr generateCenterLinePath( centerline_path->header = route_handler->getRouteHeader(); - centerline_path->drivable_area = util::generateDrivableArea( - *centerline_path, drivable_lanes, p.drivable_area_resolution, p.vehicle_length, planner_data); + util::generateDrivableArea(*centerline_path, drivable_lanes, p.vehicle_length, planner_data); return centerline_path; } diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 64a79e87bd76..5fea6f0a13e8 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -454,7 +454,8 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath output_path_msg = to_path(*input_path_msg); output_path_msg.header.frame_id = "map"; output_path_msg.header.stamp = this->now(); - output_path_msg.drivable_area = input_path_msg->drivable_area; + output_path_msg.left_bound = input_path_msg->left_bound; + output_path_msg.right_bound = input_path_msg->right_bound; return output_path_msg; } @@ -475,7 +476,8 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath output_path_msg.header.stamp = this->now(); // TODO(someone): This must be updated in each scene module, but copy from input message for now. - output_path_msg.drivable_area = input_path_msg->drivable_area; + output_path_msg.left_bound = input_path_msg->left_bound; + output_path_msg.right_bound = input_path_msg->right_bound; return output_path_msg; } diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp index 0f6921691025..9c00a9839415 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp @@ -399,7 +399,8 @@ PathWithLaneId trimPathFromSelfPose( PathWithLaneId output{}; output.header = input.header; - output.drivable_area = input.drivable_area; + output.left_bound = input.left_bound; + output.right_bound = input.right_bound; double dist_sum = 0; for (size_t i = nearest_idx; i < input.points.size(); ++i) { output.points.push_back(input.points.at(i)); diff --git a/planning/obstacle_avoidance_planner/CMakeLists.txt b/planning/obstacle_avoidance_planner/CMakeLists.txt index a215e158fab0..b1b29a1a5f38 100644 --- a/planning/obstacle_avoidance_planner/CMakeLists.txt +++ b/planning/obstacle_avoidance_planner/CMakeLists.txt @@ -10,12 +10,10 @@ 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/costmap_generator.cpp src/eb_path_optimizer.cpp src/mpt_optimizer.cpp src/utils/utils.cpp src/utils/debug_utils.cpp - src/utils/cv_utils.cpp src/node.cpp ) diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp index 4fb800e4a8ea..2e1ce6502441 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp @@ -36,16 +36,6 @@ using VehicleBounds = std::vector; using SequentialBounds = std::vector; using BoundsCandidates = std::vector; -using SequentialBoundsCandidates = std::vector; - -struct CVMaps -{ - cv::Mat drivable_area; - cv::Mat clearance_map; - cv::Mat only_objects_clearance_map; - cv::Mat area_with_objects_map; - nav_msgs::msg::MapMetaData map_info; -}; struct QPParam { @@ -171,7 +161,7 @@ struct DebugData std::vector extended_fixed_traj; std::vector extended_non_fixed_traj; - SequentialBoundsCandidates sequential_bounds_candidates; + BoundsCandidates bounds_candidates; }; struct Trajectories diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/costmap_generator.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/costmap_generator.hpp deleted file mode 100644 index cdff21bd7113..000000000000 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/costmap_generator.hpp +++ /dev/null @@ -1,63 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef OBSTACLE_AVOIDANCE_PLANNER__COSTMAP_GENERATOR_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__COSTMAP_GENERATOR_HPP_ - -#include "obstacle_avoidance_planner/common_structs.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" - -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "nav_msgs/msg/occupancy_grid.hpp" - -#include "boost/optional.hpp" - -#include -#include -#include -#include - -class CostmapGenerator -{ -public: - CVMaps getMaps( - const bool enable_avoidance, const autoware_auto_planning_msgs::msg::Path & path, - const std::vector & objects, - 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, DebugData & debug_data) const; - - cv::Mat getClearanceMap(const cv::Mat & drivable_area, DebugData & debug_data) const; - - cv::Mat drawObstaclesOnImage( - const bool enable_avoidance, - const std::vector & objects, - const std::vector & path_points, - 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 * debug_avoiding_objects, - DebugData & debug_data) const; - - cv::Mat getDrivableAreaInCV( - const nav_msgs::msg::OccupancyGrid & occupancy_grid, DebugData & debug_data) const; -}; -#endif // OBSTACLE_AVOIDANCE_PLANNER__COSTMAP_GENERATOR_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp index 81453424d3a0..d48f1f384ae5 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp @@ -201,8 +201,9 @@ class MPTOptimizer std::vector getReferencePoints( const std::vector & points, - const std::unique_ptr & prev_mpt_points, const bool enable_avoidance, - const CVMaps & maps, DebugData & debug_data) const; + const std::vector & left_bound, + const std::vector & right_bound, + const std::unique_ptr & prev_mpt_points, DebugData & debug_data) const; void calcPlanningFromEgo(std::vector & ref_points) const; @@ -218,12 +219,11 @@ class MPTOptimizer const std::unique_ptr & prev_trajs) const; void calcBounds( - std::vector & ref_points, const bool enable_avoidance, const CVMaps & maps, - const std::unique_ptr & prev_trajs, DebugData & debug_data) const; + std::vector & ref_points, + const std::vector & left_bound, + const std::vector & right_bound, DebugData & debug_data) const; - void calcVehicleBounds( - std::vector & ref_points, const CVMaps & maps, DebugData & debug_data, - const bool enable_avoidance) const; + void calcVehicleBounds(std::vector & ref_points, DebugData & debug_data) const; // void calcFixState( // std::vector & ref_points, @@ -269,19 +269,6 @@ class MPTOptimizer std::vector getMPTFixedPoints( const std::vector & ref_points) const; - BoundsCandidates getBoundsCandidates( - const bool enable_avoidance, const geometry_msgs::msg::Pose & avoiding_point, - const CVMaps & maps, DebugData & debug_data) const; - - CollisionType getCollisionType( - const CVMaps & maps, const bool enable_avoidance, - const geometry_msgs::msg::Pose & avoiding_point, const double traversed_dist, - const double bound_angle) const; - - boost::optional getClearance( - const cv::Mat & clearance_map, const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & map_info) const; - ObjectiveMatrix getObjectiveMatrix( const MPTMatrix & mpt_mat, const ValueMatrix & obj_mat, [[maybe_unused]] const std::vector & ref_points, DebugData & debug_data) const; @@ -299,7 +286,9 @@ class MPTOptimizer const bool enable_avoidance, const std::vector & smoothed_points, const std::vector & path_points, - const std::unique_ptr & prev_trajs, const CVMaps & maps, + const std::vector & left_bound, + const std::vector & right_bound, + const std::unique_ptr & prev_trajs, const geometry_msgs::msg::Pose & current_ego_pose, const double current_ego_vel, DebugData & debug_data); }; diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index 4e57bac6a084..6b9683fb2440 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -16,7 +16,6 @@ #include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/costmap_generator.hpp" #include "obstacle_avoidance_planner/eb_path_optimizer.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "opencv2/core.hpp" @@ -191,7 +190,6 @@ class ObstacleAvoidancePlanner : public rclcpp::Node double max_delta_time_sec_for_replan_; // core algorithm - std::unique_ptr costmap_generator_ptr_; std::unique_ptr eb_path_optimizer_ptr_; std::unique_ptr mpt_optimizer_ptr_; @@ -266,7 +264,7 @@ class ObstacleAvoidancePlanner : public rclcpp::Node Trajectory generateTrajectory(const PlannerData & planner_data); - Trajectories optimizeTrajectory(const PlannerData & planner_data, const CVMaps & cv_maps); + Trajectories optimizeTrajectory(const PlannerData & planner_data); Trajectories getPrevTrajs(const std::vector & path_points) const; @@ -274,8 +272,7 @@ class ObstacleAvoidancePlanner : public rclcpp::Node const std::vector & path_points, std::vector & traj_points) const; void insertZeroVelocityOutsideDrivableArea( - const PlannerData & planner_data, std::vector & traj_points, - const CVMaps & cv_maps); + const PlannerData & planner_data, std::vector & traj_points); void publishDebugDataInOptimization( const PlannerData & planner_data, const std::vector & traj_points); diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/cv_utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/cv_utils.hpp deleted file mode 100644 index da5edace4353..000000000000 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/cv_utils.hpp +++ /dev/null @@ -1,121 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__UTILS__CV_UTILS_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__UTILS__CV_UTILS_HPP_ - -#include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/eb_path_optimizer.hpp" -#include "opencv2/core.hpp" -#include "opencv2/imgproc/imgproc_c.h" -#include "opencv2/opencv.hpp" - -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "geometry_msgs/msg/point32.hpp" - -#include "boost/optional/optional_fwd.hpp" - -#include -#include - -namespace util -{ -struct Footprint; -} - -struct Edges -{ - int front_idx; - int back_idx; - geometry_msgs::msg::Point extended_front; - geometry_msgs::msg::Point extended_back; - geometry_msgs::msg::Point origin; -}; - -struct PolygonPoints -{ - std::vector points_in_image; - std::vector points_in_map; -}; - -namespace cv_utils -{ -void getOccupancyGridValue( - const nav_msgs::msg::OccupancyGrid & og, const int i, const int j, unsigned char & value); - -void putOccupancyGridValue( - nav_msgs::msg::OccupancyGrid & og, const int i, const int j, const unsigned char value); -} // namespace cv_utils - -namespace cv_polygon_utils -{ -PolygonPoints getPolygonPoints( - const std::vector & points, - const nav_msgs::msg::MapMetaData & map_info); - -PolygonPoints getPolygonPoints( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info); - -PolygonPoints getPolygonPointsFromBB( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info); - -PolygonPoints getPolygonPointsFromCircle( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info); - -PolygonPoints getPolygonPointsFromPolygon( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info); - -std::vector getCVPolygon( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const PolygonPoints & polygon_points, - const std::vector & path_points, - const cv::Mat & clearance_map, const nav_msgs::msg::MapMetaData & map_info); - -std::vector getDefaultCVPolygon( - const std::vector & points_in_image); - -std::vector getExtendedCVPolygon( - const std::vector & points_in_image, - const std::vector & points_in_map, - const geometry_msgs::msg::Pose & nearest_path_point_pose, - const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info); - -boost::optional getEdges( - const std::vector & points_in_image, - const std::vector & points_in_map, - const geometry_msgs::msg::Pose & nearest_path_point_pose, - const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info); -} // namespace cv_polygon_utils - -namespace cv_drivable_area_utils -{ -bool isOutsideDrivableAreaFromRectangleFootprint( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, - const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, - const VehicleParam & vehicle_param); - -bool isOutsideDrivableAreaFromCirclesFootprint( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, - const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, - const std::vector vehicle_circle_longitudinal_offsets, - const double vehicle_circle_radius); -} // namespace cv_drivable_area_utils - -#endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__CV_UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/debug_utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/debug_utils.hpp index 083b5dd00efe..506587f63f5e 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/debug_utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/debug_utils.hpp @@ -42,9 +42,6 @@ visualization_msgs::msg::MarkerArray getDebugVisualizationMarker( visualization_msgs::msg::MarkerArray getDebugVisualizationWallMarker( DebugData & debug_data, const VehicleParam & vehicle_param); - -nav_msgs::msg::OccupancyGrid getDebugCostmap( - const cv::Mat & clearance_map, const nav_msgs::msg::OccupancyGrid & occupancy_grid); } // namespace debug_utils #endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__DEBUG_UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp index 03129dd1e4bd..1f1b3dbdf924 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp @@ -354,4 +354,12 @@ namespace utils void logOSQPSolutionStatus(const int solution_status, const std::string & msg); } // namespace utils +namespace drivable_area_utils +{ +bool isOutsideDrivableAreaFromRectangleFootprint( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, + const std::vector left_bound, + const std::vector right_bound, const VehicleParam & vehicle_param); +} + #endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/src/costmap_generator.cpp b/planning/obstacle_avoidance_planner/src/costmap_generator.cpp deleted file mode 100644 index 0936ccc9854c..000000000000 --- a/planning/obstacle_avoidance_planner/src/costmap_generator.cpp +++ /dev/null @@ -1,337 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "obstacle_avoidance_planner/costmap_generator.hpp" - -#include "obstacle_avoidance_planner/utils/cv_utils.hpp" -#include "obstacle_avoidance_planner/utils/utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - -namespace -{ -cv::Point toCVPoint(const geometry_msgs::msg::Point & p) -{ - cv::Point cv_point; - cv_point.x = p.x; - cv_point.y = p.y; - return cv_point; -} - -bool isAvoidingObjectType( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const TrajectoryParam & traj_param) -{ - if ( - (object.classification.at(0).label == object.classification.at(0).UNKNOWN && - traj_param.is_avoiding_unknown) || - (object.classification.at(0).label == object.classification.at(0).CAR && - traj_param.is_avoiding_car) || - (object.classification.at(0).label == object.classification.at(0).TRUCK && - traj_param.is_avoiding_truck) || - (object.classification.at(0).label == object.classification.at(0).BUS && - traj_param.is_avoiding_bus) || - (object.classification.at(0).label == object.classification.at(0).BICYCLE && - traj_param.is_avoiding_bicycle) || - (object.classification.at(0).label == object.classification.at(0).MOTORCYCLE && - traj_param.is_avoiding_motorbike) || - (object.classification.at(0).label == object.classification.at(0).PEDESTRIAN && - traj_param.is_avoiding_pedestrian)) { - return true; - } - return false; -} - -bool arePointsInsideDriveableArea( - const std::vector & image_points, const cv::Mat & clearance_map) -{ - bool points_inside_area = false; - for (const auto & image_point : image_points) { - const float clearance = - clearance_map.ptr(static_cast(image_point.y))[static_cast(image_point.x)]; - if (clearance > 0) { - points_inside_area = true; - } - } - return points_inside_area; -} - -bool isAvoidingObject( - const PolygonPoints & polygon_points, - const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info, - const std::vector & path_points, - const TrajectoryParam & traj_param) -{ - if (path_points.empty()) { - return false; - } - if (!isAvoidingObjectType(object, traj_param)) { - return false; - } - const auto image_point = geometry_utils::transformMapToOptionalImage( - object.kinematics.initial_pose_with_covariance.pose.position, map_info); - if (!image_point) { - return false; - } - - // TODO(murooka) remove findNearestIndex without any constraints - const int nearest_idx = motion_utils::findNearestIndex( - path_points, object.kinematics.initial_pose_with_covariance.pose.position); - const auto nearest_path_point = path_points[nearest_idx]; - const auto rel_p = geometry_utils::transformToRelativeCoordinate2D( - object.kinematics.initial_pose_with_covariance.pose.position, nearest_path_point.pose); - // skip object located back the beginning of path points - if (nearest_idx == 0 && rel_p.x < 0) { - return false; - } - - /* - const float object_clearance_from_road = - clearance_map.ptr( - static_cast(image_point.get().y))[static_cast(image_point.get().x)] * - map_info.resolution; - */ - const geometry_msgs::msg::Vector3 twist = - object.kinematics.initial_twist_with_covariance.twist.linear; - const double vel = std::sqrt(twist.x * twist.x + twist.y * twist.y + twist.z * twist.z); - /* - const auto nearest_path_point_image = - geometry_utils::transformMapToOptionalImage(nearest_path_point.pose.position, map_info); - if (!nearest_path_point_image) { - return false; - } - const float nearest_path_point_clearance = - clearance_map.ptr(static_cast( - nearest_path_point_image.get().y))[static_cast(nearest_path_point_image.get().x)] * - map_info.resolution; - */ - const double lateral_offset_to_path = motion_utils::calcLateralOffset( - path_points, object.kinematics.initial_pose_with_covariance.pose.position); - if ( - // nearest_path_point_clearance - traj_param.center_line_width * 0.5 < - // object_clearance_from_road || - std::abs(lateral_offset_to_path) < traj_param.center_line_width * 0.5 || - vel > traj_param.max_avoiding_objects_velocity_ms || - !arePointsInsideDriveableArea(polygon_points.points_in_image, clearance_map)) { - return false; - } - return true; -} -} // namespace - -namespace tier4_autoware_utils -{ -template <> -geometry_msgs::msg::Point getPoint(const cv::Point & p) -{ - geometry_msgs::msg::Point geom_p; - geom_p.x = p.x; - geom_p.y = p.y; - - return geom_p; -} -} // namespace tier4_autoware_utils - -CVMaps CostmapGenerator::getMaps( - const bool enable_avoidance, const autoware_auto_planning_msgs::msg::Path & path, - const std::vector & objects, - const TrajectoryParam & traj_param, DebugData & debug_data) -{ - stop_watch_.tic(__func__); - - // make cv_maps - CVMaps cv_maps; - - cv_maps.drivable_area = getDrivableAreaInCV(path.drivable_area, debug_data); - cv_maps.clearance_map = getClearanceMap(cv_maps.drivable_area, debug_data); - - std::vector debug_avoiding_objects; - cv::Mat objects_image = drawObstaclesOnImage( - enable_avoidance, objects, path.points, path.drivable_area.info, cv_maps.drivable_area, - cv_maps.clearance_map, traj_param, &debug_avoiding_objects, debug_data); - - cv_maps.area_with_objects_map = - getAreaWithObjects(cv_maps.drivable_area, objects_image, debug_data); - cv_maps.only_objects_clearance_map = getClearanceMap(objects_image, debug_data); - cv_maps.map_info = path.drivable_area.info; - - // debug data - debug_data.clearance_map = cv_maps.clearance_map; - debug_data.only_object_clearance_map = cv_maps.only_objects_clearance_map; - debug_data.area_with_objects_map = cv_maps.area_with_objects_map; - debug_data.avoiding_objects = debug_avoiding_objects; - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - return cv_maps; -} - -cv::Mat CostmapGenerator::getDrivableAreaInCV( - const nav_msgs::msg::OccupancyGrid & occupancy_grid, DebugData & debug_data) const -{ - stop_watch_.tic(__func__); - - cv::Mat drivable_area = cv::Mat(occupancy_grid.info.width, occupancy_grid.info.height, CV_8UC1); - - drivable_area.forEach([&](unsigned char & value, const int * position) -> void { - cv_utils::getOccupancyGridValue(occupancy_grid, position[0], position[1], value); - }); - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - return drivable_area; -} - -cv::Mat CostmapGenerator::getClearanceMap( - const cv::Mat & drivable_area, DebugData & debug_data) const -{ - stop_watch_.tic(__func__); - - cv::Mat clearance_map; - cv::distanceTransform(drivable_area, clearance_map, cv::DIST_L2, 5); - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - return clearance_map; -} - -cv::Mat CostmapGenerator::drawObstaclesOnImage( - const bool enable_avoidance, - const std::vector & objects, - const std::vector & path_points, - 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 * debug_avoiding_objects, - DebugData & debug_data) const -{ - stop_watch_.tic(__func__); - - std::vector path_points_inside_area; - for (const auto & point : path_points) { - std::vector points; - geometry_msgs::msg::Point image_point; - if (!geometry_utils::transformMapToImage(point.pose.position, map_info, image_point)) { - continue; - } - const float clearance = - clearance_map.ptr(static_cast(image_point.y))[static_cast(image_point.x)]; - if (clearance < 1e-5) { - continue; - } - path_points_inside_area.push_back(point); - } - - // NOTE: objects image is too sparse so that creating cost map is heavy. - // Then, objects image is created by filling dilated drivable area, - // instead of "cv::Mat objects_image = cv::Mat::ones(clearance_map.rows, clearance_map.cols, - // CV_8UC1) * 255;". - constexpr double dilate_margin = 1.0; - cv::Mat objects_image; - const int dilate_size = static_cast( - (1.8 + dilate_margin) / - (std::sqrt(2) * map_info.resolution)); // TODO(murooka) use clearance_from_object - cv::dilate(drivable_area, objects_image, cv::Mat(), cv::Point(-1, -1), dilate_size); - - if (!enable_avoidance) { - return objects_image; - } - - // fill object - std::vector> cv_polygons; - std::vector> obj_cog_info; - std::vector obj_positions; - for (const auto & object : objects) { - const PolygonPoints polygon_points = cv_polygon_utils::getPolygonPoints(object, map_info); - if (isAvoidingObject( - polygon_points, object, clearance_map, map_info, path_points_inside_area, traj_param)) { - const double lon_dist_to_path = motion_utils::calcSignedArcLength( - path_points, 0, object.kinematics.initial_pose_with_covariance.pose.position); - const double lat_dist_to_path = motion_utils::calcLateralOffset( - path_points, object.kinematics.initial_pose_with_covariance.pose.position); - obj_cog_info.push_back({lon_dist_to_path, lat_dist_to_path}); - obj_positions.push_back(object.kinematics.initial_pose_with_covariance.pose.position); - - cv_polygons.push_back(cv_polygon_utils::getCVPolygon( - object, polygon_points, path_points_inside_area, clearance_map, map_info)); - debug_avoiding_objects->push_back(object); - } - } - for (const auto & polygon : cv_polygons) { - cv::fillConvexPoly(objects_image, polygon, cv::Scalar(0)); - } - - // fill between objects in the same side - const auto get_closest_obj_point = [&](size_t idx) { - // TODO(murooka) remove findNearestIndex without any constraints - const auto & path_point = - path_points.at(motion_utils::findNearestIndex(path_points, obj_positions.at(idx))); - double min_dist = std::numeric_limits::min(); - size_t min_idx = 0; - for (size_t p_idx = 0; p_idx < cv_polygons.at(idx).size(); ++p_idx) { - const double dist = - tier4_autoware_utils::calcDistance2d(cv_polygons.at(idx).at(p_idx), path_point); - if (dist < min_dist) { - min_dist = dist; - min_idx = p_idx; - } - } - - geometry_msgs::msg::Point geom_point; - geom_point.x = cv_polygons.at(idx).at(min_idx).x; - geom_point.y = cv_polygons.at(idx).at(min_idx).y; - return geom_point; - }; - - std::vector> cv_between_polygons; - for (size_t i = 0; i < obj_positions.size(); ++i) { - for (size_t j = i + 1; j < obj_positions.size(); ++j) { - const auto & obj_info1 = obj_cog_info.at(i); - const auto & obj_info2 = obj_cog_info.at(j); - - // RCLCPP_ERROR_STREAM(rclcpp::get_logger("lat"), obj_info1.at(1) << " " << obj_info2.at(1)); - // RCLCPP_ERROR_STREAM(rclcpp::get_logger("lon"), obj_info1.at(0) << " " << obj_info2.at(0)); - - constexpr double max_lon_dist_to_convex_obstacles = 30.0; - if ( - obj_info1.at(1) * obj_info2.at(1) < 0 || - std::abs(obj_info1.at(0) - obj_info2.at(0)) > max_lon_dist_to_convex_obstacles) { - continue; - } - - std::vector cv_points; - cv_points.push_back(toCVPoint(obj_positions.at(i))); - cv_points.push_back(toCVPoint(get_closest_obj_point(i))); - cv_points.push_back(toCVPoint(get_closest_obj_point(j))); - cv_points.push_back(toCVPoint(obj_positions.at(j))); - - cv_between_polygons.push_back(cv_points); - } - } - /* - for (const auto & polygon : cv_between_polygons) { - cv::fillConvexPoly(objects_image, polygon, cv::Scalar(0)); - } - */ - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - - return objects_image; -} - -cv::Mat CostmapGenerator::getAreaWithObjects( - const cv::Mat & drivable_area, const cv::Mat & objects_image, DebugData & debug_data) const -{ - stop_watch_.tic(__func__); - - cv::Mat area_with_objects = cv::min(objects_image, drivable_area); - - debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; - return area_with_objects; -} diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 56d74c9eab4e..acc5ab823ca5 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -18,9 +18,11 @@ #include "obstacle_avoidance_planner/utils/utils.hpp" #include "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "tf2/utils.h" +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "nav_msgs/msg/map_meta_data.hpp" +#include "boost/assign/list_of.hpp" #include "boost/optional.hpp" #include @@ -73,44 +75,6 @@ Bounds findWidestBounds(const BoundsCandidates & front_bounds_candidates) } */ -Bounds findNearestBounds( - const geometry_msgs::msg::Pose & bounds_pose, const BoundsCandidates & front_bounds_candidates, - const geometry_msgs::msg::Point & target_pos) -{ - double min_dist = std::numeric_limits::max(); - size_t min_dist_index = 0; - if (front_bounds_candidates.size() != 1) { - for (size_t candidate_idx = 0; candidate_idx < front_bounds_candidates.size(); - ++candidate_idx) { - const auto & front_bounds_candidate = front_bounds_candidates.at(candidate_idx); - - const double bound_center = - (front_bounds_candidate.upper_bound + front_bounds_candidate.lower_bound) / 2.0; - const auto center_pos = - tier4_autoware_utils::calcOffsetPose(bounds_pose, 0.0, bound_center, 0.0); - const double dist = tier4_autoware_utils::calcDistance2d(center_pos, target_pos); - - if (dist < min_dist) { - min_dist_index = candidate_idx; - min_dist = dist; - } - } - } - return front_bounds_candidates.at(min_dist_index); -} - -double calcOverlappedBoundsSignedLength( - const Bounds & prev_front_continuous_bounds, const Bounds & front_bounds_candidate) -{ - const double min_ub = - std::min(front_bounds_candidate.upper_bound, prev_front_continuous_bounds.upper_bound); - const double max_lb = - std::max(front_bounds_candidate.lower_bound, prev_front_continuous_bounds.lower_bound); - - const double overlapped_signed_length = min_ub - max_lb; - return overlapped_signed_length; -} - geometry_msgs::msg::Pose calcVehiclePose( const ReferencePoint & ref_point, const double lat_error, const double yaw_error, const double offset) @@ -202,6 +166,47 @@ size_t findNearestIndexWithSoftYawConstraints( return nearest_idx_optional ? *nearest_idx_optional : motion_utils::findNearestIndex(points_with_yaw, pose.position); } + +boost::optional intersection( + const Eigen::Vector2d & start_point1, const Eigen::Vector2d & end_point1, + const Eigen::Vector2d & start_point2, const Eigen::Vector2d & end_point2) +{ + using Point = boost::geometry::model::d2::point_xy; + using Line = boost::geometry::model::linestring; + + const Line line1 = + boost::assign::list_of(start_point1(0), start_point1(1))(end_point1(0), end_point1(1)); + const Line line2 = + boost::assign::list_of(start_point2(0), start_point2(1))(end_point2(0), end_point2(1)); + + std::vector output; + boost::geometry::intersection(line1, line2, output); + if (output.empty()) { + return {}; + } + + const Eigen::Vector2d output_point{output.front().x(), output.front().y()}; + return output_point; +} + +double calcLateralDistToBound( + const Eigen::Vector2d & current_point, const Eigen::Vector2d & edge_point, + const std::vector & bound, const bool is_right_bound = false) +{ + for (size_t i = 0; i < bound.size() - 1; ++i) { + const Eigen::Vector2d current_bound_point = {bound.at(i).x, bound.at(i).y}; + const Eigen::Vector2d next_bound_point = {bound.at(i + 1).x, bound.at(i + 1).y}; + + const auto intersects_point = + intersection(current_point, edge_point, current_bound_point, next_bound_point); + if (intersects_point) { + const double dist = (*intersects_point - current_point).norm(); + return is_right_bound ? -dist : dist; + } + } + + return is_right_bound ? -5.0 : 5.0; +} } // namespace MPTOptimizer::MPTOptimizer( @@ -221,7 +226,9 @@ boost::optional MPTOptimizer::getModelPredictiveTrajecto const bool enable_avoidance, const std::vector & smoothed_points, const std::vector & path_points, - const std::unique_ptr & prev_trajs, const CVMaps & maps, + const std::vector & left_bound, + const std::vector & right_bound, + const std::unique_ptr & prev_trajs, const geometry_msgs::msg::Pose & current_ego_pose, const double current_ego_vel, DebugData & debug_data) { @@ -238,7 +245,7 @@ boost::optional MPTOptimizer::getModelPredictiveTrajecto } std::vector full_ref_points = - getReferencePoints(smoothed_points, prev_trajs, enable_avoidance, maps, debug_data); + getReferencePoints(smoothed_points, left_bound, right_bound, prev_trajs, debug_data); if (full_ref_points.empty()) { RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, @@ -322,8 +329,9 @@ boost::optional MPTOptimizer::getModelPredictiveTrajecto std::vector MPTOptimizer::getReferencePoints( const std::vector & smoothed_points, - const std::unique_ptr & prev_trajs, const bool enable_avoidance, - const CVMaps & maps, DebugData & debug_data) const + const std::vector & left_bound, + const std::vector & right_bound, + const std::unique_ptr & prev_trajs, DebugData & debug_data) const { stop_watch_.tic(__func__); @@ -406,8 +414,8 @@ std::vector MPTOptimizer::getReferencePoints( } // set bounds information - calcBounds(ref_points, enable_avoidance, maps, prev_trajs, debug_data); - calcVehicleBounds(ref_points, maps, debug_data, enable_avoidance); + calcBounds(ref_points, left_bound, right_bound, debug_data); + calcVehicleBounds(ref_points, debug_data); // set extra information (alpha and has_object_collision) // NOTE: This must be after bounds calculation. @@ -1376,109 +1384,73 @@ void MPTOptimizer::addSteerWeightR( } void MPTOptimizer::calcBounds( - std::vector & ref_points, const bool enable_avoidance, const CVMaps & maps, - const std::unique_ptr & prev_trajs, DebugData & debug_data) const + std::vector & ref_points, + const std::vector & left_bound, + const std::vector & right_bound, DebugData & debug_data) const { stop_watch_.tic(__func__); - // search bounds candidate for each ref points - SequentialBoundsCandidates sequential_bounds_candidates; - for (const auto & ref_point : ref_points) { - const auto bounds_candidates = - getBoundsCandidates(enable_avoidance, convertRefPointsToPose(ref_point), maps, debug_data); - sequential_bounds_candidates.push_back(bounds_candidates); + if (left_bound.empty() || right_bound.empty()) { + std::cerr << "[ObstacleAvoidancePlanner]: Boundary is empty when calculating bounds" + << std::endl; + return; } - debug_data.sequential_bounds_candidates = sequential_bounds_candidates; - - // search continuous and widest bounds only for front point - for (size_t i = 0; i < sequential_bounds_candidates.size(); ++i) { - // NOTE: back() is the front avoiding circle - const auto & bounds_candidates = sequential_bounds_candidates.at(i); - - // extract only continuous bounds; - if (i == 0) { // TODO(murooka) use previous bounds, not widest bounds - const auto target_pos = [&]() { - if (prev_trajs && !prev_trajs->mpt_ref_points.empty()) { - return prev_trajs->mpt_ref_points.front().p; - } - return ref_points.at(i).p; - }(); - - geometry_msgs::msg::Pose ref_pose; - ref_pose.position = ref_points.at(i).p; - ref_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_points.at(i).yaw); - - const auto & nearest_bounds = findNearestBounds(ref_pose, bounds_candidates, target_pos); - ref_points.at(i).bounds = nearest_bounds; - } else { - const auto & prev_ref_point = ref_points.at(i - 1); - const auto & prev_continuous_bounds = prev_ref_point.bounds; - - BoundsCandidates filtered_bounds_candidates; - for (const auto & bounds_candidate : bounds_candidates) { - // Step 1. Bounds is continuous to the previous one, - // and the overlapped signed length is longer than vehicle width - // overlapped_signed_length already considers vehicle width. - const double overlapped_signed_length = - calcOverlappedBoundsSignedLength(prev_continuous_bounds, bounds_candidate); - if (overlapped_signed_length < 0) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, - "non-overlapped length bounds is ignored."); - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, - "In detail, prev: lower=%f, upper=%f, candidate: lower=%f, upper=%f", - prev_continuous_bounds.lower_bound, prev_continuous_bounds.upper_bound, - bounds_candidate.lower_bound, bounds_candidate.upper_bound); - continue; - } - // Step 2. Bounds is longer than vehicle width. - // bounds_length already considers vehicle width. - const double bounds_length = bounds_candidate.upper_bound - bounds_candidate.lower_bound; - if (bounds_length < 0) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, - "negative length bounds is ignored."); - continue; - } - - filtered_bounds_candidates.push_back(bounds_candidate); - } - - // Step 3. Nearest bounds to trajectory - if (!filtered_bounds_candidates.empty()) { - const auto nearest_bounds = std::min_element( - filtered_bounds_candidates.begin(), filtered_bounds_candidates.end(), - [](const auto & a, const auto & b) { - return std::min(std::abs(a.lower_bound), std::abs(a.upper_bound)) < - std::min(std::abs(b.lower_bound), std::abs(b.upper_bound)); - }); - if ( - filtered_bounds_candidates.begin() <= nearest_bounds && - nearest_bounds < filtered_bounds_candidates.end()) { - ref_points.at(i).bounds = *nearest_bounds; - continue; - } - } + /* + const double min_soft_road_clearance = vehicle_param_.width / 2.0 + + mpt_param_.soft_clearance_from_road + + mpt_param_.extra_desired_clearance_from_road; + */ + const double min_soft_road_clearance = vehicle_param_.width / 2.0; - // invalid bounds - RCLCPP_WARN_EXPRESSION( - rclcpp::get_logger("getBounds: not front"), is_showing_debug_info_, "invalid bounds"); - const auto invalid_bounds = - Bounds{-5.0, 5.0, CollisionType::OUT_OF_ROAD, CollisionType::OUT_OF_ROAD}; - ref_points.at(i).bounds = invalid_bounds; - } + // search bounds candidate for each ref points + debug_data.bounds_candidates.clear(); + for (size_t i = 0; i < ref_points.size() - 1; ++i) { + const auto curr_ref_position = convertRefPointsToPose(ref_points.at(i)).position; + const auto next_ref_position = convertRefPointsToPose(ref_points.at(i + 1)).position; + const Eigen::Vector2d current_ref_point = {curr_ref_position.x, curr_ref_position.y}; + const Eigen::Vector2d next_ref_point = {next_ref_position.x, next_ref_position.y}; + const Eigen::Vector2d current_to_next_vec = next_ref_point - current_ref_point; + const Eigen::Vector2d left_vertical_vec = {-current_to_next_vec(1), current_to_next_vec(0)}; + const Eigen::Vector2d right_vertical_vec = {current_to_next_vec(1), -current_to_next_vec(0)}; + + const Eigen::Vector2d left_point = current_ref_point + left_vertical_vec.normalized() * 5.0; + const Eigen::Vector2d right_point = current_ref_point + right_vertical_vec.normalized() * 5.0; + const double lat_dist_to_left_bound = std::min( + calcLateralDistToBound(current_ref_point, left_point, left_bound) - min_soft_road_clearance, + 5.0); + const double lat_dist_to_right_bound = std::max( + calcLateralDistToBound(current_ref_point, right_point, right_bound, true) + + min_soft_road_clearance, + -5.0); + + ref_points.at(i).bounds = Bounds{ + lat_dist_to_right_bound, lat_dist_to_left_bound, CollisionType::NO_COLLISION, + CollisionType::NO_COLLISION}; + debug_data.bounds_candidates.push_back(ref_points.at(i).bounds); } + // Terminal Boundary + const double lat_dist_to_left_bound = + -motion_utils::calcLateralOffset( + left_bound, convertRefPointsToPose(ref_points.back()).position) - + min_soft_road_clearance; + const double lat_dist_to_right_bound = + -motion_utils::calcLateralOffset( + right_bound, convertRefPointsToPose(ref_points.back()).position) + + min_soft_road_clearance; + ref_points.back().bounds = Bounds{ + lat_dist_to_right_bound, lat_dist_to_left_bound, CollisionType::NO_COLLISION, + CollisionType::NO_COLLISION}; + debug_data.bounds_candidates.push_back(ref_points.back().bounds); + debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; return; } void MPTOptimizer::calcVehicleBounds( - std::vector & ref_points, [[maybe_unused]] const CVMaps & maps, - DebugData & debug_data, [[maybe_unused]] const bool enable_avoidance) const + std::vector & ref_points, DebugData & debug_data) const { stop_watch_.tic(__func__); @@ -1558,168 +1530,3 @@ void MPTOptimizer::calcVehicleBounds( debug_data.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; } - -BoundsCandidates MPTOptimizer::getBoundsCandidates( - const bool enable_avoidance, const geometry_msgs::msg::Pose & avoiding_point, const CVMaps & maps, - [[maybe_unused]] DebugData & debug_data) const -{ - BoundsCandidates bounds_candidate; - - constexpr double max_search_lane_width = 5.0; - const auto search_widths = mpt_param_.bounds_search_widths; - - // search right to left - const double bound_angle = - tier4_autoware_utils::normalizeRadian(tf2::getYaw(avoiding_point.orientation) + M_PI_2); - - double traversed_dist = -max_search_lane_width; - double current_right_bound = -max_search_lane_width; - - // calculate the initial position is empty or not - // 0.drivable, 1.out of map, 2.out of road, 3. object - CollisionType previous_collision_type = - getCollisionType(maps, enable_avoidance, avoiding_point, traversed_dist, bound_angle); - - const auto has_collision = [&](const CollisionType & collision_type) -> bool { - return collision_type == CollisionType::OUT_OF_ROAD || collision_type == CollisionType::OBJECT; - }; - CollisionType latest_right_bound_collision_type = previous_collision_type; - - while (traversed_dist < max_search_lane_width) { - for (size_t search_idx = 0; search_idx < search_widths.size(); ++search_idx) { - const double ds = search_widths.at(search_idx); - while (true) { - const CollisionType current_collision_type = - getCollisionType(maps, enable_avoidance, avoiding_point, traversed_dist, bound_angle); - - if (has_collision(current_collision_type)) { // currently collision - if (!has_collision(previous_collision_type)) { - // if target_position becomes collision from no collision or out_of_sight - if (search_idx == search_widths.size() - 1) { - const double left_bound = traversed_dist - ds / 2.0; - bounds_candidate.push_back(Bounds{ - current_right_bound, left_bound, latest_right_bound_collision_type, - current_collision_type}); - previous_collision_type = current_collision_type; - } - break; - } - } else if (current_collision_type == CollisionType::OUT_OF_SIGHT) { // currently - // out_of_sight - if (previous_collision_type == CollisionType::NO_COLLISION) { - // if target_position becomes out_of_sight from no collision - if (search_idx == search_widths.size() - 1) { - const double left_bound = max_search_lane_width; - bounds_candidate.push_back(Bounds{ - current_right_bound, left_bound, latest_right_bound_collision_type, - current_collision_type}); - previous_collision_type = current_collision_type; - } - break; - } - } else { // currently no collision - if (has_collision(previous_collision_type)) { - // if target_position becomes no collision from collision - if (search_idx == search_widths.size() - 1) { - current_right_bound = traversed_dist - ds / 2.0; - latest_right_bound_collision_type = previous_collision_type; - previous_collision_type = current_collision_type; - } - break; - } - } - - // if target_position is longer than max_search_lane_width - if (traversed_dist >= max_search_lane_width) { - if (!has_collision(previous_collision_type)) { - if (search_idx == search_widths.size() - 1) { - const double left_bound = traversed_dist - ds / 2.0; - bounds_candidate.push_back(Bounds{ - current_right_bound, left_bound, latest_right_bound_collision_type, - CollisionType::OUT_OF_ROAD}); - } - } - break; - } - - // go forward with ds - traversed_dist += ds; - previous_collision_type = current_collision_type; - } - - if (search_idx != search_widths.size() - 1) { - // go back with ds since target_position became empty or road/object - // NOTE: if ds is the last of search_widths, don't have to go back - traversed_dist -= ds; - } - } - } - - // if empty - // TODO(murooka) sometimes this condition realizes - if (bounds_candidate.empty()) { - RCLCPP_WARN_EXPRESSION( - rclcpp::get_logger("getBoundsCandidate"), is_showing_debug_info_, "empty bounds candidate"); - // NOTE: set invalid bounds so that MPT won't be solved - const auto invalid_bounds = - Bounds{-5.0, 5.0, CollisionType::OUT_OF_ROAD, CollisionType::OUT_OF_ROAD}; - bounds_candidate.push_back(invalid_bounds); - } - - return bounds_candidate; -} - -// 0.NO_COLLISION, 1.OUT_OF_SIGHT, 2.OUT_OF_ROAD, 3.OBJECT -CollisionType MPTOptimizer::getCollisionType( - const CVMaps & maps, const bool enable_avoidance, const geometry_msgs::msg::Pose & avoiding_point, - const double traversed_dist, const double bound_angle) const -{ - // calculate clearance - const double min_soft_road_clearance = vehicle_param_.width / 2.0 + - mpt_param_.soft_clearance_from_road + - mpt_param_.extra_desired_clearance_from_road; - const double min_obj_clearance = vehicle_param_.width / 2.0 + mpt_param_.clearance_from_object + - mpt_param_.soft_clearance_from_road; - - // calculate target position - const geometry_msgs::msg::Point target_pos = tier4_autoware_utils::createPoint( - avoiding_point.position.x + traversed_dist * std::cos(bound_angle), - avoiding_point.position.y + traversed_dist * std::sin(bound_angle), 0.0); - - const auto opt_road_clearance = getClearance(maps.clearance_map, target_pos, maps.map_info); - const auto opt_obj_clearance = - getClearance(maps.only_objects_clearance_map, target_pos, maps.map_info); - - // object has more priority than road, so its condition exists first - if (enable_avoidance && opt_obj_clearance) { - const bool is_obj = opt_obj_clearance.get() < min_obj_clearance; - if (is_obj) { - return CollisionType::OBJECT; - } - } - - if (opt_road_clearance) { - const bool out_of_road = opt_road_clearance.get() < min_soft_road_clearance; - if (out_of_road) { - return CollisionType::OUT_OF_ROAD; - } else { - return CollisionType::NO_COLLISION; - } - } - - return CollisionType::OUT_OF_SIGHT; -} - -boost::optional MPTOptimizer::getClearance( - const cv::Mat & clearance_map, const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & map_info) const -{ - const auto image_point = geometry_utils::transformMapToOptionalImage(map_point, map_info); - if (!image_point) { - return boost::none; - } - const float clearance = clearance_map.ptr(static_cast( - image_point.get().y))[static_cast(image_point.get().x)] * - map_info.resolution; - return clearance; -} diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 4120034db374..277508933512 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -16,7 +16,6 @@ #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/motion_utils.hpp" -#include "obstacle_avoidance_planner/utils/cv_utils.hpp" #include "obstacle_avoidance_planner/utils/debug_utils.hpp" #include "obstacle_avoidance_planner/utils/utils.hpp" #include "rclcpp/time.hpp" @@ -847,8 +846,6 @@ void ObstacleAvoidancePlanner::resetPlanning() { RCLCPP_WARN(get_logger(), "[ObstacleAvoidancePlanner] Reset planning"); - costmap_generator_ptr_ = std::make_unique(); - eb_path_optimizer_ptr_ = std::make_unique( is_showing_debug_info_, traj_param_, eb_param_, vehicle_param_); @@ -869,12 +866,14 @@ void ObstacleAvoidancePlanner::onPath(const Path::SharedPtr path_ptr) { stop_watch_.tic(__func__); - if ( - path_ptr->points.empty() || path_ptr->drivable_area.data.empty() || !current_twist_ptr_ || - !objects_ptr_) { + if (path_ptr->points.empty() || !current_twist_ptr_ || !objects_ptr_) { return; } + if (path_ptr->left_bound.empty() || path_ptr->right_bound.empty()) { + std::cerr << "Right or left bound is empty" << std::endl; + } + // create planner data PlannerData planner_data; planner_data.path = *path_ptr; @@ -956,12 +955,8 @@ std::vector ObstacleAvoidancePlanner::generateOptimizedTrajecto } prev_replanned_time_ptr_ = std::make_unique(this->now()); - // create clearance maps - const CVMaps cv_maps = costmap_generator_ptr_->getMaps( - enable_avoidance_, path, planner_data.objects, traj_param_, debug_data_); - // calculate trajectory with EB and MPT - auto optimal_trajs = optimizeTrajectory(planner_data, cv_maps); + auto optimal_trajs = optimizeTrajectory(planner_data); // calculate velocity // NOTE: Velocity is not considered in optimization. @@ -969,8 +964,7 @@ std::vector ObstacleAvoidancePlanner::generateOptimizedTrajecto // insert 0 velocity when trajectory is over drivable area if (is_stopping_if_outside_drivable_area_) { - insertZeroVelocityOutsideDrivableArea( - planner_data, optimal_trajs.model_predictive_trajectory, cv_maps); + insertZeroVelocityOutsideDrivableArea(planner_data, optimal_trajs.model_predictive_trajectory); } publishDebugDataInOptimization(planner_data, optimal_trajs.model_predictive_trajectory); @@ -1122,8 +1116,7 @@ bool ObstacleAvoidancePlanner::isEgoNearToPrevTrajectory(const geometry_msgs::ms return true; } -Trajectories ObstacleAvoidancePlanner::optimizeTrajectory( - const PlannerData & planner_data, const CVMaps & cv_maps) +Trajectories ObstacleAvoidancePlanner::optimizeTrajectory(const PlannerData & planner_data) { stop_watch_.tic(__func__); @@ -1176,8 +1169,8 @@ Trajectories ObstacleAvoidancePlanner::optimizeTrajectory( // MPT: optimize trajectory to be kinematically feasible and collision free const auto mpt_trajs = mpt_optimizer_ptr_->getModelPredictiveTrajectory( - enable_avoidance_, eb_traj.get(), p.path.points, prev_optimal_trajs_ptr_, cv_maps, p.ego_pose, - p.ego_vel, debug_data_); + enable_avoidance_, eb_traj.get(), p.path.points, p.path.left_bound, p.path.right_bound, + prev_optimal_trajs_ptr_, p.ego_pose, p.ego_vel, debug_data_); if (!mpt_trajs) { return getPrevTrajs(p.path.points); } @@ -1231,8 +1224,7 @@ void ObstacleAvoidancePlanner::calcVelocity( } void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( - const PlannerData & planner_data, std::vector & traj_points, - const CVMaps & cv_maps) + const PlannerData & planner_data, std::vector & traj_points) { if (traj_points.empty()) { return; @@ -1240,9 +1232,6 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( stop_watch_.tic(__func__); - const auto & map_info = cv_maps.map_info; - const auto & road_clearance_map = cv_maps.clearance_map; - const size_t nearest_idx = findEgoNearestIndex(traj_points, planner_data.ego_pose); // NOTE: Some end trajectory points will be ignored to check if outside the drivable area @@ -1258,12 +1247,18 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( return traj_points.size(); }(); + const auto left_bound = planner_data.path.left_bound; + const auto right_bound = planner_data.path.right_bound; + if (left_bound.empty() || right_bound.empty()) { + return; + } + for (size_t i = nearest_idx; i < end_idx; ++i) { const auto & traj_point = traj_points.at(i); // calculate the first point being outside drivable area - const bool is_outside = cv_drivable_area_utils::isOutsideDrivableAreaFromRectangleFootprint( - traj_point, road_clearance_map, map_info, vehicle_param_); + const bool is_outside = drivable_area_utils::isOutsideDrivableAreaFromRectangleFootprint( + traj_point, left_bound, right_bound, vehicle_param_); // only insert zero velocity to the first point outside drivable area if (is_outside) { @@ -1616,25 +1611,6 @@ void ObstacleAvoidancePlanner::publishDebugDataInMain(const Path & path) const debug_extended_non_fixed_traj_pub_->publish(debug_extended_non_fixed_traj); } - { // publish clearance map - stop_watch_.tic("publishClearanceMap"); - - if (is_publishing_area_with_objects_) { // false - debug_area_with_objects_pub_->publish( - debug_utils::getDebugCostmap(debug_data_.area_with_objects_map, path.drivable_area)); - } - if (is_publishing_object_clearance_map_) { // false - debug_object_clearance_map_pub_->publish( - debug_utils::getDebugCostmap(debug_data_.only_object_clearance_map, path.drivable_area)); - } - if (is_publishing_clearance_map_) { // true - debug_clearance_map_pub_->publish( - debug_utils::getDebugCostmap(debug_data_.clearance_map, path.drivable_area)); - } - debug_data_.msg_stream << " getDebugCostMap * 3:= " << stop_watch_.toc("publishClearanceMap") - << " [ms]\n"; - } - debug_data_.msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"; } diff --git a/planning/obstacle_avoidance_planner/src/utils/cv_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/cv_utils.cpp deleted file mode 100644 index a543df9d54a9..000000000000 --- a/planning/obstacle_avoidance_planner/src/utils/cv_utils.cpp +++ /dev/null @@ -1,470 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "obstacle_avoidance_planner/utils/cv_utils.hpp" - -#include "obstacle_avoidance_planner/utils/utils.hpp" -#include "tf2/utils.h" -#include "tier4_autoware_utils/system/stop_watch.hpp" - -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "nav_msgs/msg/occupancy_grid.hpp" - -#include "boost/optional.hpp" - -#include -#include -#include - -namespace -{ -boost::optional getDistance( - const cv::Mat & clearance_map, const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & map_info) -{ - const auto image_point = geometry_utils::transformMapToOptionalImage(map_point, map_info); - if (!image_point) { - return boost::none; - } - const float clearance = clearance_map.ptr(static_cast( - image_point.get().y))[static_cast(image_point.get().x)] * - map_info.resolution; - return clearance; -} - -bool isOutsideDrivableArea( - const geometry_msgs::msg::Point & pos, const cv::Mat & road_clearance_map, - const nav_msgs::msg::MapMetaData & map_info, const double max_dist) -{ - const auto dist = getDistance(road_clearance_map, pos, map_info); - if (dist) { - return dist.get() < max_dist; - } - - return false; -} -} // namespace - -namespace cv_utils -{ -void getOccupancyGridValue( - const nav_msgs::msg::OccupancyGrid & og, const int i, const int j, unsigned char & value) -{ - int i_flip = og.info.width - i - 1; - int j_flip = og.info.height - j - 1; - if (og.data[i_flip + j_flip * og.info.width] > 0) { - value = 0; - } else { - value = 255; - } -} - -void putOccupancyGridValue( - nav_msgs::msg::OccupancyGrid & og, const int i, const int j, const unsigned char value) -{ - int i_flip = og.info.width - i - 1; - int j_flip = og.info.height - j - 1; - og.data[i_flip + j_flip * og.info.width] = value; -} -} // namespace cv_utils - -namespace cv_polygon_utils -{ -PolygonPoints getPolygonPoints( - const std::vector & points, - const nav_msgs::msg::MapMetaData & map_info) -{ - std::vector points_in_image; - std::vector points_in_map; - for (const auto & point : points) { - const auto image_point = geometry_utils::transformMapToOptionalImage(point, map_info); - if (image_point) { - points_in_image.push_back(image_point.get()); - points_in_map.push_back(point); - } - } - PolygonPoints polygon_points; - polygon_points.points_in_image = points_in_image; - polygon_points.points_in_map = points_in_map; - return polygon_points; -} - -PolygonPoints getPolygonPoints( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info) -{ - std::vector points_in_image; - std::vector points_in_map; - PolygonPoints polygon_points; - if (object.shape.type == object.shape.BOUNDING_BOX) { - polygon_points = getPolygonPointsFromBB(object, map_info); - } else if (object.shape.type == object.shape.CYLINDER) { - polygon_points = getPolygonPointsFromCircle(object, map_info); - } else if (object.shape.type == object.shape.POLYGON) { - polygon_points = getPolygonPointsFromPolygon(object, map_info); - } - return polygon_points; -} - -PolygonPoints getPolygonPointsFromBB( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info) -{ - std::vector points_in_image; - std::vector points_in_map; - const double dim_x = object.shape.dimensions.x; - const double dim_y = object.shape.dimensions.y; - const std::vector rel_x = {0.5 * dim_x, 0.5 * dim_x, -0.5 * dim_x, -0.5 * dim_x}; - const std::vector rel_y = {0.5 * dim_y, -0.5 * dim_y, -0.5 * dim_y, 0.5 * dim_y}; - const geometry_msgs::msg::Pose object_pose = object.kinematics.initial_pose_with_covariance.pose; - for (size_t i = 0; i < rel_x.size(); i++) { - geometry_msgs::msg::Point rel_point; - rel_point.x = rel_x[i]; - rel_point.y = rel_y[i]; - auto abs_point = geometry_utils::transformToAbsoluteCoordinate2D(rel_point, object_pose); - geometry_msgs::msg::Point image_point; - if (geometry_utils::transformMapToImage(abs_point, map_info, image_point)) { - points_in_image.push_back(image_point); - points_in_map.push_back(abs_point); - } - } - PolygonPoints polygon_points; - polygon_points.points_in_image = points_in_image; - polygon_points.points_in_map = points_in_map; - return polygon_points; -} - -PolygonPoints getPolygonPointsFromCircle( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info) -{ - std::vector points_in_image; - std::vector points_in_map; - const double radius = object.shape.dimensions.x; - const geometry_msgs::msg::Point center = - object.kinematics.initial_pose_with_covariance.pose.position; - constexpr int num_sampling_points = 5; - for (int i = 0; i < num_sampling_points; ++i) { - std::vector deltas = {0, 1.0}; - for (const auto & delta : deltas) { - geometry_msgs::msg::Point point; - point.x = std::cos( - ((i + delta) / static_cast(num_sampling_points)) * 2.0 * M_PI + - M_PI / static_cast(num_sampling_points)) * - (radius / 2.0) + - center.x; - point.y = std::sin( - ((i + delta) / static_cast(num_sampling_points)) * 2.0 * M_PI + - M_PI / static_cast(num_sampling_points)) * - (radius / 2.0) + - center.y; - point.z = center.z; - geometry_msgs::msg::Point image_point; - if (geometry_utils::transformMapToImage(point, map_info, image_point)) { - points_in_image.push_back(image_point); - points_in_map.push_back(point); - } - } - } - PolygonPoints polygon_points; - polygon_points.points_in_image = points_in_image; - polygon_points.points_in_map = points_in_map; - return polygon_points; -} - -PolygonPoints getPolygonPointsFromPolygon( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const nav_msgs::msg::MapMetaData & map_info) -{ - std::vector points_in_image; - std::vector points_in_map; - for (const auto & polygon_p : object.shape.footprint.points) { - geometry_msgs::msg::Point rel_point; - rel_point.x = polygon_p.x; - rel_point.y = polygon_p.y; - geometry_msgs::msg::Point point = geometry_utils::transformToAbsoluteCoordinate2D( - rel_point, object.kinematics.initial_pose_with_covariance.pose); - const auto image_point = geometry_utils::transformMapToOptionalImage(point, map_info); - if (image_point) { - points_in_image.push_back(image_point.get()); - points_in_map.push_back(point); - } - } - PolygonPoints polygon_points; - polygon_points.points_in_image = points_in_image; - polygon_points.points_in_map = points_in_map; - return polygon_points; -} - -std::vector getCVPolygon( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const PolygonPoints & polygon_points, - const std::vector & path_points, - const cv::Mat & clearance_map, const nav_msgs::msg::MapMetaData & map_info) -{ - // TODO(murooka) remove findNearestIndex without any constraints - const int nearest_idx = motion_utils::findNearestIndex( - path_points, object.kinematics.initial_pose_with_covariance.pose.position); - const auto nearest_path_point = path_points[nearest_idx]; - if (path_points.empty()) { - return getDefaultCVPolygon(polygon_points.points_in_image); - } - return getExtendedCVPolygon( - polygon_points.points_in_image, polygon_points.points_in_map, nearest_path_point.pose, object, - clearance_map, map_info); -} - -std::vector getDefaultCVPolygon( - const std::vector & points_in_image) -{ - std::vector cv_polygon; - for (const auto & point : points_in_image) { - cv::Point image_point = cv::Point(point.x, point.y); - cv_polygon.push_back(image_point); - } - return cv_polygon; -} - -std::vector getExtendedCVPolygon( - const std::vector & points_in_image, - const std::vector & points_in_map, - const geometry_msgs::msg::Pose & nearest_path_point_pose, - const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info) -{ - const boost::optional optional_edges = getEdges( - points_in_image, points_in_map, nearest_path_point_pose, object, clearance_map, map_info); - if (!optional_edges) { - return getDefaultCVPolygon(points_in_image); - } - const Edges edges = optional_edges.get(); - - // TODO(murooka) remove findNearestIndex without any constraints - const int nearest_polygon_idx = motion_utils::findNearestIndex(points_in_image, edges.origin); - std::vector cv_polygon; - if (edges.back_idx == nearest_polygon_idx || edges.front_idx == nearest_polygon_idx) { - // make polygon only with edges and extended edges - } else if (edges.back_idx < nearest_polygon_idx) { - // back_idx -> nearest_idx -> frond_idx - if (edges.back_idx < edges.front_idx && nearest_polygon_idx < edges.front_idx) { - for (int i = edges.back_idx + 1; i < edges.front_idx; i++) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - // back_idx -> vector_front -> vector_back -> nearest_idx -> frond_idx - } else if (edges.back_idx < edges.front_idx && nearest_polygon_idx > edges.front_idx) { - for (int i = edges.back_idx - 1; i >= 0; i--) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - for (int i = points_in_image.size() - 1; i > edges.front_idx; i--) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - // back_idx -> vector_back -> vector_front -> nearest_idx -> front_idx - } else { - for (size_t i = edges.back_idx + 1; i < points_in_image.size(); i++) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - for (int i = 0; i < edges.front_idx; i++) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - } - } else { - // back_idx -> nearest_idx -> front_idx - if (edges.back_idx >= edges.front_idx && nearest_polygon_idx > edges.front_idx) { - for (int i = edges.back_idx - 1; i > edges.front_idx; i--) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - // back_idx -> vector_back -> vector_front -> nearest_idx -> front_idx - } else { - if (edges.back_idx >= edges.front_idx && nearest_polygon_idx < edges.front_idx) { - for (size_t i = edges.back_idx + 1; i < points_in_image.size(); i++) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - for (int i = 0; i < edges.front_idx; i++) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - } else { // back_idx -> vector_front -> vector_back -> nearest_idx -> front_idx - for (int i = edges.back_idx - 1; i >= 0; i--) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - for (int i = points_in_image.size() - 1; i > edges.front_idx; i--) { - cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); - } - } - } - } - cv_polygon.push_back( - cv::Point(points_in_image[edges.front_idx].x, points_in_image[edges.front_idx].y)); - cv_polygon.push_back(cv::Point(edges.extended_front.x, edges.extended_front.y)); - cv_polygon.push_back(cv::Point(edges.extended_back.x, edges.extended_back.y)); - cv_polygon.push_back( - cv::Point(points_in_image[edges.back_idx].x, points_in_image[edges.back_idx].y)); - return cv_polygon; -} - -boost::optional getEdges( - const std::vector & points_in_image, - const std::vector & points_in_map, - const geometry_msgs::msg::Pose & nearest_path_point_pose, - const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info) -{ - // calculate perpendicular point to object along with path point orientation - const double yaw = tf2::getYaw(nearest_path_point_pose.orientation); - const Eigen::Vector2d rel_path_vec(std::cos(yaw), std::sin(yaw)); - const Eigen::Vector2d obj_vec( - object.kinematics.initial_pose_with_covariance.pose.position.x, - object.kinematics.initial_pose_with_covariance.pose.position.y); - const double inner_product = rel_path_vec[0] * (obj_vec[0] - nearest_path_point_pose.position.x) + - rel_path_vec[1] * (obj_vec[1] - nearest_path_point_pose.position.y); - geometry_msgs::msg::Point origin; - origin.x = nearest_path_point_pose.position.x + rel_path_vec[0] * inner_product; - origin.y = nearest_path_point_pose.position.y + rel_path_vec[1] * inner_product; - const Eigen::Vector2d obj2origin(origin.x - obj_vec[0], origin.y - obj_vec[1]); - - // calculate origin for casting ray to edges - const auto path_point_image = - geometry_utils::transformMapToImage(nearest_path_point_pose.position, map_info); - constexpr double ray_origin_dist_scale = 1.0; - const float clearance = clearance_map.ptr(static_cast( - path_point_image.y))[static_cast(path_point_image.x)] * - map_info.resolution * ray_origin_dist_scale; - const Eigen::Vector2d obj2ray_origin = obj2origin.normalized() * (obj2origin.norm() + clearance); - geometry_msgs::msg::Point ray_origin; - ray_origin.x = obj_vec[0] + obj2ray_origin[0]; - ray_origin.y = obj_vec[1] + obj2ray_origin[1]; - geometry_msgs::msg::Point ray_origin_image; - ray_origin_image = geometry_utils::transformMapToImage(ray_origin, map_info); - - double min_cos = std::numeric_limits::max(); - double max_cos = std::numeric_limits::lowest(); - const double path_yaw = tf2::getYaw(nearest_path_point_pose.orientation); - const double dx1 = std::cos(path_yaw); - const double dy1 = std::sin(path_yaw); - const Eigen::Vector2d path_point_vec(dx1, dy1); - const double path_point_vec_norm = path_point_vec.norm(); - Edges edges; - for (size_t i = 0; i < points_in_image.size(); i++) { - const double dx2 = points_in_map[i].x - ray_origin.x; - const double dy2 = points_in_map[i].y - ray_origin.y; - const Eigen::Vector2d path_point2point(dx2, dy2); - const double inner_product = path_point_vec.dot(path_point2point); - const double cos = inner_product / (path_point_vec_norm * path_point2point.norm()); - if (cos > max_cos) { - max_cos = cos; - edges.front_idx = i; - } - if (cos < min_cos) { - min_cos = cos; - edges.back_idx = i; - } - } - - const double max_sin = std::sqrt(1 - max_cos * max_cos); - const double min_sin = std::sqrt(1 - min_cos * min_cos); - const Eigen::Vector2d point2front_edge( - points_in_image[edges.front_idx].x - ray_origin_image.x, - points_in_image[edges.front_idx].y - ray_origin_image.y); - const Eigen::Vector2d point2back_edge( - points_in_image[edges.back_idx].x - ray_origin_image.x, - points_in_image[edges.back_idx].y - ray_origin_image.y); - const Eigen::Vector2d point2extended_front_edge = - point2front_edge.normalized() * (clearance * 2 / max_sin) * (1 / map_info.resolution); - const Eigen::Vector2d point2extended_back_edge = - point2back_edge.normalized() * (clearance * 2 / min_sin) * (1 / map_info.resolution); - - const double dist2extended_front_edge = point2extended_front_edge.norm() * map_info.resolution; - const double dist2front_edge = point2front_edge.norm() * map_info.resolution; - const double dist2extended_back_edge = point2extended_back_edge.norm() * map_info.resolution; - const double dist2back_edge = point2back_edge.norm() * map_info.resolution; - if ( - dist2extended_front_edge < clearance * 2 || dist2extended_back_edge < clearance * 2 || - dist2front_edge > dist2extended_front_edge || dist2back_edge > dist2extended_back_edge) { - return boost::none; - } - geometry_msgs::msg::Point extended_front; - geometry_msgs::msg::Point extended_back; - extended_front.x = point2extended_front_edge(0) + ray_origin_image.x; - extended_front.y = point2extended_front_edge(1) + ray_origin_image.y; - extended_back.x = point2extended_back_edge(0) + ray_origin_image.x; - extended_back.y = point2extended_back_edge(1) + ray_origin_image.y; - edges.extended_front = extended_front; - edges.extended_back = extended_back; - edges.origin = ray_origin_image; - return edges; -} -} // namespace cv_polygon_utils - -namespace cv_drivable_area_utils -{ -bool isOutsideDrivableAreaFromRectangleFootprint( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, - const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, - const VehicleParam & vehicle_param) -{ - const double base_to_right = (vehicle_param.wheel_tread / 2.0) + vehicle_param.right_overhang; - const double base_to_left = (vehicle_param.wheel_tread / 2.0) + vehicle_param.left_overhang; - - const double base_to_front = vehicle_param.length - vehicle_param.rear_overhang; - const double base_to_rear = vehicle_param.rear_overhang; - - const auto top_left_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_left, 0.0) - .position; - const auto top_right_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, base_to_right, 0.0) - .position; - const auto bottom_right_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, base_to_right, 0.0) - .position; - const auto bottom_left_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_left, 0.0) - .position; - - constexpr double epsilon = 1e-8; - const bool out_top_left = - isOutsideDrivableArea(top_left_pos, road_clearance_map, map_info, epsilon); - const bool out_top_right = - isOutsideDrivableArea(top_right_pos, road_clearance_map, map_info, epsilon); - const bool out_bottom_left = - isOutsideDrivableArea(bottom_left_pos, road_clearance_map, map_info, epsilon); - const bool out_bottom_right = - isOutsideDrivableArea(bottom_right_pos, road_clearance_map, map_info, epsilon); - - if (out_top_left || out_top_right || out_bottom_left || out_bottom_right) { - return true; - } - - return false; -} - -[[maybe_unused]] bool isOutsideDrivableAreaFromCirclesFootprint( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, - const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, - const std::vector vehicle_circle_longitudinal_offsets, const double vehicle_circle_radius) -{ - for (const double offset : vehicle_circle_longitudinal_offsets) { - const auto avoiding_pos = - tier4_autoware_utils::calcOffsetPose(traj_point.pose, offset, 0.0, 0.0).position; - - const bool outside_drivable_area = - isOutsideDrivableArea(avoiding_pos, road_clearance_map, map_info, vehicle_circle_radius); - if (outside_drivable_area) { - return true; - } - } - - return false; -} - -} // namespace cv_drivable_area_utils diff --git a/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp index f8f494684d7c..7cab7cb64d20 100644 --- a/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp @@ -15,7 +15,6 @@ #include "obstacle_avoidance_planner/utils/debug_utils.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "obstacle_avoidance_planner/utils/cv_utils.hpp" #include "obstacle_avoidance_planner/utils/utils.hpp" #include "tf2/utils.h" @@ -438,9 +437,9 @@ visualization_msgs::msg::MarkerArray getRectanglesNumMarkerArray( } visualization_msgs::msg::MarkerArray getBoundsCandidatesLineMarkerArray( - const std::vector & ref_points, - std::vector> & bounds_candidates, const double r, const double g, - const double b, [[maybe_unused]] const double vehicle_width, const size_t sampling_num) + const std::vector & ref_points, std::vector & bounds_candidates, + const double r, const double g, const double b, [[maybe_unused]] const double vehicle_width, + const size_t sampling_num) { const auto current_time = rclcpp::Clock().now(); visualization_msgs::msg::MarkerArray msg; @@ -459,23 +458,19 @@ visualization_msgs::msg::MarkerArray getBoundsCandidatesLineMarkerArray( } const auto & bound_candidates = bounds_candidates.at(i); - for (size_t j = 0; j < bound_candidates.size(); ++j) { - geometry_msgs::msg::Pose pose; - pose.position = ref_points.at(i).p; - pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_points.at(i).yaw); - - // lower bound - const double lb_y = bound_candidates.at(j).lower_bound; - const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; - marker.points.push_back(lb); - - // upper bound - const double ub_y = bound_candidates.at(j).upper_bound; - const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; - marker.points.push_back(ub); - - msg.markers.push_back(marker); - } + geometry_msgs::msg::Pose pose; + pose.position = ref_points.at(i).p; + pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_points.at(i).yaw); + + // lower bound + const double lb_y = bound_candidates.lower_bound; + const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; + marker.points.push_back(lb); + + // upper bound + const double ub_y = bound_candidates.upper_bound; + const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; + marker.points.push_back(ub); } return msg; @@ -766,8 +761,8 @@ visualization_msgs::msg::MarkerArray getDebugVisualizationMarker( // bounds candidates appendMarkerArray( getBoundsCandidatesLineMarkerArray( - debug_data.ref_points, debug_data.sequential_bounds_candidates, 0.2, 0.99, 0.99, - vehicle_param.width, debug_data.mpt_visualize_sampling_num), + debug_data.ref_points, debug_data.bounds_candidates, 0.2, 0.99, 0.99, vehicle_param.width, + debug_data.mpt_visualize_sampling_num), &vis_marker_array); // vehicle circle line @@ -817,19 +812,4 @@ visualization_msgs::msg::MarkerArray getDebugVisualizationWallMarker( } return vis_marker_array; } - -nav_msgs::msg::OccupancyGrid getDebugCostmap( - const cv::Mat & clearance_map, const nav_msgs::msg::OccupancyGrid & occupancy_grid) -{ - if (clearance_map.empty()) return nav_msgs::msg::OccupancyGrid(); - - cv::Mat tmp; - clearance_map.copyTo(tmp); - cv::normalize(tmp, tmp, 0, 255, cv::NORM_MINMAX, CV_8UC1); - nav_msgs::msg::OccupancyGrid clearance_map_in_og = occupancy_grid; - tmp.forEach([&](const unsigned char & value, const int * position) -> void { - cv_utils::putOccupancyGridValue(clearance_map_in_og, position[0], position[1], value); - }); - return clearance_map_in_og; -} } // namespace debug_utils diff --git a/planning/obstacle_avoidance_planner/src/utils/utils.cpp b/planning/obstacle_avoidance_planner/src/utils/utils.cpp index cc3edd1c4222..66bd14c7c7fb 100644 --- a/planning/obstacle_avoidance_planner/src/utils/utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/utils.cpp @@ -601,3 +601,108 @@ void logOSQPSolutionStatus(const int solution_status, const std::string & msg) } } } // namespace utils + +namespace +{ +geometry_msgs::msg::Point getStartPoint( + const std::vector & bound, const geometry_msgs::msg::Point & point) +{ + const size_t segment_idx = motion_utils::findNearestSegmentIndex(bound, point); + const auto & curr_seg_point = bound.at(segment_idx); + const auto & next_seg_point = bound.at(segment_idx); + const Eigen::Vector2d first_to_target{point.x - curr_seg_point.x, point.y - curr_seg_point.y}; + const Eigen::Vector2d first_to_second{ + next_seg_point.x - curr_seg_point.x, next_seg_point.y - curr_seg_point.y}; + const double length = first_to_target.dot(first_to_second.normalized()); + + if (length < 0.0) { + return bound.front(); + } + + const auto first_point = motion_utils::calcLongitudinalOffsetPoint(bound, segment_idx, length); + + if (first_point) { + return *first_point; + } + + return bound.front(); +} + +bool isOutsideDrivableArea( + const geometry_msgs::msg::Point & point, + const std::vector & left_bound, + const std::vector & right_bound) +{ + if (left_bound.empty() || right_bound.empty()) { + return false; + } + + constexpr double min_dist = 0.1; + const auto left_start_point = getStartPoint(left_bound, right_bound.front()); + const auto right_start_point = getStartPoint(right_bound, left_bound.front()); + + // ignore point in front of the front line + const std::vector front_bound = {left_start_point, right_start_point}; + const double lat_dist_to_front_bound = motion_utils::calcLateralOffset(front_bound, point); + if (lat_dist_to_front_bound > min_dist) { + return false; + } + + // left bound check + const double lat_dist_to_left_bound = motion_utils::calcLateralOffset(left_bound, point); + if (lat_dist_to_left_bound > min_dist) { + return true; + } + + // right bound check + const double lat_dist_to_right_bound = motion_utils::calcLateralOffset(right_bound, point); + if (lat_dist_to_right_bound < -min_dist) { + return true; + } + + return false; +} +} // namespace + +namespace drivable_area_utils +{ +bool isOutsideDrivableAreaFromRectangleFootprint( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, + const std::vector left_bound, + const std::vector right_bound, const VehicleParam & vehicle_param) +{ + if (left_bound.empty() || right_bound.empty()) { + return false; + } + + const double base_to_right = (vehicle_param.wheel_tread / 2.0) + vehicle_param.right_overhang; + const double base_to_left = (vehicle_param.wheel_tread / 2.0) + vehicle_param.left_overhang; + + const double base_to_front = vehicle_param.length - vehicle_param.rear_overhang; + const double base_to_rear = vehicle_param.rear_overhang; + + const auto top_left_pos = + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_left, 0.0) + .position; + const auto top_right_pos = + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, base_to_right, 0.0) + .position; + const auto bottom_right_pos = + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, base_to_right, 0.0) + .position; + const auto bottom_left_pos = + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_left, 0.0) + .position; + + const bool out_top_left = isOutsideDrivableArea(top_left_pos, left_bound, right_bound); + const bool out_top_right = isOutsideDrivableArea(top_right_pos, left_bound, right_bound); + const bool out_bottom_left = isOutsideDrivableArea(bottom_left_pos, left_bound, right_bound); + const bool out_bottom_right = isOutsideDrivableArea(bottom_right_pos, left_bound, right_bound); + + if (out_top_left || out_top_right || out_bottom_left || out_bottom_right) { + return true; + } + + return false; +} +} // namespace drivable_area_utils diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index f3bce09fa861..8fe5e350a559 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -107,7 +107,8 @@ PathWithLaneId removeOverlappingPoints(const PathWithLaneId & input_path) filtered_path.points.push_back(pt); } } - filtered_path.drivable_area = input_path.drivable_area; + filtered_path.left_bound = input_path.left_bound; + filtered_path.right_bound = input_path.right_bound; return filtered_path; } diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/collision_free_optimizer_node.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/collision_free_optimizer_node.hpp index e76cd4b69365..55f91bddbdab 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/collision_free_optimizer_node.hpp +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/collision_free_optimizer_node.hpp @@ -20,7 +20,6 @@ #include "motion_utils/motion_utils.hpp" #include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/costmap_generator.hpp" #include "obstacle_avoidance_planner/eb_path_optimizer.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "rclcpp/clock.hpp" @@ -67,7 +66,6 @@ class CollisionFreeOptimizerNode : public rclcpp::Node double max_delta_time_sec_for_replan_; // logic - std::unique_ptr costmap_generator_ptr_; std::unique_ptr eb_path_optimizer_ptr_; std::unique_ptr mpt_optimizer_ptr_; diff --git a/planning/static_centerline_optimizer/src/collision_free_optimizer_node.cpp b/planning/static_centerline_optimizer/src/collision_free_optimizer_node.cpp index f9d843928647..5dd83d6d4b31 100644 --- a/planning/static_centerline_optimizer/src/collision_free_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/collision_free_optimizer_node.cpp @@ -371,8 +371,6 @@ void CollisionFreeOptimizerNode::resetPlanning() { RCLCPP_WARN(get_logger(), "[ObstacleAvoidancePlanner] Reset planning"); - costmap_generator_ptr_ = std::make_unique(); - eb_path_optimizer_ptr_ = std::make_unique( is_showing_debug_info_, traj_param_, eb_param_, vehicle_param_); @@ -391,7 +389,7 @@ void CollisionFreeOptimizerNode::resetPrevOptimization() Trajectory CollisionFreeOptimizerNode::pathCallback(const Path::ConstSharedPtr path_ptr) { - if (path_ptr->points.empty() || path_ptr->drivable_area.data.empty()) { + if (path_ptr->points.empty()) { return Trajectory{}; } @@ -411,8 +409,6 @@ Trajectory CollisionFreeOptimizerNode::pathCallback(const Path::ConstSharedPtr p // cv_maps const auto predicted_objects = PredictedObjects{}.objects; - const CVMaps cv_maps = - costmap_generator_ptr_->getMaps(false, *path_ptr, predicted_objects, traj_param_, debug_data_); const size_t initial_target_index = 3; auto target_pose = resampled_path.points.at(initial_target_index).pose; // TODO(murooka) @@ -424,8 +420,8 @@ Trajectory CollisionFreeOptimizerNode::pathCallback(const Path::ConstSharedPtr p .pose; const auto mpt_trajs = mpt_optimizer_ptr_->getModelPredictiveTrajectory( - false, resampled_traj_points, resampled_path.points, prev_optimal_trajs_ptr_, cv_maps, - target_pose, 0.0, debug_data_); + false, resampled_traj_points, resampled_path.points, resampled_path.left_bound, + resampled_path.right_bound, prev_optimal_trajs_ptr_, target_pose, 0.0, debug_data_); if (!mpt_trajs) { break; } diff --git a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp index a756309aefd6..df2e01cbcd31 100644 --- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp @@ -45,7 +45,8 @@ Path convert_to_path(const PathWithLaneId & path_with_lane_id) { Path path; path.header = path_with_lane_id.header; - path.drivable_area = path_with_lane_id.drivable_area; + path.left_bound = path_with_lane_id.left_bound; + path.right_bound = path_with_lane_id.right_bound; for (const auto & point : path_with_lane_id.points) { path.points.push_back(point.point); } diff --git a/planning/static_centerline_optimizer/src/utils.cpp b/planning/static_centerline_optimizer/src/utils.cpp index bd0197773401..f97d08b113e2 100644 --- a/planning/static_centerline_optimizer/src/utils.cpp +++ b/planning/static_centerline_optimizer/src/utils.cpp @@ -92,11 +92,10 @@ PathWithLaneId get_path_with_lane_id( planner_data->parameters.ego_nearest_yaw_threshold = ego_nearest_yaw_threshold; // generate drivable area and store it in path with lane id - constexpr double drivable_area_resolution = 0.1; constexpr double vehicle_length = 0.0; const auto drivable_lanes = behavior_path_planner::util::generateDrivableLanes(lanelets); - path_with_lane_id.drivable_area = behavior_path_planner::util::generateDrivableArea( - path_with_lane_id, drivable_lanes, drivable_area_resolution, vehicle_length, planner_data); + behavior_path_planner::util::generateDrivableArea( + path_with_lane_id, drivable_lanes, vehicle_length, planner_data); return path_with_lane_id; }