diff --git a/planning/behavior_path_avoidance_by_lane_change_module/CMakeLists.txt b/planning/behavior_path_avoidance_by_lane_change_module/CMakeLists.txt new file mode 100644 index 0000000000000..bca909c7cef7e --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_avoidance_by_lane_change_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene.cpp + src/manager.cpp + src/interface.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_behavior_path_planner_node_interface.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/config/avoidance_by_lc/avoidance_by_lc.param.yaml b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml similarity index 100% rename from planning/behavior_path_planner/config/avoidance_by_lc/avoidance_by_lc.param.yaml rename to planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp new file mode 100644 index 0000000000000..e177244930da6 --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp @@ -0,0 +1,33 @@ +// Copyright 2022 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 BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_ + +#include "behavior_path_avoidance_module/data_structs.hpp" + +#include + +namespace behavior_path_planner +{ +struct AvoidanceByLCParameters : public AvoidanceParameters +{ + // execute only when the target object longitudinal distance is larger than this param. + double execute_object_longitudinal_margin{0.0}; + + // execute only when lane change end point is before the object. + bool execute_only_when_lane_change_finish_before_object{false}; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp new file mode 100644 index 0000000000000..0bc08ccd400ca --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp @@ -0,0 +1,48 @@ +// 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 BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__INTERFACE_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__INTERFACE_HPP_ + +#include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" +#include "behavior_path_avoidance_by_lane_change_module/scene.hpp" +#include "behavior_path_planner/scene_module/lane_change/interface.hpp" + +#include + +#include +#include +#include + +namespace behavior_path_planner +{ +class AvoidanceByLaneChangeInterface : public LaneChangeInterface +{ +public: + AvoidanceByLaneChangeInterface( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::shared_ptr & avoidance_by_lane_change_parameters, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map); + + bool isExecutionRequested() const override; + +protected: + void updateRTCStatus(const double start_distance, const double finish_distance) override; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__INTERFACE_HPP_ diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp new file mode 100644 index 0000000000000..b1beae9c6fc03 --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.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 BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__MANAGER_HPP_ + +#include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" +#include "behavior_path_avoidance_by_lane_change_module/interface.hpp" +#include "behavior_path_planner/scene_module/lane_change/manager.hpp" + +#include + +#include +#include +#include +#include + +namespace behavior_path_planner +{ +using route_handler::Direction; + +class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager +{ +public: + AvoidanceByLaneChangeModuleManager() + : LaneChangeModuleManager( + "avoidance_by_lc", route_handler::Direction::NONE, + LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) + { + } + + void init(rclcpp::Node * node) override; + + std::unique_ptr createNewSceneModuleInstance() override; + +private: + std::shared_ptr avoidance_parameters_; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp similarity index 81% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp rename to planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp index 67fd2c1ba0c77..1e667c207c64f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp @@ -12,18 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_ +#include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" #include "behavior_path_planner/scene_module/lane_change/normal.hpp" #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using geometry_msgs::msg::Pose; -using geometry_msgs::msg::Twist; using AvoidanceDebugData = DebugData; class AvoidanceByLaneChange : public NormalLaneChange @@ -56,4 +54,4 @@ class AvoidanceByLaneChange : public NormalLaneChange }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_avoidance_by_lane_change_module/package.xml b/planning/behavior_path_avoidance_by_lane_change_module/package.xml new file mode 100644 index 0000000000000..3bb32bdf76bf0 --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/package.xml @@ -0,0 +1,36 @@ + + + + behavior_path_avoidance_by_lane_change_module + 0.1.0 + The behavior_path_avoidance_by_lane_change_module package + + Satoshi Ota + Zulfaqar Azmi + Fumiya Watanabe + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + behavior_path_avoidance_module + behavior_path_planner + behavior_path_planner_common + motion_utils + pluginlib + rclcpp + rtc_interface + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_avoidance_by_lane_change_module/plugins.xml b/planning/behavior_path_avoidance_by_lane_change_module/plugins.xml new file mode 100644 index 0000000000000..092d54c096ae9 --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp new file mode 100644 index 0000000000000..2e451461abab7 --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -0,0 +1,62 @@ +// 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. + +#include "behavior_path_avoidance_by_lane_change_module/interface.hpp" + +#include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "behavior_path_planner_common/marker_utils/utils.hpp" + +#include +#include +#include +#include +#include + +namespace behavior_path_planner +{ +AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::shared_ptr & avoidance_by_lane_change_parameters, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map) +: LaneChangeInterface{ + name, + node, + parameters, + rtc_interface_ptr_map, + objects_of_interest_marker_interface_ptr_map, + std::make_unique(parameters, avoidance_by_lane_change_parameters)} +{ +} + +bool AvoidanceByLaneChangeInterface::isExecutionRequested() const +{ + return module_type_->specialRequiredCheck() && module_type_->isLaneChangeRequired(); +} + +void AvoidanceByLaneChangeInterface::updateRTCStatus( + const double start_distance, const double finish_distance) +{ + const auto direction = std::invoke([&]() -> std::string { + const auto dir = module_type_->getDirection(); + return (dir == Direction::LEFT) ? "left" : "right"; + }); + + rtc_interface_ptr_map_.at(direction)->updateCooperateStatus( + uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now()); +} +} // namespace behavior_path_planner diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp new file mode 100644 index 0000000000000..61b0a36ae0fb0 --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -0,0 +1,176 @@ +// 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. + +#include "behavior_path_avoidance_by_lane_change_module/manager.hpp" + +#include "tier4_autoware_utils/ros/parameter.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + +#include + +#include +#include +#include + +namespace behavior_path_planner +{ + +void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) +{ + using autoware_auto_perception_msgs::msg::ObjectClassification; + using tier4_autoware_utils::getOrDeclareParameter; + + // init manager interface + initInterface(node, {"left", "right"}); + + // init lane change manager + LaneChangeModuleManager::init(node); + + AvoidanceByLCParameters p{}; + // unique parameters + { + const std::string ns = "avoidance_by_lane_change."; + p.execute_object_longitudinal_margin = + getOrDeclareParameter(*node, ns + "execute_object_longitudinal_margin"); + p.execute_only_when_lane_change_finish_before_object = + getOrDeclareParameter(*node, ns + "execute_only_when_lane_change_finish_before_object"); + } + + // general params + { + const std::string ns = "avoidance."; + p.resample_interval_for_planning = + getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); + p.resample_interval_for_output = + getOrDeclareParameter(*node, ns + "resample_interval_for_output"); + } + + // target object + { + const auto get_object_param = [&](std::string && ns) { + ObjectParameter param{}; + param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); + param.moving_speed_threshold = + getOrDeclareParameter(*node, ns + "moving_speed_threshold"); + param.moving_time_threshold = + getOrDeclareParameter(*node, ns + "moving_time_threshold"); + param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); + param.envelope_buffer_margin = + getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); + param.avoid_margin_lateral = + getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); + param.safety_buffer_lateral = + getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); + return param; + }; + + const std::string ns = "avoidance_by_lane_change.target_object."; + p.object_parameters.emplace( + ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle.")); + p.object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car.")); + p.object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck.")); + p.object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer.")); + p.object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus.")); + p.object_parameters.emplace( + ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian.")); + p.object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle.")); + p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); + + p.lower_distance_for_polygon_expansion = + getOrDeclareParameter(*node, ns + "lower_distance_for_polygon_expansion"); + p.upper_distance_for_polygon_expansion = + getOrDeclareParameter(*node, ns + "upper_distance_for_polygon_expansion"); + } + + // target filtering + { + const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { + if (p.object_parameters.count(object_type) == 0) { + return; + } + p.object_parameters.at(object_type).is_avoidance_target = + getOrDeclareParameter(*node, ns); + }; + + const std::string ns = "avoidance.target_filtering."; + set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); + set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); + set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); + set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); + set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); + set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); + set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); + set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); + + p.object_check_goal_distance = + getOrDeclareParameter(*node, ns + "object_check_goal_distance"); + p.threshold_distance_object_is_on_center = + getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); + p.object_check_shiftable_ratio = + getOrDeclareParameter(*node, ns + "object_check_shiftable_ratio"); + p.object_check_min_road_shoulder_width = + getOrDeclareParameter(*node, ns + "object_check_min_road_shoulder_width"); + p.object_last_seen_threshold = + getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); + } + + { + const std::string ns = "avoidance.target_filtering.force_avoidance."; + p.enable_force_avoidance_for_stopped_vehicle = + getOrDeclareParameter(*node, ns + "enable"); + p.threshold_time_force_avoidance_for_stopped_vehicle = + getOrDeclareParameter(*node, ns + "time_threshold"); + p.object_ignore_section_traffic_light_in_front_distance = + getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); + p.object_ignore_section_crosswalk_in_front_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.front_distance"); + p.object_ignore_section_crosswalk_behind_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); + } + + { + const std::string ns = "avoidance.target_filtering.detection_area."; + p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); + p.object_check_min_forward_distance = + getOrDeclareParameter(*node, ns + "min_forward_distance"); + p.object_check_max_forward_distance = + getOrDeclareParameter(*node, ns + "max_forward_distance"); + p.object_check_backward_distance = + getOrDeclareParameter(*node, ns + "backward_distance"); + } + + // safety check + { + const std::string ns = "avoidance.safety_check."; + p.hysteresis_factor_expand_rate = + getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); + } + + avoidance_parameters_ = std::make_shared(p); +} + +std::unique_ptr +AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() +{ + return std::make_unique( + name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_); +} + +} // namespace behavior_path_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::AvoidanceByLaneChangeModuleManager, + behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp similarity index 97% rename from planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp rename to planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index befd458153e6e..bfa873d9518ef 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -12,15 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp" +#include "behavior_path_avoidance_by_lane_change_module/scene.hpp" -#include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_avoidance_module/utils.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" -#include -#include - #include #include diff --git a/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..2264669fdcdbe --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,129 @@ +// 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. + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "behavior_path_planner/behavior_path_planner_node.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include + +#include +#include + +using behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + + std::vector module_names; + module_names.emplace_back("behavior_path_planner::AvoidanceByLaneChangeModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, + {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + ament_index_cpp::get_package_share_directory("behavior_path_planner") + + "/config/lane_change/lane_change.param.yaml", + ament_index_cpp::get_package_share_directory("behavior_path_avoidance_module") + + "/config/avoidance.param.yaml", + ament_index_cpp::get_package_share_directory("behavior_path_avoidance_by_lane_change_module") + + "/config/avoidance_by_lc.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_path_avoidance_module/CMakeLists.txt b/planning/behavior_path_avoidance_module/CMakeLists.txt new file mode 100644 index 0000000000000..c94591f9bbd30 --- /dev/null +++ b/planning/behavior_path_avoidance_module/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_avoidance_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene.cpp + src/debug.cpp + src/utils.cpp + src/manager.cpp + src/shift_line_generator.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_utils.cpp + test/test_behavior_path_planner_node_interface.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml similarity index 100% rename from planning/behavior_path_planner/config/avoidance/avoidance.param.yaml rename to planning/behavior_path_avoidance_module/config/avoidance.param.yaml diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp similarity index 98% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp rename to planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 68c64ff9b7112..8c45230959c6c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" @@ -622,4 +622,4 @@ struct DebugData } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp similarity index 90% rename from planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp rename to planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp index 12b54a5031041..2bf3158da9ab6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__MARKER_UTILS__AVOIDANCE__DEBUG_HPP_ -#define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__AVOIDANCE__DEBUG_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_ -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" +#include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_planner_common/marker_utils/utils.hpp" #include @@ -78,4 +78,4 @@ std::string toStrInfo(const behavior_path_planner::ShiftLine & sp); std::string toStrInfo(const behavior_path_planner::AvoidLine & ap); -#endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__AVOIDANCE__DEBUG_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp similarity index 96% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp rename to planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index 54a38b828e987..6717cefefc9f6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__HELPER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__HELPER_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__HELPER_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__HELPER_HPP_ -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_avoidance_module/data_structs.hpp" +#include "behavior_path_avoidance_module/utils.hpp" #include @@ -331,4 +331,4 @@ class AvoidanceHelper }; } // namespace behavior_path_planner::helper::avoidance -#endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__HELPER_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__HELPER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp similarity index 80% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp rename to planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp index 0fe8bd7efa17b..72cfbe6f0a1ed 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__MANAGER_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" +#include "behavior_path_avoidance_module/data_structs.hpp" +#include "behavior_path_avoidance_module/scene.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -51,4 +51,4 @@ class AvoidanceModuleManager : public SceneModuleManagerInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__MANAGER_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp similarity index 96% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp rename to planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index b01cb62f22372..6a66b485d1c59 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_MODULE_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__SCENE_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__SCENE_HPP_ -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/avoidance/helper.hpp" -#include "behavior_path_planner/utils/avoidance/shift_line_generator.hpp" +#include "behavior_path_avoidance_module/data_structs.hpp" +#include "behavior_path_avoidance_module/helper.hpp" +#include "behavior_path_avoidance_module/shift_line_generator.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" @@ -450,4 +450,4 @@ class AvoidanceModule : public SceneModuleInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_MODULE_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/shift_line_generator.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp similarity index 95% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/shift_line_generator.hpp rename to planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp index 054a6e15ef6bf..edfe9ab9663ce 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/shift_line_generator.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__SHIFT_LINE_GENERATOR_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__SHIFT_LINE_GENERATOR_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/avoidance/helper.hpp" +#include "behavior_path_avoidance_module/data_structs.hpp" +#include "behavior_path_avoidance_module/helper.hpp" #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -248,4 +248,4 @@ class ShiftLineGenerator } // namespace behavior_path_planner::utils::avoidance -#endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__SHIFT_LINE_GENERATOR_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp similarity index 96% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp rename to planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index e7611208962f0..c954ab9022853 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_ -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" +#include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" @@ -180,4 +180,4 @@ double calcDistanceToAvoidStartLine( const std::shared_ptr & parameters); } // namespace behavior_path_planner::utils::avoidance -#endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_ diff --git a/planning/behavior_path_avoidance_module/package.xml b/planning/behavior_path_avoidance_module/package.xml new file mode 100644 index 0000000000000..68cc911e9373d --- /dev/null +++ b/planning/behavior_path_avoidance_module/package.xml @@ -0,0 +1,53 @@ + + + + behavior_path_avoidance_module + 0.1.0 + The behavior_path_avoidance_module package + + Takamasa Horibe + Satoshi Ota + Fumiya Watanabe + Kyoichi Sugahara + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_auto_tf2 + autoware_perception_msgs + behavior_path_planner + behavior_path_planner_common + geometry_msgs + lanelet2_extension + libboost-dev + magic_enum + motion_utils + objects_of_interest_marker_interface + pluginlib + rclcpp + route_handler + rtc_interface + sensor_msgs + signal_processing + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_planning_msgs + vehicle_info_util + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_avoidance_module/plugins.xml b/planning/behavior_path_avoidance_module/plugins.xml new file mode 100644 index 0000000000000..f25677dad1e9a --- /dev/null +++ b/planning/behavior_path_avoidance_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp similarity index 99% rename from planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp rename to planning/behavior_path_avoidance_module/src/debug.cpp index 7587c3bb3663e..3ae210e0b1d78 100644 --- a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/marker_utils/avoidance/debug.hpp" +#include "behavior_path_avoidance_module/debug.hpp" #include "behavior_path_planner_common/utils/utils.hpp" diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp similarity index 99% rename from planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp rename to planning/behavior_path_avoidance_module/src/manager.cpp index e17804f10deaa..f4997d2af12f1 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_avoidance_module/src/manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/avoidance/manager.hpp" +#include "behavior_path_avoidance_module/manager.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp similarity index 99% rename from planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp rename to planning/behavior_path_avoidance_module/src/scene.cpp index df93df75410e1..9b0c85f60ba29 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" +#include "behavior_path_avoidance_module/scene.hpp" -#include "behavior_path_planner/marker_utils/avoidance/debug.hpp" -#include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_avoidance_module/debug.hpp" +#include "behavior_path_avoidance_module/utils.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" #include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" diff --git a/planning/behavior_path_planner/src/utils/avoidance/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp similarity index 99% rename from planning/behavior_path_planner/src/utils/avoidance/shift_line_generator.cpp rename to planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index c7fbaa931737c..48d7e3a4baf8b 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/avoidance/shift_line_generator.hpp" +#include "behavior_path_avoidance_module/shift_line_generator.hpp" -#include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_avoidance_module/utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" #include diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp similarity index 99% rename from planning/behavior_path_planner/src/utils/avoidance/utils.cpp rename to planning/behavior_path_avoidance_module/src/utils.cpp index 6fd1943369cb9..b469e9070baad 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -14,8 +14,8 @@ #include "behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_avoidance_module/data_structs.hpp" +#include "behavior_path_avoidance_module/utils.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/traffic_light_utils.hpp" diff --git a/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..83514127b0a8e --- /dev/null +++ b/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,126 @@ +// 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. + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "behavior_path_planner/behavior_path_planner_node.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include + +#include +#include + +using behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + + std::vector module_names; + module_names.emplace_back("behavior_path_planner::AvoidanceModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + ament_index_cpp::get_package_share_directory("behavior_path_planner") + + "/config/lane_change/lane_change.param.yaml", + ament_index_cpp::get_package_share_directory("behavior_path_avoidance_module") + + "/config/avoidance.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_path_planner/test/test_avoidance_utils.cpp b/planning/behavior_path_avoidance_module/test/test_utils.cpp similarity index 92% rename from planning/behavior_path_planner/test/test_avoidance_utils.cpp rename to planning/behavior_path_avoidance_module/test/test_utils.cpp index fc2bea1a5b288..e5c6fd2e72092 100644 --- a/planning/behavior_path_planner/test/test_avoidance_utils.cpp +++ b/planning/behavior_path_avoidance_module/test/test_utils.cpp @@ -11,8 +11,9 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/avoidance/utils.hpp" + +#include "behavior_path_avoidance_module/data_structs.hpp" +#include "behavior_path_avoidance_module/utils.hpp" #include #include diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 2ea43bf5a4ef1..b99bc2a4eb2e4 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -12,8 +12,6 @@ pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) ament_auto_add_library(${PROJECT_NAME}_lib SHARED src/planner_manager.cpp src/behavior_path_planner_node.cpp - src/scene_module/avoidance/avoidance_module.cpp - src/scene_module/avoidance/manager.cpp src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp src/scene_module/dynamic_avoidance/manager.cpp src/scene_module/start_planner/start_planner_module.cpp @@ -25,11 +23,8 @@ ament_auto_add_library(${PROJECT_NAME}_lib SHARED src/scene_module/lane_change/interface.cpp src/scene_module/lane_change/normal.cpp src/scene_module/lane_change/external_request.cpp - src/scene_module/lane_change/avoidance_by_lane_change.cpp src/scene_module/lane_change/manager.cpp src/utils/start_goal_planner_common/utils.cpp - src/utils/avoidance/shift_line_generator.cpp - src/utils/avoidance/utils.cpp src/utils/lane_change/utils.cpp src/utils/side_shift/util.cpp src/utils/goal_planner/util.cpp @@ -44,7 +39,6 @@ ament_auto_add_library(${PROJECT_NAME}_lib SHARED src/utils/start_planner/freespace_pull_out.cpp src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp - src/marker_utils/avoidance/debug.cpp src/marker_utils/lane_change/debug.cpp ) @@ -71,14 +65,6 @@ if(BUILD_TESTING) ${PROJECT_NAME}_lib ) - ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_avoidance_module - test/test_avoidance_utils.cpp - ) - - target_link_libraries(test_${CMAKE_PROJECT_NAME}_avoidance_module - ${PROJECT_NAME}_lib - ) - ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_lane_change_module test/test_lane_change_utils.cpp ) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp index 2e3cbd992e27f..b7cef007d6d9f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp @@ -15,7 +15,6 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ -#include "behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp" #include "behavior_path_planner/scene_module/lane_change/base_class.hpp" #include "behavior_path_planner/scene_module/lane_change/external_request.hpp" #include "behavior_path_planner/scene_module/lane_change/normal.hpp" @@ -145,23 +144,6 @@ class LaneChangeInterface : public SceneModuleInterface bool is_abort_approval_requested_{false}; }; - -class AvoidanceByLaneChangeInterface : public LaneChangeInterface -{ -public: - AvoidanceByLaneChangeInterface( - const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters, - const std::shared_ptr & avoidance_by_lane_change_parameters, - const std::unordered_map> & rtc_interface_ptr_map, - std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); - - bool isExecutionRequested() const override; - -protected: - void updateRTCStatus(const double start_distance, const double finish_distance) override; -}; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp index ed476adcbd834..024c6685b28c4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp @@ -94,24 +94,6 @@ class ExternalRequestLaneChangeLeftModuleManager : public LaneChangeModuleManage { } }; - -class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager -{ -public: - AvoidanceByLaneChangeModuleManager() - : LaneChangeModuleManager( - "avoidance_by_lc", route_handler::Direction::NONE, - LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) - { - } - - void init(rclcpp::Node * node) override; - - std::unique_ptr createNewSceneModuleInstance() override; - -private: - std::shared_ptr avoidance_parameters_; -}; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index f11d48a24c79c..6f34bc79fe5f1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -14,8 +14,8 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include @@ -147,15 +147,6 @@ enum class LaneChangeModuleType { EXTERNAL_REQUEST, AVOIDANCE_BY_LANE_CHANGE, }; - -struct AvoidanceByLCParameters : public AvoidanceParameters -{ - // execute only when the target object longitudinal distance is larger than this param. - double execute_object_longitudinal_margin{0.0}; - - // execute only when lane change end point is before the object. - bool execute_only_when_lane_change_finish_before_object{false}; -}; } // namespace behavior_path_planner namespace behavior_path_planner::data::lane_change diff --git a/planning/behavior_path_planner/plugins.xml b/planning/behavior_path_planner/plugins.xml index 561246fcf75e4..5196b20dc5abd 100644 --- a/planning/behavior_path_planner/plugins.xml +++ b/planning/behavior_path_planner/plugins.xml @@ -1,5 +1,4 @@ - @@ -8,5 +7,4 @@ - diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 413350e3eb23b..26fc423fce02f 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -474,38 +474,4 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( // not in the vicinity of the end of the path. return original return original_turn_signal_info; } - -AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( - const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters, - const std::shared_ptr & avoidance_by_lane_change_parameters, - const std::unordered_map> & rtc_interface_ptr_map, - std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: LaneChangeInterface{ - name, - node, - parameters, - rtc_interface_ptr_map, - objects_of_interest_marker_interface_ptr_map, - std::make_unique(parameters, avoidance_by_lane_change_parameters)} -{ -} - -bool AvoidanceByLaneChangeInterface::isExecutionRequested() const -{ - return module_type_->specialRequiredCheck() && module_type_->isLaneChangeRequired(); -} - -void AvoidanceByLaneChangeInterface::updateRTCStatus( - const double start_distance, const double finish_distance) -{ - const auto direction = std::invoke([&]() -> std::string { - const auto dir = module_type_->getDirection(); - return (dir == Direction::LEFT) ? "left" : "right"; - }); - - rtc_interface_ptr_map_.at(direction)->updateCooperateStatus( - uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now()); -} } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 29b7cad533a66..ed40476723dba 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -246,148 +246,6 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(*node, ns + "execute_object_longitudinal_margin"); - p.execute_only_when_lane_change_finish_before_object = - getOrDeclareParameter(*node, ns + "execute_only_when_lane_change_finish_before_object"); - } - - // general params - { - const std::string ns = "avoidance."; - p.resample_interval_for_planning = - getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); - p.resample_interval_for_output = - getOrDeclareParameter(*node, ns + "resample_interval_for_output"); - } - - // target object - { - const auto get_object_param = [&](std::string && ns) { - ObjectParameter param{}; - param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); - param.moving_speed_threshold = - getOrDeclareParameter(*node, ns + "moving_speed_threshold"); - param.moving_time_threshold = - getOrDeclareParameter(*node, ns + "moving_time_threshold"); - param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); - param.envelope_buffer_margin = - getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); - param.avoid_margin_lateral = - getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); - param.safety_buffer_lateral = - getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); - return param; - }; - - const std::string ns = "avoidance_by_lane_change.target_object."; - p.object_parameters.emplace( - ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle.")); - p.object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car.")); - p.object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck.")); - p.object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer.")); - p.object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus.")); - p.object_parameters.emplace( - ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian.")); - p.object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle.")); - p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); - - p.lower_distance_for_polygon_expansion = - getOrDeclareParameter(*node, ns + "lower_distance_for_polygon_expansion"); - p.upper_distance_for_polygon_expansion = - getOrDeclareParameter(*node, ns + "upper_distance_for_polygon_expansion"); - } - - // target filtering - { - const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { - if (p.object_parameters.count(object_type) == 0) { - return; - } - p.object_parameters.at(object_type).is_avoidance_target = - getOrDeclareParameter(*node, ns); - }; - - const std::string ns = "avoidance.target_filtering."; - set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); - set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); - set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); - set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); - set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); - set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); - set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); - set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); - - p.object_check_goal_distance = - getOrDeclareParameter(*node, ns + "object_check_goal_distance"); - p.threshold_distance_object_is_on_center = - getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); - p.object_check_shiftable_ratio = - getOrDeclareParameter(*node, ns + "object_check_shiftable_ratio"); - p.object_check_min_road_shoulder_width = - getOrDeclareParameter(*node, ns + "object_check_min_road_shoulder_width"); - p.object_last_seen_threshold = - getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); - } - - { - const std::string ns = "avoidance.target_filtering.force_avoidance."; - p.enable_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "enable"); - p.threshold_time_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "time_threshold"); - p.object_ignore_section_traffic_light_in_front_distance = - getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); - p.object_ignore_section_crosswalk_in_front_distance = - getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.front_distance"); - p.object_ignore_section_crosswalk_behind_distance = - getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); - } - - { - const std::string ns = "avoidance.target_filtering.detection_area."; - p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); - p.object_check_min_forward_distance = - getOrDeclareParameter(*node, ns + "min_forward_distance"); - p.object_check_max_forward_distance = - getOrDeclareParameter(*node, ns + "max_forward_distance"); - p.object_check_backward_distance = - getOrDeclareParameter(*node, ns + "backward_distance"); - } - - // safety check - { - const std::string ns = "avoidance.safety_check."; - p.hysteresis_factor_expand_rate = - getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); - } - - avoidance_parameters_ = std::make_shared(p); -} - -std::unique_ptr -AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() -{ - return std::make_unique( - name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); -} - } // namespace behavior_path_planner #include @@ -403,6 +261,3 @@ PLUGINLIB_EXPORT_CLASS( PLUGINLIB_EXPORT_CLASS( behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, behavior_path_planner::SceneModuleManagerInterface) -PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::AvoidanceByLaneChangeModuleManager, - behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp index 9235e8e8e3de6..c2b115ae98afe 100644 --- a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp @@ -49,7 +49,6 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("behavior_path_planner"); std::vector module_names; - module_names.emplace_back("behavior_path_planner::AvoidanceModuleManager"); module_names.emplace_back("behavior_path_planner::DynamicAvoidanceModuleManager"); module_names.emplace_back("behavior_path_planner::SideShiftModuleManager"); module_names.emplace_back("behavior_path_planner::StartPlannerModuleManager"); @@ -58,7 +57,6 @@ std::shared_ptr generateNode() module_names.emplace_back("behavior_path_planner::LaneChangeLeftModuleManager"); module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeRightModuleManager"); module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"); - module_names.emplace_back("behavior_path_planner::AvoidanceByLaneChangeModuleManager"); std::vector params; params.emplace_back("launch_modules", module_names); @@ -72,12 +70,10 @@ std::shared_ptr generateNode() behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - behavior_path_planner_dir + "/config/avoidance/avoidance.param.yaml", behavior_path_planner_dir + "/config/dynamic_avoidance/dynamic_avoidance.param.yaml", behavior_path_planner_dir + "/config/lane_change/lane_change.param.yaml", behavior_path_planner_dir + "/config/start_planner/start_planner.param.yaml", behavior_path_planner_dir + "/config/goal_planner/goal_planner.param.yaml", - behavior_path_planner_dir + "/config/avoidance_by_lc/avoidance_by_lc.param.yaml", behavior_path_planner_dir + "/config/side_shift/side_shift.param.yaml"}); return std::make_shared(node_options);