diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 1cc1411f8a12e..a35b5205cf6b4 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -26,6 +26,7 @@ + @@ -128,6 +129,11 @@ value="$(eval "'$(var behavior_velocity_planner_launch_modules)' + 'autoware::behavior_velocity_planner::BlindSpotModulePlugin, '")" if="$(var launch_blind_spot_module)" /> + + + @@ -214,6 +222,7 @@ + diff --git a/planning/autoware_rtc_interface/src/rtc_interface.cpp b/planning/autoware_rtc_interface/src/rtc_interface.cpp index 7fcd0596b6b32..bc22a011977be 100644 --- a/planning/autoware_rtc_interface/src/rtc_interface.cpp +++ b/planning/autoware_rtc_interface/src/rtc_interface.cpp @@ -70,7 +70,7 @@ Module getModuleType(const std::string & module_name) module.type = Module::BLIND_SPOT; } else if (module_name == "crosswalk") { module.type = Module::CROSSWALK; - } else if (module_name == "detection_area") { + } else if (module_name == "detection_area" || module_name == "detection_lane") { module.type = Module::DETECTION_AREA; } else if (module_name == "intersection") { module.type = Module::INTERSECTION; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/CMakeLists.txt new file mode 100644 index 0000000000000..bd3adf29ef2df --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/CMakeLists.txt @@ -0,0 +1,43 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_velocity_detection_lane_module) + +find_package(autoware_cmake REQUIRED) + +autoware_package() + +find_package(PCL REQUIRED COMPONENTS common) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wno-error=deprecated-declarations) +endif() + +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) + +generate_parameter_library(detection_lane_parameters + param/detection_lane_parameters.yaml +) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +target_link_libraries(${PROJECT_NAME} + ${PCL_LIBRARIES} + detection_lane_parameters +) + +# if(BUILD_TESTING) +# find_package(ament_lint_auto REQUIRED) +# ament_lint_auto_find_test_dependencies() +# ament_add_ros_isolated_gtest(test_${PROJECT_NAME} +# test/test_utils.cpp +# test/test_node_interface.cpp +# ) +# target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) +# endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/README.md new file mode 100644 index 0000000000000..d5e8ffe6ba5f1 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/README.md @@ -0,0 +1 @@ +## Detection Lane diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/config/detection_lane.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/config/detection_lane.param.yaml new file mode 100644 index 0000000000000..ee86f17a2d225 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/config/detection_lane.param.yaml @@ -0,0 +1,58 @@ +/**: + ros__parameters: + detection_lane: + pointcloud: + range: + min: 80.0 + max: 140.0 + offset: + left: -0.3 + right: -0.3 + voxel_grid_filter: + x: 0.1 + y: 0.1 + z: 0.5 + clustering: + cluster_tolerance: 0.5 #[m] + min_cluster_height: 0.1 + min_cluster_size: 5 + max_cluster_size: 10000 + velocity_estimation: + observation_time: 0.3 + latency: 0.3 + + filter: + min_velocity: 1.0 + + stop: + margin: 0.0 + on_time_buffer: 1.0 + off_time_buffer: 0.5 + use_sudden_stop: true + + hold: + margin: 0.0 + + dead_line: + enable: true + margin: 0.5 + + enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + + intersection_names: [shiojiri, komatsu] + + config: + shiojiri: + mgrs_grid: 53SQA + lane_id: 192 + stop_line_id: 88642 + lane_1st_ids: [53437, 53400, 53506] + ttc_threshold: 20.0 + + komatsu: + mgrs_grid: 53SPA + lane_id: 185 + stop_line_id: 128271 + lane_1st_ids: [128324, 128080, 128176, 128228, 128122, 128074, 128248, 176] + lane_2nd_ids: [128323, 128079, 128175, 128227, 128226, 128073, 128247, 175] + ttc_threshold: 20.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/package.xml new file mode 100644 index 0000000000000..ed4af087c699c --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/package.xml @@ -0,0 +1,47 @@ + + + + autoware_behavior_velocity_detection_lane_module + 0.43.0 + The autoware_behavior_velocity_detection_lane_module package + + Kyoichi Sugahara + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Kyoichi Sugahara + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_behavior_velocity_planner + autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface + autoware_lanelet2_extension + autoware_motion_utils + autoware_planning_msgs + autoware_route_handler + autoware_signal_processing + autoware_utils + eigen + generate_parameter_library + geometry_msgs + libboost-dev + pluginlib + rclcpp + tf2 + tf2_eigen + tf2_geometry_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/param/detection_lane_parameters.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/param/detection_lane_parameters.yaml new file mode 100644 index 0000000000000..043c34a21ee49 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/param/detection_lane_parameters.yaml @@ -0,0 +1,116 @@ +detection_lane: + detection_lane: + pointcloud: + range: + min: + type: double + validation: + gt_eq<>: [0.0] + max: + type: double + validation: + gt_eq<>: [0.0] + offset: + left: + type: double + right: + type: double + voxel_grid_filter: + x: + type: double + validation: + gt<>: [0.0] + y: + type: double + validation: + gt<>: [0.0] + z: + type: double + validation: + gt<>: [0.0] + clustering: + cluster_tolerance: + type: double + validation: + gt<>: [0.0] + min_cluster_height: + type: double + validation: + gt<>: [0.0] + min_cluster_size: + type: int + validation: + gt<>: [0] + max_cluster_size: + type: int + validation: + gt<>: [0] + velocity_estimation: + observation_time: + type: double + validation: + gt_eq<>: [0.0] + latency: + type: double + validation: + gt_eq<>: [0.0] + + filter: + min_velocity: + type: double + + stop: + margin: + type: double + on_time_buffer: + type: double + off_time_buffer: + type: double + use_sudden_stop: + type: bool + + hold: + margin: + type: double + + dead_line: + enable: + type: bool + margin: + type: double + + intersection_names: + type: string_array + + config: + __map_intersection_names: + mgrs_grid: + type: string + lane_id: + type: int + stop_line_id: + type: int + lane_1st_ids: + type: int_array + validation: + not_empty<>: [] + unique<>: [] + lane_2nd_ids: + type: int_array + default_value: [] + validation: + unique<>: [] + lane_3rd_ids: + type: int_array + default_value: [] + validation: + unique<>: [] + lane_4th_ids: + type: int_array + default_value: [] + validation: + unique<>: [] + ttc_threshold: + type: double + validation: + gt_eq<>: [0.0] diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/plugins.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/plugins.xml new file mode 100644 index 0000000000000..0879f92bd94a1 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/src/manager.cpp new file mode 100644 index 0000000000000..0a113c007dd27 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/src/manager.cpp @@ -0,0 +1,182 @@ +// Copyright 2025 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 "manager.hpp" + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +using autoware_utils::get_or_declare_parameter; +using lanelet::autoware::DetectionArea; + +lanelet::ConstLanelet combineLaneletsShape(const lanelet::ConstLanelets & lanelets) +{ + lanelet::Points3d lefts; + lanelet::Points3d rights; + lanelet::Points3d centers; + for (const auto & llt : lanelets) { + for (const auto & pt : llt.leftBound()) { + lefts.push_back(lanelet::Point3d(pt)); + } + for (const auto & pt : llt.rightBound()) { + rights.push_back(lanelet::Point3d(pt)); + } + for (const auto & pt : llt.centerline()) { + centers.push_back(lanelet::Point3d(pt)); + } + } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts); + const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights); + const auto center_line = lanelet::LineString3d(lanelet::InvalId, centers); + auto combined_lanelet = lanelet::Lanelet(lanelets.back().id(), left_bound, right_bound); + combined_lanelet.setCenterline(center_line); + return combined_lanelet; +} + +DetectionLaneModuleManager::DetectionLaneModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterfaceWithRTC( + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")), + param_listener_{ + std::make_unique(node.get_node_parameters_interface())}, + sub_map_projector_info_{&node, "/map/map_projector_info", rclcpp::QoS{1}.transient_local()} +{ +} + +void DetectionLaneModuleManager::launchNewModules( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) +{ + const auto p = param_listener_->get_params(); + + const auto map_projector_info = sub_map_projector_info_.take_data(); + if (!map_projector_info) { + if (mgrs_grid_ == "") { + RCLCPP_ERROR(logger_, "projector info hasn't been published."); + return; + } + } else { + mgrs_grid_ = map_projector_info->mgrs_grid; + } + + const auto routing_graph = planner_data_->route_handler_->getRoutingGraphPtr(); + const auto lanelet_map = planner_data_->route_handler_->getLaneletMapPtr(); + + const auto lanelets = + planning_utils::getLaneletsOnPath(path, lanelet_map, planner_data_->current_odometry->pose); + + for (const auto & lanelet : lanelets) { + const auto itr = std::find_if( + p.config.intersection_names_map.begin(), p.config.intersection_names_map.end(), + [&lanelet, this](const auto & config) { + return config.second.mgrs_grid == mgrs_grid_ && config.second.lane_id == lanelet.id(); + }); + + if (itr == p.config.intersection_names_map.end()) { + continue; + } + + if (!isModuleRegistered(itr->second.lane_id)) { + const auto passing_lane = lanelet_map->laneletLayer.get(itr->second.lane_id); + const auto stop_line = lanelet_map->lineStringLayer.get(itr->second.stop_line_id); + const auto detection_lanes = [&lanelet_map, &itr]() { + lanelet::ConstLanelets ret{}; + + { + lanelet::ConstLanelets detection_lanes{}; + for (const auto & id : itr->second.lane_1st_ids) { + detection_lanes.push_back(lanelet_map->laneletLayer.get(id)); + } + if (!detection_lanes.empty()) { + ret.push_back(combineLaneletsShape(detection_lanes)); + } + } + + { + lanelet::ConstLanelets detection_lanes{}; + for (const auto & id : itr->second.lane_2nd_ids) { + detection_lanes.push_back(lanelet_map->laneletLayer.get(id)); + } + if (!detection_lanes.empty()) { + ret.push_back(combineLaneletsShape(detection_lanes)); + } + } + + { + lanelet::ConstLanelets detection_lanes{}; + for (const auto & id : itr->second.lane_3rd_ids) { + detection_lanes.push_back(lanelet_map->laneletLayer.get(id)); + } + if (!detection_lanes.empty()) { + ret.push_back(combineLaneletsShape(detection_lanes)); + } + } + + { + lanelet::ConstLanelets detection_lanes{}; + for (const auto & id : itr->second.lane_4th_ids) { + detection_lanes.push_back(lanelet_map->laneletLayer.get(id)); + } + if (!detection_lanes.empty()) { + ret.push_back(combineLaneletsShape(detection_lanes)); + } + } + + return ret; + }(); + + registerModule(std::make_shared( + itr->second.lane_id, passing_lane, stop_line, detection_lanes, itr->second.ttc_threshold, + p.detection_lane, logger_.get_child("detection_lane_module"), clock_, time_keeper_, + planning_factor_interface_)); + generate_uuid(itr->second.lane_id); + updateRTCStatus( + getUUID(itr->second.lane_id), true, State::WAITING_FOR_EXECUTION, + std::numeric_limits::lowest(), path.header.stamp); + } + } +} + +std::function &)> +DetectionLaneModuleManager::getModuleExpiredFunction( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) +{ + const auto lanelets = planning_utils::getLaneletsOnPath( + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + return [lanelets](const std::shared_ptr & scene_module) { + const auto id = scene_module->getModuleId(); + return !std::any_of( + lanelets.begin(), lanelets.end(), [&id](const auto & lanelet) { return lanelet.id() == id; }); + }; +} + +} // namespace autoware::behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::behavior_velocity_planner::DetectionLaneModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/src/manager.hpp new file mode 100644 index 0000000000000..9cae6daea1b45 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/src/manager.hpp @@ -0,0 +1,71 @@ +// Copyright 2025 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 MANAGER_HPP_ +#define MANAGER_HPP_ + +#include "scene.hpp" + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +class DetectionLaneModuleManager : public SceneModuleManagerInterfaceWithRTC +{ +public: + explicit DetectionLaneModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "detection_lane"; } + + RequiredSubscriptionInfo getRequiredSubscriptions() const override + { + RequiredSubscriptionInfo required_subscription_info; + required_subscription_info.no_ground_pointcloud = true; + return required_subscription_info; + } + +private: + std::unique_ptr param_listener_; + + void launchNewModules(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> + getModuleExpiredFunction( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; + + autoware_utils::InterProcessPollingSubscriber< + autoware_map_msgs::msg::MapProjectorInfo, autoware_utils::polling_policy::Newest> + sub_map_projector_info_; + + std::string mgrs_grid_{""}; +}; + +class DetectionLaneModulePlugin : public PluginWrapper +{ +}; + +} // namespace autoware::behavior_velocity_planner + +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/src/scene.cpp new file mode 100644 index 0000000000000..2387e85b8b0a4 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/src/scene.cpp @@ -0,0 +1,532 @@ +// Copyright 2025 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 "scene.hpp" + +#include "autoware/signal_processing/lowpass_filter_1d.hpp" +#include "utils.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +using autoware::motion_utils::calcLongitudinalOffsetPose; +using autoware::motion_utils::calcSignedArcLength; + +DetectionLaneModule::DetectionLaneModule( + const int64_t module_id, const lanelet::ConstLanelet & passing_lane, + const lanelet::ConstLineString3d & stop_line, const lanelet::ConstLanelets & detection_lanes, + const double ttc_threshold, const detection_lane::Params::DetectionLane & planner_param, + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface) +: SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), + turn_direction_{passing_lane.attributeOr("turn_direction", "else")}, + stop_line_{stop_line}, + ttc_threshold_{ttc_threshold}, + planner_param_(planner_param) +{ + for (const auto & detection_lane : detection_lanes) { + std::vector intersects; + boost::geometry::intersection( + lanelet::utils::to2D(detection_lane.centerline()).basicLineString(), + lanelet::utils::to2D(passing_lane.centerline()).basicLineString(), intersects); + + if (intersects.size() != 1) { + RCLCPP_ERROR(logger, "something wrong."); + } + + geometry_msgs::msg::Pose conflict_point; + conflict_point.position = + autoware_utils::create_point(intersects.front().x(), intersects.front().y(), 0.0); + + lanelet::ConstLanelets lanelets{detection_lane}; + + const auto conflict_point_coordinate = + lanelet::utils::getArcCoordinates(lanelets, conflict_point).length; + + const auto expand_lanelets = lanelet::utils::getExpandedLanelets( + lanelets, planner_param_.pointcloud.offset.left, + -1.0 * planner_param_.pointcloud.offset.right); + + const auto polygon = + lanelet::utils::getPolygonFromArcLength( + expand_lanelets, conflict_point_coordinate - planner_param_.pointcloud.range.max, + conflict_point_coordinate - planner_param_.pointcloud.range.min) + .basicPolygon(); + + detection_areas_info_.emplace_back(lanelets, polygon, conflict_point_coordinate); + } + + const auto traffic_light_regulatory_elements = + passing_lane.regulatoryElementsAs(); + if (!traffic_light_regulatory_elements.empty()) { + traffic_light_regulatory_element_id_ = traffic_light_regulatory_elements.front()->id(); + } +} + +bool DetectionLaneModule::modifyPathVelocity([[maybe_unused]] PathWithLaneId * path) +{ + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + debug_data_ = DebugData(); + + post_process(); + + const auto original_path = *path; + + const auto obstacle_pointcloud = filter_pointcloud(debug_data_); + auto pointcloud_objects = get_pointcloud_object( + planner_data_->current_odometry->header.stamp, obstacle_pointcloud, detection_areas_info_, + debug_data_); + + fill_ttc(pointcloud_objects); + + debug_data_.pointcloud_objects = pointcloud_objects; + + // Get self pose + const auto & self_pose = planner_data_->current_odometry->pose; + const size_t current_seg_idx = findEgoSegmentIndex(path->points); + + // Get stop point + const auto extended_stop_line = planning_utils::extendLine( + stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length); + const auto stop_point = arc_lane_utils::createTargetPoint( + original_path, extended_stop_line, planner_param_.stop.margin, + planner_data_->vehicle_info_.max_longitudinal_offset_m); + if (!stop_point) { + return true; + } + + const auto & stop_point_idx = stop_point->first; + const auto & stop_pose = stop_point->second; + const size_t stop_line_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( + path->points, stop_pose.position, stop_point_idx); + + auto modified_stop_pose = stop_pose; + size_t modified_stop_line_seg_idx = stop_line_seg_idx; + + const auto is_stopped = planner_data_->isVehicleStopped(0.0); + const auto stop_dist = calcSignedArcLength( + path->points, self_pose.position, current_seg_idx, stop_pose.position, stop_line_seg_idx); + + // Don't re-approach when the ego stops closer to the stop point than hold_stop_margin_distance + if (is_stopped && stop_dist < planner_param_.hold.margin) { + const auto ego_pos_on_path = + calcLongitudinalOffsetPose(original_path.points, self_pose.position, 0.0); + + if (!ego_pos_on_path) { + return false; + } + + modified_stop_pose = ego_pos_on_path.value(); + modified_stop_line_seg_idx = current_seg_idx; + } + + setDistance(stop_dist); + + // Check state + setSafe(is_safe(pointcloud_objects) || is_prioritized_lane()); + + if (isActivated()) { + return true; + } + + // Force ignore objects after dead_line + if (planner_param_.dead_line.enable) { + // Use '-' for margin because it's the backward distance from stop line + const auto dead_line_point = arc_lane_utils::createTargetPoint( + original_path, extended_stop_line, -planner_param_.dead_line.margin, + planner_data_->vehicle_info_.max_longitudinal_offset_m); + + if (dead_line_point) { + const size_t dead_line_point_idx = dead_line_point->first; + const auto & dead_line_pose = dead_line_point->second; + + const size_t dead_line_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( + path->points, dead_line_pose.position, dead_line_point_idx); + + debug_data_.dead_line_poses.push_back(dead_line_pose); + + const double dist_from_ego_to_dead_line = calcSignedArcLength( + original_path.points, self_pose.position, current_seg_idx, dead_line_pose.position, + dead_line_seg_idx); + if (dist_from_ego_to_dead_line < 0.0) { + RCLCPP_WARN_THROTTLE(logger_, *clock_, 1000, "exceed dead line."); + setSafe(true); + return true; + } + } + } + + // Ignore objects if braking distance is not enough + if (!planner_param_.stop.use_sudden_stop) { + const auto v_now = planner_data_->current_velocity->twist.linear.x; + const auto a_now = planner_data_->current_acceleration->accel.accel.linear.x; + const double pass_judge_line_distance = planning_utils::calcJudgeLineDistWithAccLimit( + v_now, a_now, planner_data_->delay_response_time); + constexpr double buffer = 3.0; + if (stop_dist + buffer < pass_judge_line_distance) { + RCLCPP_WARN_THROTTLE(logger_, *clock_, 1000, "avoid sudden stop."); + setSafe(true); + return true; + } + } + + planning_utils::insertStopPoint(modified_stop_pose.position, modified_stop_line_seg_idx, *path); + + debug_data_.stop_poses.push_back(stop_point->second); + + return true; +} + +auto DetectionLaneModule::filter_pointcloud([[maybe_unused]] DebugData & debug) const + -> PointCloud::Ptr +{ + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const auto p = planner_param_.pointcloud; + + auto output = std::make_shared(*planner_data_->no_ground_pointcloud); + + { + debug.pointcloud_nums.push_back(output->size()); + } + + { + output = + filter_by_range(output, planner_data_->current_odometry->pose.position, p.range.min, true); + debug.pointcloud_nums.push_back(output->size()); + } + + // { + // autoware_utils::ScopedTimeTrack st("voxel_grid_filter", *time_keeper_); + + // pcl::VoxelGrid filter; + // filter.setInputCloud(output); + // filter.setLeafSize(p.voxel_grid_filter.x, p.voxel_grid_filter.y, p.voxel_grid_filter.z); + // filter.filter(*output); + // debug.pointcloud_nums.push_back(output->size()); + // } + + return output; +} + +auto DetectionLaneModule::get_pointcloud_object( + const rclcpp::Time & now, const PointCloud::Ptr & pointcloud_ptr, + const DetectionAreasInfo & detection_areas_info, DebugData & debug) -> PointCloudObjects +{ + autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + PointCloudObjects ret{}; + for (const auto & info : detection_areas_info) { + const auto pointcloud = + *get_clustered_pointcloud(get_obstacle_points({info.polygon}, *pointcloud_ptr), debug); + + const auto path = planner_data_->route_handler_->getCenterLinePath( + info.lanelets, 0.0, std::numeric_limits::max()); + // const auto resampled_path = behavior_path_planner::utils::resamplePathWithSpline(path, 2.0); + const auto resampled_path = path; + + std::optional opt_object = std::nullopt; + for (const auto & point : pointcloud) { + const auto p_geom = autoware_utils::create_point(point.x, point.y, point.z); + const size_t src_seg_idx = + autoware::motion_utils::findNearestSegmentIndex(resampled_path.points, p_geom); + const double signed_length_src_offset = + autoware::motion_utils::calcLongitudinalOffsetToSegment( + resampled_path.points, src_seg_idx, p_geom); + + const double obj_arc_length = + autoware::motion_utils::calcSignedArcLength( + resampled_path.points, src_seg_idx, resampled_path.points.size() - 1) - + signed_length_src_offset; + const auto pose_on_center_line = autoware::motion_utils::calcLongitudinalOffsetPose( + resampled_path.points, src_seg_idx, signed_length_src_offset); + + if (!pose_on_center_line.has_value()) { + continue; + } + + if (!opt_object.has_value()) { + PointCloudObject object; + object.last_update_time = now; + object.pose = pose_on_center_line.value(); + object.furthest_lane = info.lanelets.back(); + object.tracking_duration = 0.0; + object.distance = obj_arc_length; + object.velocity = 0.0; + opt_object = object; + } else if (opt_object.value().distance > obj_arc_length) { + opt_object.value().last_update_time = now; + opt_object.value().pose = pose_on_center_line.value(); + opt_object.value().furthest_lane = info.lanelets.back(); + opt_object.value().tracking_duration = 0.0; + opt_object.value().distance = obj_arc_length; + opt_object.value().velocity = 0.0; + } + } + if (opt_object.has_value()) { + opt_object.value().distance -= + lanelet::utils::getLaneletLength2d(info.lanelets) - info.conflict_point_coordinate; + fill_velocity(opt_object.value()); + ret.push_back(opt_object.value()); + } + } + + return ret; +} + +auto DetectionLaneModule::get_clustered_pointcloud( + const PointCloud::Ptr in, DebugData & debug) const -> PointCloud::Ptr +{ + const auto p = planner_param_.pointcloud; + + const auto points_belonging_to_cluster_hulls = std::make_shared(); + // eliminate noisy points by only considering points belonging to clusters of at least a certain + // size + if (in->empty()) return std::make_shared(); + const std::vector cluster_indices = std::invoke([&]() { + std::vector cluster_idx; + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + tree->setInputCloud(in); + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(p.clustering.cluster_tolerance); + ec.setMinClusterSize(p.clustering.min_cluster_size); + ec.setMaxClusterSize(p.clustering.max_cluster_size); + ec.setSearchMethod(tree); + ec.setInputCloud(in); + ec.extract(cluster_idx); + return cluster_idx; + }); + for (const auto & indices : cluster_indices) { + PointCloud::Ptr cluster(new PointCloud); + bool cluster_surpasses_threshold_height{false}; + for (const auto & index : indices.indices) { + const auto & point = (*in)[index]; + cluster_surpasses_threshold_height = (cluster_surpasses_threshold_height) + ? cluster_surpasses_threshold_height + : (point.z > p.clustering.min_cluster_height); + cluster->push_back(point); + } + if (!cluster_surpasses_threshold_height) continue; + // Make a 2d convex hull for the objects + pcl::ConvexHull hull; + hull.setDimension(2); + hull.setInputCloud(cluster); + std::vector polygons; + PointCloud::Ptr surface_hull(new PointCloud); + hull.reconstruct(*surface_hull, polygons); + autoware_utils::Polygon3d hull_polygon; + for (const auto & p : *surface_hull) { + points_belonging_to_cluster_hulls->push_back(p); + const auto point = autoware_utils::Point3d{p.x, p.y, p.z}; + boost::geometry::append(hull_polygon.outer(), point); + } + debug.hull_polygons.push_back(hull_polygon); + } + + { + const auto obstacle_pointcloud = std::make_shared(); + pcl::toROSMsg(*points_belonging_to_cluster_hulls, *obstacle_pointcloud); + obstacle_pointcloud->header.stamp = planner_data_->current_odometry->header.stamp; + obstacle_pointcloud->header.frame_id = "map"; + debug.obstacle_pointcloud = obstacle_pointcloud; + } + + return points_belonging_to_cluster_hulls; +} + +void DetectionLaneModule::fill_ttc(PointCloudObjects & objects) const +{ + for (auto & object : objects) { + object.ttc = object.distance / std::max(planner_param_.filter.min_velocity, object.velocity); + object.safe = object.ttc > ttc_threshold_; + object.ignore = object.velocity < planner_param_.filter.min_velocity; + } +} + +void DetectionLaneModule::fill_velocity(PointCloudObject & pointcloud_object) +{ + const auto update_history = [this](const auto & pointcloud_object) { + if (history_.count(pointcloud_object.furthest_lane.id()) == 0) { + history_.emplace(pointcloud_object.furthest_lane.id(), pointcloud_object); + } else { + history_.at(pointcloud_object.furthest_lane.id()) = pointcloud_object; + } + }; + + const auto fill_velocity = [this](auto & pointcloud_object, const auto & previous_data) { + const auto dx = previous_data.distance - pointcloud_object.distance; + const auto dt = (pointcloud_object.last_update_time - previous_data.last_update_time).seconds(); + + if (dt < 1e-6) { + pointcloud_object.velocity = previous_data.velocity; + pointcloud_object.tracking_duration = previous_data.tracking_duration + dt; + pointcloud_object.distance_with_delay_compensation = + pointcloud_object.distance - pointcloud_object.velocity * planner_param_.pointcloud.latency; + return; + } + + constexpr double assumed_acceleration = 30.0; + const auto raw_velocity = dx / dt; + const auto is_reliable = previous_data.tracking_duration > + planner_param_.pointcloud.velocity_estimation.observation_time; + + if ( + is_reliable && std::abs(raw_velocity - previous_data.velocity) / dt > assumed_acceleration) { + // closest point may jumped. reset tracking history. + pointcloud_object.velocity = 0.0; + pointcloud_object.tracking_duration = 0.0; + } else { + // keep tracking. + pointcloud_object.velocity = + autoware::signal_processing::lowpassFilter(raw_velocity, previous_data.velocity, 0.5); + pointcloud_object.tracking_duration = previous_data.tracking_duration + dt; + } + + pointcloud_object.distance_with_delay_compensation = + pointcloud_object.distance - pointcloud_object.velocity * planner_param_.pointcloud.latency; + }; + + if (history_.count(pointcloud_object.furthest_lane.id()) == 0) { + update_history(pointcloud_object); + return; + } + + fill_velocity(pointcloud_object, history_.at(pointcloud_object.furthest_lane.id())); + update_history(pointcloud_object); +} + +bool DetectionLaneModule::is_prioritized_lane() const +{ + const auto signal = planner_data_->getTrafficSignal( + traffic_light_regulatory_element_id_, true /* this module keeps last observation*/); + + if (!signal.has_value()) { + return false; + } + + constexpr double threshold = 1.0; + if ((clock_->now() - signal.value().stamp).seconds() > threshold) { + return false; + } + + if (turn_direction_ == "right") { + return is_green_arrow( + signal.value().signal, autoware_perception_msgs::msg::TrafficLightElement::RIGHT_ARROW); + } + + if (turn_direction_ == "left") { + return is_green_arrow( + signal.value().signal, autoware_perception_msgs::msg::TrafficLightElement::LEFT_ARROW); + } + + return false; +} + +bool DetectionLaneModule::is_safe(const PointCloudObjects & pointcloud_objects) +{ + const auto is_safe = std::all_of( + pointcloud_objects.begin(), pointcloud_objects.end(), + [](const auto & object) { return object.ignore || object.safe; }); + + const auto now = clock_->now(); + if (is_safe) { + last_safe_time_ = now; + if ((now - last_unsafe_time_).seconds() > planner_param_.stop.off_time_buffer) { + return true; + } + } else { + last_unsafe_time_ = now; + if ((now - last_safe_time_).seconds() < planner_param_.stop.on_time_buffer) { + return true; + } + } + + return false; +} + +auto DetectionLaneModule::createDebugMarkerArray() -> visualization_msgs::msg::MarkerArray +{ + visualization_msgs::msg::MarkerArray msg; + + const auto add = [&msg](const visualization_msgs::msg::MarkerArray & added) { + autoware_utils::append_marker_array(added, &msg); + }; + + add(lanelet::visualization::laneletsAsTriangleMarkerArray( + "detection_lanes", get_detection_lanes(), + autoware_utils::create_marker_color(1.0, 0.0, 0.42, 0.1))); + + add(create_polygon_marker_array( + get_detection_polygons(), "detection_areas", + autoware_utils::create_marker_color(1.0, 0.0, 0.42, 0.999))); + + add(create_polygon_marker_array( + debug_data_.hull_polygons, "hull_polygons", + autoware_utils::create_marker_color(0.0, 0.0, 1.0, 0.999))); + + add(create_pointcloud_object_marker_array(debug_data_.pointcloud_objects, "pointcloud_objects")); + + std::for_each(msg.markers.begin(), msg.markers.end(), [](auto & marker) { + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + }); + + return msg; +} + +auto DetectionLaneModule::createVirtualWalls() -> autoware::motion_utils::VirtualWalls +{ + autoware::motion_utils::VirtualWalls virtual_walls; + autoware::motion_utils::VirtualWall wall; + wall.text = "detection_lane"; + + wall.style = autoware::motion_utils::VirtualWallType::stop; + for (const auto & p : debug_data_.stop_poses) { + wall.pose = autoware_utils::calc_offset_pose( + p, planner_data_->vehicle_info_.max_longitudinal_offset_m, 0.0, 0.0); + virtual_walls.push_back(wall); + } + + wall.style = autoware::motion_utils::VirtualWallType::deadline; + for (const auto & p : debug_data_.dead_line_poses) { + wall.pose = autoware_utils::calc_offset_pose( + p, planner_data_->vehicle_info_.max_longitudinal_offset_m, 0.0, 0.0); + virtual_walls.push_back(wall); + } + return virtual_walls; +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/src/scene.hpp new file mode 100644 index 0000000000000..85d34da978d1c --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/src/scene.hpp @@ -0,0 +1,123 @@ +// Copyright 2025 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 SCENE_HPP_ +#define SCENE_HPP_ + +#include "structs.hpp" + +#include +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +class DetectionLaneModule : public SceneModuleInterfaceWithRTC +{ +public: + DetectionLaneModule( + const int64_t module_id, const lanelet::ConstLanelet & passing_lane, + const lanelet::ConstLineString3d & stop_line, const lanelet::ConstLanelets & detection_lanes, + const double ttc_threshold, const detection_lane::Params::DetectionLane & planner_param, + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface); + + bool modifyPathVelocity(PathWithLaneId * path) override; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + + autoware::motion_utils::VirtualWalls createVirtualWalls() override; + +private: + auto get_clustered_pointcloud(const PointCloud::Ptr in, DebugData & debug) const + -> PointCloud::Ptr; + + auto get_detection_polygons() const -> lanelet::BasicPolygons3d + { + lanelet::BasicPolygons3d ret{}; + for (const auto & info : detection_areas_info_) { + ret.push_back(info.polygon); + } + return ret; + } + + auto get_detection_lanes() const -> lanelet::ConstLanelets + { + lanelet::ConstLanelets ret{}; + for (const auto & info : detection_areas_info_) { + ret.insert(ret.end(), info.lanelets.begin(), info.lanelets.end()); + } + return ret; + } + + auto get_pointcloud_object( + const rclcpp::Time & now, const PointCloud::Ptr & pointcloud_ptr, + const DetectionAreasInfo & detection_areas_info, DebugData & debug) -> PointCloudObjects; + + void fill_ttc(PointCloudObjects & objects) const; + + void fill_velocity(PointCloudObject & pointcloud_object); + + auto filter_pointcloud(DebugData & debug) const -> PointCloud::Ptr; + + bool is_prioritized_lane() const; + + bool is_safe(const PointCloudObjects & pointcloud_objects); + + void post_process() + { + auto itr = history_.begin(); + while (itr != history_.end()) { + if ((clock_->now() - itr->second.last_update_time).seconds() > 1.0) { + itr = history_.erase(itr); + } else { + itr++; + } + } + } + + DetectionAreasInfo detection_areas_info_{}; + + std::string turn_direction_{""}; + + lanelet::ConstLineString3d stop_line_; + + lanelet::Id traffic_light_regulatory_element_id_; + + double ttc_threshold_; + + detection_lane::Params::DetectionLane planner_param_; + + rclcpp::Time last_safe_time_{clock_->now()}; + + rclcpp::Time last_unsafe_time_{clock_->now()}; + + std::map history_; + + DebugData debug_data_; +}; +} // namespace autoware::behavior_velocity_planner + +#endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/src/structs.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/src/structs.hpp new file mode 100644 index 0000000000000..3874b7c2aa7c6 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/src/structs.hpp @@ -0,0 +1,104 @@ +// Copyright 2025 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 STRUCTS_HPP_ +#define STRUCTS_HPP_ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +using autoware_internal_planning_msgs::msg::PathWithLaneId; + +using sensor_msgs::msg::PointCloud2; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +using PointCloud = pcl::PointCloud; + +struct DetectionAreaInfo +{ + DetectionAreaInfo( + const lanelet::ConstLanelets & _lanelets, const lanelet::BasicPolygon3d & _polygon, + const double _conflict_point_coordinate) + : lanelets{_lanelets}, polygon{_polygon}, conflict_point_coordinate{_conflict_point_coordinate} + { + } + lanelet::ConstLanelets lanelets; + + lanelet::BasicPolygon3d polygon; + + double conflict_point_coordinate; +}; + +using DetectionAreasInfo = std::vector; + +struct PointCloudObject +{ + rclcpp::Time last_update_time; + + geometry_msgs::msg::Pose pose; + + lanelet::ConstLanelet furthest_lane; + + double tracking_duration; + + double distance; + + double distance_with_delay_compensation; + + double velocity; + + double ttc; + + bool safe; + + bool ignore; +}; + +using PointCloudObjects = std::vector; + +struct DebugData +{ + PointCloudObjects pointcloud_objects; + + std::vector stop_poses; + + std::vector dead_line_poses; + + std::vector hull_polygons; + + std::vector pointcloud_nums{}; + + sensor_msgs::msg::PointCloud2::SharedPtr obstacle_pointcloud; +}; +} // namespace autoware::behavior_velocity_planner + +#endif // STRUCTS_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/src/utils.hpp new file mode 100644 index 0000000000000..f1d3b2991efb2 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/src/utils.hpp @@ -0,0 +1,262 @@ +// Copyright 2025 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 UTILS_HPP_ +#define UTILS_HPP_ + +#include "structs.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +visualization_msgs::msg::MarkerArray create_polygon_marker_array( + const lanelet::BasicPolygons3d & polygons, const std::string & ns, + const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::MarkerArray msg; + + size_t i = 0; + for (const auto & polygon : polygons) { + auto marker = autoware_utils::create_default_marker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i++, + visualization_msgs::msg::Marker::LINE_STRIP, + autoware_utils::create_marker_scale(0.2, 0.0, 0.0), color); + + for (const auto & p : polygon) { + marker.points.push_back(autoware_utils::create_point(p.x(), p.y(), p.z())); + } + if (!marker.points.empty()) { + marker.points.push_back(marker.points.front()); + } + msg.markers.push_back(marker); + } + + return msg; +} + +visualization_msgs::msg::MarkerArray create_polygon_marker_array( + const std::vector & polygons, const std::string & ns, + const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::MarkerArray msg; + + size_t i = 0; + for (const auto & polygon : polygons) { + auto marker = autoware_utils::create_default_marker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i++, + visualization_msgs::msg::Marker::LINE_STRIP, + autoware_utils::create_marker_scale(0.05, 0.0, 0.0), color); + + for (const auto & p : polygon.outer()) { + marker.points.push_back(autoware_utils::create_point(p.x(), p.y(), p.z())); + } + if (!marker.points.empty()) { + marker.points.push_back(marker.points.front()); + } + msg.markers.push_back(marker); + } + + return msg; +} + +visualization_msgs::msg::MarkerArray create_pointcloud_object_marker_array( + const PointCloudObjects & objects, const std::string & ns) +{ + visualization_msgs::msg::MarkerArray msg; + + size_t i = 0L; + for (const auto & object : objects) { + const auto sphere_color = [&object]() { + if (object.tracking_duration < 0.3) { + return autoware_utils::create_marker_color(1.0, 0.67, 0.0, 0.999); + } + if (object.ignore) { + return autoware_utils::create_marker_color(0.5, 0.5, 0.5, 0.999); + } + return object.safe ? autoware_utils::create_marker_color(0.16, 1.0, 0.69, 0.999) + : autoware_utils::create_marker_color(1.0, 0.0, 0.42, 0.999); + }(); + + { + auto marker = autoware_utils::create_default_marker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns + "_sphere", i++, + visualization_msgs::msg::Marker::SPHERE, autoware_utils::create_marker_scale(1.0, 1.0, 1.0), + sphere_color); + marker.pose = object.pose; + msg.markers.push_back(marker); + } + + { + const auto x_scale = object.velocity * 0.36; + auto marker = autoware_utils::create_default_marker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns + "_arrow", i++, + visualization_msgs::msg::Marker::ARROW, + autoware_utils::create_marker_scale(x_scale, 0.3, 0.3), sphere_color); + marker.pose = object.pose; + msg.markers.push_back(marker); + } + + { + auto marker = autoware_utils::create_default_marker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns + "_text", i++, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + autoware_utils::create_marker_scale(0.5, 0.5, 0.5), + autoware_utils::create_marker_color(1.0, 1.0, 1.0, 0.999)); + + std::ostringstream ss; + ss << std::fixed << std::setprecision(2); + ss << "TrackingDuration:" << object.tracking_duration + << "[s]\nDistance(w/DC):" << object.distance + << "[m]\nDistance(w/oDC):" << object.distance_with_delay_compensation + << "[m]\nVelocity:" << object.velocity << "[m/s]\nTTC" << object.ttc + << "[s]\nFurthestLaneID:" << object.furthest_lane.id(); + + marker.text = ss.str(); + marker.pose = object.pose; + marker.pose.position.z += 1.0; + msg.markers.push_back(marker); + } + } + + return msg; +} + +std::pair get_smallest_enclosing_circle( + const lanelet::BasicPolygon2d & poly) +{ + // The `eps` is used to avoid precision bugs in circle inclusion checks. + // If the value of `eps` is too small, this function doesn't work well. More than 1e-10 is + // recommended. + const double eps = 1e-5; + lanelet::BasicPoint2d center(0.0, 0.0); + double radius_squared = 0.0; + + auto cross = [](const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2) -> double { + return p1.x() * p2.y() - p1.y() * p2.x(); + }; + + auto make_circle_3 = [&]( + const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2, + const lanelet::BasicPoint2d & p3) -> void { + // reference for circumcenter vector https://en.wikipedia.org/wiki/Circumscribed_circle + const double a = (p2 - p3).squaredNorm(); + const double b = (p3 - p1).squaredNorm(); + const double c = (p1 - p2).squaredNorm(); + const double s = cross(p2 - p1, p3 - p1); + if (std::abs(s) < eps) return; + center = (a * (b + c - a) * p1 + b * (c + a - b) * p2 + c * (a + b - c) * p3) / (4 * s * s); + radius_squared = (center - p1).squaredNorm() + eps; + }; + + auto make_circle_2 = + [&](const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2) -> void { + center = (p1 + p2) * 0.5; + radius_squared = (center - p1).squaredNorm() + eps; + }; + + auto in_circle = [&](const lanelet::BasicPoint2d & p) -> bool { + return (center - p).squaredNorm() <= radius_squared; + }; + + // mini disc + for (size_t i = 1; i < poly.size(); i++) { + const auto p1 = poly[i]; + if (in_circle(p1)) continue; + + // mini disc with point + const auto p0 = poly[0]; + make_circle_2(p0, p1); + for (size_t j = 0; j < i; j++) { + const auto p2 = poly[j]; + if (in_circle(p2)) continue; + + // mini disc with two points + make_circle_2(p1, p2); + for (size_t k = 0; k < j; k++) { + const auto p3 = poly[k]; + if (in_circle(p3)) continue; + + // mini disc with tree points + make_circle_3(p1, p2, p3); + } + } + } + + return std::make_pair(center, radius_squared); +} + +PointCloud::Ptr filter_by_range( + const PointCloud::Ptr & points, const geometry_msgs::msg::Point & center, const double range, + const bool outer = false) +{ + PointCloud ret; + const auto threshold = range * range; + for (const auto & p : *points) { + const double squared_dist = + (center.x - p.x) * (center.x - p.x) + (center.y - p.y) * (center.y - p.y); + if (outer) { + if (squared_dist > threshold) { + ret.push_back(p); + } + } else { + if (squared_dist < threshold) { + ret.push_back(p); + } + } + } + return std::make_shared(ret); +} + +PointCloud::Ptr get_obstacle_points( + const lanelet::BasicPolygons3d & polygons, const PointCloud & points) +{ + PointCloud ret; + for (const auto & polygon : polygons) { + const auto circle = get_smallest_enclosing_circle(lanelet::utils::to2D(polygon)); + for (const auto & p : points) { + const double squared_dist = (circle.first.x() - p.x) * (circle.first.x() - p.x) + + (circle.first.y() - p.y) * (circle.first.y() - p.y); + if (squared_dist > circle.second) { + continue; + } + if (boost::geometry::within( + autoware_utils::Point2d{p.x, p.y}, lanelet::utils::to2D(polygon))) { + ret.push_back(p); + } + } + } + return std::make_shared(ret); +} + +bool is_green_arrow( + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state, const uint8_t & lamp_shape) +{ + const auto it_lamp = + std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_shape](const auto & x) { + return x.shape == lamp_shape && + x.color == autoware_perception_msgs::msg::TrafficLightElement::GREEN; + }); + + return it_lamp != tl_state.elements.end(); +} +} // namespace autoware::behavior_velocity_planner + +#endif // UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..a359a3a64e416 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/test/test_node_interface.cpp @@ -0,0 +1,75 @@ +// Copyright 2024 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 + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "detection_area", "autoware::behavior_velocity_planner::DetectionAreaModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/test/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/test/test_utils.cpp new file mode 100644 index 0000000000000..9b4dc5193f655 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_lane_module/test/test_utils.cpp @@ -0,0 +1,183 @@ +// Copyright 2024 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 "../src/utils.hpp" + +#include +#include +#include +#include + +#include +#include + +TEST(TestUtils, getStopLine) +{ + using autoware::behavior_velocity_planner::detection_area::get_stop_line_geometry2d; + lanelet::LineString3d line; + line.push_back(lanelet::Point3d(lanelet::InvalId, 0.0, -1.0)); + line.push_back(lanelet::Point3d(lanelet::InvalId, 0.0, 1.0)); + lanelet::Polygons3d detection_areas; + lanelet::Polygon3d area; + area.push_back(lanelet::Point3d(lanelet::InvalId, 1.0, -1.0)); + area.push_back(lanelet::Point3d(lanelet::InvalId, 1.0, 1.0)); + area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, 1.0)); + area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, -1.0)); + detection_areas.push_back(area); + auto detection_area = + lanelet::autoware::DetectionArea::make(lanelet::InvalId, {}, detection_areas, line); + { + const double extend_length = 0.0; + const auto stop_line = get_stop_line_geometry2d(*detection_area, extend_length); + ASSERT_EQ(stop_line.size(), 2UL); + EXPECT_EQ(stop_line[0].x(), line[0].x()); + EXPECT_EQ(stop_line[0].y(), line[0].y()); + EXPECT_EQ(stop_line[1].x(), line[1].x()); + EXPECT_EQ(stop_line[1].y(), line[1].y()); + } + // extended line + for (auto extend_length = -2.0; extend_length < 2.0; extend_length += 0.1) { + const auto stop_line = get_stop_line_geometry2d(*detection_area, extend_length); + ASSERT_EQ(stop_line.size(), 2UL); + EXPECT_EQ(stop_line[0].x(), line[0].x()); + EXPECT_EQ(stop_line[0].y(), line[0].y() - extend_length); + EXPECT_EQ(stop_line[1].x(), line[1].x()); + EXPECT_EQ(stop_line[1].y(), line[1].y() + extend_length); + } +} + +TEST(TestUtils, getObstaclePoints) +{ + using autoware::behavior_velocity_planner::detection_area::get_obstacle_points; + lanelet::ConstPolygons3d detection_areas; + lanelet::Polygon3d area; + area.push_back(lanelet::Point3d(lanelet::InvalId, 1.0, -1.0)); + area.push_back(lanelet::Point3d(lanelet::InvalId, 1.0, 1.0)); + area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, 1.0)); + area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, -1.0)); + detection_areas.push_back(area); + pcl::PointCloud points; + // empty points + { + const auto obstacle_points = get_obstacle_points(detection_areas, points); + EXPECT_TRUE(obstacle_points.empty()); + } + // add points outside the detection area + points.emplace_back(0.0, 0.0, 0.0); + points.emplace_back(4.0, 4.0, 0.0); + { + const auto obstacle_points = get_obstacle_points(detection_areas, points); + EXPECT_TRUE(obstacle_points.empty()); + } + // add point on the edge of the detection area (will not be found) + points.emplace_back(1.0, 1.0, 0.0); + { + const auto obstacle_points = get_obstacle_points(detection_areas, points); + EXPECT_TRUE(obstacle_points.empty()); + } + // add point inside the detection area (will be found) + points.emplace_back(2.0, 0.0, 0.0); + { + const auto obstacle_points = get_obstacle_points(detection_areas, points); + ASSERT_EQ(obstacle_points.size(), 1UL); + EXPECT_EQ(obstacle_points[0].x, points[3].x); + EXPECT_EQ(obstacle_points[0].y, points[3].y); + } + // add a detection area that covers all points + lanelet::Polygon3d full_area; + full_area.push_back(lanelet::Point3d(lanelet::InvalId, -10.0, -10.0)); + full_area.push_back(lanelet::Point3d(lanelet::InvalId, -10.0, 10.0)); + full_area.push_back(lanelet::Point3d(lanelet::InvalId, 10.0, 10.0)); + full_area.push_back(lanelet::Point3d(lanelet::InvalId, 10.0, -10.0)); + detection_areas.push_back(full_area); + { + const auto obstacle_points = get_obstacle_points(detection_areas, points); + ASSERT_EQ(obstacle_points.size(), 2UL); // only the 1st point found for each area are returned + EXPECT_EQ(obstacle_points[0].x, points[3].x); + EXPECT_EQ(obstacle_points[0].y, points[3].y); + EXPECT_EQ(obstacle_points[1].x, points[0].x); + EXPECT_EQ(obstacle_points[1].y, points[0].y); + } +} + +TEST(TestUtils, canClearStopState) +{ + using autoware::behavior_velocity_planner::detection_area::can_clear_stop_state; + std::shared_ptr last_obstacle_found_time = nullptr; + // can clear if we never found an obstacle + for (auto now_s = 0; now_s <= 10; now_s += 1) { + for (auto now_ns = 0; now_ns <= 1e9; now_ns += 1e8) { + for (double state_clear_time = 0.0; state_clear_time <= 10.0; state_clear_time += 0.1) { + const auto can_clear = can_clear_stop_state( + last_obstacle_found_time, rclcpp::Time(now_s, now_ns, RCL_CLOCK_UNINITIALIZED), + state_clear_time); + EXPECT_TRUE(can_clear); + } + } + } + last_obstacle_found_time = std::make_shared(1.0, 0.0); + const auto state_clear_time = 1.0; + // special case for negative time difference which may occur with simulated time + EXPECT_TRUE(can_clear_stop_state(last_obstacle_found_time, rclcpp::Time(0, 0), state_clear_time)); + EXPECT_TRUE( + can_clear_stop_state(last_obstacle_found_time, rclcpp::Time(0, 9 * 1e8), state_clear_time)); + // cannot clear before the time has passed since the obstacle disappeared + EXPECT_FALSE( + can_clear_stop_state(last_obstacle_found_time, rclcpp::Time(1, 1), state_clear_time)); + EXPECT_FALSE( + can_clear_stop_state(last_obstacle_found_time, rclcpp::Time(1, 1e9 - 1), state_clear_time)); + // can clear after the time has passed + EXPECT_TRUE(can_clear_stop_state(last_obstacle_found_time, rclcpp::Time(2, 1), state_clear_time)); + EXPECT_TRUE( + can_clear_stop_state(last_obstacle_found_time, rclcpp::Time(100, 0), state_clear_time)); + // negative time + const auto negative_state_clear_time = -1.0; + EXPECT_TRUE( + can_clear_stop_state(last_obstacle_found_time, rclcpp::Time(0, 0), negative_state_clear_time)); + EXPECT_TRUE( + can_clear_stop_state(last_obstacle_found_time, rclcpp::Time(1, 0), negative_state_clear_time)); + EXPECT_TRUE( + can_clear_stop_state(last_obstacle_found_time, rclcpp::Time(2, 0), negative_state_clear_time)); +} + +TEST(TestUtils, hasEnoughBrakingDistance) +{ + using autoware::behavior_velocity_planner::detection_area::has_enough_braking_distance; + // prepare a stop pose 10m away from the self pose + geometry_msgs::msg::Pose self_pose; + self_pose.position.x = 0.0; + self_pose.position.y = 0.0; + geometry_msgs::msg::Pose line_pose; + line_pose.position.x = 10.0; + line_pose.position.y = 0.0; + // can always brake at zero velocity + for (auto pass_judge_line_distance = 0.0; pass_judge_line_distance <= 20.0; + pass_judge_line_distance += 0.1) { + double current_velocity = 0.0; + EXPECT_TRUE(has_enough_braking_distance( + self_pose, line_pose, pass_judge_line_distance, current_velocity)); + } + // if velocity is not zero, can brake if the pass judge line distance is lower than 10m + const double current_velocity = 5.0; + for (auto pass_judge_line_distance = 0.0; pass_judge_line_distance < 10.0; + pass_judge_line_distance += 0.1) { + EXPECT_TRUE(has_enough_braking_distance( + self_pose, line_pose, pass_judge_line_distance, current_velocity)); + } + for (auto pass_judge_line_distance = 10.0; pass_judge_line_distance <= 20.0; + pass_judge_line_distance += 0.1) { + EXPECT_FALSE(has_enough_braking_distance( + self_pose, line_pose, pass_judge_line_distance, current_velocity)); + } +}