From 65665ce2a55393fb564d934d478eeb944c12ff26 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Thu, 13 Jun 2024 17:04:06 +0900 Subject: [PATCH] refactor(dynamic_obstacle_stop): move to motion_velocity_planner (#7460) Signed-off-by: Maxime CLEMENT --- .github/CODEOWNERS | 2 +- .../behavior_planning.launch.xml | 7 - .../motion_planning.launch.xml | 13 +- planning/.pages | 2 +- .../behavior_velocity_planner.launch.xml | 2 - .../plugins.xml | 3 - .../src/manager.cpp | 76 ------ .../src/manager.hpp | 58 ----- .../src/scene_dynamic_obstacle_stop.cpp | 166 ------------- .../src/scene_dynamic_obstacle_stop.hpp | 62 ----- .../CMakeLists.txt | 4 +- .../README.md | 42 ++-- .../config/dynamic_obstacle_stop.param.yaml | 2 +- .../DynamicObstacleStop-Collision.drawio.svg | 0 .../DynamicObstacleStop-Filtering.drawio.svg | 0 ...amicObstacleStop-ImmediatePaths.drawio.svg | 0 .../DynamicObstacleStop-StopPoint.drawio.svg | 0 .../docs/dynamic_obstacle_stop_rviz.png | Bin .../package.xml | 4 +- .../plugins.xml | 3 + .../src/collision.cpp | 12 +- .../src/collision.hpp | 13 +- .../src/debug.cpp | 4 +- .../src/debug.hpp | 4 +- .../src/dynamic_obstacle_stop_module.cpp | 224 ++++++++++++++++++ .../src/dynamic_obstacle_stop_module.hpp | 58 +++++ .../src/footprint.cpp | 16 +- .../src/footprint.hpp | 8 +- .../src/object_filtering.cpp | 16 +- .../src/object_filtering.hpp | 6 +- .../src/object_stop_decision.cpp | 12 +- .../src/object_stop_decision.hpp | 12 +- .../src/types.hpp | 17 +- .../planner_data.hpp | 4 +- .../package.xml | 1 + .../test/src/test_node_interface.cpp | 4 +- 36 files changed, 385 insertions(+), 472 deletions(-) delete mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml delete mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp delete mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp delete mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp delete mode 100644 planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp rename planning/{behavior_velocity_dynamic_obstacle_stop_module => motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module}/CMakeLists.txt (56%) rename planning/{behavior_velocity_dynamic_obstacle_stop_module => motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module}/README.md (56%) rename planning/{behavior_velocity_dynamic_obstacle_stop_module => motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module}/config/dynamic_obstacle_stop.param.yaml (87%) rename planning/{behavior_velocity_dynamic_obstacle_stop_module => motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module}/docs/DynamicObstacleStop-Collision.drawio.svg (100%) rename planning/{behavior_velocity_dynamic_obstacle_stop_module => motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module}/docs/DynamicObstacleStop-Filtering.drawio.svg (100%) rename planning/{behavior_velocity_dynamic_obstacle_stop_module => motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module}/docs/DynamicObstacleStop-ImmediatePaths.drawio.svg (100%) rename planning/{behavior_velocity_dynamic_obstacle_stop_module => motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module}/docs/DynamicObstacleStop-StopPoint.drawio.svg (100%) rename planning/{behavior_velocity_dynamic_obstacle_stop_module => motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module}/docs/dynamic_obstacle_stop_rviz.png (100%) rename planning/{behavior_velocity_dynamic_obstacle_stop_module => motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module}/package.xml (90%) create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/plugins.xml rename planning/{behavior_velocity_dynamic_obstacle_stop_module => motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module}/src/collision.cpp (87%) rename planning/{behavior_velocity_dynamic_obstacle_stop_module => motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module}/src/collision.hpp (77%) rename planning/{behavior_velocity_dynamic_obstacle_stop_module => motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module}/src/debug.cpp (96%) rename planning/{behavior_velocity_dynamic_obstacle_stop_module => motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module}/src/debug.hpp (91%) create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp rename planning/{behavior_velocity_dynamic_obstacle_stop_module => motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module}/src/footprint.cpp (83%) rename planning/{behavior_velocity_dynamic_obstacle_stop_module => motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module}/src/footprint.hpp (90%) rename planning/{behavior_velocity_dynamic_obstacle_stop_module => motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module}/src/object_filtering.cpp (89%) rename planning/{behavior_velocity_dynamic_obstacle_stop_module => motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module}/src/object_filtering.hpp (85%) rename planning/{behavior_velocity_dynamic_obstacle_stop_module => motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module}/src/object_stop_decision.cpp (83%) rename planning/{behavior_velocity_dynamic_obstacle_stop_module => motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module}/src/object_stop_decision.hpp (87%) rename planning/{behavior_velocity_dynamic_obstacle_stop_module => motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module}/src/types.hpp (80%) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index c3c284b9256c4..52538dc9b1120 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -192,13 +192,13 @@ planning/autoware_behavior_path_side_shift_module/** fumiya.watanabe@tier4.jp ky planning/behavior_path_start_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/autoware_behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/autoware_behavior_velocity_crosswalk_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yuki.takagi@tier4.jp -planning/behavior_velocity_dynamic_obstacle_stop_module/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/autoware_behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/autoware_behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/autoware_behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/autoware_behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** maxime.clement@tier4.jp planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** maxime.clement@tier4.jp 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 9121be27825aa..3deff32d6e297 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 @@ -33,7 +33,6 @@ - @@ -168,11 +167,6 @@ value="$(eval "'$(var behavior_velocity_planner_launch_modules)' + 'autoware::behavior_velocity_planner::NoDrivableLaneModulePlugin, '")" if="$(var launch_no_drivable_lane_module)" /> - @@ -256,7 +250,6 @@ - diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index 94f641200868e..ae43c2d81efb6 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -2,9 +2,9 @@ + - @@ -19,11 +19,11 @@ value="$(eval "'$(var motion_velocity_planner_launch_modules)' + 'autoware::motion_velocity_planner::ObstacleVelocityLimiterModule, '")" if="$(var launch_obstacle_velocity_limiter_module)" /> - - - - - + @@ -140,6 +140,7 @@ + diff --git a/planning/.pages b/planning/.pages index 5fe671a3629cc..d072126b506e8 100644 --- a/planning/.pages +++ b/planning/.pages @@ -24,7 +24,6 @@ nav: - 'Blind Spot': planning/autoware_behavior_velocity_blind_spot_module - 'Crosswalk': planning/autoware_behavior_velocity_crosswalk_module - 'Detection Area': planning/behavior_velocity_detection_area_module - - 'Dynamic Obstacle Stop': planning/behavior_velocity_dynamic_obstacle_stop_module - 'Intersection': planning/autoware_behavior_velocity_intersection_module - 'No Drivable Lane': planning/behavior_velocity_no_drivable_lane_module - 'No Stopping Area': planning/autoware_behavior_velocity_no_stopping_area_module @@ -64,6 +63,7 @@ nav: - 'Motion Velocity Planner': - 'About Motion Velocity Planner': planning/motion_velocity_planner/autoware_motion_velocity_planner_node/ - 'Available Modules': + - 'Dynamic Obstacle Stop': planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/ - 'Out of Lane': planning/motion_velocity_planner/autoware_motion_velocity_planner_out_of_lane_module/ - 'Obstacle Velocity Limiter': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/ - 'Velocity Smoother': diff --git a/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index f52d9637c2134..3c4f4c3fdd64c 100644 --- a/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -25,7 +25,6 @@ - @@ -68,7 +67,6 @@ - diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml b/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml deleted file mode 100644 index ba2d2ba61ad50..0000000000000 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp deleted file mode 100644 index f594432c9e452..0000000000000 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp +++ /dev/null @@ -1,76 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "manager.hpp" - -#include "scene_dynamic_obstacle_stop.hpp" - -#include - -#include - -namespace autoware::behavior_velocity_planner -{ -using tier4_autoware_utils::getOrDeclareParameter; - -DynamicObstacleStopModuleManager::DynamicObstacleStopModuleManager(rclcpp::Node & node) -: SceneModuleManagerInterface(node, getModuleName()), module_id_(0UL) -{ - const std::string ns(getModuleName()); - auto & pp = planner_param_; - - pp.extra_object_width = getOrDeclareParameter(node, ns + ".extra_object_width"); - pp.minimum_object_velocity = getOrDeclareParameter(node, ns + ".minimum_object_velocity"); - pp.stop_distance_buffer = getOrDeclareParameter(node, ns + ".stop_distance_buffer"); - pp.time_horizon = getOrDeclareParameter(node, ns + ".time_horizon"); - pp.hysteresis = getOrDeclareParameter(node, ns + ".hysteresis"); - pp.add_duration_buffer = getOrDeclareParameter(node, ns + ".add_stop_duration_buffer"); - pp.remove_duration_buffer = - getOrDeclareParameter(node, ns + ".remove_stop_duration_buffer"); - pp.minimum_object_distance_from_ego_path = - getOrDeclareParameter(node, ns + ".minimum_object_distance_from_ego_path"); - pp.ignore_unavoidable_collisions = - getOrDeclareParameter(node, ns + ".ignore_unavoidable_collisions"); - - const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); - pp.ego_lateral_offset = - std::max(std::abs(vehicle_info.min_lateral_offset_m), vehicle_info.max_lateral_offset_m); - pp.ego_longitudinal_offset = vehicle_info.max_longitudinal_offset_m; -} - -void DynamicObstacleStopModuleManager::launchNewModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) -{ - if (path.points.empty()) return; - // general - if (!isModuleRegistered(module_id_)) { - registerModule(std::make_shared( - module_id_, planner_param_, logger_.get_child("dynamic_obstacle_stop_module"), clock_)); - } -} - -std::function &)> -DynamicObstacleStopModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) -{ - return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { - return false; - }; -} -} // namespace autoware::behavior_velocity_planner - -#include -PLUGINLIB_EXPORT_CLASS( - autoware::behavior_velocity_planner::DynamicObstacleStopModulePlugin, - autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp deleted file mode 100644 index d5c403e26792c..0000000000000 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MANAGER_HPP_ -#define MANAGER_HPP_ - -#include "scene_dynamic_obstacle_stop.hpp" - -#include -#include -#include -#include - -#include - -#include -#include -#include - -namespace autoware::behavior_velocity_planner -{ -class DynamicObstacleStopModuleManager : public SceneModuleManagerInterface -{ -public: - explicit DynamicObstacleStopModuleManager(rclcpp::Node & node); - - const char * getModuleName() override { return "dynamic_obstacle_stop"; } - -private: - using PlannerParam = dynamic_obstacle_stop::PlannerParam; - - PlannerParam planner_param_; - int64_t module_id_; - - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - - std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; -}; - -class DynamicObstacleStopModulePlugin : public PluginWrapper -{ -}; - -} // namespace autoware::behavior_velocity_planner - -#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp deleted file mode 100644 index 9c5eced187aa8..0000000000000 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp +++ /dev/null @@ -1,166 +0,0 @@ -// Copyright 2023-2024 TIER IV, Inc. All rights reserved. -// -// 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_dynamic_obstacle_stop.hpp" - -#include "collision.hpp" -#include "debug.hpp" -#include "footprint.hpp" -#include "object_filtering.hpp" -#include "object_stop_decision.hpp" -#include "types.hpp" - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop -{ - -using visualization_msgs::msg::Marker; -using visualization_msgs::msg::MarkerArray; - -DynamicObstacleStopModule::DynamicObstacleStopModule( - const int64_t module_id, PlannerParam planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), params_(std::move(planner_param)) -{ - velocity_factor_.init(PlanningBehavior::ROUTE_OBSTACLE); -} - -bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) -{ - debug_data_.reset_data(); - *stop_reason = planning_utils::initializeStopReason(StopReason::OBSTACLE_STOP); - if (!path || path->points.size() < 2) return true; - - tier4_autoware_utils::StopWatch stopwatch; - stopwatch.tic(); - stopwatch.tic("preprocessing"); - EgoData ego_data; - ego_data.pose = planner_data_->current_odometry->pose; - ego_data.path.points = path->points; - motion_utils::removeOverlapPoints(ego_data.path.points); - ego_data.first_path_idx = - motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position); - ego_data.longitudinal_offset_to_first_path_idx = motion_utils::calcLongitudinalOffsetToSegment( - ego_data.path.points, ego_data.first_path_idx, ego_data.pose.position); - const auto min_stop_distance = - motion_utils::calcDecelDistWithJerkAndAccConstraints( - planner_data_->current_velocity->twist.linear.x, 0.0, - planner_data_->current_acceleration->accel.accel.linear.x, - planner_data_->max_stop_acceleration_threshold, -planner_data_->max_stop_jerk_threshold, - planner_data_->max_stop_jerk_threshold) - .value_or(0.0); - ego_data.earliest_stop_pose = motion_utils::calcLongitudinalOffsetPose( - ego_data.path.points, ego_data.pose.position, min_stop_distance); - - make_ego_footprint_rtree(ego_data, params_); - double hysteresis = - std::find_if( - object_map_.begin(), object_map_.end(), - [](const auto & pair) { return pair.second.should_be_avoided(); }) == object_map_.end() - ? 0.0 - : params_.hysteresis; - const auto dynamic_obstacles = - filter_predicted_objects(*planner_data_->predicted_objects, ego_data, params_, hysteresis); - - const auto preprocessing_duration_us = stopwatch.toc("preprocessing"); - - stopwatch.tic("footprints"); - const auto obstacle_forward_footprints = - make_forward_footprints(dynamic_obstacles, params_, hysteresis); - const auto footprints_duration_us = stopwatch.toc("footprints"); - stopwatch.tic("collisions"); - auto collisions = find_collisions(ego_data, dynamic_obstacles, obstacle_forward_footprints); - update_object_map(object_map_, collisions, clock_->now(), ego_data.path.points, params_); - std::optional earliest_collision = - find_earliest_collision(object_map_, ego_data); - const auto collisions_duration_us = stopwatch.toc("collisions"); - if (earliest_collision) { - const auto arc_length_diff = motion_utils::calcSignedArcLength( - ego_data.path.points, *earliest_collision, ego_data.pose.position); - const auto can_stop_before_limit = arc_length_diff < min_stop_distance - - params_.ego_longitudinal_offset - - params_.stop_distance_buffer; - const auto stop_pose = can_stop_before_limit - ? motion_utils::calcLongitudinalOffsetPose( - ego_data.path.points, *earliest_collision, - -params_.stop_distance_buffer - params_.ego_longitudinal_offset) - : ego_data.earliest_stop_pose; - debug_data_.stop_pose = stop_pose; - if (stop_pose) { - motion_utils::insertStopPoint(*stop_pose, 0.0, path->points); - const auto stop_pose_reached = - planner_data_->current_velocity->twist.linear.x < 1e-3 && - tier4_autoware_utils::calcDistance2d(ego_data.pose, *stop_pose) < 1e-3; - velocity_factor_.set( - path->points, ego_data.pose, *stop_pose, - stop_pose_reached ? VelocityFactor::STOPPED : VelocityFactor::APPROACHING, - "dynamic_obstacle_stop"); - } - } - - const auto total_time_us = stopwatch.toc(); - RCLCPP_DEBUG( - logger_, - "Total time = %2.2fus\n\tpreprocessing = %2.2fus\n\tfootprints = " - "%2.2fus\n\tcollisions = %2.2fus\n", - total_time_us, preprocessing_duration_us, footprints_duration_us, collisions_duration_us); - debug_data_.ego_footprints = ego_data.path_footprints; - debug_data_.obstacle_footprints = obstacle_forward_footprints; - debug_data_.z = ego_data.pose.position.z; - return true; -} - -MarkerArray DynamicObstacleStopModule::createDebugMarkerArray() -{ - const auto z = debug_data_.z; - MarkerArray array; - std::string ns = "collisions"; - const auto collision_markers = debug::make_collision_markers(object_map_, ns, z, clock_->now()); - debug::add_markers(array, debug_data_.prev_collisions_nb, collision_markers, ns); - ns = "dynamic_obstacles_footprints"; - const auto obstacle_footprint_markers = - debug::make_polygon_markers(debug_data_.obstacle_footprints, ns, z); - debug::add_markers(array, debug_data_.prev_dynamic_obstacles_nb, obstacle_footprint_markers, ns); - ns = "ego_footprints"; - const auto ego_footprint_markers = debug::make_polygon_markers(debug_data_.ego_footprints, ns, z); - debug::add_markers(array, debug_data_.prev_ego_footprints_nb, ego_footprint_markers, ns); - return array; -} - -motion_utils::VirtualWalls DynamicObstacleStopModule::createVirtualWalls() -{ - motion_utils::VirtualWalls virtual_walls{}; - if (debug_data_.stop_pose) { - motion_utils::VirtualWall virtual_wall; - virtual_wall.text = "dynamic_obstacle_stop"; - virtual_wall.longitudinal_offset = params_.ego_longitudinal_offset; - virtual_wall.style = motion_utils::VirtualWallType::stop; - virtual_wall.pose = *debug_data_.stop_pose; - virtual_walls.push_back(virtual_wall); - } - return virtual_walls; -} - -} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp deleted file mode 100644 index 8018412ba2b4b..0000000000000 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp +++ /dev/null @@ -1,62 +0,0 @@ -// Copyright 2023-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. - -#ifndef SCENE_DYNAMIC_OBSTACLE_STOP_HPP_ -#define SCENE_DYNAMIC_OBSTACLE_STOP_HPP_ - -#include "object_stop_decision.hpp" -#include "types.hpp" - -#include -#include -#include - -#include -#include - -#include -#include -#include -#include - -namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop -{ -class DynamicObstacleStopModule : public SceneModuleInterface -{ -public: - DynamicObstacleStopModule( - const int64_t module_id, PlannerParam planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); - - /// @brief insert stop or slow down points to prevent dangerously entering another lane - /// @param [inout] path the path to update - /// @param [inout] stop_reason reason for stopping - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; - - visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; - -private: - PlannerParam params_; - ObjectStopDecisionMap object_map_; - -protected: - int64_t module_id_{}; - - // Debug - mutable DebugData debug_data_; -}; -} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop - -#endif // SCENE_DYNAMIC_OBSTACLE_STOP_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/CMakeLists.txt similarity index 56% rename from planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt rename to planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/CMakeLists.txt index 481ac245cb432..30d147de3d786 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_dynamic_obstacle_stop_module) +project(autoware_motion_velocity_dynamic_obstacle_stop_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED DIRECTORY src diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/README.md similarity index 56% rename from planning/behavior_velocity_dynamic_obstacle_stop_module/README.md rename to planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/README.md index ee44ac08afe96..805e5f27d4129 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/README.md +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/README.md @@ -10,11 +10,11 @@ The _immediate_ path of an object is the area that the object would traverse dur ### Activation Timing -This module is activated if the launch parameter `launch_dynamic_obstacle_stop_module` is set to true in the behavior planning launch file. +This module is activated if the launch parameter `launch_dynamic_obstacle_stop_module` is set to true in the motion planning launch file. ### Inner-workings / Algorithms -The module insert a stop point where the ego path collides with the immediate path of an object. +The module insert a stop point where the ego trajectory collides with the immediate path of an object. The overall module flow can be summarized with the following 4 steps. 1. Filter dynamic objects. @@ -25,7 +25,7 @@ The overall module flow can be summarized with the following 4 steps. In addition to these 4 steps, 2 mechanisms are in place to make the stop point of this module more stable: an hysteresis and a decision duration buffer. The `hysteresis` parameter is used when a stop point was already being inserted in the previous iteration -and it increases the range where dynamic objects are considered close enough to the ego path to be used by the module. +and it increases the range where dynamic objects are considered close enough to the ego trajectory to be used by the module. #### Filter dynamic objects @@ -37,12 +37,12 @@ An object is considered by the module only if it meets all of the following cond - it is moving at a velocity higher than defined by the `minimum_object_velocity` parameter; - it is not too close to the current position of the ego vehicle; - it is not unavoidable (only if parameter `ignore_unavoidable_collisions` is set to `true`); -- it is close to the ego path. +- it is close to the ego trajectory. An object is considered unavoidable if it is heading towards the ego vehicle such that even if ego stops, a collision would still occur (assuming the object keeps driving in a straight line). For the last condition, -the object is considered close enough if its lateral distance from the ego path is less than the threshold parameter `minimum_object_distance_from_ego_path` plus half the width of ego and of the object (including the `extra_object_width` parameter). +the object is considered close enough if its lateral distance from the ego trajectory is less than the threshold parameter `minimum_object_distance_from_ego_trajectory` plus half the width of ego and of the object (including the `extra_object_width` parameter). In addition, the value of the `hysteresis` parameter is added to the minimum distance if a stop point was inserted in the previous iteration. #### Calculate immediate path rectangles @@ -57,17 +57,17 @@ and its length is the current speed of the object multiplied by the `time_horizo ![Earliest collision example](docs/DynamicObstacleStop-Collision.drawio.svg) -We build the ego path footprints as the set of ego footprint polygons projected on each path point. -We then calculate the intersections between these ego path footprints and the previously calculated immediate path rectangles. -An intersection is ignored if the object is not driving toward ego, i.e., the absolute angle between the object and the path point is larger than $\frac{3 \pi}{4}$. +We build the ego trajectory footprints as the set of ego footprint polygons projected on each trajectory point. +We then calculate the intersections between these ego trajectory footprints and the previously calculated immediate path rectangles. +An intersection is ignored if the object is not driving toward ego, i.e., the absolute angle between the object and the trajectory point is larger than $\frac{3 \pi}{4}$. -The collision point with the lowest arc length when projected on the ego path will be used to calculate the final stop point. +The collision point with the lowest arc length when projected on the ego trajectory will be used to calculate the final stop point. #### Insert stop point ![Stop point example](docs/DynamicObstacleStop-StopPoint.drawio.svg) -Before inserting a stop point, we calculate the range of path arc lengths where it can be inserted. +Before inserting a stop point, we calculate the range of trajectory arc lengths where it can be inserted. The minimum is calculated to satisfy the acceleration and jerk constraints of the vehicle. If a stop point was inserted in the previous iteration of the module, its arc length is used as the maximum. Finally, @@ -84,14 +84,14 @@ Timers and collision points are tracked for each dynamic object independently. ### Module Parameters -| Parameter | Type | Description | -| --------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------ | -| `extra_object_width` | double | [m] extra width around detected objects | -| `minimum_object_velocity` | double | [m/s] objects with a velocity bellow this value are ignored | -| `stop_distance_buffer` | double | [m] extra distance to add between the stop point and the collision point | -| `time_horizon` | double | [s] time horizon used for collision checks | -| `hysteresis` | double | [m] once a collision has been detected, this hysteresis is used on the collision detection | -| `add_stop_duration_buffer` | double | [s] duration where a collision must be continuously detected before a stop decision is added | -| `remove_stop_duration_buffer` | double | [s] duration between no collision being detected and the stop decision being remove | -| `minimum_object_distance_from_ego_path` | double | [m] minimum distance between the footprints of ego and an object to consider for collision | -| `ignore_unavoidable_collisions` | bool | [-] if true, ignore collisions that cannot be avoided by stopping (assuming the obstacle continues going straight) | +| Parameter | Type | Description | +| --------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------ | +| `extra_object_width` | double | [m] extra width around detected objects | +| `minimum_object_velocity` | double | [m/s] objects with a velocity bellow this value are ignored | +| `stop_distance_buffer` | double | [m] extra distance to add between the stop point and the collision point | +| `time_horizon` | double | [s] time horizon used for collision checks | +| `hysteresis` | double | [m] once a collision has been detected, this hysteresis is used on the collision detection | +| `add_stop_duration_buffer` | double | [s] duration where a collision must be continuously detected before a stop decision is added | +| `remove_stop_duration_buffer` | double | [s] duration between no collision being detected and the stop decision being remove | +| `minimum_object_distance_from_ego_trajectory` | double | [m] minimum distance between the footprints of ego and an object to consider for collision | +| `ignore_unavoidable_collisions` | bool | [-] if true, ignore collisions that cannot be avoided by stopping (assuming the obstacle continues going straight) | diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml similarity index 87% rename from planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml rename to planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml index ac95cd75c8571..10d749e57a9c7 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml @@ -8,5 +8,5 @@ hysteresis: 1.0 # [m] once a collision has been detected, this hysteresis is used on the collision detection add_stop_duration_buffer : 0.5 # [s] duration where a collision must be continuously detected before a stop decision is added remove_stop_duration_buffer : 1.0 # [s] duration between no collision being detected and the stop decision being remove - minimum_object_distance_from_ego_path: 1.0 # [m] minimum distance between the footprints of ego and an object to consider for collision + minimum_object_distance_from_ego_trajectory: 1.0 # [m] minimum distance between the footprints of ego and an object to consider for collision ignore_unavoidable_collisions : true # if true, ignore collisions that cannot be avoided by stopping (assuming the obstacle continues going straight) diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Collision.drawio.svg b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Collision.drawio.svg similarity index 100% rename from planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Collision.drawio.svg rename to planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Collision.drawio.svg diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Filtering.drawio.svg b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Filtering.drawio.svg similarity index 100% rename from planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Filtering.drawio.svg rename to planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Filtering.drawio.svg diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-ImmediatePaths.drawio.svg b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-ImmediatePaths.drawio.svg similarity index 100% rename from planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-ImmediatePaths.drawio.svg rename to planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-ImmediatePaths.drawio.svg diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-StopPoint.drawio.svg b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-StopPoint.drawio.svg similarity index 100% rename from planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-StopPoint.drawio.svg rename to planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-StopPoint.drawio.svg diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/dynamic_obstacle_stop_rviz.png b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/docs/dynamic_obstacle_stop_rviz.png similarity index 100% rename from planning/behavior_velocity_dynamic_obstacle_stop_module/docs/dynamic_obstacle_stop_rviz.png rename to planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/docs/dynamic_obstacle_stop_rviz.png diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml similarity index 90% rename from planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml rename to planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml index bdf842b4fa55f..c0eca8028cb95 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml @@ -1,7 +1,7 @@ - behavior_velocity_dynamic_obstacle_stop_module + autoware_motion_velocity_dynamic_obstacle_stop_module 0.1.0 dynamic_obstacle_stop module to stop ego when in the immediate path of a dynamic object @@ -12,7 +12,7 @@ ament_cmake_auto autoware_cmake - autoware_behavior_velocity_planner_common + autoware_motion_velocity_planner_common autoware_perception_msgs autoware_planning_msgs autoware_route_handler diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/plugins.xml b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/plugins.xml new file mode 100644 index 0000000000000..e82dc688f1f34 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.cpp similarity index 87% rename from planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp rename to planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.cpp index 10843fd3aef2b..c79c6c32802a3 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.cpp @@ -24,7 +24,7 @@ #include #include -namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::motion_velocity_planner::dynamic_obstacle_stop { std::optional find_closest_collision_point( @@ -37,9 +37,9 @@ std::optional find_closest_collision_point( ego_data.rtree.query( boost::geometry::index::intersects(object_footprint), std::back_inserter(rough_collisions)); for (const auto & rough_collision : rough_collisions) { - const auto path_idx = rough_collision.second; - const auto & ego_footprint = ego_data.path_footprints[path_idx]; - const auto & ego_pose = ego_data.path.points[path_idx].point.pose; + const auto traj_idx = rough_collision.second; + const auto & ego_footprint = ego_data.trajectory_footprints[traj_idx]; + const auto & ego_pose = ego_data.trajectory[traj_idx].pose; const auto angle_diff = tier4_autoware_utils::normalizeRadian( tf2::getYaw(ego_pose.orientation) - tf2::getYaw(object_pose.orientation)); if (std::abs(angle_diff) > (M_PI_2 + M_PI_4)) { // TODO(Maxime): make this angle a parameter @@ -49,7 +49,7 @@ std::optional find_closest_collision_point( for (const auto & coll_p : collision_points) { auto p = geometry_msgs::msg::Point().set__x(coll_p.x()).set__y(coll_p.y()); const auto dist_to_ego = - motion_utils::calcSignedArcLength(ego_data.path.points, ego_data.pose.position, p); + motion_utils::calcSignedArcLength(ego_data.trajectory, ego_data.pose.position, p); if (dist_to_ego < closest_dist) { closest_dist = dist_to_ego; closest_collision_point = p; @@ -80,4 +80,4 @@ std::vector find_collisions( return collisions; } -} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.hpp similarity index 77% rename from planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp rename to planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.hpp index 8431221a27d6b..6b995d715c190 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.hpp @@ -20,10 +20,11 @@ #include #include -namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::motion_velocity_planner::dynamic_obstacle_stop { /// @brief find the collision point closest to ego along an object footprint -/// @param [in] ego_data ego data including its path and footprints used for finding a collision +/// @param [in] ego_data ego data including its trajectory and footprints used for finding a +/// collision /// @param [in] object_pose pose of the dynamic object /// @param [in] object_footprint footprint of the object used for finding a collision /// @return the collision point closest to ego (if any) @@ -31,16 +32,16 @@ std::optional find_closest_collision_point( const EgoData & ego_data, const geometry_msgs::msg::Pose & object_pose, const tier4_autoware_utils::Polygon2d & object_footprint); -/// @brief find the earliest collision along the ego path -/// @param [in] ego_data ego data including its path and footprint +/// @brief find the earliest collision along the ego trajectory +/// @param [in] ego_data ego data including its trajectory and footprint /// @param [in] objects obstacles /// @param [in] obstacle_forward_footprints obstacle footprints to check for collisions -/// @return the point of earliest collision along the ego path +/// @return the point of earliest collision along the ego trajectory std::vector find_collisions( const EgoData & ego_data, const std::vector & objects, const tier4_autoware_utils::MultiPolygon2d & obstacle_forward_footprints); -} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop #endif // COLLISION_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.cpp similarity index 96% rename from planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp rename to planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.cpp index 5a457d651cd9e..bfca43edaeac9 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.cpp @@ -23,7 +23,7 @@ #include #include -namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop::debug +namespace autoware::motion_velocity_planner::dynamic_obstacle_stop::debug { std::vector make_delete_markers( @@ -110,4 +110,4 @@ std::vector make_polygon_markers( } return markers; } -} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop::debug +} // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop::debug diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.hpp similarity index 91% rename from planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp rename to planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.hpp index c567948927089..c708f0c86a983 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/debug.hpp @@ -28,7 +28,7 @@ #include #include -namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop::debug +namespace autoware::motion_velocity_planner::dynamic_obstacle_stop::debug { std::vector make_delete_markers( const size_t from, const size_t to, const std::string & ns); @@ -40,6 +40,6 @@ std::vector make_collision_markers( const rclcpp::Time & now); std::vector make_polygon_markers( const tier4_autoware_utils::MultiPolygon2d & footprints, const std::string & ns, const double z); -} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop::debug +} // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop::debug #endif // DEBUG_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp new file mode 100644 index 0000000000000..4d4b9b32a0f32 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp @@ -0,0 +1,224 @@ +// Copyright 2023-2024 TIER IV, Inc. All rights reserved. +// +// 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 "dynamic_obstacle_stop_module.hpp" + +#include "collision.hpp" +#include "debug.hpp" +#include "footprint.hpp" +#include "object_filtering.hpp" +#include "object_stop_decision.hpp" +#include "types.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ + +void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & module_name) +{ + module_name_ = module_name; + logger_ = node.get_logger().get_child(ns_); + clock_ = node.get_clock(); + velocity_factor_interface_.init(motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + + debug_publisher_ = + node.create_publisher("~/" + ns_ + "/debug_markers", 1); + virtual_wall_publisher_ = + node.create_publisher("~/" + ns_ + "/virtual_walls", 1); + + using tier4_autoware_utils::getOrDeclareParameter; + auto & p = params_; + p.extra_object_width = getOrDeclareParameter(node, ns_ + ".extra_object_width"); + p.minimum_object_velocity = getOrDeclareParameter(node, ns_ + ".minimum_object_velocity"); + p.stop_distance_buffer = getOrDeclareParameter(node, ns_ + ".stop_distance_buffer"); + p.time_horizon = getOrDeclareParameter(node, ns_ + ".time_horizon"); + p.hysteresis = getOrDeclareParameter(node, ns_ + ".hysteresis"); + p.add_duration_buffer = getOrDeclareParameter(node, ns_ + ".add_stop_duration_buffer"); + p.remove_duration_buffer = + getOrDeclareParameter(node, ns_ + ".remove_stop_duration_buffer"); + p.minimum_object_distance_from_ego_trajectory = + getOrDeclareParameter(node, ns_ + ".minimum_object_distance_from_ego_trajectory"); + p.ignore_unavoidable_collisions = + getOrDeclareParameter(node, ns_ + ".ignore_unavoidable_collisions"); + + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); + p.ego_lateral_offset = + std::max(std::abs(vehicle_info.min_lateral_offset_m), vehicle_info.max_lateral_offset_m); + p.ego_longitudinal_offset = vehicle_info.max_longitudinal_offset_m; +} + +void DynamicObstacleStopModule::update_parameters(const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + auto & p = params_; + updateParam(parameters, ns_ + ".extra_object_width", p.extra_object_width); + updateParam(parameters, ns_ + ".minimum_object_velocity", p.minimum_object_velocity); + updateParam(parameters, ns_ + ".stop_distance_buffer", p.stop_distance_buffer); + updateParam(parameters, ns_ + ".time_horizon", p.time_horizon); + updateParam(parameters, ns_ + ".hysteresis", p.hysteresis); + updateParam(parameters, ns_ + ".add_stop_duration_buffer", p.add_duration_buffer); + updateParam(parameters, ns_ + ".remove_stop_duration_buffer", p.remove_duration_buffer); + updateParam( + parameters, ns_ + ".minimum_object_distance_from_ego_trajectory", + p.minimum_object_distance_from_ego_trajectory); + updateParam(parameters, ns_ + ".ignore_unavoidable_collisions", p.ignore_unavoidable_collisions); +} + +VelocityPlanningResult DynamicObstacleStopModule::plan( + const std::vector & ego_trajectory_points, + const std::shared_ptr planner_data) +{ + VelocityPlanningResult result; + debug_data_.reset_data(); + if (ego_trajectory_points.size() < 2) return result; + + tier4_autoware_utils::StopWatch stopwatch; + stopwatch.tic(); + stopwatch.tic("preprocessing"); + dynamic_obstacle_stop::EgoData ego_data; + ego_data.pose = planner_data->current_odometry.pose.pose; + ego_data.trajectory = ego_trajectory_points; + motion_utils::removeOverlapPoints(ego_data.trajectory); + ego_data.first_trajectory_idx = + motion_utils::findNearestSegmentIndex(ego_data.trajectory, ego_data.pose.position); + ego_data.longitudinal_offset_to_first_trajectory_idx = + motion_utils::calcLongitudinalOffsetToSegment( + ego_data.trajectory, ego_data.first_trajectory_idx, ego_data.pose.position); + const auto min_stop_distance = motion_utils::calcDecelDistWithJerkAndAccConstraints( + planner_data->current_odometry.twist.twist.linear.x, 0.0, + planner_data->current_acceleration.accel.accel.linear.x, + planner_data->velocity_smoother_->getMinDecel(), + planner_data->velocity_smoother_->getMaxJerk(), + planner_data->velocity_smoother_->getMinJerk()) + .value_or(0.0); + ego_data.earliest_stop_pose = motion_utils::calcLongitudinalOffsetPose( + ego_data.trajectory, ego_data.pose.position, min_stop_distance); + + dynamic_obstacle_stop::make_ego_footprint_rtree(ego_data, params_); + double hysteresis = + std::find_if( + object_map_.begin(), object_map_.end(), + [](const auto & pair) { return pair.second.should_be_avoided(); }) == object_map_.end() + ? 0.0 + : params_.hysteresis; + const auto dynamic_obstacles = dynamic_obstacle_stop::filter_predicted_objects( + planner_data->predicted_objects, ego_data, params_, hysteresis); + + const auto preprocessing_duration_us = stopwatch.toc("preprocessing"); + + stopwatch.tic("footprints"); + const auto obstacle_forward_footprints = + dynamic_obstacle_stop::make_forward_footprints(dynamic_obstacles, params_, hysteresis); + const auto footprints_duration_us = stopwatch.toc("footprints"); + stopwatch.tic("collisions"); + auto collisions = dynamic_obstacle_stop::find_collisions( + ego_data, dynamic_obstacles, obstacle_forward_footprints); + update_object_map(object_map_, collisions, clock_->now(), ego_data.trajectory, params_); + std::optional earliest_collision = + dynamic_obstacle_stop::find_earliest_collision(object_map_, ego_data); + const auto collisions_duration_us = stopwatch.toc("collisions"); + if (earliest_collision) { + const auto arc_length_diff = motion_utils::calcSignedArcLength( + ego_data.trajectory, *earliest_collision, ego_data.pose.position); + const auto can_stop_before_limit = arc_length_diff < min_stop_distance - + params_.ego_longitudinal_offset - + params_.stop_distance_buffer; + const auto stop_pose = can_stop_before_limit + ? motion_utils::calcLongitudinalOffsetPose( + ego_data.trajectory, *earliest_collision, + -params_.stop_distance_buffer - params_.ego_longitudinal_offset) + : ego_data.earliest_stop_pose; + debug_data_.stop_pose = stop_pose; + if (stop_pose) { + result.stop_points.push_back(stop_pose->position); + const auto stop_pose_reached = + planner_data->current_odometry.twist.twist.linear.x < 1e-3 && + tier4_autoware_utils::calcDistance2d(ego_data.pose, *stop_pose) < 1e-3; + velocity_factor_interface_.set( + ego_trajectory_points, ego_data.pose, *stop_pose, + stop_pose_reached ? motion_utils::VelocityFactor::STOPPED + : motion_utils::VelocityFactor::APPROACHING, + "dynamic_obstacle_stop"); + result.velocity_factor = velocity_factor_interface_.get(); + create_virtual_walls(); + } + } + + debug_publisher_->publish(create_debug_marker_array()); + virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers()); + + const auto total_time_us = stopwatch.toc(); + RCLCPP_DEBUG( + logger_, + "Total time = %2.2fus\n\tpreprocessing = %2.2fus\n\tfootprints = " + "%2.2fus\n\tcollisions = %2.2fus\n", + total_time_us, preprocessing_duration_us, footprints_duration_us, collisions_duration_us); + debug_data_.ego_footprints = ego_data.trajectory_footprints; + debug_data_.obstacle_footprints = obstacle_forward_footprints; + debug_data_.z = ego_data.pose.position.z; + return result; +} + +visualization_msgs::msg::MarkerArray DynamicObstacleStopModule::create_debug_marker_array() +{ + const auto z = debug_data_.z; + visualization_msgs::msg::MarkerArray array; + std::string ns = "collisions"; + const auto collision_markers = + dynamic_obstacle_stop::debug::make_collision_markers(object_map_, ns, z, clock_->now()); + dynamic_obstacle_stop::debug::add_markers( + array, debug_data_.prev_collisions_nb, collision_markers, ns); + ns = "dynamic_obstacles_footprints"; + const auto obstacle_footprint_markers = + dynamic_obstacle_stop::debug::make_polygon_markers(debug_data_.obstacle_footprints, ns, z); + dynamic_obstacle_stop::debug::add_markers( + array, debug_data_.prev_dynamic_obstacles_nb, obstacle_footprint_markers, ns); + ns = "ego_footprints"; + const auto ego_footprint_markers = + dynamic_obstacle_stop::debug::make_polygon_markers(debug_data_.ego_footprints, ns, z); + dynamic_obstacle_stop::debug::add_markers( + array, debug_data_.prev_ego_footprints_nb, ego_footprint_markers, ns); + return array; +} + +void DynamicObstacleStopModule::create_virtual_walls() +{ + if (debug_data_.stop_pose) { + motion_utils::VirtualWall virtual_wall; + virtual_wall.text = "dynamic_obstacle_stop"; + virtual_wall.longitudinal_offset = params_.ego_longitudinal_offset; + virtual_wall.style = motion_utils::VirtualWallType::stop; + virtual_wall.pose = *debug_data_.stop_pose; + virtual_wall_marker_creator.add_virtual_wall(virtual_wall); + } +} + +} // namespace autoware::motion_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::motion_velocity_planner::DynamicObstacleStopModule, + autoware::motion_velocity_planner::PluginModuleInterface) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp new file mode 100644 index 0000000000000..eb707f8fe0b79 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp @@ -0,0 +1,58 @@ +// Copyright 2023-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. + +#ifndef DYNAMIC_OBSTACLE_STOP_MODULE_HPP_ +#define DYNAMIC_OBSTACLE_STOP_MODULE_HPP_ + +#include "object_stop_decision.hpp" +#include "types.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class DynamicObstacleStopModule : public PluginModuleInterface +{ +public: + void init(rclcpp::Node & node, const std::string & module_name) override; + void update_parameters(const std::vector & parameters) override; + VelocityPlanningResult plan( + const std::vector & ego_trajectory_points, + const std::shared_ptr planner_data) override; + std::string get_module_name() const override { return module_name_; } + +private: + visualization_msgs::msg::MarkerArray create_debug_marker_array(); + void create_virtual_walls(); + + inline static const std::string ns_ = "dynamic_obstacle_stop"; + std::string module_name_; + rclcpp::Clock::SharedPtr clock_{}; + + dynamic_obstacle_stop::PlannerParam params_; + dynamic_obstacle_stop::ObjectStopDecisionMap object_map_; + + // Debug + mutable dynamic_obstacle_stop::DebugData debug_data_; +}; +} // namespace autoware::motion_velocity_planner + +#endif // DYNAMIC_OBSTACLE_STOP_MODULE_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp similarity index 83% rename from planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp rename to planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp index 5f005348c4cea..d5b0ad3d117d2 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp @@ -26,7 +26,7 @@ #include #include -namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::motion_velocity_planner::dynamic_obstacle_stop { tier4_autoware_utils::MultiPolygon2d make_forward_footprints( const std::vector & obstacles, @@ -71,14 +71,14 @@ tier4_autoware_utils::Polygon2d project_to_pose( void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params) { - for (const auto & p : ego_data.path.points) - ego_data.path_footprints.push_back(tier4_autoware_utils::toFootprint( - p.point.pose, params.ego_longitudinal_offset, 0.0, params.ego_lateral_offset * 2.0)); - for (auto i = 0UL; i < ego_data.path_footprints.size(); ++i) { - const auto box = - boost::geometry::return_envelope(ego_data.path_footprints[i]); + for (const auto & p : ego_data.trajectory) + ego_data.trajectory_footprints.push_back(tier4_autoware_utils::toFootprint( + p.pose, params.ego_longitudinal_offset, 0.0, params.ego_lateral_offset * 2.0)); + for (auto i = 0UL; i < ego_data.trajectory_footprints.size(); ++i) { + const auto box = boost::geometry::return_envelope( + ego_data.trajectory_footprints[i]); ego_data.rtree.insert(std::make_pair(box, i)); } } -} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.hpp similarity index 90% rename from planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp rename to planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.hpp index c75d7bb9cc49e..8ceb19d679ea0 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.hpp @@ -21,7 +21,7 @@ #include -namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::motion_velocity_planner::dynamic_obstacle_stop { /// @brief create the footprint of the given obstacles and their projection over a fixed time /// horizon @@ -46,10 +46,10 @@ tier4_autoware_utils::Polygon2d make_forward_footprint( /// @return footprint projected to the given pose tier4_autoware_utils::Polygon2d project_to_pose( const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose); -/// @brief create the rtree indexing the ego footprint along the path -/// @param [inout] ego_data ego data with its path and the rtree to populate +/// @brief create the rtree indexing the ego footprint along the trajectory +/// @param [inout] ego_data ego data with its trajectory and the rtree to populate /// @param [in] params parameters void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params); -} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop #endif // FOOTPRINT_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp similarity index 89% rename from planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp rename to planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp index b851e1a8fb35b..0bb3c90ad262b 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp @@ -21,12 +21,12 @@ #include #include -namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::motion_velocity_planner::dynamic_obstacle_stop { /// @brief filter the given predicted objects /// @param objects predicted objects -/// @param ego_data ego data, including its path and pose +/// @param ego_data ego data, including its trajectory and pose /// @param params parameters /// @param hysteresis [m] extra distance threshold used for filtering /// @return filtered predicted objects @@ -47,15 +47,15 @@ std::vector filter_predicted_obj }; const auto is_in_range = [&](const auto & o) { const auto distance = std::abs(motion_utils::calcLateralOffset( - ego_data.path.points, o.kinematics.initial_pose_with_covariance.pose.position)); - return distance <= params.minimum_object_distance_from_ego_path + params.ego_lateral_offset + - o.shape.dimensions.y / 2.0 + hysteresis; + ego_data.trajectory, o.kinematics.initial_pose_with_covariance.pose.position)); + return distance <= params.minimum_object_distance_from_ego_trajectory + + params.ego_lateral_offset + o.shape.dimensions.y / 2.0 + hysteresis; }; const auto is_not_too_close = [&](const auto & o) { const auto obj_arc_length = motion_utils::calcSignedArcLength( - ego_data.path.points, ego_data.pose.position, + ego_data.trajectory, ego_data.pose.position, o.kinematics.initial_pose_with_covariance.pose.position); - return obj_arc_length > ego_data.longitudinal_offset_to_first_path_idx + + return obj_arc_length > ego_data.longitudinal_offset_to_first_trajectory_idx + params.ego_longitudinal_offset + o.shape.dimensions.x / 2.0; }; const auto is_unavoidable = [&](const auto & o) { @@ -91,4 +91,4 @@ std::vector filter_predicted_obj } return filtered_objects; } -} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp similarity index 85% rename from planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp rename to planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp index 71da351d2f10b..b366b5fc37a5e 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp @@ -21,12 +21,12 @@ #include -namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::motion_velocity_planner::dynamic_obstacle_stop { /// @brief filter the given predicted objects /// @param objects predicted objects -/// @param ego_data ego data, including its path and pose +/// @param ego_data ego data, including its trajectory and pose /// @param params parameters /// @param hysteresis [m] extra distance threshold used for filtering /// @return filtered predicted objects @@ -34,6 +34,6 @@ std::vector filter_predicted_obj const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis); -} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop #endif // OBJECT_FILTERING_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp similarity index 83% rename from planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp rename to planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp index 0f6fdbfadbf86..ce5416c12ae78 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp @@ -18,13 +18,11 @@ #include -namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::motion_velocity_planner::dynamic_obstacle_stop { void update_object_map( ObjectStopDecisionMap & object_map, const std::vector & collisions, - const rclcpp::Time & now, - const std::vector & path_points, - const PlannerParam & params) + const rclcpp::Time & now, const TrajectoryPoints & trajectory, const PlannerParam & params) { for (auto & [object, decision] : object_map) decision.collision_detected = false; for (const auto & collision : collisions) { @@ -32,7 +30,7 @@ void update_object_map( search->second.collision_detected = true; const auto is_closer_collision_point = motion_utils::calcSignedArcLength( - path_points, search->second.collision_point, collision.point) < 0.0; + trajectory, search->second.collision_point, collision.point) < 0.0; if (is_closer_collision_point) search->second.collision_point = collision.point; } else { object_map[collision.object_uuid].collision_point = collision.point; @@ -56,7 +54,7 @@ std::optional find_earliest_collision( for (auto & [object_uuid, decision] : object_map) { if (decision.should_be_avoided()) { const auto arc_length = motion_utils::calcSignedArcLength( - ego_data.path.points, ego_data.pose.position, decision.collision_point); + ego_data.trajectory, ego_data.pose.position, decision.collision_point); if (arc_length < earliest_collision_arc_length) { earliest_collision_arc_length = arc_length; earliest_collision = decision.collision_point; @@ -66,4 +64,4 @@ std::optional find_earliest_collision( return earliest_collision; } -} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp similarity index 87% rename from planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp rename to planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp index cc13614e80782..4b4373c82592a 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp @@ -24,7 +24,7 @@ #include #include -namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::motion_velocity_planner::dynamic_obstacle_stop { /// @brief representation of a decision to stop for a dynamic object struct ObjectStopDecision @@ -56,21 +56,19 @@ using ObjectStopDecisionMap = std::unordered_map & collisions, - const rclcpp::Time & now, - const std::vector & path_points, - const PlannerParam & params); + const rclcpp::Time & now, const TrajectoryPoints & trajectory, const PlannerParam & params); -/// @brief find the earliest collision requiring a stop along the ego path +/// @brief find the earliest collision requiring a stop along the ego trajectory /// @param object_map map with the objects to avoid and their corresponding collision points /// @param ego_data ego data /// @return the earliest collision point (if any) std::optional find_earliest_collision( const ObjectStopDecisionMap & object_map, const EgoData & ego_data); -} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop #endif // OBJECT_STOP_DECISION_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp similarity index 80% rename from planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp rename to planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp index 32ef89413b2f8..93170bead32e8 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp @@ -19,8 +19,8 @@ #include #include +#include #include -#include #include @@ -29,8 +29,9 @@ #include #include -namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::motion_velocity_planner::dynamic_obstacle_stop { +using TrajectoryPoints = std::vector; using BoxIndexPair = std::pair; using Rtree = boost::geometry::index::rtree>; @@ -46,17 +47,17 @@ struct PlannerParam double remove_duration_buffer; double ego_longitudinal_offset; double ego_lateral_offset; - double minimum_object_distance_from_ego_path; + double minimum_object_distance_from_ego_trajectory; bool ignore_unavoidable_collisions; }; struct EgoData { - tier4_planning_msgs::msg::PathWithLaneId path{}; - size_t first_path_idx{}; - double longitudinal_offset_to_first_path_idx; // [m] + TrajectoryPoints trajectory{}; + size_t first_trajectory_idx{}; + double longitudinal_offset_to_first_trajectory_idx; // [m] geometry_msgs::msg::Pose pose; - tier4_autoware_utils::MultiPolygon2d path_footprints; + tier4_autoware_utils::MultiPolygon2d trajectory_footprints; Rtree rtree; std::optional earliest_stop_pose; }; @@ -84,6 +85,6 @@ struct Collision geometry_msgs::msg::Point point; std::string object_uuid; }; -} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop #endif // TYPES_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp index 494deae1d060c..576ad94ed9210 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp @@ -67,8 +67,8 @@ struct PlannerData std::shared_ptr route_handler; // nearest search - double ego_nearest_dist_threshold; - double ego_nearest_yaw_threshold; + double ego_nearest_dist_threshold{}; + double ego_nearest_yaw_threshold{}; // other internal data // traffic_light_id_map_raw is the raw observation, while traffic_light_id_map_keep_last keeps the diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml index 5de50947c4c28..26bbcce3ee2a4 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml @@ -46,6 +46,7 @@ ament_cmake_ros ament_lint_auto autoware_lint_common + autoware_motion_velocity_dynamic_obstacle_stop_module autoware_motion_velocity_obstacle_velocity_limiter_module autoware_motion_velocity_out_of_lane_module autoware_planning_test_manager diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp index 3c7b7d1df9bf5..c35749ef33710 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp @@ -61,6 +61,7 @@ std::shared_ptr generateNode() std::vector module_names; module_names.emplace_back("autoware::motion_velocity_planner::OutOfLaneModule"); module_names.emplace_back("autoware::motion_velocity_planner::ObstacleVelocityLimiterModule"); + module_names.emplace_back("autoware::motion_velocity_planner::DynamicObstacleStopModule"); std::vector params; params.emplace_back("launch_modules", module_names); @@ -75,7 +76,8 @@ std::shared_ptr generateNode() velocity_smoother_dir + "/config/Analytical.param.yaml", motion_velocity_planner_dir + "/config/motion_velocity_planner.param.yaml", get_motion_velocity_module_config("out_of_lane"), - get_motion_velocity_module_config("obstacle_velocity_limiter")}); + get_motion_velocity_module_config("obstacle_velocity_limiter"), + get_motion_velocity_module_config("dynamic_obstacle_stop")}); return std::make_shared(node_options); }