diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 1ee1d82297baf..603ddac6802a6 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -194,10 +194,10 @@ planning/planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp planning/route_handler/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp zulfaqar.azmi@tier4.jp planning/rtc_interface/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp planning/rtc_replayer/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp -planning/sampling_based_planner/bezier_sampler/** maxime.clement@tier4.jp -planning/sampling_based_planner/frenet_planner/** maxime.clement@tier4.jp -planning/sampling_based_planner/path_sampler/** maxime.clement@tier4.jp -planning/sampling_based_planner/sampler_common/** maxime.clement@tier4.jp +planning/sampling_based_planner/autoware_bezier_sampler/** maxime.clement@tier4.jp +planning/sampling_based_planner/autoware_frenet_planner/** maxime.clement@tier4.jp +planning/sampling_based_planner/autoware_path_sampler/** maxime.clement@tier4.jp +planning/sampling_based_planner/autoware_sampler_common/** maxime.clement@tier4.jp planning/scenario_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/autoware_surround_obstacle_checker/** satoshi.ota@tier4.jp sensing/gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index 5fcbfe512e3e1..b5173d6a98b2e 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -80,7 +80,7 @@ - + diff --git a/planning/.pages b/planning/.pages index 4f1d928252185..f7acd1b5efad5 100644 --- a/planning/.pages +++ b/planning/.pages @@ -56,10 +56,10 @@ nav: - 'About Path Smoother': planning/path_smoother - 'Elastic Band': planning/path_smoother/docs/eb - 'Sampling Based Planner': - - 'Path Sample': planning/sampling_based_planner/path_sampler - - 'Common library': planning/sampling_based_planner/sampler_common - - 'Frenet Planner': planning/sampling_based_planner/frenet_planner - - 'Bezier Sampler': planning/sampling_based_planner/bezier_sampler + - 'Path Sample': planning/sampling_based_planner/autoware_path_sampler + - 'Common library': planning/sampling_based_planner/autoware_sampler_common + - 'Frenet Planner': planning/sampling_based_planner/autoware_frenet_planner + - 'Bezier Sampler': planning/sampling_based_planner/autoware_bezier_sampler - 'Surround Obstacle Checker': - 'About Surround Obstacle Checker': planning/autoware_surround_obstacle_checker - 'About Surround Obstacle Checker (Japanese)': planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index e4fe3d0e9d3e4..a646621e5b089 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -38,13 +38,14 @@ autoware_adapi_v1_msgs autoware_behavior_path_planner_common + autoware_frenet_planner + autoware_path_sampler autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager autoware_vehicle_msgs behaviortree_cpp_v3 freespace_planning_algorithms - frenet_planner geometry_msgs interpolation lane_departure_checker @@ -54,7 +55,6 @@ magic_enum motion_utils object_recognition_utils - path_sampler pluginlib rclcpp rclcpp_components diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp index 3f3d144952d8e..0a5dec8d07b4f 100644 --- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp +++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp @@ -21,19 +21,19 @@ #include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware_behavior_path_planner_common/utils/path_utils.hpp" #include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware_bezier_sampler/bezier_sampling.hpp" +#include "autoware_frenet_planner/frenet_planner.hpp" +#include "autoware_sampler_common/constraints/footprint.hpp" +#include "autoware_sampler_common/constraints/hard_constraint.hpp" +#include "autoware_sampler_common/constraints/soft_constraint.hpp" +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" #include "behavior_path_sampling_planner_module/sampling_planner_parameters.hpp" #include "behavior_path_sampling_planner_module/util.hpp" -#include "bezier_sampler/bezier_sampling.hpp" -#include "frenet_planner/frenet_planner.hpp" #include "lanelet2_extension/utility/query.hpp" #include "lanelet2_extension/utility/utilities.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "rclcpp/rclcpp.hpp" -#include "sampler_common/constraints/footprint.hpp" -#include "sampler_common/constraints/hard_constraint.hpp" -#include "sampler_common/constraints/soft_constraint.hpp" -#include "sampler_common/structures.hpp" -#include "sampler_common/transform/spline_transform.hpp" #include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include "tier4_autoware_utils/math/constants.hpp" @@ -73,7 +73,7 @@ struct SamplingPlannerData struct SamplingPlannerDebugData { - std::vector sampled_candidates{}; + std::vector sampled_candidates{}; size_t previous_sampled_candidates_nb = 0UL; std::vector obstacles{}; std::vector footprints{}; @@ -231,25 +231,25 @@ class SamplingPlannerModule : public SceneModuleInterface void updateDebugMarkers(); void prepareConstraints( - sampler_common::Constraints & constraints, + autoware::sampler_common::Constraints & constraints, const PredictedObjects::ConstSharedPtr & predicted_objects, const std::vector & left_bound, const std::vector & right_bound) const; - frenet_planner::SamplingParameters prepareSamplingParameters( - const sampler_common::State & initial_state, - const sampler_common::transform::Spline2D & path_spline, + autoware::frenet_planner::SamplingParameters prepareSamplingParameters( + const autoware::sampler_common::State & initial_state, + const autoware::sampler_common::transform::Spline2D & path_spline, const SamplingPlannerInternalParameters & internal_params_); PathWithLaneId convertFrenetPathToPathWithLaneID( - const frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets, + const autoware::frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets, const double path_z); // member // std::shared_ptr params_; std::shared_ptr internal_params_; vehicle_info_util::VehicleInfo vehicle_info_{}; - std::optional prev_sampling_path_ = std::nullopt; + std::optional prev_sampling_path_ = std::nullopt; // move to utils void extendOutputDrivableArea( diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp index 6e00fefe89574..4ddfbf436ffaf 100644 --- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp +++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp @@ -15,8 +15,8 @@ #ifndef BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_ #define BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_ -#include "bezier_sampler/bezier_sampling.hpp" -#include "sampler_common/structures.hpp" +#include "autoware_bezier_sampler/bezier_sampling.hpp" +#include "autoware_sampler_common/structures.hpp" #include namespace behavior_path_planner @@ -78,12 +78,12 @@ struct Sampling std::vector target_lateral_positions{}; int nb_target_lateral_positions{}; Frenet frenet; - bezier_sampler::SamplingParameters bezier{}; + autoware::bezier_sampler::SamplingParameters bezier{}; }; struct SamplingPlannerInternalParameters { - sampler_common::Constraints constraints; + autoware::sampler_common::Constraints constraints; Sampling sampling; Preprocessing preprocessing{}; }; diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp index ac8ba788cb9bd..2825ead2a9fee 100644 --- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp +++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp @@ -14,8 +14,8 @@ #ifndef BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__UTIL_HPP_ #define BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__UTIL_HPP_ -#include "sampler_common/structures.hpp" -#include "sampler_common/transform/spline_transform.hpp" +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" #include @@ -40,18 +40,20 @@ struct SoftConstraintsInputs lanelet::ConstLanelets closest_lanelets_to_goal; behavior_path_planner::PlanResult reference_path; behavior_path_planner::PlanResult prev_module_path; - std::optional prev_path; + std::optional prev_path; lanelet::ConstLanelets current_lanes; }; using SoftConstraintsFunctionVector = std::vector>; + autoware::sampler_common::Path &, const autoware::sampler_common::Constraints &, + const SoftConstraintsInputs &)>>; using HardConstraintsFunctionVector = std::vector>; + autoware::sampler_common::Path &, const autoware::sampler_common::Constraints &, + const MultiPoint2d &)>>; inline std::vector evaluateSoftConstraints( - sampler_common::Path & path, const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, const autoware::sampler_common::Constraints & constraints, const SoftConstraintsFunctionVector & soft_constraints_functions, const SoftConstraintsInputs & input_data) { @@ -71,7 +73,7 @@ inline std::vector evaluateSoftConstraints( } inline std::vector evaluateHardConstraints( - sampler_common::Path & path, const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, const autoware::sampler_common::Constraints & constraints, const MultiPoint2d & footprint, const HardConstraintsFunctionVector & hard_constraints) { std::vector constraints_results; @@ -86,11 +88,11 @@ inline std::vector evaluateHardConstraints( return constraints_results; } -inline sampler_common::State getInitialState( +inline autoware::sampler_common::State getInitialState( const geometry_msgs::msg::Pose & pose, - const sampler_common::transform::Spline2D & reference_spline) + const autoware::sampler_common::transform::Spline2D & reference_spline) { - sampler_common::State initial_state; + autoware::sampler_common::State initial_state; Point2d initial_state_pose{pose.position.x, pose.position.y}; const auto rpy = tier4_autoware_utils::getRPY(pose.orientation); initial_state.pose = initial_state_pose; diff --git a/planning/behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_sampling_planner_module/package.xml index e6fb0d4d0f573..0583056db4293 100644 --- a/planning/behavior_path_sampling_planner_module/package.xml +++ b/planning/behavior_path_sampling_planner_module/package.xml @@ -14,17 +14,17 @@ autoware_cmake autoware_behavior_path_planner_common + autoware_bezier_sampler + autoware_frenet_planner + autoware_path_sampler autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager autoware_vehicle_msgs - bezier_sampler - frenet_planner geometry_msgs interpolation lanelet2_extension motion_utils - path_sampler pluginlib rclcpp rclcpp_components diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp index 4b0ecdcd24e14..8788b446e4384 100644 --- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp +++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp @@ -44,14 +44,16 @@ SamplingPlannerModule::SamplingPlannerModule( // check if the path is empty hard_constraints_.emplace_back( []( - sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + [[maybe_unused]] const autoware::sampler_common::Constraints & constraints, [[maybe_unused]] const MultiPoint2d & footprint) -> bool { return !path.points.empty() && !path.poses.empty(); }); hard_constraints_.emplace_back( []( - sampler_common::Path & path, const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + const autoware::sampler_common::Constraints & constraints, const MultiPoint2d & footprint) -> bool { if (!footprint.empty()) { path.constraint_results.inside_drivable_area = @@ -71,7 +73,8 @@ SamplingPlannerModule::SamplingPlannerModule( hard_constraints_.emplace_back( []( - sampler_common::Path & path, const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + const autoware::sampler_common::Constraints & constraints, [[maybe_unused]] const MultiPoint2d & footprint) -> bool { if (path.curvatures.empty()) { path.constraint_results.valid_curvature = false; @@ -96,8 +99,8 @@ SamplingPlannerModule::SamplingPlannerModule( // Yaw difference soft constraint cost -> Considering implementation // soft_constraints_.emplace_back( // [&]( - // sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & - // constraints, + // autoware::sampler_common::Path & path, [[maybe_unused]] const + // autoware::sampler_common::Constraints & constraints, // [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { // if (path.points.empty()) return 0.0; // const auto & goal_pose_yaw = @@ -109,7 +112,8 @@ SamplingPlannerModule::SamplingPlannerModule( // Remaining path length soft_constraints_.emplace_back( [&]( - sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + [[maybe_unused]] const autoware::sampler_common::Constraints & constraints, [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { if (path.points.empty()) return 0.0; if (path.poses.empty()) return 0.0; @@ -142,8 +146,8 @@ SamplingPlannerModule::SamplingPlannerModule( // Distance to centerline soft_constraints_.emplace_back( [&]( - [[maybe_unused]] sampler_common::Path & path, - [[maybe_unused]] const sampler_common::Constraints & constraints, + [[maybe_unused]] autoware::sampler_common::Path & path, + [[maybe_unused]] const autoware::sampler_common::Constraints & constraints, [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { if (path.poses.empty()) return 0.0; const auto & last_pose = path.poses.back(); @@ -159,7 +163,8 @@ SamplingPlannerModule::SamplingPlannerModule( // // Curvature cost soft_constraints_.emplace_back( []( - sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + [[maybe_unused]] const autoware::sampler_common::Constraints & constraints, [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { if (path.curvatures.empty()) return std::numeric_limits::max(); @@ -244,7 +249,7 @@ bool SamplingPlannerModule::isReferencePathSafe() const std::vector hard_constraints_results; auto transform_to_sampling_path = [](const PlanResult plan) { - sampler_common::Path path; + autoware::sampler_common::Path path; for (size_t i = 0; i < plan->points.size(); ++i) { const auto x = plan->points[i].point.pose.position.x; const auto y = plan->points[i].point.pose.position.y; @@ -258,17 +263,20 @@ bool SamplingPlannerModule::isReferencePathSafe() const } return path; }; - sampler_common::Path reference_path = transform_to_sampling_path(prev_module_reference_path); - const auto footprint = sampler_common::constraints::buildFootprintPoints( + autoware::sampler_common::Path reference_path = + transform_to_sampling_path(prev_module_reference_path); + const auto footprint = autoware::sampler_common::constraints::buildFootprintPoints( reference_path, internal_params_->constraints); behavior_path_planner::HardConstraintsFunctionVector hard_constraints_reference_path; hard_constraints_reference_path.emplace_back( []( - sampler_common::Path & path, const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + const autoware::sampler_common::Constraints & constraints, const MultiPoint2d & footprint) -> bool { path.constraint_results.collision_free = - !sampler_common::constraints::has_collision(footprint, constraints.obstacle_polygons); + !autoware::sampler_common::constraints::has_collision( + footprint, constraints.obstacle_polygons); return path.constraint_results.collision_free; }); evaluateHardConstraints( @@ -295,7 +303,7 @@ SamplingPlannerData SamplingPlannerModule::createPlannerData( } PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID( - const frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets, + const autoware::frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets, const double path_z) { auto quaternion_from_rpy = [](double roll, double pitch, double yaw) -> tf2::Quaternion { @@ -348,12 +356,12 @@ PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID( } void SamplingPlannerModule::prepareConstraints( - sampler_common::Constraints & constraints, + autoware::sampler_common::Constraints & constraints, const PredictedObjects::ConstSharedPtr & predicted_objects, const std::vector & left_bound, const std::vector & right_bound) const { - constraints.obstacle_polygons = sampler_common::MultiPolygon2d(); + constraints.obstacle_polygons = autoware::sampler_common::MultiPolygon2d(); constraints.rtree.clear(); size_t i = 0; for (const auto & o : predicted_objects->objects) { @@ -370,7 +378,7 @@ void SamplingPlannerModule::prepareConstraints( // TODO(Maxime): directly use lines instead of polygon - sampler_common::Polygon2d drivable_area_polygon; + autoware::sampler_common::Polygon2d drivable_area_polygon; for (const auto & p : right_bound) { drivable_area_polygon.outer().emplace_back(p.x, p.y); } @@ -392,7 +400,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan() if (reference_path_ptr->points.empty()) { return {}; } - auto reference_spline = [&]() -> sampler_common::transform::Spline2D { + auto reference_spline = [&]() -> autoware::sampler_common::transform::Spline2D { std::vector x; std::vector y; x.reserve(reference_path_ptr->points.size()); @@ -404,19 +412,19 @@ BehaviorModuleOutput SamplingPlannerModule::plan() return {x, y}; }(); - frenet_planner::FrenetState frenet_initial_state; - frenet_planner::SamplingParameters sampling_parameters; + autoware::frenet_planner::FrenetState frenet_initial_state; + autoware::frenet_planner::SamplingParameters sampling_parameters; const auto & pose = planner_data_->self_odometry->pose.pose; - sampler_common::State initial_state = + autoware::sampler_common::State initial_state = behavior_path_planner::getInitialState(pose, reference_spline); sampling_parameters = prepareSamplingParameters(initial_state, reference_spline, *internal_params_); auto set_frenet_state = []( - const sampler_common::State & initial_state, - const sampler_common::transform::Spline2D & reference_spline, - frenet_planner::FrenetState & frenet_initial_state) + const autoware::sampler_common::State & initial_state, + const autoware::sampler_common::transform::Spline2D & reference_spline, + autoware::frenet_planner::FrenetState & frenet_initial_state) { frenet_initial_state.position = initial_state.frenet; @@ -496,14 +504,16 @@ BehaviorModuleOutput SamplingPlannerModule::plan() const int path_divisions = internal_params_->sampling.previous_path_reuse_points_nb; const bool is_extend_previous_path = path_divisions > 0; - std::vector frenet_paths; + std::vector frenet_paths; // Extend prev path if (prev_path_is_valid && is_extend_previous_path) { - frenet_planner::Path prev_path_frenet = prev_sampling_path_.value(); + autoware::frenet_planner::Path prev_path_frenet = prev_sampling_path_.value(); frenet_paths.push_back(prev_path_frenet); - auto get_subset = [](const frenet_planner::Path & path, size_t offset) -> frenet_planner::Path { - frenet_planner::Path s; + auto get_subset = []( + const autoware::frenet_planner::Path & path, + size_t offset) -> autoware::frenet_planner::Path { + autoware::frenet_planner::Path s; s.points = {path.points.begin(), path.points.begin() + offset}; s.curvatures = {path.curvatures.begin(), path.curvatures.begin() + offset}; s.yaws = {path.yaws.begin(), path.yaws.begin() + offset}; @@ -512,7 +522,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan() return s; }; - sampler_common::State current_state; + autoware::sampler_common::State current_state; current_state.pose = {ego_pose.position.x, ego_pose.position.y}; const auto closest_iter = std::min_element( @@ -537,22 +547,22 @@ BehaviorModuleOutput SamplingPlannerModule::plan() const auto reused_path = get_subset(prev_path_frenet, reuse_idx); geometry_msgs::msg::Pose future_pose = reused_path.poses.back(); - sampler_common::State future_state = + autoware::sampler_common::State future_state = behavior_path_planner::getInitialState(future_pose, reference_spline); - frenet_planner::FrenetState frenet_reuse_state; + autoware::frenet_planner::FrenetState frenet_reuse_state; set_frenet_state(future_state, reference_spline, frenet_reuse_state); - frenet_planner::SamplingParameters extension_sampling_parameters = + autoware::frenet_planner::SamplingParameters extension_sampling_parameters = prepareSamplingParameters(future_state, reference_spline, *internal_params_); - auto extension_frenet_paths = frenet_planner::generatePaths( + auto extension_frenet_paths = autoware::frenet_planner::generatePaths( reference_spline, frenet_reuse_state, extension_sampling_parameters); for (auto & p : extension_frenet_paths) { if (!p.points.empty()) frenet_paths.push_back(reused_path.extend(p)); } } } else { - frenet_paths = - frenet_planner::generatePaths(reference_spline, frenet_initial_state, sampling_parameters); + frenet_paths = autoware::frenet_planner::generatePaths( + reference_spline, frenet_initial_state, sampling_parameters); } const auto path_for_calculating_bounds = getPreviousModuleOutput().reference_path; @@ -587,8 +597,8 @@ BehaviorModuleOutput SamplingPlannerModule::plan() std::vector> hard_constraints_results_full; std::vector> soft_constraints_results_full; for (auto & path : frenet_paths) { - const auto footprint = - sampler_common::constraints::buildFootprintPoints(path, internal_params_->constraints); + const auto footprint = autoware::sampler_common::constraints::buildFootprintPoints( + path, internal_params_->constraints); std::vector hard_constraints_results = evaluateHardConstraints(path, internal_params_->constraints, footprint, hard_constraints_); path.constraint_results.valid_curvature = true; @@ -598,7 +608,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan() soft_constraints_results_full.push_back(soft_constraints_results); } - std::vector candidate_paths; + std::vector candidate_paths; const auto move_to_paths = [&candidate_paths](auto & paths_to_move) { candidate_paths.insert( candidate_paths.end(), std::make_move_iterator(paths_to_move.begin()), @@ -917,9 +927,9 @@ DrivableLanes SamplingPlannerModule::generateExpandDrivableLanes( return current_drivable_lanes; } -frenet_planner::SamplingParameters SamplingPlannerModule::prepareSamplingParameters( - const sampler_common::State & initial_state, - const sampler_common::transform::Spline2D & path_spline, +autoware::frenet_planner::SamplingParameters SamplingPlannerModule::prepareSamplingParameters( + const autoware::sampler_common::State & initial_state, + const autoware::sampler_common::transform::Spline2D & path_spline, const SamplingPlannerInternalParameters & params_) { // calculate target lateral positions @@ -953,10 +963,10 @@ frenet_planner::SamplingParameters SamplingPlannerModule::prepareSamplingParamet } else { target_lateral_positions = params_.sampling.target_lateral_positions; } - frenet_planner::SamplingParameters sampling_parameters; + autoware::frenet_planner::SamplingParameters sampling_parameters; sampling_parameters.resolution = params_.sampling.resolution; const auto max_s = path_spline.lastS(); - frenet_planner::SamplingParameter p; + autoware::frenet_planner::SamplingParameter p; for (const auto target_length : params_.sampling.target_lengths) { p.target_state.position.s = std::min(max_s, path_spline.frenet(initial_state.pose).s + std::max(0.0, target_length)); diff --git a/planning/sampling_based_planner/bezier_sampler/CMakeLists.txt b/planning/sampling_based_planner/autoware_bezier_sampler/CMakeLists.txt similarity index 80% rename from planning/sampling_based_planner/bezier_sampler/CMakeLists.txt rename to planning/sampling_based_planner/autoware_bezier_sampler/CMakeLists.txt index 76d0fc99843f1..8d5bd554ab282 100644 --- a/planning/sampling_based_planner/bezier_sampler/CMakeLists.txt +++ b/planning/sampling_based_planner/autoware_bezier_sampler/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(bezier_sampler) +project(autoware_bezier_sampler) find_package(autoware_cmake REQUIRED) autoware_package() @@ -7,7 +7,7 @@ autoware_package() find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) -ament_auto_add_library(bezier_sampler SHARED +ament_auto_add_library(autoware_bezier_sampler SHARED src/bezier.cpp src/bezier_sampling.cpp ) diff --git a/planning/sampling_based_planner/bezier_sampler/README.md b/planning/sampling_based_planner/autoware_bezier_sampler/README.md similarity index 100% rename from planning/sampling_based_planner/bezier_sampler/README.md rename to planning/sampling_based_planner/autoware_bezier_sampler/README.md diff --git a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier.hpp b/planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier.hpp similarity index 94% rename from planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier.hpp rename to planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier.hpp index 93c6ec6b87f36..5d16c55d793af 100644 --- a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier.hpp +++ b/planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEZIER_SAMPLER__BEZIER_HPP_ -#define BEZIER_SAMPLER__BEZIER_HPP_ +#ifndef AUTOWARE_BEZIER_SAMPLER__BEZIER_HPP_ +#define AUTOWARE_BEZIER_SAMPLER__BEZIER_HPP_ #include #include -namespace bezier_sampler +namespace autoware::bezier_sampler { // Coefficients for matrix calculation of the quintic Bézier curve. @@ -70,6 +70,6 @@ class Bezier /// @brief calculate the curvature for the given parameter t [[nodiscard]] double curvature(const double t) const; }; -} // namespace bezier_sampler +} // namespace autoware::bezier_sampler -#endif // BEZIER_SAMPLER__BEZIER_HPP_ +#endif // AUTOWARE_BEZIER_SAMPLER__BEZIER_HPP_ diff --git a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp b/planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier_sampling.hpp similarity index 80% rename from planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp rename to planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier_sampling.hpp index 18df9dcb796d8..a832ef1f5cd39 100644 --- a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp +++ b/planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier_sampling.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_ -#define BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_ +#ifndef AUTOWARE_BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_ +#define AUTOWARE_BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_ -#include +#include +#include #include #include -#include #include -namespace bezier_sampler +namespace autoware::bezier_sampler { struct SamplingParameters { @@ -38,13 +38,13 @@ struct SamplingParameters /// @details from Section IV of A. Artuñedoet al.: Real-Time Motion Planning Approach for Automated /// Driving in Urban Environments std::vector sample( - const sampler_common::State & initial, const sampler_common::State & final, + const autoware::sampler_common::State & initial, const autoware::sampler_common::State & final, const SamplingParameters & params); /// @brief generate a Bezier curve for the given states, velocities, and accelerations Bezier generate( const Eigen::Vector2d & initial_pose, const Eigen::Vector2d & final_pose, const Eigen::Vector2d & initial_velocity, const Eigen::Vector2d & initial_acceleration, const Eigen::Vector2d & final_velocity, const Eigen::Vector2d & final_acceleration); -} // namespace bezier_sampler +} // namespace autoware::bezier_sampler -#endif // BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_ +#endif // AUTOWARE_BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_ diff --git a/planning/sampling_based_planner/bezier_sampler/package.xml b/planning/sampling_based_planner/autoware_bezier_sampler/package.xml similarity index 89% rename from planning/sampling_based_planner/bezier_sampler/package.xml rename to planning/sampling_based_planner/autoware_bezier_sampler/package.xml index efe65e830871a..62dce46816119 100644 --- a/planning/sampling_based_planner/bezier_sampler/package.xml +++ b/planning/sampling_based_planner/autoware_bezier_sampler/package.xml @@ -1,6 +1,6 @@ - bezier_sampler + autoware_bezier_sampler 0.0.1 Package to sample trajectories using Bézier curves Maxime CLEMENT @@ -11,8 +11,8 @@ autoware_cmake + autoware_sampler_common eigen - sampler_common ament_cmake_ros ament_lint_auto diff --git a/planning/sampling_based_planner/bezier_sampler/src/bezier.cpp b/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier.cpp similarity index 96% rename from planning/sampling_based_planner/bezier_sampler/src/bezier.cpp rename to planning/sampling_based_planner/autoware_bezier_sampler/src/bezier.cpp index fc4c09840f507..9a882ad06659a 100644 --- a/planning/sampling_based_planner/bezier_sampler/src/bezier.cpp +++ b/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include -namespace bezier_sampler +namespace autoware::bezier_sampler { Bezier::Bezier(Eigen::Matrix control_points) : control_points_(std::move(control_points)) @@ -117,4 +117,4 @@ double Bezier::heading(const double t) const return std::atan2(vel.y(), vel.x()); } -} // namespace bezier_sampler +} // namespace autoware::bezier_sampler diff --git a/planning/sampling_based_planner/bezier_sampler/src/bezier_sampling.cpp b/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier_sampling.cpp similarity index 94% rename from planning/sampling_based_planner/bezier_sampler/src/bezier_sampling.cpp rename to planning/sampling_based_planner/autoware_bezier_sampler/src/bezier_sampling.cpp index a9fa318980250..e9a1a9ef32de5 100644 --- a/planning/sampling_based_planner/bezier_sampler/src/bezier_sampling.cpp +++ b/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier_sampling.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include -namespace bezier_sampler +namespace autoware::bezier_sampler { std::vector sample( - const sampler_common::State & initial, const sampler_common::State & final, + const autoware::sampler_common::State & initial, const autoware::sampler_common::State & final, const SamplingParameters & params) { std::vector curves; @@ -75,4 +75,4 @@ Bezier generate( (1.0 / 20.0) * final_acceleration.transpose(); return Bezier(control_points); } -} // namespace bezier_sampler +} // namespace autoware::bezier_sampler diff --git a/planning/sampling_based_planner/frenet_planner/CMakeLists.txt b/planning/sampling_based_planner/autoware_frenet_planner/CMakeLists.txt similarity index 78% rename from planning/sampling_based_planner/frenet_planner/CMakeLists.txt rename to planning/sampling_based_planner/autoware_frenet_planner/CMakeLists.txt index 0d7918daa215b..f928702a387b9 100644 --- a/planning/sampling_based_planner/frenet_planner/CMakeLists.txt +++ b/planning/sampling_based_planner/autoware_frenet_planner/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(frenet_planner) +project(autoware_frenet_planner) find_package(autoware_cmake REQUIRED) autoware_package() @@ -7,7 +7,7 @@ autoware_package() find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) -ament_auto_add_library(frenet_planner SHARED +ament_auto_add_library(autoware_frenet_planner SHARED DIRECTORY src/ ) diff --git a/planning/sampling_based_planner/frenet_planner/README.md b/planning/sampling_based_planner/autoware_frenet_planner/README.md similarity index 100% rename from planning/sampling_based_planner/frenet_planner/README.md rename to planning/sampling_based_planner/autoware_frenet_planner/README.md diff --git a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/conversions.hpp b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/conversions.hpp similarity index 81% rename from planning/sampling_based_planner/frenet_planner/include/frenet_planner/conversions.hpp rename to planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/conversions.hpp index f8575d8f65936..dc504ba1386eb 100644 --- a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/conversions.hpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/conversions.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef FRENET_PLANNER__CONVERSIONS_HPP_ -#define FRENET_PLANNER__CONVERSIONS_HPP_ +#ifndef AUTOWARE_FRENET_PLANNER__CONVERSIONS_HPP_ +#define AUTOWARE_FRENET_PLANNER__CONVERSIONS_HPP_ -#include "frenet_planner/polynomials.hpp" +#include "autoware_frenet_planner/polynomials.hpp" -#include +#include #include -namespace frenet_planner +namespace autoware::frenet_planner { /// @brief calculate the lengths and yaws vectors of the given trajectory @@ -44,6 +44,6 @@ inline void calculateLengthsAndYaws(Trajectory & trajectory) const auto last_yaw = trajectory.yaws.empty() ? 0.0 : trajectory.yaws.back(); trajectory.yaws.push_back(last_yaw); } -} // namespace frenet_planner +} // namespace autoware::frenet_planner -#endif // FRENET_PLANNER__CONVERSIONS_HPP_ +#endif // AUTOWARE_FRENET_PLANNER__CONVERSIONS_HPP_ diff --git a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/frenet_planner.hpp similarity index 67% rename from planning/sampling_based_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp rename to planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/frenet_planner.hpp index 7d60214a52722..88eb2f3244e40 100644 --- a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/frenet_planner.hpp @@ -12,34 +12,34 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef FRENET_PLANNER__FRENET_PLANNER_HPP_ -#define FRENET_PLANNER__FRENET_PLANNER_HPP_ +#ifndef AUTOWARE_FRENET_PLANNER__FRENET_PLANNER_HPP_ +#define AUTOWARE_FRENET_PLANNER__FRENET_PLANNER_HPP_ -#include "frenet_planner/structures.hpp" -#include "sampler_common/structures.hpp" -#include "sampler_common/transform/spline_transform.hpp" +#include "autoware_frenet_planner/structures.hpp" +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include #include -namespace frenet_planner +namespace autoware::frenet_planner { /// @brief generate trajectories relative to the reference for the given initial state and sampling /// parameters std::vector generateTrajectories( - const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, - const SamplingParameters & sampling_parameters); + const autoware::sampler_common::transform::Spline2D & reference_spline, + const FrenetState & initial_state, const SamplingParameters & sampling_parameters); /// @brief generate trajectories relative to the reference for the given initial state and sampling /// parameters std::vector generateLowVelocityTrajectories( - const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, - const SamplingParameters & sampling_parameters); + const autoware::sampler_common::transform::Spline2D & reference_spline, + const FrenetState & initial_state, const SamplingParameters & sampling_parameters); /// @brief generate paths relative to the reference for the given initial state and sampling /// parameters std::vector generatePaths( - const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, - const SamplingParameters & sampling_parameters); + const autoware::sampler_common::transform::Spline2D & reference_spline, + const FrenetState & initial_state, const SamplingParameters & sampling_parameters); /// @brief generate a candidate path /// @details one polynomial for lateral motion (d) is calculated over the longitudinal displacement /// (s): d(s). @@ -58,11 +58,12 @@ Trajectory generateLowVelocityCandidate( const FrenetState & initial_state, const FrenetState & target_state, const double duration, const double time_resolution); /// @brief calculate the cartesian frame of the given path -void calculateCartesian(const sampler_common::transform::Spline2D & reference, Path & path); +void calculateCartesian( + const autoware::sampler_common::transform::Spline2D & reference, Path & path); /// @brief calculate the cartesian frame of the given trajectory void calculateCartesian( - const sampler_common::transform::Spline2D & reference, Trajectory & trajectory); + const autoware::sampler_common::transform::Spline2D & reference, Trajectory & trajectory); -} // namespace frenet_planner +} // namespace autoware::frenet_planner -#endif // FRENET_PLANNER__FRENET_PLANNER_HPP_ +#endif // AUTOWARE_FRENET_PLANNER__FRENET_PLANNER_HPP_ diff --git a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/polynomials.hpp b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/polynomials.hpp similarity index 88% rename from planning/sampling_based_planner/frenet_planner/include/frenet_planner/polynomials.hpp rename to planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/polynomials.hpp index 65157c76d5db3..02217ef8ad444 100644 --- a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/polynomials.hpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/polynomials.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef FRENET_PLANNER__POLYNOMIALS_HPP_ -#define FRENET_PLANNER__POLYNOMIALS_HPP_ +#ifndef AUTOWARE_FRENET_PLANNER__POLYNOMIALS_HPP_ +#define AUTOWARE_FRENET_PLANNER__POLYNOMIALS_HPP_ -namespace frenet_planner +namespace autoware::frenet_planner { class Polynomial { @@ -50,6 +50,6 @@ class Polynomial /// @brief Get the jerk at the given time [[nodiscard]] double jerk(const double t) const; }; -} // namespace frenet_planner +} // namespace autoware::frenet_planner -#endif // FRENET_PLANNER__POLYNOMIALS_HPP_ +#endif // AUTOWARE_FRENET_PLANNER__POLYNOMIALS_HPP_ diff --git a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/structures.hpp b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/structures.hpp similarity index 73% rename from planning/sampling_based_planner/frenet_planner/include/frenet_planner/structures.hpp rename to planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/structures.hpp index c44cb5d814d71..d2a71d06a7933 100644 --- a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/structures.hpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/structures.hpp @@ -12,52 +12,54 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef FRENET_PLANNER__STRUCTURES_HPP_ -#define FRENET_PLANNER__STRUCTURES_HPP_ +#ifndef AUTOWARE_FRENET_PLANNER__STRUCTURES_HPP_ +#define AUTOWARE_FRENET_PLANNER__STRUCTURES_HPP_ -#include "frenet_planner/polynomials.hpp" +#include "autoware_frenet_planner/polynomials.hpp" -#include +#include #include #include -namespace frenet_planner +namespace autoware::frenet_planner { struct FrenetState { - sampler_common::FrenetPoint position = {0, 0}; + autoware::sampler_common::FrenetPoint position = {0, 0}; double lateral_velocity{}; double longitudinal_velocity{}; double lateral_acceleration{}; double longitudinal_acceleration{}; }; -struct Path : sampler_common::Path +struct Path : autoware::sampler_common::Path { - std::vector frenet_points{}; + std::vector frenet_points{}; std::optional lateral_polynomial{}; Path() = default; - explicit Path(const sampler_common::Path & path) : sampler_common::Path(path) {} + explicit Path(const autoware::sampler_common::Path & path) : autoware::sampler_common::Path(path) + { + } void clear() override { - sampler_common::Path::clear(); + autoware::sampler_common::Path::clear(); frenet_points.clear(); lateral_polynomial.reset(); } void reserve(const size_t size) override { - sampler_common::Path::reserve(size); + autoware::sampler_common::Path::reserve(size); frenet_points.reserve(size); } [[nodiscard]] Path extend(const Path & path) const { - Path extended_traj(sampler_common::Path::extend(path)); + Path extended_traj(autoware::sampler_common::Path::extend(path)); extended_traj.frenet_points.insert( extended_traj.frenet_points.end(), frenet_points.begin(), frenet_points.end()); extended_traj.frenet_points.insert( @@ -69,7 +71,7 @@ struct Path : sampler_common::Path [[nodiscard]] Path * subset(const size_t from_idx, const size_t to_idx) const override { - auto * subpath = new Path(*sampler_common::Path::subset(from_idx, to_idx)); + auto * subpath = new Path(*autoware::sampler_common::Path::subset(from_idx, to_idx)); assert(to_idx >= from_idx); subpath->reserve(to_idx - from_idx); @@ -87,19 +89,22 @@ struct SamplingParameter FrenetState target_state; }; -struct Trajectory : sampler_common::Trajectory +struct Trajectory : autoware::sampler_common::Trajectory { - std::vector frenet_points{}; + std::vector frenet_points{}; std::optional lateral_polynomial{}; std::optional longitudinal_polynomial{}; SamplingParameter sampling_parameter; Trajectory() = default; - explicit Trajectory(const sampler_common::Trajectory & traj) : sampler_common::Trajectory(traj) {} + explicit Trajectory(const autoware::sampler_common::Trajectory & traj) + : autoware::sampler_common::Trajectory(traj) + { + } void clear() override { - sampler_common::Trajectory::clear(); + autoware::sampler_common::Trajectory::clear(); frenet_points.clear(); lateral_polynomial.reset(); longitudinal_polynomial.reset(); @@ -107,13 +112,13 @@ struct Trajectory : sampler_common::Trajectory void reserve(const size_t size) override { - sampler_common::Trajectory::reserve(size); + autoware::sampler_common::Trajectory::reserve(size); frenet_points.reserve(size); } [[nodiscard]] Trajectory extend(const Trajectory & traj) const { - Trajectory extended_traj(sampler_common::Trajectory::extend(traj)); + Trajectory extended_traj(autoware::sampler_common::Trajectory::extend(traj)); extended_traj.frenet_points.insert( extended_traj.frenet_points.end(), frenet_points.begin(), frenet_points.end()); extended_traj.frenet_points.insert( @@ -126,7 +131,8 @@ struct Trajectory : sampler_common::Trajectory [[nodiscard]] Trajectory * subset(const size_t from_idx, const size_t to_idx) const override { - auto * sub_trajectory = new Trajectory(*sampler_common::Trajectory::subset(from_idx, to_idx)); + auto * sub_trajectory = + new Trajectory(*autoware::sampler_common::Trajectory::subset(from_idx, to_idx)); assert(to_idx >= from_idx); sub_trajectory->reserve(to_idx - from_idx); @@ -151,6 +157,6 @@ struct SamplingParameters std::vector parameters; double resolution; }; -} // namespace frenet_planner +} // namespace autoware::frenet_planner -#endif // FRENET_PLANNER__STRUCTURES_HPP_ +#endif // AUTOWARE_FRENET_PLANNER__STRUCTURES_HPP_ diff --git a/planning/sampling_based_planner/frenet_planner/launch/frenet_planner.launch b/planning/sampling_based_planner/autoware_frenet_planner/launch/frenet_planner.launch similarity index 78% rename from planning/sampling_based_planner/frenet_planner/launch/frenet_planner.launch rename to planning/sampling_based_planner/autoware_frenet_planner/launch/frenet_planner.launch index 9570d488eaecc..69e5961b9cdb8 100644 --- a/planning/sampling_based_planner/frenet_planner/launch/frenet_planner.launch +++ b/planning/sampling_based_planner/autoware_frenet_planner/launch/frenet_planner.launch @@ -3,7 +3,7 @@ - + diff --git a/planning/sampling_based_planner/frenet_planner/package.xml b/planning/sampling_based_planner/autoware_frenet_planner/package.xml similarity index 76% rename from planning/sampling_based_planner/frenet_planner/package.xml rename to planning/sampling_based_planner/autoware_frenet_planner/package.xml index c378c6ae25e9b..1ba7c8a148007 100644 --- a/planning/sampling_based_planner/frenet_planner/package.xml +++ b/planning/sampling_based_planner/autoware_frenet_planner/package.xml @@ -1,8 +1,8 @@ - frenet_planner + autoware_frenet_planner 0.0.1 - The frenet_planner package for trajectory generation in Frenet frame + The autoware_frenet_planner package for trajectory generation in Frenet frame Maxime CLEMENT Maxime CLEMENT Apache License 2.0 @@ -12,7 +12,7 @@ autoware_cmake autoware_auto_common - sampler_common + autoware_sampler_common ament_cmake_ros ament_lint_auto diff --git a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp similarity index 89% rename from planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp rename to planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp index 6f0bef882743c..ba3156c6b085d 100644 --- a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "frenet_planner/frenet_planner.hpp" +#include "autoware_frenet_planner/frenet_planner.hpp" #include +#include +#include +#include +#include +#include #include -#include -#include -#include -#include -#include #include "autoware_planning_msgs/msg/path.hpp" #include @@ -31,11 +31,11 @@ #include #include -namespace frenet_planner +namespace autoware::frenet_planner { std::vector generateTrajectories( - const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, - const SamplingParameters & sampling_parameters) + const autoware::sampler_common::transform::Spline2D & reference_spline, + const FrenetState & initial_state, const SamplingParameters & sampling_parameters) { std::vector trajectories; trajectories.reserve(sampling_parameters.parameters.size()); @@ -54,8 +54,8 @@ std::vector generateTrajectories( } std::vector generateLowVelocityTrajectories( - const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, - const SamplingParameters & sampling_parameters) + const autoware::sampler_common::transform::Spline2D & reference_spline, + const FrenetState & initial_state, const SamplingParameters & sampling_parameters) { std::vector trajectories; trajectories.reserve(sampling_parameters.parameters.size()); @@ -73,8 +73,8 @@ std::vector generateLowVelocityTrajectories( } std::vector generatePaths( - const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, - const SamplingParameters & sampling_parameters) + const autoware::sampler_common::transform::Spline2D & reference_spline, + const FrenetState & initial_state, const SamplingParameters & sampling_parameters) { std::vector candidates; candidates.reserve(sampling_parameters.parameters.size()); @@ -148,7 +148,8 @@ Path generateCandidate( return path; } -void calculateCartesian(const sampler_common::transform::Spline2D & reference, Path & path) +void calculateCartesian( + const autoware::sampler_common::transform::Spline2D & reference, Path & path) { if (!path.frenet_points.empty()) { path.points.reserve(path.frenet_points.size()); @@ -191,7 +192,7 @@ void calculateCartesian(const sampler_common::transform::Spline2D & reference, P } } void calculateCartesian( - const sampler_common::transform::Spline2D & reference, Trajectory & trajectory) + const autoware::sampler_common::transform::Spline2D & reference, Trajectory & trajectory) { if (!trajectory.frenet_points.empty()) { trajectory.points.reserve(trajectory.frenet_points.size()); @@ -240,4 +241,4 @@ void calculateCartesian( } } -} // namespace frenet_planner +} // namespace autoware::frenet_planner diff --git a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/polynomials.cpp b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/polynomials.cpp similarity index 94% rename from planning/sampling_based_planner/frenet_planner/src/frenet_planner/polynomials.cpp rename to planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/polynomials.cpp index 13f71dc88f9d6..b785255cd856c 100644 --- a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/polynomials.cpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/polynomials.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include -namespace frenet_planner +namespace autoware::frenet_planner { // @brief Create a polynomial connecting the given initial and target configuration over the given // duration @@ -72,4 +72,4 @@ double Polynomial::jerk(const double t) const const double t2 = t * t; return 60 * a_ * t2 + 24 * b_ * t + 6 * c_; } -} // namespace frenet_planner +} // namespace autoware::frenet_planner diff --git a/planning/sampling_based_planner/path_sampler/CMakeLists.txt b/planning/sampling_based_planner/autoware_path_sampler/CMakeLists.txt similarity index 52% rename from planning/sampling_based_planner/path_sampler/CMakeLists.txt rename to planning/sampling_based_planner/autoware_path_sampler/CMakeLists.txt index 5a5b99f0aff3a..170646b0c0c9a 100644 --- a/planning/sampling_based_planner/path_sampler/CMakeLists.txt +++ b/planning/sampling_based_planner/autoware_path_sampler/CMakeLists.txt @@ -1,16 +1,16 @@ cmake_minimum_required(VERSION 3.14) -project(path_sampler) +project(autoware_path_sampler) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(path_sampler SHARED +ament_auto_add_library(autoware_path_sampler SHARED DIRECTORY src ) # register node -rclcpp_components_register_node(path_sampler - PLUGIN "path_sampler::PathSampler" +rclcpp_components_register_node(autoware_path_sampler + PLUGIN "autoware::path_sampler::PathSampler" EXECUTABLE path_sampler_exe ) diff --git a/planning/sampling_based_planner/path_sampler/README.md b/planning/sampling_based_planner/autoware_path_sampler/README.md similarity index 100% rename from planning/sampling_based_planner/path_sampler/README.md rename to planning/sampling_based_planner/autoware_path_sampler/README.md diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/common_structs.hpp similarity index 87% rename from planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp rename to planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/common_structs.hpp index 7cf88cb75e13a..5a8183d8e5926 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/common_structs.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SAMPLER__COMMON_STRUCTS_HPP_ -#define PATH_SAMPLER__COMMON_STRUCTS_HPP_ +#ifndef AUTOWARE_PATH_SAMPLER__COMMON_STRUCTS_HPP_ +#define AUTOWARE_PATH_SAMPLER__COMMON_STRUCTS_HPP_ -#include "path_sampler/type_alias.hpp" +#include "autoware_path_sampler/type_alias.hpp" +#include "autoware_sampler_common/structures.hpp" #include "rclcpp/rclcpp.hpp" -#include "sampler_common/structures.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" @@ -28,7 +28,7 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { struct PlannerData { @@ -61,7 +61,7 @@ struct TimeKeeper latest_stream.str(""); if (enable_calculation_time_info) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("path_sampler.time"), msg); + RCLCPP_INFO_STREAM(rclcpp::get_logger("autoware_path_sampler.time"), msg); } } @@ -87,7 +87,7 @@ struct TimeKeeper struct DebugData { - std::vector sampled_candidates{}; + std::vector sampled_candidates{}; size_t previous_sampled_candidates_nb = 0UL; std::vector obstacles{}; std::vector footprints{}; @@ -131,6 +131,6 @@ struct EgoNearestParam double dist_threshold{0.0}; double yaw_threshold{0.0}; }; -} // namespace path_sampler +} // namespace autoware::path_sampler -#endif // PATH_SAMPLER__COMMON_STRUCTS_HPP_ +#endif // AUTOWARE_PATH_SAMPLER__COMMON_STRUCTS_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/node.hpp similarity index 77% rename from planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp rename to planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/node.hpp index ab9bc71bc79e4..e44c38aed2b53 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/node.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SAMPLER__NODE_HPP_ -#define PATH_SAMPLER__NODE_HPP_ +#ifndef AUTOWARE_PATH_SAMPLER__NODE_HPP_ +#define AUTOWARE_PATH_SAMPLER__NODE_HPP_ -#include "path_sampler/common_structs.hpp" -#include "path_sampler/parameters.hpp" -#include "path_sampler/type_alias.hpp" +#include "autoware_path_sampler/common_structs.hpp" +#include "autoware_path_sampler/parameters.hpp" +#include "autoware_path_sampler/type_alias.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" #include "rclcpp/rclcpp.hpp" -#include "sampler_common/transform/spline_transform.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" -#include +#include #include #include @@ -30,7 +30,7 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { class PathSampler : public rclcpp::Node { @@ -54,7 +54,7 @@ class PathSampler : public rclcpp::Node PredictedObjects::SharedPtr in_objects_ptr_ = std::make_shared(); // variables for previous information - std::optional prev_path_; + std::optional prev_path_; // interface publisher rclcpp::Publisher::SharedPtr traj_pub_; @@ -86,8 +86,9 @@ class PathSampler : public rclcpp::Node const std::vector & traj_points, const std::vector & optimized_points) const; void resetPreviousData(); - sampler_common::State getPlanningState( - sampler_common::State & state, const sampler_common::transform::Spline2D & path_spline) const; + autoware::sampler_common::State getPlanningState( + autoware::sampler_common::State & state, + const autoware::sampler_common::transform::Spline2D & path_spline) const; // sub-functions of generateTrajectory void copyZ( @@ -95,13 +96,14 @@ class PathSampler : public rclcpp::Node void copyVelocity( const std::vector & from_traj, std::vector & to_traj, const geometry_msgs::msg::Pose & ego_pose); - sampler_common::Path generatePath(const PlannerData & planner_data); - std::vector generateCandidatesFromPreviousPath( - const PlannerData & planner_data, const sampler_common::transform::Spline2D & path_spline); + autoware::sampler_common::Path generatePath(const PlannerData & planner_data); + std::vector generateCandidatesFromPreviousPath( + const PlannerData & planner_data, + const autoware::sampler_common::transform::Spline2D & path_spline); std::vector generateTrajectoryPoints(const PlannerData & planner_data); void publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const; void publishDebugMarker(const std::vector & traj_points) const; }; -} // namespace path_sampler +} // namespace autoware::path_sampler -#endif // PATH_SAMPLER__NODE_HPP_ +#endif // AUTOWARE_PATH_SAMPLER__NODE_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/parameters.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/parameters.hpp similarity index 77% rename from planning/sampling_based_planner/path_sampler/include/path_sampler/parameters.hpp rename to planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/parameters.hpp index 03887d8357e0d..0ce0ea875c066 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/parameters.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/parameters.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SAMPLER__PARAMETERS_HPP_ -#define PATH_SAMPLER__PARAMETERS_HPP_ +#ifndef AUTOWARE_PATH_SAMPLER__PARAMETERS_HPP_ +#define AUTOWARE_PATH_SAMPLER__PARAMETERS_HPP_ -#include "bezier_sampler/bezier_sampling.hpp" -#include "sampler_common/structures.hpp" +#include "autoware_bezier_sampler/bezier_sampling.hpp" +#include "autoware_sampler_common/structures.hpp" #include struct Parameters { - sampler_common::Constraints constraints; + autoware::sampler_common::Constraints constraints; struct { bool enable_frenet{}; @@ -37,7 +37,7 @@ struct Parameters std::vector target_lateral_velocities{}; std::vector target_lateral_accelerations{}; } frenet; - bezier_sampler::SamplingParameters bezier{}; + autoware::bezier_sampler::SamplingParameters bezier{}; } sampling; struct @@ -54,4 +54,4 @@ struct Parameters } path_reuse{}; }; -#endif // PATH_SAMPLER__PARAMETERS_HPP_ +#endif // AUTOWARE_PATH_SAMPLER__PARAMETERS_HPP_ diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/path_generation.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/path_generation.hpp new file mode 100644 index 0000000000000..ef234f1d8207a --- /dev/null +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/path_generation.hpp @@ -0,0 +1,52 @@ +// Copyright 2023 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 AUTOWARE_PATH_SAMPLER__PATH_GENERATION_HPP_ +#define AUTOWARE_PATH_SAMPLER__PATH_GENERATION_HPP_ + +#include "autoware_bezier_sampler/bezier_sampling.hpp" +#include "autoware_frenet_planner/structures.hpp" +#include "autoware_path_sampler/parameters.hpp" +#include "autoware_sampler_common/constraints/hard_constraint.hpp" +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" + +#include + +#include + +namespace autoware::path_sampler +{ +/** + * @brief generate candidate paths for the given problem inputs + * @param [in] initial_state initial ego state + * @param [in] path_spline spline of the reference path + * @param [in] base_length base length of the reuse path (= 0.0 if not reusing a previous path) + * @param [in] params parameters + * @return candidate paths + */ +std::vector generateCandidatePaths( + const autoware::sampler_common::State & initial_state, + const autoware::sampler_common::transform::Spline2D & path_spline, const double base_length, + const Parameters & params); + +std::vector generateBezierPaths( + const autoware::sampler_common::State & initial_state, const double base_length, + const autoware::sampler_common::transform::Spline2D & path_spline, const Parameters & params); +std::vector generateFrenetPaths( + const autoware::sampler_common::State & initial_state, const double base_length, + const autoware::sampler_common::transform::Spline2D & path_spline, const Parameters & params); +} // namespace autoware::path_sampler + +#endif // AUTOWARE_PATH_SAMPLER__PATH_GENERATION_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/prepare_inputs.hpp similarity index 64% rename from planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp rename to planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/prepare_inputs.hpp index a9a2728b27c49..1b10be1121481 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/prepare_inputs.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SAMPLER__PREPARE_INPUTS_HPP_ -#define PATH_SAMPLER__PREPARE_INPUTS_HPP_ +#ifndef AUTOWARE_PATH_SAMPLER__PREPARE_INPUTS_HPP_ +#define AUTOWARE_PATH_SAMPLER__PREPARE_INPUTS_HPP_ -#include "frenet_planner/structures.hpp" -#include "path_sampler/parameters.hpp" -#include "path_sampler/type_alias.hpp" -#include "sampler_common/transform/spline_transform.hpp" +#include "autoware_frenet_planner/structures.hpp" +#include "autoware_path_sampler/parameters.hpp" +#include "autoware_path_sampler/type_alias.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" #include #include @@ -35,20 +35,20 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { /// @brief prepare constraints void prepareConstraints( - sampler_common::Constraints & constraints, const PredictedObjects & predicted_objects, + autoware::sampler_common::Constraints & constraints, const PredictedObjects & predicted_objects, const std::vector & left_bound, const std::vector & right_bound); /// @brief prepare sampling parameters to generate Frenet paths -frenet_planner::SamplingParameters prepareSamplingParameters( - const sampler_common::State & initial_state, const double base_length, - const sampler_common::transform::Spline2D & path_spline, const Parameters & params); +autoware::frenet_planner::SamplingParameters prepareSamplingParameters( + const autoware::sampler_common::State & initial_state, const double base_length, + const autoware::sampler_common::transform::Spline2D & path_spline, const Parameters & params); /// @brief prepare the 2D spline representation of the given Path message -sampler_common::transform::Spline2D preparePathSpline( +autoware::sampler_common::transform::Spline2D preparePathSpline( const std::vector & path_msg, const bool smooth_path); -} // namespace path_sampler +} // namespace autoware::path_sampler -#endif // PATH_SAMPLER__PREPARE_INPUTS_HPP_ +#endif // AUTOWARE_PATH_SAMPLER__PREPARE_INPUTS_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/type_alias.hpp similarity index 88% rename from planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp rename to planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/type_alias.hpp index 90e655c086444..95d833c4df47d 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/type_alias.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SAMPLER__TYPE_ALIAS_HPP_ -#define PATH_SAMPLER__TYPE_ALIAS_HPP_ +#ifndef AUTOWARE_PATH_SAMPLER__TYPE_ALIAS_HPP_ +#define AUTOWARE_PATH_SAMPLER__TYPE_ALIAS_HPP_ #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/path.hpp" @@ -28,7 +28,7 @@ #include "tier4_debug_msgs/msg/string_stamped.hpp" #include "visualization_msgs/msg/marker_array.hpp" -namespace path_sampler +namespace autoware::path_sampler { // std_msgs using std_msgs::msg::Header; @@ -46,6 +46,6 @@ using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; // debug using tier4_debug_msgs::msg::StringStamped; -} // namespace path_sampler +} // namespace autoware::path_sampler -#endif // PATH_SAMPLER__TYPE_ALIAS_HPP_ +#endif // AUTOWARE_PATH_SAMPLER__TYPE_ALIAS_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp similarity index 81% rename from planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp rename to planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp index 875b81b9cd49a..abd2fa367bc7b 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ -#define PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ +#ifndef AUTOWARE_PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ +#define AUTOWARE_PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ +#include "autoware_path_sampler/common_structs.hpp" +#include "autoware_path_sampler/type_alias.hpp" #include "eigen3/Eigen/Core" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "path_sampler/common_structs.hpp" -#include "path_sampler/type_alias.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" @@ -35,7 +35,7 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { namespace geometry_utils { @@ -52,5 +52,5 @@ bool isSamePoint(const T1 & t1, const T2 & t2) return true; } } // namespace geometry_utils -} // namespace path_sampler -#endif // PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ +} // namespace autoware::path_sampler +#endif // AUTOWARE_PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp similarity index 90% rename from planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp rename to planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp index a036dcfe638c2..4f346ae910f44 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ -#define PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ +#ifndef AUTOWARE_PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ +#define AUTOWARE_PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ +#include "autoware_path_sampler/common_structs.hpp" +#include "autoware_path_sampler/type_alias.hpp" +#include "autoware_sampler_common/structures.hpp" #include "eigen3/Eigen/Core" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "path_sampler/common_structs.hpp" -#include "path_sampler/type_alias.hpp" -#include "sampler_common/structures.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -33,7 +33,7 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { namespace trajectory_utils { @@ -95,7 +95,8 @@ std::vector convertToTrajectoryPoints(const std::vector & po return traj_points; } -inline std::vector convertToTrajectoryPoints(const sampler_common::Path & path) +inline std::vector convertToTrajectoryPoints( + const autoware::sampler_common::Path & path) { std::vector traj_points; for (auto i = 0UL; i < path.points.size(); ++i) { @@ -160,7 +161,7 @@ std::optional updateFrontPointForFix( motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position); if (0 < lon_offset_to_prev_front) { RCLCPP_WARN( - rclcpp::get_logger("path_sampler.trajectory_utils"), + rclcpp::get_logger("autoware_path_sampler.trajectory_utils"), "Fixed point will not be inserted due to the error during calculation."); return std::nullopt; } @@ -171,7 +172,7 @@ std::optional updateFrontPointForFix( constexpr double max_lat_error = 3.0; if (max_lat_error < dist) { RCLCPP_WARN( - rclcpp::get_logger("path_sampler.trajectory_utils"), + rclcpp::get_logger("autoware_path_sampler.trajectory_utils"), "New Fixed point is too far from points %f [m]", dist); } @@ -193,5 +194,5 @@ void insertStopPoint( std::vector & traj_points, const geometry_msgs::msg::Pose & input_stop_pose, const size_t stop_seg_idx); } // namespace trajectory_utils -} // namespace path_sampler -#endif // PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ +} // namespace autoware::path_sampler +#endif // AUTOWARE_PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/package.xml b/planning/sampling_based_planner/autoware_path_sampler/package.xml similarity index 92% rename from planning/sampling_based_planner/path_sampler/package.xml rename to planning/sampling_based_planner/autoware_path_sampler/package.xml index a57ff6e6033f6..011da8d469607 100644 --- a/planning/sampling_based_planner/path_sampler/package.xml +++ b/planning/sampling_based_planner/autoware_path_sampler/package.xml @@ -1,7 +1,7 @@ - path_sampler + autoware_path_sampler 0.1.0 Package for the sampling-based path planner Maxime CLEMENT @@ -13,10 +13,10 @@ autoware_cmake + autoware_bezier_sampler + autoware_frenet_planner autoware_perception_msgs autoware_planning_msgs - bezier_sampler - frenet_planner geometry_msgs interpolation motion_utils diff --git a/planning/sampling_based_planner/path_sampler/src/node.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp similarity index 94% rename from planning/sampling_based_planner/path_sampler/src/node.cpp rename to planning/sampling_based_planner/autoware_path_sampler/src/node.cpp index 97bafb68c47a8..a82c1b6a52a26 100644 --- a/planning/sampling_based_planner/path_sampler/src/node.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp @@ -12,25 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "path_sampler/node.hpp" - +#include "autoware_path_sampler/node.hpp" + +#include "autoware_path_sampler/path_generation.hpp" +#include "autoware_path_sampler/prepare_inputs.hpp" +#include "autoware_path_sampler/utils/geometry_utils.hpp" +#include "autoware_path_sampler/utils/trajectory_utils.hpp" +#include "autoware_sampler_common/constraints/hard_constraint.hpp" +#include "autoware_sampler_common/constraints/soft_constraint.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/marker/marker_helper.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "path_sampler/path_generation.hpp" -#include "path_sampler/prepare_inputs.hpp" -#include "path_sampler/utils/geometry_utils.hpp" -#include "path_sampler/utils/trajectory_utils.hpp" #include "rclcpp/time.hpp" -#include "sampler_common/constraints/hard_constraint.hpp" -#include "sampler_common/constraints/soft_constraint.hpp" #include #include #include -namespace path_sampler +namespace autoware::path_sampler { namespace { @@ -220,8 +220,9 @@ void PathSampler::objectsCallback(const PredictedObjects::SharedPtr msg) in_objects_ptr_ = msg; } -sampler_common::State PathSampler::getPlanningState( - sampler_common::State & state, const sampler_common::transform::Spline2D & path_spline) const +autoware::sampler_common::State PathSampler::getPlanningState( + autoware::sampler_common::State & state, + const autoware::sampler_common::transform::Spline2D & path_spline) const { state.frenet = path_spline.frenet(state.pose); if (params_.preprocessing.force_zero_deviation) { @@ -390,13 +391,14 @@ std::vector PathSampler::generateTrajectory(const PlannerData & return generated_traj_points; } -std::vector PathSampler::generateCandidatesFromPreviousPath( - const PlannerData & planner_data, const sampler_common::transform::Spline2D & path_spline) +std::vector PathSampler::generateCandidatesFromPreviousPath( + const PlannerData & planner_data, + const autoware::sampler_common::transform::Spline2D & path_spline) { - std::vector candidates; + std::vector candidates; if (!prev_path_ || prev_path_->points.size() < 2) return candidates; // Update previous path - sampler_common::State current_state; + autoware::sampler_common::State current_state; current_state.pose = {planner_data.ego_pose.position.x, planner_data.ego_pose.position.y}; current_state.heading = tf2::getYaw(planner_data.ego_pose.orientation); const auto closest_iter = std::min_element( @@ -419,7 +421,7 @@ std::vector PathSampler::generateCandidatesFromPreviousPat const auto reuse_step = prev_path_length / params_.sampling.previous_path_reuse_points_nb; for (double reuse_length = reuse_step; reuse_length <= prev_path_length; reuse_length += reuse_step) { - sampler_common::State reuse_state; + autoware::sampler_common::State reuse_state; size_t reuse_idx = 0; for (reuse_idx = 0; reuse_idx + 1 < prev_path_->lengths.size() && prev_path_->lengths[reuse_idx] < reuse_length; @@ -438,13 +440,13 @@ std::vector PathSampler::generateCandidatesFromPreviousPat return candidates; } -sampler_common::Path PathSampler::generatePath(const PlannerData & planner_data) +autoware::sampler_common::Path PathSampler::generatePath(const PlannerData & planner_data) { time_keeper_ptr_->tic(__func__); - sampler_common::Path generated_path{}; + autoware::sampler_common::Path generated_path{}; if (prev_path_ && prev_path_->points.size() > 1) { - sampler_common::constraints::checkHardConstraints(*prev_path_, params_.constraints); + autoware::sampler_common::constraints::checkHardConstraints(*prev_path_, params_.constraints); if (prev_path_->constraint_results.isValid()) { const auto prev_path_spline = preparePathSpline(trajectory_utils::convertToTrajectoryPoints(*prev_path_), false); @@ -460,7 +462,7 @@ sampler_common::Path PathSampler::generatePath(const PlannerData & planner_data) } const auto path_spline = preparePathSpline(planner_data.traj_points, params_.preprocessing.smooth_reference); - sampler_common::State current_state; + autoware::sampler_common::State current_state; current_state.pose = {planner_data.ego_pose.position.x, planner_data.ego_pose.position.y}; current_state.heading = tf2::getYaw(planner_data.ego_pose.orientation); @@ -476,9 +478,9 @@ sampler_common::Path PathSampler::generatePath(const PlannerData & planner_data) debug_data_.footprints.clear(); for (auto & path : candidate_paths) { const auto footprint = - sampler_common::constraints::checkHardConstraints(path, params_.constraints); + autoware::sampler_common::constraints::checkHardConstraints(path, params_.constraints); debug_data_.footprints.push_back(footprint); - sampler_common::constraints::calculateCost(path, params_.constraints, path_spline); + autoware::sampler_common::constraints::calculateCost(path, params_.constraints, path_spline); } const auto best_path_idx = [](const auto & paths) { auto min_cost = std::numeric_limits::max(); @@ -701,7 +703,7 @@ std::vector PathSampler::extendTrajectory( time_keeper_ptr_->toc(__func__, " "); return resampled_traj_points; } -} // namespace path_sampler +} // namespace autoware::path_sampler #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(path_sampler::PathSampler) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::path_sampler::PathSampler) diff --git a/planning/sampling_based_planner/path_sampler/src/path_generation.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/path_generation.cpp similarity index 72% rename from planning/sampling_based_planner/path_sampler/src/path_generation.cpp rename to planning/sampling_based_planner/autoware_path_sampler/src/path_generation.cpp index 2d1587c7b45a3..63ca5e18f423b 100644 --- a/planning/sampling_based_planner/path_sampler/src/path_generation.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/path_generation.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "path_sampler/path_generation.hpp" +#include "autoware_path_sampler/path_generation.hpp" -#include "sampler_common/structures.hpp" +#include "autoware_sampler_common/structures.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -29,14 +29,14 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { -std::vector generateCandidatePaths( - const sampler_common::State & initial_state, - const sampler_common::transform::Spline2D & path_spline, const double base_length, +std::vector generateCandidatePaths( + const autoware::sampler_common::State & initial_state, + const autoware::sampler_common::transform::Spline2D & path_spline, const double base_length, const Parameters & params) { - std::vector paths; + std::vector paths; const auto move_to_paths = [&](auto & paths_to_move) { paths.insert( paths.end(), std::make_move_iterator(paths_to_move.begin()), @@ -53,27 +53,27 @@ std::vector generateCandidatePaths( } return paths; } -std::vector generateBezierPaths( - const sampler_common::State & initial_state, const double base_length, - const sampler_common::transform::Spline2D & path_spline, const Parameters & params) +std::vector generateBezierPaths( + const autoware::sampler_common::State & initial_state, const double base_length, + const autoware::sampler_common::transform::Spline2D & path_spline, const Parameters & params) { const auto initial_s = path_spline.frenet(initial_state.pose).s; const auto max_s = path_spline.lastS(); - std::vector bezier_paths; + std::vector bezier_paths; for (const auto target_length : params.sampling.target_lengths) { if (target_length <= base_length) continue; const auto target_s = std::min(max_s, initial_s + target_length - base_length); if (target_s >= max_s) break; - sampler_common::State target_state{}; + autoware::sampler_common::State target_state{}; target_state.pose = path_spline.cartesian({target_s, 0}); target_state.curvature = path_spline.curvature(target_s); target_state.heading = path_spline.yaw(target_s); const auto bezier_samples = - bezier_sampler::sample(initial_state, target_state, params.sampling.bezier); + autoware::bezier_sampler::sample(initial_state, target_state, params.sampling.bezier); const auto step = std::min(0.1, params.sampling.resolution / target_length); for (const auto & bezier : bezier_samples) { - sampler_common::Path path; + autoware::sampler_common::Path path; path.lengths.push_back(0.0); for (double t = 0.0; t <= 1.0; t += step) { const auto x_y = bezier.valueM(t); @@ -93,14 +93,14 @@ std::vector generateBezierPaths( return bezier_paths; } -std::vector generateFrenetPaths( - const sampler_common::State & initial_state, const double base_length, - const sampler_common::transform::Spline2D & path_spline, const Parameters & params) +std::vector generateFrenetPaths( + const autoware::sampler_common::State & initial_state, const double base_length, + const autoware::sampler_common::transform::Spline2D & path_spline, const Parameters & params) { const auto sampling_parameters = prepareSamplingParameters(initial_state, base_length, path_spline, params); - frenet_planner::FrenetState initial_frenet_state; + autoware::frenet_planner::FrenetState initial_frenet_state; initial_frenet_state.position = path_spline.frenet(initial_state.pose); const auto s = initial_frenet_state.position.s; const auto d = initial_frenet_state.position.d; @@ -121,6 +121,7 @@ std::vector generateFrenetPaths( ((1 - path_curvature * d) / (cos_yaw * cos_yaw)) * (initial_state.curvature * ((1 - path_curvature * d) / cos_yaw) - path_curvature); } - return frenet_planner::generatePaths(path_spline, initial_frenet_state, sampling_parameters); + return autoware::frenet_planner::generatePaths( + path_spline, initial_frenet_state, sampling_parameters); } -} // namespace path_sampler +} // namespace autoware::path_sampler diff --git a/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp similarity index 84% rename from planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp rename to planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp index fd52764950ca9..64a09087666ff 100644 --- a/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "path_sampler/prepare_inputs.hpp" +#include "autoware_path_sampler/prepare_inputs.hpp" -#include "frenet_planner/structures.hpp" -#include "path_sampler/utils/geometry_utils.hpp" -#include "sampler_common/structures.hpp" -#include "sampler_common/transform/spline_transform.hpp" +#include "autoware_frenet_planner/structures.hpp" +#include "autoware_path_sampler/utils/geometry_utils.hpp" +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -29,22 +29,22 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { void prepareConstraints( - sampler_common::Constraints & constraints, const PredictedObjects & predicted_objects, + autoware::sampler_common::Constraints & constraints, const PredictedObjects & predicted_objects, const std::vector & left_bound, const std::vector & right_bound) { - constraints.obstacle_polygons = sampler_common::MultiPolygon2d(); + constraints.obstacle_polygons = autoware::sampler_common::MultiPolygon2d(); for (const auto & o : predicted_objects.objects) if (o.kinematics.initial_twist_with_covariance.twist.linear.x < 0.5) // TODO(Maxime): param constraints.obstacle_polygons.push_back(tier4_autoware_utils::toPolygon2d(o)); constraints.dynamic_obstacles = {}; // TODO(Maxime): not implemented // TODO(Maxime): directly use lines instead of polygon - sampler_common::Polygon2d drivable_area_polygon; + autoware::sampler_common::Polygon2d drivable_area_polygon; for (const auto & p : right_bound) drivable_area_polygon.outer().emplace_back(p.x, p.y); for (auto it = left_bound.rbegin(); it != left_bound.rend(); ++it) drivable_area_polygon.outer().emplace_back(it->x, it->y); @@ -52,9 +52,9 @@ void prepareConstraints( constraints.drivable_polygons = {drivable_area_polygon}; } -frenet_planner::SamplingParameters prepareSamplingParameters( - const sampler_common::State & initial_state, const double base_length, - const sampler_common::transform::Spline2D & path_spline, const Parameters & params) +autoware::frenet_planner::SamplingParameters prepareSamplingParameters( + const autoware::sampler_common::State & initial_state, const double base_length, + const autoware::sampler_common::transform::Spline2D & path_spline, const Parameters & params) { // calculate target lateral positions std::vector target_lateral_positions; @@ -87,10 +87,10 @@ frenet_planner::SamplingParameters prepareSamplingParameters( } else { target_lateral_positions = params.sampling.target_lateral_positions; } - frenet_planner::SamplingParameters sampling_parameters; + autoware::frenet_planner::SamplingParameters sampling_parameters; sampling_parameters.resolution = params.sampling.resolution; const auto max_s = path_spline.lastS(); - frenet_planner::SamplingParameter p; + autoware::frenet_planner::SamplingParameter p; for (const auto target_length : params.sampling.target_lengths) { p.target_state.position.s = std::min( max_s, path_spline.frenet(initial_state.pose).s + std::max(0.0, target_length - base_length)); @@ -109,7 +109,7 @@ frenet_planner::SamplingParameters prepareSamplingParameters( return sampling_parameters; } -sampler_common::transform::Spline2D preparePathSpline( +autoware::sampler_common::transform::Spline2D preparePathSpline( const std::vector & path, const bool smooth_path) { std::vector x; @@ -151,4 +151,4 @@ sampler_common::transform::Spline2D preparePathSpline( } return {x, y}; } -} // namespace path_sampler +} // namespace autoware::path_sampler diff --git a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp similarity index 95% rename from planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp rename to planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp index 07bd4f32787de..d161d60bc724d 100644 --- a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "path_sampler/utils/trajectory_utils.hpp" +#include "autoware_path_sampler/utils/trajectory_utils.hpp" +#include "autoware_path_sampler/utils/geometry_utils.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "path_sampler/utils/geometry_utils.hpp" #include "tf2/utils.h" #include "autoware_planning_msgs/msg/path_point.hpp" @@ -31,7 +31,7 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { namespace trajectory_utils { @@ -107,4 +107,4 @@ void insertStopPoint( traj_points.insert(traj_points.begin() + stop_seg_idx + 1, additional_traj_point); } } // namespace trajectory_utils -} // namespace path_sampler +} // namespace autoware::path_sampler diff --git a/planning/sampling_based_planner/sampler_common/CMakeLists.txt b/planning/sampling_based_planner/autoware_sampler_common/CMakeLists.txt similarity index 81% rename from planning/sampling_based_planner/sampler_common/CMakeLists.txt rename to planning/sampling_based_planner/autoware_sampler_common/CMakeLists.txt index 5e669a705201e..d82f38d3ec0c0 100644 --- a/planning/sampling_based_planner/sampler_common/CMakeLists.txt +++ b/planning/sampling_based_planner/autoware_sampler_common/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(sampler_common) +project(autoware_sampler_common) find_package(autoware_cmake REQUIRED) autoware_package() @@ -8,7 +8,7 @@ find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED) -ament_auto_add_library(sampler_common SHARED +ament_auto_add_library(autoware_sampler_common SHARED DIRECTORY src/ ) @@ -22,7 +22,7 @@ if(BUILD_TESTING) ) target_link_libraries(test_sampler_common - sampler_common + autoware_sampler_common ) endif() diff --git a/planning/sampling_based_planner/sampler_common/README.md b/planning/sampling_based_planner/autoware_sampler_common/README.md similarity index 100% rename from planning/sampling_based_planner/sampler_common/README.md rename to planning/sampling_based_planner/autoware_sampler_common/README.md diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/footprint.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/footprint.hpp similarity index 73% rename from planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/footprint.hpp rename to planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/footprint.hpp index 061f922001a0f..52dd1a83bd99e 100644 --- a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/footprint.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/footprint.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_ -#define SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_ +#ifndef AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_ +#define AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_ -#include "sampler_common/structures.hpp" +#include "autoware_sampler_common/structures.hpp" -namespace sampler_common::constraints +namespace autoware::sampler_common::constraints { /// @brief Calculate the footprint of the given path /// @param path sequence of pose used to build the footprint /// @param constraints input constraint object containing vehicle footprint offsets /// @return the polygon footprint of the path MultiPoint2d buildFootprintPoints(const Path & path, const Constraints & constraints); -} // namespace sampler_common::constraints +} // namespace autoware::sampler_common::constraints -#endif // SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_ +#endif // AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/hard_constraint.hpp similarity index 73% rename from planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp rename to planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/hard_constraint.hpp index a314e83b065d7..6098e7c15d33d 100644 --- a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/hard_constraint.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_ -#define SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_ +#ifndef AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_ +#define AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_ -#include "sampler_common/structures.hpp" +#include "autoware_sampler_common/structures.hpp" #include -namespace sampler_common::constraints +namespace autoware::sampler_common::constraints { /// @brief Check if the path satisfies the hard constraints MultiPoint2d checkHardConstraints(Path & path, const Constraints & constraints); @@ -27,6 +27,6 @@ bool has_collision( const double min_distance = 0.0); bool satisfyMinMax(const std::vector & values, const double min, const double max); -} // namespace sampler_common::constraints +} // namespace autoware::sampler_common::constraints -#endif // SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_ +#endif // AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/map_constraints.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/map_constraints.hpp similarity index 80% rename from planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/map_constraints.hpp rename to planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/map_constraints.hpp index ed04d8104a904..7867efa1fa2db 100644 --- a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/map_constraints.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/map_constraints.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_ -#define SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_ +#ifndef AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_ +#define AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_ -#include "sampler_common/structures.hpp" +#include "autoware_sampler_common/structures.hpp" #include #include -namespace sampler_common::constraints +namespace autoware::sampler_common::constraints { struct MapConstraints { @@ -55,6 +55,6 @@ struct MapConstraints MultiPolygon2d drivable_polygons; std::vector polygon_costs; }; -} // namespace sampler_common::constraints +} // namespace autoware::sampler_common::constraints -#endif // SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_ +#endif // AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/soft_constraint.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/soft_constraint.hpp similarity index 74% rename from planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/soft_constraint.hpp rename to planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/soft_constraint.hpp index 9fdb9fe137e49..290b6b6674e21 100644 --- a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/soft_constraint.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/soft_constraint.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_ -#define SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_ +#ifndef AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_ +#define AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_ -#include "sampler_common/structures.hpp" -#include "sampler_common/transform/spline_transform.hpp" +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" -namespace sampler_common::constraints +namespace autoware::sampler_common::constraints { /// @brief calculate the curvature cost of the given path void calculateCurvatureCost(Path & path, const Constraints & constraints); @@ -30,6 +30,6 @@ void calculateLateralDeviationCost( /// @brief calculate the overall cost of the given path void calculateCost( Path & path, const Constraints & constraints, const transform::Spline2D & reference); -} // namespace sampler_common::constraints +} // namespace autoware::sampler_common::constraints -#endif // SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_ +#endif // AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp similarity index 98% rename from planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp rename to planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp index cef791a01df44..a3ba25b6594e9 100644 --- a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SAMPLER_COMMON__STRUCTURES_HPP_ -#define SAMPLER_COMMON__STRUCTURES_HPP_ +#ifndef AUTOWARE_SAMPLER_COMMON__STRUCTURES_HPP_ +#define AUTOWARE_SAMPLER_COMMON__STRUCTURES_HPP_ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" @@ -36,7 +36,7 @@ #include #include -namespace sampler_common +namespace autoware::sampler_common { using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::MultiPoint2d; @@ -367,6 +367,6 @@ struct ReusableTrajectory Configuration planning_configuration; // planning configuration at the end of the trajectory }; -} // namespace sampler_common +} // namespace autoware::sampler_common -#endif // SAMPLER_COMMON__STRUCTURES_HPP_ +#endif // AUTOWARE_SAMPLER_COMMON__STRUCTURES_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/transform/spline_transform.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/transform/spline_transform.hpp similarity index 88% rename from planning/sampling_based_planner/sampler_common/include/sampler_common/transform/spline_transform.hpp rename to planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/transform/spline_transform.hpp index ce66141832dbf..c5f78c29cb13b 100644 --- a/planning/sampling_based_planner/sampler_common/include/sampler_common/transform/spline_transform.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/transform/spline_transform.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_ -#define SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_ +#ifndef AUTOWARE_SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_ +#define AUTOWARE_SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_ -#include "sampler_common/structures.hpp" +#include "autoware_sampler_common/structures.hpp" #include -namespace sampler_common::transform +namespace autoware::sampler_common::transform { -using sampler_common::FrenetPoint; +using autoware::sampler_common::FrenetPoint; class Spline { @@ -80,6 +80,6 @@ class Spline2D static std::vector arcLength( const std::vector & x, const std::vector & y); }; -} // namespace sampler_common::transform +} // namespace autoware::sampler_common::transform -#endif // SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_ +#endif // AUTOWARE_SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/package.xml b/planning/sampling_based_planner/autoware_sampler_common/package.xml similarity index 94% rename from planning/sampling_based_planner/sampler_common/package.xml rename to planning/sampling_based_planner/autoware_sampler_common/package.xml index 321ca2fd43fef..a1462131d62ad 100644 --- a/planning/sampling_based_planner/sampler_common/package.xml +++ b/planning/sampling_based_planner/autoware_sampler_common/package.xml @@ -1,6 +1,6 @@ - sampler_common + autoware_sampler_common 0.0.1 Common classes and functions for sampling based planners Maxime CLEMENT diff --git a/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/footprint.cpp b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/footprint.cpp similarity index 87% rename from planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/footprint.cpp rename to planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/footprint.cpp index 92c8ae65267db..1aafa598142a4 100644 --- a/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/footprint.cpp +++ b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/footprint.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "sampler_common/constraints/footprint.hpp" +#include "autoware_sampler_common/constraints/footprint.hpp" -#include "sampler_common/structures.hpp" +#include "autoware_sampler_common/structures.hpp" #include @@ -25,7 +25,7 @@ #include #include -namespace sampler_common::constraints +namespace autoware::sampler_common::constraints { namespace @@ -50,4 +50,4 @@ MultiPoint2d buildFootprintPoints(const Path & path, const Constraints & constra } return footprint; } -} // namespace sampler_common::constraints +} // namespace autoware::sampler_common::constraints diff --git a/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/hard_constraint.cpp b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/hard_constraint.cpp similarity index 89% rename from planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/hard_constraint.cpp rename to planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/hard_constraint.cpp index 7bb29c8837723..9901b3979259c 100644 --- a/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/hard_constraint.cpp +++ b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/hard_constraint.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "sampler_common/constraints/hard_constraint.hpp" +#include "autoware_sampler_common/constraints/hard_constraint.hpp" -#include "sampler_common/constraints/footprint.hpp" +#include "autoware_sampler_common/constraints/footprint.hpp" #include #include #include -namespace sampler_common::constraints +namespace autoware::sampler_common::constraints { bool satisfyMinMax(const std::vector & values, const double min, const double max) { @@ -56,4 +56,4 @@ MultiPoint2d checkHardConstraints(Path & path, const Constraints & constraints) } return footprint; } -} // namespace sampler_common::constraints +} // namespace autoware::sampler_common::constraints diff --git a/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/soft_constraint.cpp b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/soft_constraint.cpp similarity index 85% rename from planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/soft_constraint.cpp rename to planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/soft_constraint.cpp index b1d390e68c49f..71dff81929553 100644 --- a/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/soft_constraint.cpp +++ b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/soft_constraint.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "sampler_common/constraints/soft_constraint.hpp" +#include "autoware_sampler_common/constraints/soft_constraint.hpp" -#include "sampler_common/structures.hpp" -#include "sampler_common/transform/spline_transform.hpp" +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" #include -namespace sampler_common::constraints +namespace autoware::sampler_common::constraints { void calculateCurvatureCost(Path & path, const Constraints & constraints) { @@ -51,4 +51,4 @@ void calculateCost( calculateLengthCost(path, constraints); calculateLateralDeviationCost(path, constraints, reference); } -} // namespace sampler_common::constraints +} // namespace autoware::sampler_common::constraints diff --git a/planning/sampling_based_planner/sampler_common/src/sampler_common/transform/spline_transform.cpp b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/transform/spline_transform.cpp similarity index 98% rename from planning/sampling_based_planner/sampler_common/src/sampler_common/transform/spline_transform.cpp rename to planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/transform/spline_transform.cpp index cf5f1e5478fe5..ce473b1768a54 100644 --- a/planning/sampling_based_planner/sampler_common/src/sampler_common/transform/spline_transform.cpp +++ b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/transform/spline_transform.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include #include #include -namespace sampler_common::transform +namespace autoware::sampler_common::transform { Spline::Spline(const std::vector & base_index, const std::vector & base_value) { @@ -312,4 +312,4 @@ double Spline2D::yaw(const double s) const return std::atan2(y_vel, x_vel); } -} // namespace sampler_common::transform +} // namespace autoware::sampler_common::transform diff --git a/planning/sampling_based_planner/sampler_common/test/test_structures.cpp b/planning/sampling_based_planner/autoware_sampler_common/test/test_structures.cpp similarity index 96% rename from planning/sampling_based_planner/sampler_common/test/test_structures.cpp rename to planning/sampling_based_planner/autoware_sampler_common/test/test_structures.cpp index 006768c9325e2..77496209290c1 100644 --- a/planning/sampling_based_planner/sampler_common/test/test_structures.cpp +++ b/planning/sampling_based_planner/autoware_sampler_common/test/test_structures.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include TEST(Path, extendPath) { - using sampler_common::Path; + using autoware::sampler_common::Path; Path traj1; Path traj2; Path traj3 = traj1.extend(traj2); @@ -43,7 +43,7 @@ TEST(Path, extendPath) TEST(Trajectory, resample) { constexpr auto eps = 1e-6; - using sampler_common::Trajectory; + using autoware::sampler_common::Trajectory; Trajectory t; t.reserve(2); @@ -103,7 +103,7 @@ TEST(Trajectory, resample) TEST(Trajectory, resampleTime) { constexpr auto eps = 1e-6; - using sampler_common::Trajectory; + using autoware::sampler_common::Trajectory; Trajectory t; t.reserve(2); diff --git a/planning/sampling_based_planner/sampler_common/test/test_transform.cpp b/planning/sampling_based_planner/autoware_sampler_common/test/test_transform.cpp similarity index 85% rename from planning/sampling_based_planner/sampler_common/test/test_transform.cpp rename to planning/sampling_based_planner/autoware_sampler_common/test/test_transform.cpp index d7998d7b8879e..6c5f43e929766 100644 --- a/planning/sampling_based_planner/sampler_common/test/test_transform.cpp +++ b/planning/sampling_based_planner/autoware_sampler_common/test/test_transform.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include @@ -24,13 +24,13 @@ constexpr auto TOL = 1E-6; // 1µm tolerance TEST(splineTransform, makeSpline2D) { - sampler_common::transform::Spline2D spline1({0.0, 1.0, 2.0}, {0.0, 1.0, 2.0}); - sampler_common::transform::Spline2D spline2({-10.0, 5.0, -2.0}, {99.0, 3.0, -20.0}); + autoware::sampler_common::transform::Spline2D spline1({0.0, 1.0, 2.0}, {0.0, 1.0, 2.0}); + autoware::sampler_common::transform::Spline2D spline2({-10.0, 5.0, -2.0}, {99.0, 3.0, -20.0}); } TEST(splineTransform, toFrenet) { - sampler_common::transform::Spline2D spline({0.0, 1.0, 2.0}, {0.0, 0.0, 0.0}); + autoware::sampler_common::transform::Spline2D spline({0.0, 1.0, 2.0}, {0.0, 0.0, 0.0}); auto frenet = spline.frenet({0.0, 0.0}); EXPECT_NEAR(frenet.s, 0.0, TOL); EXPECT_NEAR(frenet.d, 0.0, TOL); @@ -56,7 +56,7 @@ TEST(splineTransform, toFrenet) EXPECT_NEAR(frenet.s, 1.0, TOL); EXPECT_NEAR(frenet.d, -1.0, TOL); // EDGE CASE 90 deg angle - sampler_common::transform::Spline2D spline2({0.0, 1.0, 2.0}, {0.0, 1.0, 0.0}); + autoware::sampler_common::transform::Spline2D spline2({0.0, 1.0, 2.0}, {0.0, 1.0, 0.0}); frenet = spline2.frenet({1.0, 2.0}); EXPECT_NEAR(frenet.s, 1.0, TOL); EXPECT_NEAR(frenet.d, 1.0, TOL); @@ -64,7 +64,7 @@ TEST(splineTransform, toFrenet) TEST(splineTransform, toCartesian) { - sampler_common::transform::Spline2D spline({0.0, 1.0, 2.0}, {0.0, 0.0, 0.0}); + autoware::sampler_common::transform::Spline2D spline({0.0, 1.0, 2.0}, {0.0, 0.0, 0.0}); auto cart = spline.cartesian({1.0, 0.0}); EXPECT_NEAR(cart.x(), 1.0, TOL); EXPECT_NEAR(cart.y(), 0.0, TOL); @@ -85,7 +85,7 @@ TEST(splineTransform, benchFrenet) auto y = 0.0; std::generate(xs.begin(), xs.end(), [&]() { return ++x; }); std::generate(ys.begin(), ys.end(), [&]() { return ++y; }); - sampler_common::transform::Spline2D spline(xs, ys); + autoware::sampler_common::transform::Spline2D spline(xs, ys); auto points_distribution = std::uniform_real_distribution(0.0, static_cast(size)); constexpr auto nb_iter = 1e3; for (auto iter = 0; iter < nb_iter; ++iter) { diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/path_generation.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/path_generation.hpp deleted file mode 100644 index 448400236c9f8..0000000000000 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/path_generation.hpp +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright 2023 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 PATH_SAMPLER__PATH_GENERATION_HPP_ -#define PATH_SAMPLER__PATH_GENERATION_HPP_ - -#include "bezier_sampler/bezier_sampling.hpp" -#include "frenet_planner/structures.hpp" -#include "path_sampler/parameters.hpp" -#include "sampler_common/constraints/hard_constraint.hpp" -#include "sampler_common/structures.hpp" -#include "sampler_common/transform/spline_transform.hpp" - -#include - -#include - -namespace path_sampler -{ -/** - * @brief generate candidate paths for the given problem inputs - * @param [in] initial_state initial ego state - * @param [in] path_spline spline of the reference path - * @param [in] base_length base length of the reuse path (= 0.0 if not reusing a previous path) - * @param [in] params parameters - * @return candidate paths - */ -std::vector generateCandidatePaths( - const sampler_common::State & initial_state, - const sampler_common::transform::Spline2D & path_spline, const double base_length, - const Parameters & params); - -std::vector generateBezierPaths( - const sampler_common::State & initial_state, const double base_length, - const sampler_common::transform::Spline2D & path_spline, const Parameters & params); -std::vector generateFrenetPaths( - const sampler_common::State & initial_state, const double base_length, - const sampler_common::transform::Spline2D & path_spline, const Parameters & params); -} // namespace path_sampler - -#endif // PATH_SAMPLER__PATH_GENERATION_HPP_