diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/data_structs.hpp index dd533aff1e30f..730c678b93427 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/data_structs.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ -#define AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ +#include "autoware/behavior_path_dynamic_obstacle_avoidance_module/type_alias.hpp" #include "autoware/behavior_path_planner_common/data_manager.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "autoware/behavior_path_dynamic_obstacle_avoidance_module/type_alias.hpp" #include @@ -679,4 +679,4 @@ struct DebugData } // namespace autoware::behavior_path_planner -#endif // AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/type_alias.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/type_alias.hpp index cfbd3f89308ac..74e893b6d1b6a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/type_alias.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/type_alias.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ -#define AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ #include #include @@ -71,4 +71,4 @@ using autoware::universe_utils::pose2transform; using autoware::universe_utils::toHexString; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/utils.hpp index 52c1bd55d6da0..3d2ead864975d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/utils.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ -#define AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ -#include "autoware/behavior_path_planner_common/data_manager.hpp" -#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware/behavior_path_dynamic_obstacle_avoidance_module/data_structs.hpp" #include "autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -35,7 +35,8 @@ using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVeloc using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; static constexpr const char * logger_namespace = - "planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.dynamic_obstacle_" + "planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.dynamic_" + "obstacle_" "avoidance.utils"; bool isOnRight(const ObjectData & obj); @@ -178,4 +179,4 @@ double calcDistanceToAvoidStartLine( } // namespace autoware::behavior_path_planner::utils::dynamic_obstacle_avoidance -#endif // AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml index 5eed57a4821c5..d1d87c48e7a12 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml @@ -20,28 +20,28 @@ autoware_behavior_path_planner_common autoware_lanelet2_extension autoware_motion_utils + autoware_objects_of_interest_marker_interface autoware_perception_msgs autoware_planning_msgs + autoware_route_handler + autoware_rtc_interface autoware_universe_utils + autoware_vehicle_info_utils autoware_vehicle_msgs geometry_msgs lanelet2_core + magic_enum object_recognition_utils pluginlib rclcpp - signal_processing - visualization_msgs - autoware_objects_of_interest_marker_interface - autoware_route_handler - autoware_rtc_interface - autoware_vehicle_info_utils - magic_enum sensor_msgs + signal_processing tf2 tf2_eigen tf2_geometry_msgs tf2_ros tier4_planning_msgs + visualization_msgs ament_cmake_ros ament_lint_auto diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp index ae951fc0ac5e4..29e5fe98dd0d2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp @@ -13,8 +13,8 @@ // limitations under the License. #include "autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp" -#include "autoware/behavior_path_dynamic_obstacle_avoidance_module/utils.hpp" +#include "autoware/behavior_path_dynamic_obstacle_avoidance_module/utils.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" @@ -432,8 +432,8 @@ BehaviorModuleOutput DynamicObstacleAvoidanceModule::plan() throw std::logic_error("The polygon_generation_method's string is invalid."); }(); if (obstacle_poly) { -// obstacles_for_drivable_area.push_back( -// {object.pose, obstacle_poly.value(), object.is_collision_left}); + // obstacles_for_drivable_area.push_back( + // {object.pose, obstacle_poly.value(), object.is_collision_left}); appendObjectMarker(info_marker_, object.pose); appendExtractedPolygonMarker(debug_marker_, obstacle_poly.value(), object.pose.position.z); @@ -445,15 +445,14 @@ BehaviorModuleOutput DynamicObstacleAvoidanceModule::plan() // generate drivable lanes auto current_lanelets = utils::dynamic_obstacle_avoidance::getCurrentLanesFromPath( getPreviousModuleOutput().reference_path, planner_data_); - std::for_each( - current_lanelets.begin(), current_lanelets.end(), [&](const auto & lanelet) { - current_drivable_area_info.drivable_lanes.push_back( - utils::dynamic_obstacle_avoidance::generateExpandedDrivableLanes( - lanelet, planner_data_, parameters_)); - }); - -// current_drivable_area_info.drivable_lanes = -// getPreviousModuleOutput().drivable_area_info.drivable_lanes; + std::for_each(current_lanelets.begin(), current_lanelets.end(), [&](const auto & lanelet) { + current_drivable_area_info.drivable_lanes.push_back( + utils::dynamic_obstacle_avoidance::generateExpandedDrivableLanes( + lanelet, planner_data_, parameters_)); + }); + + // current_drivable_area_info.drivable_lanes = + // getPreviousModuleOutput().drivable_area_info.drivable_lanes; current_drivable_area_info.obstacles = obstacles_for_drivable_area; current_drivable_area_info.enable_expanding_hatched_road_markings = parameters_->use_hatched_road_markings; diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/utils.cpp index 860a0b1040c36..5c486a9bdbf73 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/utils.cpp @@ -14,13 +14,13 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/behavior_path_dynamic_obstacle_avoidance_module/data_structs.hpp" +#include "autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp" +#include "autoware/behavior_path_dynamic_obstacle_avoidance_module/utils.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/traffic_light_utils.hpp" -#include "autoware/behavior_path_dynamic_obstacle_avoidance_module/data_structs.hpp" -#include "autoware/behavior_path_dynamic_obstacle_avoidance_module/utils.hpp" -#include "autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp" #include