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 2d5177d4252d8..4e39c8e43d4ab 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,6 +2,9 @@ + + + @@ -9,6 +12,21 @@ + + + + + @@ -141,6 +161,9 @@ + + + diff --git a/planning/.pages b/planning/.pages index 7444815cdd28a..735e2b2c907c0 100644 --- a/planning/.pages +++ b/planning/.pages @@ -63,6 +63,9 @@ nav: - 'Motion Velocity Planner': - 'About Motion Velocity Planner': planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/ - 'Available Modules': + - 'Obstacle Stop': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/ + - 'Obstacle Slow Down': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/ + - 'Obstacle Cruise': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/ - 'Dynamic Obstacle Stop': planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/ - 'Out of Lane': planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/ - 'Obstacle Velocity Limiter': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/ 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 index c7a103e0269ca..15fcc52f0fd24 100644 --- 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 @@ -97,19 +97,21 @@ void DynamicObstacleStopModule::update_parameters(const std::vector & ego_trajectory_points, + [[maybe_unused]] const std::vector & + raw_trajectory_points, + const std::vector & smoothed_trajectory_points, const std::shared_ptr planner_data) { VelocityPlanningResult result; debug_data_.reset_data(); - if (ego_trajectory_points.size() < 2) return result; + if (smoothed_trajectory_points.size() < 2) return result; autoware::universe_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; + ego_data.trajectory = smoothed_trajectory_points; autoware::motion_utils::removeOverlapPoints(ego_data.trajectory); ego_data.first_trajectory_idx = autoware::motion_utils::findNearestSegmentIndex(ego_data.trajectory, ego_data.pose.position); @@ -164,7 +166,7 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( if (stop_pose) { result.stop_points.push_back(stop_pose->position); planning_factor_interface_->add( - ego_trajectory_points, ego_data.pose, *stop_pose, PlanningFactor::STOP, + smoothed_trajectory_points, ego_data.pose, *stop_pose, PlanningFactor::STOP, SafetyFactorArray{}); create_virtual_walls(); } 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 index a678e7657a6c3..aca86a82798ab 100644 --- 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 @@ -36,7 +36,8 @@ class DynamicObstacleStopModule : public PluginModuleInterface void publish_planning_factor() override { planning_factor_interface_->publish(); }; void update_parameters(const std::vector & parameters) override; VelocityPlanningResult plan( - const std::vector & ego_trajectory_points, + const std::vector & raw_trajectory_points, + const std::vector & smoothed_trajectory_points, const std::shared_ptr planner_data) override; std::string get_module_name() const override { return module_name_; } diff --git a/planning/motion_velocity_planner/autoware_motion_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 index 0726fdba02de0..850255e0c5093 100644 --- a/planning/motion_velocity_planner/autoware_motion_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 @@ -96,12 +96,12 @@ bool is_unavoidable( }; std::vector filter_predicted_objects( - const std::vector & objects, const EgoData & ego_data, + const std::vector> & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis) { std::vector filtered_objects; for (const auto & object : objects) { - const auto & predicted_object = object.predicted_object; + const auto & predicted_object = object->predicted_object; const auto is_not_too_slow = predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x >= params.minimum_object_velocity; diff --git a/planning/motion_velocity_planner/autoware_motion_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 index 33abb19074099..77b3d2b1ead45 100644 --- a/planning/motion_velocity_planner/autoware_motion_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 @@ -76,7 +76,7 @@ bool is_unavoidable( /// @param hysteresis [m] extra distance threshold used for filtering /// @return filtered predicted objects std::vector filter_predicted_objects( - const std::vector & objects, const EgoData & ego_data, + const std::vector> & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis); } // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp index 8cf83f63f4d01..2e3af577a83f4 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp @@ -53,23 +53,23 @@ struct PlannerParam struct EgoData { - TrajectoryPoints trajectory; + TrajectoryPoints trajectory{}; size_t first_trajectory_idx{}; - double longitudinal_offset_to_first_trajectory_idx; // [m] - geometry_msgs::msg::Pose pose; - autoware::universe_utils::MultiPolygon2d trajectory_footprints; - Rtree rtree; - std::optional earliest_stop_pose; + double longitudinal_offset_to_first_trajectory_idx{}; // [m] + geometry_msgs::msg::Pose pose{}; + autoware::universe_utils::MultiPolygon2d trajectory_footprints{}; + Rtree rtree{}; + std::optional earliest_stop_pose{}; }; /// @brief debug data struct DebugData { - autoware::universe_utils::MultiPolygon2d obstacle_footprints; + autoware::universe_utils::MultiPolygon2d obstacle_footprints{}; size_t prev_dynamic_obstacles_nb{}; - autoware::universe_utils::MultiPolygon2d ego_footprints; + autoware::universe_utils::MultiPolygon2d ego_footprints{}; size_t prev_ego_footprints_nb{}; - std::optional stop_pose; + std::optional stop_pose{}; size_t prev_collisions_nb{}; double z{}; void reset_data() @@ -82,8 +82,8 @@ struct DebugData struct Collision { - geometry_msgs::msg::Point point; - std::string object_uuid; + geometry_msgs::msg::Point point{}; + std::string object_uuid{}; }; } // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp index 419b5d9e46bd1..3ff4cf0c9972f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp @@ -205,7 +205,7 @@ TEST(TestObjectFiltering, isUnavoidable) TEST(TestObjectFiltering, filterPredictedObjects) { using autoware::motion_velocity_planner::dynamic_obstacle_stop::filter_predicted_objects; - std::vector objects; + std::vector> objects; autoware::motion_velocity_planner::dynamic_obstacle_stop::EgoData ego_data; autoware::motion_velocity_planner::dynamic_obstacle_stop::PlannerParam params; double hysteresis{}; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/CMakeLists.txt new file mode 100644 index 0000000000000..f51e026428aec --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_motion_velocity_obstacle_cruise_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node_universe plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/README.md new file mode 100644 index 0000000000000..296bb6290e398 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/README.md @@ -0,0 +1,118 @@ +# Obstacle Cruise + +## Role + +The `obstacle_cruise` module does the cruise planning against a dynamic obstacle in front of the ego. + +## Activation + +This module is activated if the launch parameter `launch_obstacle_cruise_module` is set to true. + +## Inner-workings / Algorithms + +### Obstacle Filtering + +The obstacles meeting the following condition are determined as obstacles for cruising. + +- The lateral distance from the object to the ego's trajectory is smaller than `obstacle_filtering.max_lat_margin`. + +- The object type is for cruising according to `obstacle_filtering.object_type.*`. +- The object is not crossing the ego's trajectory (\*1). +- If the object is inside the trajectory. + - The object type is for inside cruising according to `obstacle_filtering.object_type.inside.*`. + - The object velocity is larger than `obstacle_filtering.obstacle_velocity_threshold_from_cruise`. +- If the object is outside the trajectory. + - The object type is for outside cruising according to `obstacle_filtering.object_type.outside.*`. + - The object velocity is larger than `obstacle_filtering.outside_obstacle.obstacle_velocity_threshold`. + - The highest confident predicted path collides with the ego's trajectory. + - Its collision's period is larger than `obstacle_filtering.outside_obstacle.ego_obstacle_overlap_time_threshold`. + +#### NOTE + +##### \*1: Crossing obstacles + +Crossing obstacle is the object whose orientation's yaw angle against the ego's trajectory is smaller than `obstacle_filtering.crossing_obstacle.obstacle_traj_angle_threshold`. + +##### Yield for vehicles that might cut in into the ego's lane + +It is also possible to yield (cruise) behind vehicles in neighbor lanes if said vehicles might cut in the ego vehicle's current lane. + +The obstacles meeting the following condition are determined as obstacles for yielding (cruising). + +- The object type is for cruising according to `obstacle_filtering.object_type.*` and it is moving with a speed greater than `obstacle_filtering.yield.stopped_obstacle_velocity_threshold`. +- The object is not crossing the ego's trajectory (\*1). +- There is another object of type `obstacle_filtering.object_type.*` stopped in front of the moving obstacle. +- The lateral distance (using the ego's trajectory as reference) between both obstacles is less than `obstacle_filtering.yield.max_lat_dist_between_obstacles` +- Both obstacles, moving and stopped, are within `obstacle_filtering.yield.lat_distance_threshold` and `obstacle_filtering.yield.lat_distance_threshold` + `obstacle_filtering.yield.max_lat_dist_between_obstacles` lateral distance from the ego's trajectory respectively. + +If the above conditions are met, the ego vehicle will cruise behind the moving obstacle, yielding to it so it can cut in into the ego's lane to avoid the stopped obstacle. + +### Cruise Planning + +The role of the cruise planning is keeping a safe distance with dynamic vehicle objects with smoothed velocity transition. +This includes not only cruising a front vehicle, but also reacting a cut-in and cut-out vehicle. + +The safe distance is calculated dynamically based on the Responsibility-Sensitive Safety (RSS) by the following equation. + +$$ +d_{rss} = v_{ego} t_{idling} + \frac{1}{2} a_{ego} t_{idling}^2 + \frac{v_{ego}^2}{2 a_{ego}} - \frac{v_{obstacle}^2}{2 a_{obstacle}}, +$$ + +assuming that $d_{rss}$ is the calculated safe distance, $t_{idling}$ is the idling time for the ego to detect the front vehicle's deceleration, $v_{ego}$ is the ego's current velocity, $v_{obstacle}$ is the front obstacle's current velocity, $a_{ego}$ is the ego's acceleration, and $a_{obstacle}$ is the obstacle's acceleration. +These values are parameterized as follows. Other common values such as ego's minimum acceleration is defined in `common.param.yaml`. + +| Parameter | Type | Description | +| ------------------------------------------ | ------ | ----------------------------------------------------------------------------- | +| `cruise_planning.idling_time` | double | idling time for the ego to detect the front vehicle starting deceleration [s] | +| `cruise_planning.min_ego_accel_for_rss` | double | ego's acceleration for RSS [m/ss] | +| `cruise_planning.min_object_accel_for_rss` | double | front obstacle's acceleration for RSS [m/ss] | + +The detailed formulation is as follows. + +$$ +\begin{align} +d_{error} & = d - d_{rss} \\ +d_{normalized} & = lpf(d_{error} / d_{obstacle}) \\ +d_{quad, normalized} & = sign(d_{normalized}) *d_{normalized}*d_{normalized} \\ +v_{pid} & = pid(d_{quad, normalized}) \\ +v_{add} & = v_{pid} > 0 ? v_{pid}* w_{acc} : v_{pid} \\ +v_{target} & = max(v_{ego} + v_{add}, v_{min, cruise}) +\end{align} +$$ + +| Variable | Description | +| ----------------- | --------------------------------------- | +| `d` | actual distance to obstacle | +| `d_{rss}` | ideal distance to obstacle based on RSS | +| `v_{min, cruise}` | `min_cruise_target_vel` | +| `w_{acc}` | `output_ratio_during_accel` | +| `lpf(val)` | apply low-pass filter to `val` | +| `pid(val)` | apply pid to `val` | + +#### Algorithm selection for cruise planner + +Currently, only a PID-based planner is supported. +Each planner will be explained in the following. + +| Parameter | Type | Description | +| --------------------------- | ------ | ------------------------------------------------------------ | +| `option.planning_algorithm` | string | cruise and stop planning algorithm, selected from "pid_base" | + +#### PID-based planner + +In order to keep the safe distance, the target velocity and acceleration is calculated and sent as an external velocity limit to the velocity smoothing package (`velocity_smoother` by default). +The target velocity and acceleration is respectively calculated with the PID controller according to the error between the reference safe distance and the actual distance. + +#### Optimization-based planner + +under construction + +## Debugging + +### Obstacle for cruise + +Orange sphere which is an obstacle for cruise is visualized by `obstacles_to_cruise` in the `~/debug/marker` topic. + +Orange wall which means a safe distance to cruise if the ego's front meets the wall is visualized in the `~/debug/cruise/virtual_wall` topic. + +![cruise_visualization](./docs/cruise_visualization.png) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/config/obstacle_cruise.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/config/obstacle_cruise.param.yaml new file mode 100644 index 0000000000000..214f7bb35a155 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/config/obstacle_cruise.param.yaml @@ -0,0 +1,120 @@ +/**: + ros__parameters: + obstacle_cruise: + option: + planning_algorithm: "pid_base" # currently supported algorithm is "pid_base" + + cruise_planning: + idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s] + min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss] + min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] + safe_distance_margin : 5.0 # This is also used as a stop margin [m] + + pid_based_planner: + use_velocity_limit_based_planner: true + error_function_type: quadratic # choose from linear, quadratic + + velocity_limit_based_planner: + # PID gains to keep safe distance with the front vehicle + kp: 10.0 + ki: 0.0 + kd: 2.0 + + output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + vel_to_acc_weight: 12.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] + + enable_jerk_limit_to_output_acc: false + + disable_target_acceleration: true + + velocity_insertion_based_planner: + kp_acc: 6.0 + ki_acc: 0.0 + kd_acc: 2.0 + + kp_jerk: 20.0 + ki_jerk: 0.0 + kd_jerk: 0.0 + + output_acc_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + output_jerk_ratio_during_accel: 1.0 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + + enable_jerk_limit_to_output_acc: true + + min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss] + min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s] + time_to_evaluate_rss: 0.0 + + lpf_normalized_error_cruise_dist_gain: 0.2 + + optimization_based_planner: + dense_resampling_time_interval: 0.2 + sparse_resampling_time_interval: 2.0 + dense_time_horizon: 5.0 + max_time_horizon: 25.0 + velocity_margin: 0.2 #[m/s] + + # Parameters for safe distance + t_dangerous: 0.5 + + # For initial Motion + replan_vel_deviation: 5.0 # velocity deviation to replan initial velocity [m/s] + engage_velocity: 0.25 # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed) + engage_acceleration: 0.1 # engage acceleration [m/ss] (use this acceleration when engagement) + engage_exit_ratio: 0.5 # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity. + stop_dist_to_prohibit_engage: 0.5 # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m] + + # Weights for optimization + max_s_weight: 100.0 + max_v_weight: 1.0 + over_s_safety_weight: 1000000.0 + over_s_ideal_weight: 50.0 + over_v_weight: 500000.0 + over_a_weight: 5000.0 + over_j_weight: 10000.0 + + obstacle_filtering: + object_type: + inside: + unknown: true + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: true + pedestrian: false + + outside: + unknown: false + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: false + pedestrian: false + + max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width + + # if crossing vehicle is determined as target obstacles or not + crossing_obstacle: + obstacle_velocity_threshold : 1.0 # velocity threshold for crossing obstacle for cruise or stop [m/s] + obstacle_traj_angle_threshold : 0.523599 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop + + outside_obstacle: + obstacle_velocity_threshold : 1.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] + ego_obstacle_overlap_time_threshold : 0.2 # time threshold to decide cut-in obstacle for cruise or stop [s] + max_prediction_time_for_collision_check : 10.0 # prediction time to check collision between obstacle and ego + max_lateral_time_margin: 5.0 # time threshold of lateral margin between obstacle and trajectory band with ego's width [s] + num_of_predicted_paths: 1 # number of the highest confidence predicted path to check collision between obstacle and ego + yield: + enable_yield: true + lat_distance_threshold: 5.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding + max_lat_dist_between_obstacles: 2.5 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it + max_obstacles_collision_time: 10.0 # how far the blocking obstacle + stopped_obstacle_velocity_threshold: 0.5 + + # hysteresis for cruise and stop + obstacle_velocity_threshold_from_cruise : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] + obstacle_velocity_threshold_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/docs/cruise_visualization.png b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/docs/cruise_visualization.png new file mode 100644 index 0000000000000..acb094135e884 Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/docs/cruise_visualization.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/package.xml new file mode 100644 index 0000000000000..e0cbf2c193b7a --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/package.xml @@ -0,0 +1,41 @@ + + + + autoware_motion_velocity_obstacle_cruise_module + 0.40.0 + obstacle cruise feature in motion_velocity_planner + + Takayuki Murooka + Yuki Takagi + Maxime Clement + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + autoware_motion_utils + autoware_motion_velocity_planner_common_universe + autoware_osqp_interface + autoware_perception_msgs + autoware_planning_msgs + autoware_route_handler + autoware_signal_processing + autoware_universe_utils + autoware_vehicle_info_utils + geometry_msgs + libboost-dev + pluginlib + rclcpp + tf2 + tier4_metric_msgs + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/plugins.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/plugins.xml new file mode 100644 index 0000000000000..d841b9beba5c9 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/cruise_planner_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/cruise_planner_interface.hpp new file mode 100644 index 0000000000000..63f979813e78f --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/cruise_planner_interface.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 CRUISE_PLANNER_INTERFACE_HPP_ +#define CRUISE_PLANNER_INTERFACE_HPP_ + +#include "autoware/motion_velocity_planner_common_universe/planner_data.hpp" +#include "parameters.hpp" +#include "type_alias.hpp" +#include "types.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class CruisePlannerInterface +{ +public: + CruisePlannerInterface( + rclcpp::Node & node, const CommonParam & common_param, + const CruisePlanningParam & cruise_planning_param) + { + clock_ = node.get_clock(); + logger_ = node.get_logger(); + common_param_ = common_param; + cruise_planning_param_ = cruise_planning_param; + } + + virtual ~CruisePlannerInterface() = default; + + virtual std::vector plan_cruise( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const std::vector & obstacles, std::shared_ptr debug_data_ptr, + std::unique_ptr & + planning_factor_interface, + std::optional & velocity_limit) = 0; + + virtual void update_parameters(const std::vector & parameters) = 0; + + virtual Float32MultiArrayStamped get_cruise_planning_debug_message( + [[maybe_unused]] const rclcpp::Time & current_time) const + { + return Float32MultiArrayStamped{}; + } + +protected: + rclcpp::Clock::SharedPtr clock_{}; + rclcpp::Logger logger_{rclcpp::get_logger("")}; + CommonParam common_param_; + CruisePlanningParam cruise_planning_param_; + + static double calc_distance_to_collisionPoint( + const std::vector & traj_points, + const std::shared_ptr planner_data, + const geometry_msgs::msg::Point & collision_point) + { + const double offset = planner_data->is_driving_forward + ? std::abs(planner_data->vehicle_info_.max_longitudinal_offset_m) + : std::abs(planner_data->vehicle_info_.min_longitudinal_offset_m); + + const size_t ego_segment_idx = + planner_data->find_segment_index(traj_points, planner_data->current_odometry.pose.pose); + + const size_t collision_segment_idx = + autoware::motion_utils::findNearestSegmentIndex(traj_points, collision_point); + + const auto dist_to_collision_point = autoware::motion_utils::calcSignedArcLength( + traj_points, planner_data->current_odometry.pose.pose.position, ego_segment_idx, + collision_point, collision_segment_idx); + + return dist_to_collision_point - offset; + } + + double calc_rss_distance( + const double ego_vel, const double obstacle_vel, const double margin = 0.0) const + { + const double rss_dist_with_margin = + ego_vel * cruise_planning_param_.idling_time + + std::pow(ego_vel, 2) * 0.5 / std::abs(cruise_planning_param_.min_ego_accel_for_rss) - + std::pow(obstacle_vel, 2) * 0.5 / std::abs(cruise_planning_param_.min_object_accel_for_rss) + + margin; + return rss_dist_with_margin; + } +}; +} // namespace autoware::motion_velocity_planner + +#endif // CRUISE_PLANNER_INTERFACE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/metrics_manager.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/metrics_manager.hpp new file mode 100644 index 0000000000000..11554121a0882 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/metrics_manager.hpp @@ -0,0 +1,60 @@ +// 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 METRICS_MANAGER_HPP_ +#define METRICS_MANAGER_HPP_ + +#include "type_alias.hpp" +#include "types.hpp" + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class MetricsManager +{ +public: + void init() { metrics_.clear(); } + + void calculate_metrics(const std::string & module_name, const std::string & reason) + { + // Create status + { + // Decision + Metric decision_metric; + decision_metric.name = module_name + "/decision"; + decision_metric.unit = "string"; + decision_metric.value = reason; + metrics_.push_back(decision_metric); + } + } + + MetricArray create_metric_array(const rclcpp::Time & current_time) + { + MetricArray metrics_msg; + metrics_msg.stamp = current_time; + metrics_msg.metric_array.insert( + metrics_msg.metric_array.end(), metrics_.begin(), metrics_.end()); + return metrics_msg; + } + +private: + std::vector metrics_; +}; + +} // namespace autoware::motion_velocity_planner + +#endif // METRICS_MANAGER_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp new file mode 100644 index 0000000000000..c1d02842de1bb --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp @@ -0,0 +1,826 @@ +// Copyright 2025 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 "obstacle_cruise_module.hpp" + +#include "autoware/universe_utils/ros/uuid_helper.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +namespace +{ +double calc_diff_angle_against_trajectory( + const std::vector & traj_points, const geometry_msgs::msg::Pose & target_pose) +{ + const size_t nearest_idx = + autoware::motion_utils::findNearestIndex(traj_points, target_pose.position); + const double traj_yaw = tf2::getYaw(traj_points.at(nearest_idx).pose.orientation); + + const double target_yaw = tf2::getYaw(target_pose.orientation); + + const double diff_yaw = autoware::universe_utils::normalizeRadian(target_yaw - traj_yaw); + return diff_yaw; +} + +std::vector resample_highest_confidence_predicted_paths( + const std::vector & predicted_paths, const double time_interval, + const double time_horizon, const size_t num_paths) +{ + std::vector sorted_paths = predicted_paths; + + // Sort paths by descending confidence + std::sort( + sorted_paths.begin(), sorted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence > b.confidence; }); + + std::vector selected_paths; + size_t path_count = 0; + + // Select paths that meet the confidence thresholds + for (const auto & path : sorted_paths) { + if (path_count < num_paths) { + selected_paths.push_back(path); + ++path_count; + } + } + + // Resample each selected path + std::vector resampled_paths; + for (const auto & path : selected_paths) { + if (path.path.size() < 2) { + continue; + } + resampled_paths.push_back( + autoware::object_recognition_utils::resamplePredictedPath(path, time_interval, time_horizon)); + } + + return resampled_paths; +} + +VelocityLimitClearCommand create_velocity_limit_clear_command( + const rclcpp::Time & current_time, [[maybe_unused]] const std::string & module_name) +{ + VelocityLimitClearCommand msg; + msg.stamp = current_time; + msg.sender = "obstacle_cruise"; + msg.command = true; + return msg; +} + +Float64Stamped create_float64_stamped(const rclcpp::Time & now, const float & data) +{ + Float64Stamped msg; + msg.stamp = now; + msg.data = data; + return msg; +} +} // namespace + +void ObstacleCruiseModule::init(rclcpp::Node & node, const std::string & module_name) +{ + module_name_ = module_name; + clock_ = node.get_clock(); + logger_ = node.get_logger(); + + // ros parameters + planning_algorithm_ = + getOrDeclareParameter(node, "obstacle_cruise.option.planning_algorithm"); + common_param_ = CommonParam(node); + cruise_planning_param_ = CruisePlanningParam(node); + obstacle_filtering_param_ = ObstacleFilteringParam(node); + + // common publisher + processing_time_publisher_ = + node.create_publisher("~/debug/obstacle_cruise/processing_time_ms", 1); + virtual_wall_publisher_ = + node.create_publisher("~/obstacle_cruise/virtual_walls", 1); + debug_publisher_ = node.create_publisher("~/obstacle_cruise/debug_markers", 1); + + // module publisher + metrics_pub_ = node.create_publisher("~/cruise/metrics", 10); + debug_cruise_planning_info_pub_ = + node.create_publisher("~/debug/cruise_planning_info", 1); + processing_time_detail_pub_ = node.create_publisher( + "~/debug/processing_time_detail_ms/obstacle_cruise", 1); + + // interface + objects_of_interest_marker_interface_ = std::make_unique< + autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface>( + &node, "obstacle_cruise"); + planning_factor_interface_ = + std::make_unique( + &node, "obstacle_cruise"); + + // time keeper + time_keeper_ = std::make_shared(processing_time_detail_pub_); + + // cruise planner + cruise_planner_ = create_cruise_planner(node); +} + +void ObstacleCruiseModule::update_parameters(const std::vector & parameters) +{ + cruise_planner_->update_parameters(parameters); +} + +VelocityPlanningResult ObstacleCruiseModule::plan( + const std::vector & raw_trajectory_points, + [[maybe_unused]] const std::vector & + smoothed_trajectory_points, + const std::shared_ptr planner_data) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + // 1. init variables + stop_watch_.tic(); + debug_data_ptr_ = std::make_shared(); + metrics_manager_.init(); + + // filter obstacles of predicted objects + const auto cruise_obstacles = filter_cruise_obstacle_for_predicted_object( + planner_data->current_odometry, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold, raw_trajectory_points, planner_data->objects, + rclcpp::Time(planner_data->predicted_objects_header.stamp), planner_data->is_driving_forward, + planner_data->vehicle_info_, planner_data->trajectory_polygon_collision_check); + + // plan cruise + VelocityPlanningResult result; + [[maybe_unused]] const auto cruise_traj_points = cruise_planner_->plan_cruise( + planner_data, raw_trajectory_points, cruise_obstacles, debug_data_ptr_, + planning_factor_interface_, result.velocity_limit); + metrics_manager_.calculate_metrics("PlannerInterface", "cruise"); + + // clear velocity limit if necessary + if (result.velocity_limit) { + need_to_clear_velocity_limit_ = true; + } else { + if (need_to_clear_velocity_limit_) { + // clear velocity limit + result.velocity_limit_clear_command = + create_velocity_limit_clear_command(clock_->now(), module_name_); + need_to_clear_velocity_limit_ = false; + } + } + + publish_debug_info(); + + return result; +} + +std::string ObstacleCruiseModule::get_module_name() const +{ + return module_name_; +} + +std::unique_ptr ObstacleCruiseModule::create_cruise_planner( + rclcpp::Node & node) const +{ + if (planning_algorithm_ == "pid_base") { + return std::make_unique(node, common_param_, cruise_planning_param_); + } else if (planning_algorithm_ == "optimization_base") { + return std::make_unique(node, common_param_, cruise_planning_param_); + } + throw std::logic_error("Designated algorithm is not supported."); +} + +std::vector ObstacleCruiseModule::filter_cruise_obstacle_for_predicted_object( + const Odometry & odometry, const double ego_nearest_dist_threshold, + const double ego_nearest_yaw_threshold, const std::vector & traj_points, + const std::vector> & objects, + const rclcpp::Time & predicted_objects_stamp, const bool is_driving_forward, + const VehicleInfo & vehicle_info, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const auto & current_pose = odometry.pose.pose; + + const auto & p = trajectory_polygon_collision_check; + const auto decimated_traj_points = utils::decimate_trajectory_points_from_ego( + traj_points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold, + p.decimate_trajectory_step_length, 0.0); + const auto decimated_traj_polys = polygon_utils::create_one_step_polygons( + decimated_traj_points, vehicle_info, current_pose, 0.0, p.enable_to_consider_current_pose, + p.time_to_convergence, p.decimate_trajectory_step_length); + debug_data_ptr_->decimated_traj_polys = decimated_traj_polys; + + // cruise + std::vector cruise_obstacles; + for (const auto & object : objects) { + // 1. rough filtering + // 1.1. Check if the obstacle is in front of the ego. + const double lon_dist_from_ego_to_obj = + object->get_dist_from_ego_longitudinal(traj_points, current_pose.position); + if (lon_dist_from_ego_to_obj < 0.0) { + continue; + } + + // 1.2. Check if the rough lateral distance is smaller than the threshold. + const double min_lat_dist_to_traj_poly = + utils::calc_possible_min_dist_from_obj_to_traj_poly(object, traj_points, vehicle_info); + if (obstacle_filtering_param_.max_lat_margin < min_lat_dist_to_traj_poly) { + continue; + } + + // 2. precise filtering for cruise + const auto cruise_obstacle = create_cruise_obstacle( + odometry, traj_points, decimated_traj_points, decimated_traj_polys, object, + predicted_objects_stamp, object->get_dist_to_traj_poly(decimated_traj_polys), + is_driving_forward, vehicle_info, trajectory_polygon_collision_check); + if (cruise_obstacle) { + cruise_obstacles.push_back(*cruise_obstacle); + continue; + } + + // 3. precise filtering for yield cruise + if (obstacle_filtering_param_.enable_yield) { + const auto yield_obstacles = find_yield_cruise_obstacles( + odometry, objects, predicted_objects_stamp, traj_points, vehicle_info); + if (yield_obstacles) { + for (const auto & y : yield_obstacles.value()) { + // Check if there is no member with the same UUID in cruise_obstacles + auto it = std::find_if( + cruise_obstacles.begin(), cruise_obstacles.end(), + [&y](const auto & c) { return y.uuid == c.uuid; }); + + // If no matching UUID found, insert yield obstacle into cruise_obstacles + if (it == cruise_obstacles.end()) { + cruise_obstacles.push_back(y); + } + } + } + } + } + prev_cruise_object_obstacles_ = cruise_obstacles; + + return cruise_obstacles; +} + +void ObstacleCruiseModule::publish_debug_info() +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + // 1. debug marker + MarkerArray debug_marker; + + // 1.1. obstacles + std::vector stop_collision_points; + for (size_t i = 0; i < debug_data_ptr_->obstacles_to_cruise.size(); ++i) { + // obstacle + const auto obstacle_marker = utils::get_object_marker( + debug_data_ptr_->obstacles_to_cruise.at(i).pose, i, "obstacles", 1.0, 0.6, 0.1); + debug_marker.markers.push_back(obstacle_marker); + + // collision points + for (size_t j = 0; j < debug_data_ptr_->obstacles_to_cruise.at(i).collision_points.size(); + ++j) { + stop_collision_points.push_back( + debug_data_ptr_->obstacles_to_cruise.at(i).collision_points.at(j).point); + } + } + + // 1.2. collision points + for (size_t i = 0; i < stop_collision_points.size(); ++i) { + auto collision_point_marker = autoware::universe_utils::createDefaultMarker( + "map", clock_->now(), "collision_points", i, Marker::SPHERE, + autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + collision_point_marker.pose.position = stop_collision_points.at(i); + debug_marker.markers.push_back(collision_point_marker); + } + + // 1.3. intentionally ignored obstacles + for (size_t i = 0; i < debug_data_ptr_->intentionally_ignored_obstacles.size(); ++i) { + const auto marker = utils::get_object_marker( + debug_data_ptr_->intentionally_ignored_obstacles.at(i) + ->predicted_object.kinematics.initial_pose_with_covariance.pose, + i, "intentionally_ignored_obstacles", 0.0, 1.0, 0.0); + debug_marker.markers.push_back(marker); + } + + // 1.4. detection area + auto decimated_traj_polys_marker = autoware::universe_utils::createDefaultMarker( + "map", clock_->now(), "detection_area", 0, Marker::LINE_LIST, + autoware::universe_utils::createMarkerScale(0.01, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); + for (const auto & decimated_traj_poly : debug_data_ptr_->decimated_traj_polys) { + for (size_t dp_idx = 0; dp_idx < decimated_traj_poly.outer().size(); ++dp_idx) { + const auto & current_point = decimated_traj_poly.outer().at(dp_idx); + const auto & next_point = + decimated_traj_poly.outer().at((dp_idx + 1) % decimated_traj_poly.outer().size()); + + decimated_traj_polys_marker.points.push_back( + autoware::universe_utils::createPoint(current_point.x(), current_point.y(), 0.0)); + decimated_traj_polys_marker.points.push_back( + autoware::universe_utils::createPoint(next_point.x(), next_point.y(), 0.0)); + } + } + debug_marker.markers.push_back(decimated_traj_polys_marker); + + debug_publisher_->publish(debug_marker); + + // 2. virtual wall + virtual_wall_publisher_->publish(debug_data_ptr_->cruise_wall_marker); + + // 3. cruise planning info + const auto cruise_debug_msg = cruise_planner_->get_cruise_planning_debug_message(clock_->now()); + debug_cruise_planning_info_pub_->publish(cruise_debug_msg); + + // 4. objects of interest + objects_of_interest_marker_interface_->publishMarkerArray(); + + // 5. metrics + const auto metrics_msg = metrics_manager_.create_metric_array(clock_->now()); + metrics_pub_->publish(metrics_msg); + + // 6. processing time + processing_time_publisher_->publish(create_float64_stamped(clock_->now(), stop_watch_.toc())); + + // 7. planning factor + planning_factor_interface_->publish(); +} + +std::optional ObstacleCruiseModule::create_cruise_obstacle( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector & decimated_traj_polys, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const double dist_from_obj_poly_to_traj_poly, const bool is_driving_forward, + const VehicleInfo & vehicle_info, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const +{ + const auto & obj_uuid = object->predicted_object.object_id; + const auto & obj_uuid_str = autoware::universe_utils::toHexString(obj_uuid); + + // NOTE: When driving backward, Stop will be planned instead of cruise. + // When the obstacle is crossing the ego's trajectory, cruise can be ignored. + if ( + !is_cruise_obstacle(object->predicted_object.classification.at(0).label) || + !is_driving_forward) { + return std::nullopt; + } + + if (object->get_lon_vel_relative_to_traj(traj_points) < 0.0) { + RCLCPP_DEBUG( + logger_, "[Cruise] Ignore obstacle (%s) since it's driving in opposite direction.", + obj_uuid_str.substr(0, 4).c_str()); + return std::nullopt; + } + + if (obstacle_filtering_param_.max_lat_margin < dist_from_obj_poly_to_traj_poly) { + const auto time_to_traj = dist_from_obj_poly_to_traj_poly / + std::max(1e-6, object->get_lat_vel_relative_to_traj(traj_points)); + if (time_to_traj > obstacle_filtering_param_.max_lateral_time_margin) { + RCLCPP_DEBUG( + logger_, "[Cruise] Ignore obstacle (%s) since it's far from trajectory.", + obj_uuid_str.substr(0, 4).c_str()); + return std::nullopt; + } + } + + if (is_obstacle_crossing(traj_points, object)) { + RCLCPP_DEBUG( + logger_, "[Cruise] Ignore obstacle (%s) since it's crossing the ego's trajectory..", + obj_uuid_str.substr(0, 4).c_str()); + return std::nullopt; + } + + const auto collision_points = [&]() -> std::optional> { + constexpr double epsilon = 1e-6; + if (dist_from_obj_poly_to_traj_poly < epsilon) { + // obstacle is inside the trajectory + return create_collision_points_for_inside_cruise_obstacle( + traj_points, decimated_traj_points, decimated_traj_polys, object, predicted_objects_stamp, + is_driving_forward, vehicle_info, trajectory_polygon_collision_check); + } + // obstacle is outside the trajectory + // If the ego is stopping, do not plan cruise for outside obstacles. Stop will be planned. + if (odometry.twist.twist.linear.x < 0.1) { + return std::nullopt; + } + return create_collision_points_for_outside_cruise_obstacle( + traj_points, decimated_traj_points, decimated_traj_polys, object, predicted_objects_stamp, + is_driving_forward, vehicle_info, trajectory_polygon_collision_check); + }(); + if (!collision_points) { + return std::nullopt; + } + + return CruiseObstacle{ + obj_uuid_str, + predicted_objects_stamp, + object->get_predicted_pose(clock_->now(), predicted_objects_stamp), + object->get_lon_vel_relative_to_traj(traj_points), + object->get_lat_vel_relative_to_traj(traj_points), + *collision_points}; +} + +std::optional> ObstacleCruiseModule::find_yield_cruise_obstacles( + const Odometry & odometry, const std::vector> & objects, + const rclcpp::Time & predicted_objects_stamp, const std::vector & traj_points, + const VehicleInfo & vehicle_info) +{ + const auto & current_pose = odometry.pose.pose; + + if (objects.empty() || traj_points.empty()) return std::nullopt; + + std::vector> stopped_objects; + std::vector> moving_objects; + + std::for_each(objects.begin(), objects.end(), [&](const auto & o) { + const bool is_moving = + std::hypot( + o->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, + o->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y) > + obstacle_filtering_param_.stopped_obstacle_velocity_threshold; + if (is_moving) { + const bool is_within_lat_dist_threshold = + o->get_dist_to_traj_lateral(traj_points) < + obstacle_filtering_param_.yield_lat_distance_threshold; + if (is_within_lat_dist_threshold) moving_objects.push_back(o); + return; + } + // lat threshold is larger for stopped obstacles + const bool is_within_lat_dist_threshold = + o->get_dist_to_traj_lateral(traj_points) < + obstacle_filtering_param_.yield_lat_distance_threshold + + obstacle_filtering_param_.max_lat_dist_between_obstacles; + if (is_within_lat_dist_threshold) stopped_objects.push_back(o); + return; + }); + + if (stopped_objects.empty() || moving_objects.empty()) return std::nullopt; + + std::sort(stopped_objects.begin(), stopped_objects.end(), [&](const auto & o1, const auto & o2) { + return o1->get_dist_from_ego_longitudinal(traj_points, current_pose.position) < + o2->get_dist_from_ego_longitudinal(traj_points, current_pose.position); + }); + + std::sort(moving_objects.begin(), moving_objects.end(), [&](const auto & o1, const auto & o2) { + return o1->get_dist_from_ego_longitudinal(traj_points, current_pose.position) < + o2->get_dist_from_ego_longitudinal(traj_points, current_pose.position); + }); + + std::vector yield_obstacles; + for (const auto & moving_object : moving_objects) { + for (const auto & stopped_object : stopped_objects) { + const bool is_moving_obs_behind_of_stopped_obs = + moving_object->get_dist_from_ego_longitudinal(traj_points, current_pose.position) < + stopped_object->get_dist_from_ego_longitudinal(traj_points, current_pose.position); + const bool is_moving_obs_ahead_of_ego_front = + moving_object->get_dist_from_ego_longitudinal(traj_points, current_pose.position) > + vehicle_info.vehicle_length_m; + + if (!is_moving_obs_ahead_of_ego_front || !is_moving_obs_behind_of_stopped_obs) continue; + + const double lateral_distance_between_obstacles = std::abs( + moving_object->get_dist_to_traj_lateral(traj_points) - + stopped_object->get_dist_to_traj_lateral(traj_points)); + + const double longitudinal_distance_between_obstacles = std::abs( + moving_object->get_dist_from_ego_longitudinal(traj_points, current_pose.position) - + stopped_object->get_dist_from_ego_longitudinal(traj_points, current_pose.position)); + + const double moving_object_speed = std::hypot( + moving_object->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, + moving_object->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y); + + const bool are_obstacles_aligned = lateral_distance_between_obstacles < + obstacle_filtering_param_.max_lat_dist_between_obstacles; + const bool obstacles_collide_within_threshold_time = + longitudinal_distance_between_obstacles / moving_object_speed < + obstacle_filtering_param_.max_obstacles_collision_time; + if (are_obstacles_aligned && obstacles_collide_within_threshold_time) { + const auto yield_obstacle = + create_yield_cruise_obstacle(moving_object, predicted_objects_stamp, traj_points); + if (yield_obstacle) { + yield_obstacles.push_back(*yield_obstacle); + using autoware::objects_of_interest_marker_interface::ColorName; + objects_of_interest_marker_interface_->insertObjectData( + stopped_object->predicted_object.kinematics.initial_pose_with_covariance.pose, + stopped_object->predicted_object.shape, ColorName::RED); + } + } + } + } + if (yield_obstacles.empty()) return std::nullopt; + return yield_obstacles; +} + +std::optional> +ObstacleCruiseModule::create_collision_points_for_inside_cruise_obstacle( + const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector & decimated_traj_polys, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const bool is_driving_forward, const VehicleInfo & vehicle_info, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const +{ + const auto & obj_uuid = object->predicted_object.object_id; + const auto & obj_uuid_str = autoware::universe_utils::toHexString(obj_uuid); + + // check label + if (!is_inside_cruise_obstacle(object->predicted_object.classification.at(0).label)) { + RCLCPP_DEBUG( + logger_, "[Cruise] Ignore inside obstacle (%s) since its type is not designated.", + obj_uuid_str.substr(0, 4).c_str()); + return std::nullopt; + } + + { // consider hysteresis + // const bool is_prev_obstacle_stop = get_obstacle_from_uuid(prev_stop_obstacles_, + // obstacle.uuid).has_value(); + const bool is_prev_obstacle_cruise = + utils::get_obstacle_from_uuid(prev_cruise_object_obstacles_, obj_uuid_str).has_value(); + + if (is_prev_obstacle_cruise) { + if ( + object->get_lon_vel_relative_to_traj(traj_points) < + obstacle_filtering_param_.obstacle_velocity_threshold_from_cruise) { + return std::nullopt; + } + // NOTE: else is keeping cruise + } else { + // If previous obstacle is stop or does not exist. + if ( + object->get_lon_vel_relative_to_traj(traj_points) < + obstacle_filtering_param_.obstacle_velocity_threshold_to_cruise) { + return std::nullopt; + } + // NOTE: else is cruise from stop + } + } + + // Get highest confidence predicted path + constexpr double prediction_resampling_time_interval = 0.1; + constexpr double prediction_resampling_time_horizon = 10.0; + std::vector predicted_paths; + for (const auto & path : object->predicted_object.kinematics.predicted_paths) { + predicted_paths.push_back(path); + } + const auto resampled_predicted_paths = resample_highest_confidence_predicted_paths( + predicted_paths, prediction_resampling_time_interval, prediction_resampling_time_horizon, 1); + + if (resampled_predicted_paths.empty()) { + return std::nullopt; + } + + // calculate nearest collision point + std::vector collision_index; + const auto collision_points = polygon_utils::get_collision_points( + decimated_traj_points, decimated_traj_polys, predicted_objects_stamp, + resampled_predicted_paths.front(), object->predicted_object.shape, clock_->now(), + is_driving_forward, collision_index, + utils::calc_object_possible_max_dist_from_center(object->predicted_object.shape) + + trajectory_polygon_collision_check.decimate_trajectory_step_length + + std::hypot( + vehicle_info.vehicle_length_m, + vehicle_info.vehicle_width_m * 0.5 + obstacle_filtering_param_.max_lat_margin)); + return collision_points; +} + +std::optional> +ObstacleCruiseModule::create_collision_points_for_outside_cruise_obstacle( + const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector & decimated_traj_polys, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const bool is_driving_forward, const VehicleInfo & vehicle_info, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const +{ + const auto & obj_uuid = object->predicted_object.object_id; + const auto & obj_uuid_str = autoware::universe_utils::toHexString(obj_uuid); + + // check label + if (!is_outside_cruise_obstacle(object->predicted_object.classification.at(0).label)) { + RCLCPP_DEBUG( + logger_, "[Cruise] Ignore outside obstacle (%s) since its type is not designated.", + obj_uuid_str.substr(0, 4).c_str()); + return std::nullopt; + } + + if ( + std::hypot( + object->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, + object->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y) < + obstacle_filtering_param_.outside_obstacle_velocity_threshold) { + RCLCPP_DEBUG( + logger_, "[Cruise] Ignore outside obstacle (%s) since the obstacle velocity is low.", + obj_uuid_str.substr(0, 4).c_str()); + return std::nullopt; + } + + // Get the highest confidence predicted paths + constexpr double prediction_resampling_time_interval = 0.1; + constexpr double prediction_resampling_time_horizon = 10.0; + std::vector predicted_paths; + for (const auto & path : object->predicted_object.kinematics.predicted_paths) { + predicted_paths.push_back(path); + } + + const auto resampled_predicted_paths = resample_highest_confidence_predicted_paths( + predicted_paths, prediction_resampling_time_interval, prediction_resampling_time_horizon, + obstacle_filtering_param_.num_of_predicted_paths_for_outside_cruise_obstacle); + + // calculate collision condition for cruise + std::vector collision_index; + const auto get_collision_points = [&]() -> std::vector { + for (const auto & predicted_path : resampled_predicted_paths) { + const auto collision_points = polygon_utils::get_collision_points( + decimated_traj_points, decimated_traj_polys, predicted_objects_stamp, predicted_path, + object->predicted_object.shape, clock_->now(), is_driving_forward, collision_index, + utils::calc_object_possible_max_dist_from_center(object->predicted_object.shape) + + trajectory_polygon_collision_check.decimate_trajectory_step_length + + std::hypot( + vehicle_info.vehicle_length_m, + vehicle_info.vehicle_width_m * 0.5 + obstacle_filtering_param_.max_lat_margin), + obstacle_filtering_param_.max_prediction_time_for_collision_check); + if (!collision_points.empty()) { + return collision_points; + } + } + return {}; + }; + + const auto collision_points = get_collision_points(); + + if (collision_points.empty()) { + // Ignore vehicle obstacles outside the trajectory without collision + RCLCPP_DEBUG( + logger_, "[Cruise] Ignore outside obstacle (%s) since there are no collision points.", + obj_uuid_str.substr(0, 4).c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(object); + return std::nullopt; + } + + const double overlap_time = + (rclcpp::Time(collision_points.back().stamp) - rclcpp::Time(collision_points.front().stamp)) + .seconds(); + if (overlap_time < obstacle_filtering_param_.ego_obstacle_overlap_time_threshold) { + // Ignore vehicle obstacles outside the trajectory, whose predicted path + // overlaps the ego trajectory in a certain time. + RCLCPP_DEBUG( + logger_, "[Cruise] Ignore outside obstacle (%s) since it will not collide with the ego.", + obj_uuid_str.substr(0, 4).c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(object); + return std::nullopt; + } + + // Ignore obstacles behind the ego vehicle. + // Note: Only using isFrontObstacle(), behind obstacles cannot be filtered + // properly when the trajectory is crossing or overlapping. + const size_t first_collision_index = collision_index.front(); + if (!is_front_collide_obstacle(traj_points, object, first_collision_index)) { + return std::nullopt; + } + return collision_points; +} + +std::optional ObstacleCruiseModule::create_yield_cruise_obstacle( + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const std::vector & traj_points) +{ + if (traj_points.empty()) return std::nullopt; + // check label + + const auto & obj_uuid = object->predicted_object.object_id; + const auto & obj_uuid_str = autoware::universe_utils::toHexString(obj_uuid); + + if (!is_outside_cruise_obstacle(object->predicted_object.classification.at(0).label)) { + RCLCPP_DEBUG( + logger_, "[Cruise] Ignore yield obstacle (%s) since its type is not designated.", + obj_uuid_str.substr(0, 4).c_str()); + return std::nullopt; + } + + if ( + std::hypot( + object->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, + object->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y) < + obstacle_filtering_param_.outside_obstacle_velocity_threshold) { + RCLCPP_DEBUG( + logger_, "[Cruise] Ignore yield obstacle (%s) since the obstacle velocity is low.", + obj_uuid_str.substr(0, 4).c_str()); + return std::nullopt; + } + + if (is_obstacle_crossing(traj_points, object)) { + RCLCPP_DEBUG( + logger_, "[Cruise] Ignore yield obstacle (%s) since it's crossing the ego's trajectory..", + obj_uuid_str.substr(0, 4).c_str()); + return std::nullopt; + } + + const auto collision_points = [&]() -> std::optional> { + const auto obstacle_idx = autoware::motion_utils::findNearestIndex( + traj_points, object->predicted_object.kinematics.initial_pose_with_covariance.pose); + if (!obstacle_idx) return std::nullopt; + const auto collision_traj_point = traj_points.at(obstacle_idx.value()); + const auto object_time = clock_->now() + traj_points.at(obstacle_idx.value()).time_from_start; + + polygon_utils::PointWithStamp collision_traj_point_with_stamp; + collision_traj_point_with_stamp.stamp = object_time; + collision_traj_point_with_stamp.point.x = collision_traj_point.pose.position.x; + collision_traj_point_with_stamp.point.y = collision_traj_point.pose.position.y; + collision_traj_point_with_stamp.point.z = collision_traj_point.pose.position.z; + std::vector collision_points_vector{ + collision_traj_point_with_stamp}; + return collision_points_vector; + }(); + + if (!collision_points) return std::nullopt; + // check if obstacle is driving on the opposite direction + if (object->get_lon_vel_relative_to_traj(traj_points) < 0.0) return std::nullopt; + return CruiseObstacle{ + obj_uuid_str, + predicted_objects_stamp, + object->get_predicted_pose(clock_->now(), predicted_objects_stamp), + object->get_lon_vel_relative_to_traj(traj_points), + object->get_lat_vel_relative_to_traj(traj_points), + collision_points.value(), + true}; +} + +bool ObstacleCruiseModule::is_inside_cruise_obstacle(const uint8_t label) const +{ + const auto & types = obstacle_filtering_param_.inside_object_types; + return std::find(types.begin(), types.end(), label) != types.end(); +} + +bool ObstacleCruiseModule::is_outside_cruise_obstacle(const uint8_t label) const +{ + const auto & types = obstacle_filtering_param_.outside_object_types; + return std::find(types.begin(), types.end(), label) != types.end(); +} + +bool ObstacleCruiseModule::is_cruise_obstacle(const uint8_t label) const +{ + return is_inside_cruise_obstacle(label) || is_outside_cruise_obstacle(label); +} + +bool ObstacleCruiseModule::is_front_collide_obstacle( + const std::vector & traj_points, + const std::shared_ptr object, const size_t first_collision_idx) const +{ + const auto obstacle_idx = autoware::motion_utils::findNearestIndex( + traj_points, object->predicted_object.kinematics.initial_pose_with_covariance.pose.position); + + const double obstacle_to_col_points_distance = + autoware::motion_utils::calcSignedArcLength(traj_points, obstacle_idx, first_collision_idx); + const double object_possible_max_dist_from_center = + utils::calc_object_possible_max_dist_from_center(object->predicted_object.shape); + + // If the obstacle is far in front of the collision point, the obstacle is behind the ego. + return obstacle_to_col_points_distance > -object_possible_max_dist_from_center; +} + +bool ObstacleCruiseModule::is_obstacle_crossing( + const std::vector & traj_points, + const std::shared_ptr object) const +{ + const double diff_angle = calc_diff_angle_against_trajectory( + traj_points, object->predicted_object.kinematics.initial_pose_with_covariance.pose); + + // NOTE: Currently predicted objects does not have orientation availability even + // though sometimes orientation is not available. + const bool is_obstacle_crossing_trajectory = + obstacle_filtering_param_.crossing_obstacle_traj_angle_threshold < std::abs(diff_angle) && + obstacle_filtering_param_.crossing_obstacle_traj_angle_threshold < M_PI - std::abs(diff_angle); + if (!is_obstacle_crossing_trajectory) { + return false; + } + + // Only obstacles crossing the ego's trajectory with high speed are considered. + return true; +} +} // namespace autoware::motion_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::motion_velocity_planner::ObstacleCruiseModule, + autoware::motion_velocity_planner::PluginModuleInterface) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp new file mode 100644 index 0000000000000..d9e65c2a3b7db --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp @@ -0,0 +1,137 @@ +// 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 OBSTACLE_CRUISE_MODULE_HPP_ +#define OBSTACLE_CRUISE_MODULE_HPP_ + +#include "autoware/motion_velocity_planner_common_universe/polygon_utils.hpp" +#include "autoware/motion_velocity_planner_common_universe/utils.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" +#include "cruise_planner_interface.hpp" +#include "metrics_manager.hpp" +#include "optimization_based_planner/optimization_based_planner.hpp" +#include "parameters.hpp" +#include "pid_based_planner/pid_based_planner.hpp" +#include "type_alias.hpp" +#include "types.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class ObstacleCruiseModule : 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 & raw_trajectory_points, + const std::vector & smoothed_trajectory_points, + const std::shared_ptr planner_data) override; + std::string get_module_name() const override; + +private: + std::string module_name_; + rclcpp::Clock::SharedPtr clock_{}; + + // ros parameters + std::string planning_algorithm_; + CommonParam common_param_; + CruisePlanningParam cruise_planning_param_; + ObstacleFilteringParam obstacle_filtering_param_; + + // common publisher + rclcpp::Publisher::SharedPtr debug_cruise_planning_info_pub_; + + // module publisher + rclcpp::Publisher::SharedPtr metrics_pub_; + std::unique_ptr + objects_of_interest_marker_interface_; + rclcpp::Publisher::SharedPtr + processing_time_detail_pub_; + + // cruise planner + std::unique_ptr cruise_planner_; + + // internal variables + autoware::universe_utils::StopWatch stop_watch_; + std::vector prev_cruise_object_obstacles_; + mutable std::shared_ptr debug_data_ptr_; + bool need_to_clear_velocity_limit_{false}; + MetricsManager metrics_manager_; + mutable std::shared_ptr time_keeper_; + + std::unique_ptr create_cruise_planner(rclcpp::Node & node) const; + std::vector filter_cruise_obstacle_for_predicted_object( + const Odometry & odometry, const double ego_nearest_dist_threshold, + const double ego_nearest_yaw_threshold, const std::vector & traj_points, + const std::vector> & objects, + const rclcpp::Time & predicted_objects_stamp, const bool is_driving_forward, + const VehicleInfo & vehicle_info, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check); + void publish_debug_info(); + std::optional create_cruise_obstacle( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector & decimated_traj_polys, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const double dist_from_obj_poly_to_traj_poly, const bool is_driving_forward, + const VehicleInfo & vehicle_info, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const; + std::optional> find_yield_cruise_obstacles( + const Odometry & odometry, const std::vector> & objects, + const rclcpp::Time & predicted_objects_stamp, const std::vector & traj_points, + const VehicleInfo & vehicle_info); + std::optional> + create_collision_points_for_inside_cruise_obstacle( + const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector & decimated_traj_polys, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const bool is_driving_forward, const VehicleInfo & vehicle_info, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const; + std::optional> + create_collision_points_for_outside_cruise_obstacle( + const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector & decimated_traj_polys, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const bool is_driving_forward, const VehicleInfo & vehicle_info, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const; + std::optional create_yield_cruise_obstacle( + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const std::vector & traj_points); + bool is_inside_cruise_obstacle(const uint8_t label) const; + bool is_outside_cruise_obstacle(const uint8_t label) const; + bool is_cruise_obstacle(const uint8_t label) const; + bool is_front_collide_obstacle( + const std::vector & traj_points, + const std::shared_ptr object, const size_t first_collision_idx) const; + bool is_obstacle_crossing( + const std::vector & traj_points, + const std::shared_ptr object) const; +}; +} // namespace autoware::motion_velocity_planner + +#endif // OBSTACLE_CRUISE_MODULE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/optimization_based_planner.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/optimization_based_planner.cpp new file mode 100644 index 0000000000000..c2489bbf3039f --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/optimization_based_planner.cpp @@ -0,0 +1,754 @@ +// 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 "optimization_based_planner.hpp" + +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/zero_order_hold.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_velocity_planner_common_universe/utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" + +#include +#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +namespace autoware::motion_velocity_planner +{ +constexpr double ZERO_VEL_THRESHOLD = 0.01; +constexpr double CLOSE_S_DIST_THRESHOLD = 1e-3; + +OptimizationBasedPlanner::OptimizationBasedPlanner( + rclcpp::Node & node, const CommonParam & common_param, + const CruisePlanningParam & cruise_planning_param) +: CruisePlannerInterface(node, common_param, cruise_planning_param) +{ + smoothed_traj_sub_ = node.create_subscription( + "/planning/scenario_planning/trajectory", rclcpp::QoS{1}, + [this](const Trajectory::ConstSharedPtr msg) { smoothed_trajectory_ptr_ = msg; }); + + // parameter + dense_resampling_time_interval_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.dense_resampling_time_interval"); + sparse_resampling_time_interval_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.sparse_resampling_time_interval"); + dense_time_horizon_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.dense_time_horizon"); + max_time_horizon_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.max_time_horizon"); + + t_dangerous_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.t_dangerous"); + velocity_margin_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.velocity_margin"); + + replan_vel_deviation_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.replan_vel_deviation"); + engage_velocity_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.engage_velocity"); + engage_acceleration_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.engage_acceleration"); + engage_exit_ratio_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.engage_exit_ratio"); + stop_dist_to_prohibit_engage_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.stop_dist_to_prohibit_engage"); + + const double max_s_weight = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.max_s_weight"); + const double max_v_weight = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.max_v_weight"); + const double over_s_safety_weight = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.over_s_safety_weight"); + const double over_s_ideal_weight = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.over_s_ideal_weight"); + const double over_v_weight = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.over_v_weight"); + const double over_a_weight = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.over_a_weight"); + const double over_j_weight = node.declare_parameter( + "obstacle_cruise.cruise_planning.optimization_based_planner.over_j_weight"); + + // velocity optimizer + velocity_optimizer_ptr_ = std::make_shared( + max_s_weight, max_v_weight, over_s_safety_weight, over_s_ideal_weight, over_v_weight, + over_a_weight, over_j_weight); + + // publisher + optimized_sv_pub_ = node.create_publisher("~/optimized_sv_trajectory", 1); + optimized_st_graph_pub_ = node.create_publisher("~/optimized_st_graph", 1); + boundary_pub_ = node.create_publisher("~/boundary", 1); + debug_wall_marker_pub_ = node.create_publisher("~/debug/wall_marker", 1); +} + +std::vector OptimizationBasedPlanner::plan_cruise( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const std::vector & obstacles, + [[maybe_unused]] std::shared_ptr debug_data_ptr, + [[maybe_unused]] std::unique_ptr & + planning_factor_interface, + [[maybe_unused]] std::optional & velocity_limit) +{ + // Create Time Vector defined by resampling time interval + const std::vector time_vec = createTimeVector(); + if (time_vec.size() < 2) { + RCLCPP_ERROR( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "Resolution size is not enough"); + prev_output_ = stop_traj_points; + return stop_traj_points; + } + + // Get the nearest point on the trajectory + const size_t closest_idx = + planner_data->find_segment_index(stop_traj_points, planner_data->current_odometry.pose.pose); + + // Compute maximum velocity + double v_max = 0.0; + for (const auto & point : stop_traj_points) { + v_max = std::max(v_max, static_cast(point.longitudinal_velocity_mps)); + } + + // Get Current Velocity + double v0; + double a0; + std::tie(v0, a0) = calcInitialMotion(planner_data, stop_traj_points, prev_output_); + a0 = std::min(common_param_.max_accel, std::max(a0, common_param_.min_accel)); + + // Check trajectory size + if (stop_traj_points.size() - closest_idx <= 2) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "The number of points on the trajectory is too small or failed to calculate front offset"); + prev_output_ = stop_traj_points; + return stop_traj_points; + } + + // Check if reached goal + if (checkHasReachedGoal(planner_data, stop_traj_points)) { + prev_output_ = stop_traj_points; + return stop_traj_points; + } + + // Get S Boundaries from the obstacle + const auto s_boundaries = getSBoundaries(planner_data, stop_traj_points, obstacles, time_vec); + if (!s_boundaries) { + prev_output_ = stop_traj_points; + return stop_traj_points; + } + + // Optimization + VelocityOptimizer::OptimizationData data; + data.time_vec = time_vec; + data.s0 = 0.0; + data.a0 = a0; + data.v_max = v_max; + data.a_max = common_param_.max_accel; + data.a_min = common_param_.min_accel; + data.j_max = common_param_.max_jerk; + data.j_min = common_param_.min_jerk; + data.limit_a_max = common_param_.limit_max_accel; + data.limit_a_min = common_param_.limit_min_accel; + data.limit_j_max = common_param_.limit_max_jerk; + data.limit_j_min = common_param_.limit_min_jerk; + data.t_dangerous = t_dangerous_; + data.idling_time = cruise_planning_param_.idling_time; + data.s_boundary = *s_boundaries; + data.v0 = v0; + RCLCPP_DEBUG(rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), "v0 %f", v0); + + const auto optimized_result = velocity_optimizer_ptr_->optimize(data); + + // Publish Debug trajectories + const double traj_front_to_vehicle_offset = + autoware::motion_utils::calcSignedArcLength(stop_traj_points, 0, closest_idx); + publishDebugTrajectory( + planner_data, traj_front_to_vehicle_offset, time_vec, *s_boundaries, optimized_result); + + // Transformation from t to s + const auto processed_result = + processOptimizedResult(data.v0, optimized_result, traj_front_to_vehicle_offset); + if (!processed_result) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "Processed Result is empty"); + prev_output_ = stop_traj_points; + return stop_traj_points; + } + const auto & opt_position = processed_result->s; + const auto & opt_velocity = processed_result->v; + + // Check Size + if (opt_position.size() == 1 && opt_velocity.front() < ZERO_VEL_THRESHOLD) { + auto output = stop_traj_points; + output.at(closest_idx).longitudinal_velocity_mps = data.v0; + for (size_t i = closest_idx + 1; i < output.size(); ++i) { + output.at(i).longitudinal_velocity_mps = 0.0; + } + prev_output_ = output; + return output; + } else if (opt_position.size() == 1) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "Optimized Trajectory is too small"); + prev_output_ = stop_traj_points; + return stop_traj_points; + } + + // Get Zero Velocity Position + double closest_stop_dist = std::numeric_limits::max(); + for (size_t i = 0; i < opt_velocity.size(); ++i) { + if (opt_velocity.at(i) < ZERO_VEL_THRESHOLD) { + closest_stop_dist = std::min(closest_stop_dist, opt_position.at(i)); + break; + } + } + const auto traj_stop_dist = + autoware::motion_utils::calcDistanceToForwardStopPoint(stop_traj_points, closest_idx); + if (traj_stop_dist) { + closest_stop_dist = std::min(*traj_stop_dist + traj_front_to_vehicle_offset, closest_stop_dist); + } + + // Resample Optimum Velocity + size_t break_id = stop_traj_points.size(); + std::vector resampled_opt_position; + for (size_t i = closest_idx; i < stop_traj_points.size(); ++i) { + const double query_s = std::max( + autoware::motion_utils::calcSignedArcLength(stop_traj_points, 0, i), opt_position.front()); + if (query_s > opt_position.back()) { + break_id = i; + break; + } + resampled_opt_position.push_back(query_s); + } + // resample optimum velocity for original each position + auto resampled_opt_velocity = + autoware::interpolation::lerp(opt_position, opt_velocity, resampled_opt_position); + for (size_t i = break_id; i < stop_traj_points.size(); ++i) { + resampled_opt_velocity.push_back(stop_traj_points.at(i).longitudinal_velocity_mps); + } + + // Create Output Data + std::vector output = stop_traj_points; + for (size_t i = 0; i < closest_idx; ++i) { + output.at(i).longitudinal_velocity_mps = data.v0; + } + for (size_t i = closest_idx; i < output.size(); ++i) { + output.at(i).longitudinal_velocity_mps = + resampled_opt_velocity.at(i - closest_idx) + velocity_margin_; + } + output.back().longitudinal_velocity_mps = 0.0; // terminal velocity is zero + + // Insert Closest Stop Point + autoware::motion_utils::insertStopPoint(0, closest_stop_dist, output); + + prev_output_ = output; + return output; +} + +std::vector OptimizationBasedPlanner::createTimeVector() +{ + std::vector time_vec; + for (double t = 0.0; t < dense_time_horizon_; t += dense_resampling_time_interval_) { + time_vec.push_back(t); + } + if (dense_time_horizon_ - time_vec.back() < 1e-3) { + time_vec.back() = dense_time_horizon_; + } else { + time_vec.push_back(dense_time_horizon_); + } + + for (double t = dense_time_horizon_ + sparse_resampling_time_interval_; t < max_time_horizon_; + t += sparse_resampling_time_interval_) { + time_vec.push_back(t); + } + if (max_time_horizon_ - time_vec.back() < 1e-3) { + time_vec.back() = max_time_horizon_; + } else { + time_vec.push_back(max_time_horizon_); + } + + return time_vec; +} + +// v0, a0 +std::tuple OptimizationBasedPlanner::calcInitialMotion( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const std::vector & prev_traj_points) +{ + const auto & ego_vel = planner_data->current_odometry.twist.twist.linear.x; + const auto & ego_pose = planner_data->current_odometry.pose.pose; + const auto & input_traj_points = stop_traj_points; + const double vehicle_speed{std::abs(ego_vel)}; + const auto current_closest_point = autoware::motion_utils::calcInterpolatedPoint( + autoware::motion_utils::convertToTrajectory(input_traj_points), + planner_data->current_odometry.pose.pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); + const double target_vel{std::abs(current_closest_point.longitudinal_velocity_mps)}; + + double initial_vel{}; + double initial_acc{}; + + // first time + if (prev_traj_points.empty()) { + initial_vel = vehicle_speed; + initial_acc = 0.0; + return std::make_tuple(initial_vel, initial_acc); + } + + TrajectoryPoint prev_output_closest_point; + if (smoothed_trajectory_ptr_) { + prev_output_closest_point = autoware::motion_utils::calcInterpolatedPoint( + *smoothed_trajectory_ptr_, ego_pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); + } else { + prev_output_closest_point = autoware::motion_utils::calcInterpolatedPoint( + autoware::motion_utils::convertToTrajectory(prev_traj_points), ego_pose, + planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); + } + + // when velocity tracking deviation is large + const double desired_vel{prev_output_closest_point.longitudinal_velocity_mps}; + const double vel_error{vehicle_speed - std::abs(desired_vel)}; + if (std::abs(vel_error) > replan_vel_deviation_) { + initial_vel = vehicle_speed; // use current vehicle speed + initial_acc = prev_output_closest_point.acceleration_mps2; + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "calcInitialMotion : Large deviation error for speed control. Use current speed for " + "initial value, desired_vel = %f, vehicle_speed = %f, vel_error = %f, error_thr = %f", + desired_vel, vehicle_speed, vel_error, replan_vel_deviation_); + return std::make_tuple(initial_vel, initial_acc); + } + + // if current vehicle velocity is low && base_desired speed is high, + // use engage_velocity for engage vehicle + const double engage_vel_thr = engage_velocity_ * engage_exit_ratio_; + if (vehicle_speed < engage_vel_thr) { + if (target_vel >= engage_velocity_) { + const auto stop_dist = autoware::motion_utils::calcDistanceToForwardStopPoint( + input_traj_points, ego_pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); + if (!stop_dist.has_value()) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "calcInitialMotion : vehicle speed is low (%.3f), and desired speed is high (%.3f). Use " + "engage speed (%.3f) until vehicle speed reaches engage_vel_thr (%.3f)", + vehicle_speed, target_vel, engage_velocity_, engage_vel_thr); + return std::make_tuple(engage_velocity_, engage_acceleration_); + } else if (stop_dist.value() > stop_dist_to_prohibit_engage_) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "calcInitialMotion : vehicle speed is low (%.3f), and desired speed is high (%.3f). Use " + "engage speed (%.3f) until vehicle speed reaches engage_vel_thr (%.3f). stop_dist = %.3f", + vehicle_speed, target_vel, engage_velocity_, engage_vel_thr, stop_dist.value()); + return std::make_tuple(engage_velocity_, engage_acceleration_); + } else { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "calcInitialMotion : stop point is close (%.3f[m]). no engage.", stop_dist.value()); + } + } else if (target_vel > 0.0) { + auto clock{rclcpp::Clock{RCL_ROS_TIME}}; + RCLCPP_WARN_THROTTLE( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), clock, 3000, + "calcInitialMotion : target velocity(%.3f[m/s]) is lower than engage velocity(%.3f[m/s]). ", + target_vel, engage_velocity_); + } + } + + // normal update: use closest in prev_output + initial_vel = prev_output_closest_point.longitudinal_velocity_mps; + initial_acc = prev_output_closest_point.acceleration_mps2; + return std::make_tuple(initial_vel, initial_acc); +} + +bool OptimizationBasedPlanner::checkHasReachedGoal( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points) +{ + // If goal is close and current velocity is low, we don't optimize trajectory + const auto closest_stop_dist = autoware::motion_utils::calcDistanceToForwardStopPoint( + stop_traj_points, planner_data->current_odometry.pose.pose, + planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); + if ( + closest_stop_dist && *closest_stop_dist < 0.5 && + planner_data->current_odometry.twist.twist.linear.x < 0.6) { + return true; + } + + return false; +} + +std::optional OptimizationBasedPlanner::getSBoundaries( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const std::vector & obstacles, const std::vector & time_vec) +{ + if (stop_traj_points.empty()) { + return std::nullopt; + } + + const auto traj_length = calcTrajectoryLengthFromCurrentPose( + stop_traj_points, planner_data->current_odometry.pose.pose, planner_data); + if (!traj_length) { + return {}; + } + + SBoundaries s_boundaries(time_vec.size()); + for (size_t i = 0; i < s_boundaries.size(); ++i) { + s_boundaries.at(i).max_s = *traj_length; + } + + double min_slow_down_point_length = std::numeric_limits::max(); + std::optional min_slow_down_idx = {}; + for (size_t o_idx = 0; o_idx < obstacles.size(); ++o_idx) { + const auto & obj = obstacles.at(o_idx); + if (obj.collision_points.empty()) { + continue; + } + + // Step1 Get S boundary from the obstacle + const auto obj_s_boundaries = + getSBoundaries(planner_data, stop_traj_points, obj, time_vec, *traj_length); + if (!obj_s_boundaries) { + continue; + } + + // Step2 update s boundaries + for (size_t i = 0; i < obj_s_boundaries->size(); ++i) { + if (obj_s_boundaries->at(i).max_s < s_boundaries.at(i).max_s) { + s_boundaries.at(i) = obj_s_boundaries->at(i); + } + } + + // Step3 search nearest obstacle to follow for rviz marker + const double obj_vel = std::abs(obj.velocity); + const double rss_dist = + calc_rss_distance(planner_data->current_odometry.twist.twist.linear.x, obj_vel); + + const auto & safe_distance_margin = cruise_planning_param_.safe_distance_margin; + const double ego_obj_length = autoware::motion_utils::calcSignedArcLength( + stop_traj_points, planner_data->current_odometry.pose.pose.position, + obj.collision_points.front().point); + const double slow_down_point_length = ego_obj_length - (rss_dist + safe_distance_margin); + + if (slow_down_point_length < min_slow_down_point_length) { + min_slow_down_point_length = slow_down_point_length; + min_slow_down_idx = o_idx; + } + } + + // Publish wall marker for slowing down or stopping + if (min_slow_down_idx) { + const auto & current_time = clock_->now(); + + const auto marker_pose = autoware::motion_utils::calcLongitudinalOffsetPose( + stop_traj_points, planner_data->current_odometry.pose.pose.position, + min_slow_down_point_length); + + if (marker_pose) { + MarkerArray wall_msg; + + const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( + marker_pose.value(), "obstacle to follow", current_time, 0); + autoware::universe_utils::appendMarkerArray(markers, &wall_msg); + + // publish rviz marker + debug_wall_marker_pub_->publish(wall_msg); + } + } + + return s_boundaries; +} + +std::optional OptimizationBasedPlanner::getSBoundaries( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, const CruiseObstacle & object, + const std::vector & time_vec, const double traj_length) +{ + if (object.collision_points.empty()) { + return {}; + } + + const bool onEgoTrajectory = + checkOnTrajectory(planner_data, stop_traj_points, object.collision_points.front()); + const auto & safe_distance_margin = cruise_planning_param_.safe_distance_margin; + + // If the object is on the current ego trajectory, + // we assume the object travels along ego trajectory + if (onEgoTrajectory) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "On Trajectory Object"); + + return getSBoundariesForOnTrajectoryObject( + stop_traj_points, planner_data, time_vec, safe_distance_margin, object, traj_length); + } + + // Ignore low velocity objects that are not on the trajectory + return getSBoundariesForOffTrajectoryObject( + stop_traj_points, planner_data, time_vec, safe_distance_margin, object, traj_length); +} + +std::optional OptimizationBasedPlanner::getSBoundariesForOnTrajectoryObject( + const std::vector & traj_points, + const std::shared_ptr planner_data, const std::vector & time_vec, + const double safety_distance, const CruiseObstacle & object, const double traj_length) const +{ + const double & min_object_accel_for_rss = cruise_planning_param_.min_object_accel_for_rss; + + SBoundaries s_boundaries(time_vec.size()); + for (size_t i = 0; i < s_boundaries.size(); ++i) { + s_boundaries.at(i).max_s = traj_length; + } + + const double v_obj = std::abs(object.velocity); + + const double dist_to_collision_point = calc_distance_to_collisionPoint( + traj_points, planner_data, object.collision_points.front().point); + + double current_s_obj = std::max(dist_to_collision_point - safety_distance, 0.0); + const double current_v_obj = v_obj; + const double initial_s_upper_bound = + current_s_obj + (current_v_obj * current_v_obj) / (2 * std::fabs(min_object_accel_for_rss)); + s_boundaries.front().max_s = std::clamp(initial_s_upper_bound, 0.0, s_boundaries.front().max_s); + s_boundaries.front().is_object = true; + for (size_t i = 1; i < time_vec.size(); ++i) { + const double dt = time_vec.at(i) - time_vec.at(i - 1); + current_s_obj = current_s_obj + current_v_obj * dt; + + const double s_upper_bound = + current_s_obj + (current_v_obj * current_v_obj) / (2 * std::fabs(min_object_accel_for_rss)); + s_boundaries.at(i).max_s = std::clamp(s_upper_bound, 0.0, s_boundaries.at(i).max_s); + s_boundaries.at(i).is_object = true; + } + + return s_boundaries; +} + +std::optional OptimizationBasedPlanner::getSBoundariesForOffTrajectoryObject( + const std::vector & traj_points, + const std::shared_ptr planner_data, const std::vector & time_vec, + const double safety_distance, const CruiseObstacle & object, const double traj_length) +{ + const auto & current_time = clock_->now(); + const double & min_object_accel_for_rss = cruise_planning_param_.min_object_accel_for_rss; + + const double v_obj = std::abs(object.velocity); + + SBoundaries s_boundaries(time_vec.size()); + for (size_t i = 0; i < s_boundaries.size(); ++i) { + s_boundaries.at(i).max_s = traj_length; + } + + for (const auto & collision_point : object.collision_points) { + const double object_time = (rclcpp::Time(collision_point.stamp) - current_time).seconds(); + if (object_time < 0) { + // Ignore Past Positions + continue; + } + + const double dist_to_collision_point = + calc_distance_to_collisionPoint(traj_points, planner_data, collision_point.point); + if (!dist_to_collision_point) { + continue; + } + + const double current_s_obj = std::max(dist_to_collision_point - safety_distance, 0.0); + const double s_upper_bound = + current_s_obj + (v_obj * v_obj) / (2 * std::fabs(min_object_accel_for_rss)); + + size_t object_time_segment_idx = 0; + for (size_t i = 0; i < time_vec.size() - 1; ++i) { + if (time_vec.at(i) < object_time && object_time < time_vec.at(i + 1)) { + object_time_segment_idx = i; + break; + } + } + + for (size_t i = 0; i <= object_time_segment_idx + 1; ++i) { + if (time_vec.at(i) < object_time && s_upper_bound < s_boundaries.at(i).max_s) { + s_boundaries.at(i).max_s = std::max(0.0, s_upper_bound); + s_boundaries.at(i).is_object = true; + } + } + } + + return s_boundaries; +} + +bool OptimizationBasedPlanner::checkOnTrajectory( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const polygon_utils::PointWithStamp & point) +{ + // If the collision point is in the future, we return false + if ((rclcpp::Time(point.stamp) - clock_->now()).seconds() > 0.1) { + return false; + } + + const double lateral_offset = + std::fabs(autoware::motion_utils::calcLateralOffset(stop_traj_points, point.point)); + + if (lateral_offset < planner_data->vehicle_info_.max_lateral_offset_m + 0.1) { + return true; + } + + return false; +} + +std::optional OptimizationBasedPlanner::calcTrajectoryLengthFromCurrentPose( + const std::vector & traj_points, const geometry_msgs::msg::Pose & ego_pose, + const std::shared_ptr planner_data) +{ + const size_t ego_segment_idx = planner_data->find_segment_index(traj_points, ego_pose); + + const double traj_length = autoware::motion_utils::calcSignedArcLength( + traj_points, ego_pose.position, ego_segment_idx, traj_points.size() - 1); + + const auto dist_to_closest_stop_point = autoware::motion_utils::calcDistanceToForwardStopPoint( + traj_points, ego_pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); + if (dist_to_closest_stop_point) { + return std::min(traj_length, dist_to_closest_stop_point.value()); + } + + return traj_length; +} + +std::optional +OptimizationBasedPlanner::processOptimizedResult( + const double v0, const VelocityOptimizer::OptimizationResult & opt_result, const double offset) +{ + if ( + opt_result.t.empty() || opt_result.s.empty() || opt_result.v.empty() || opt_result.a.empty() || + opt_result.j.empty()) { + return std::nullopt; + } + + size_t break_id = opt_result.s.size(); + VelocityOptimizer::OptimizationResult processed_result; + processed_result.t.push_back(0.0); + processed_result.s.push_back(offset); + processed_result.v.push_back(v0); + processed_result.a.push_back(opt_result.a.front()); + processed_result.j.push_back(opt_result.j.front()); + + for (size_t i = 1; i < opt_result.s.size(); ++i) { + const double prev_s = processed_result.s.back(); + const double current_s = std::max(opt_result.s.at(i), 0.0) + offset; + const double current_v = opt_result.v.at(i); + if (prev_s >= current_s) { + processed_result.v.back() = 0.0; + break_id = i; + break; + } else if (current_v < ZERO_VEL_THRESHOLD) { + break_id = i; + break; + } else if (std::fabs(current_s - prev_s) < CLOSE_S_DIST_THRESHOLD) { + processed_result.v.back() = current_v; + processed_result.s.back() = current_s; + continue; + } + + processed_result.t.push_back(opt_result.t.at(i)); + processed_result.s.push_back(current_s); + processed_result.v.push_back(current_v); + processed_result.a.push_back(opt_result.a.at(i)); + processed_result.j.push_back(opt_result.j.at(i)); + } + + // Padding Zero Velocity after break id + for (size_t i = break_id; i < opt_result.s.size(); ++i) { + const double prev_s = processed_result.s.back(); + const double current_s = std::max(opt_result.s.at(i), 0.0) + offset; + if (prev_s >= current_s) { + processed_result.v.back() = 0.0; + continue; + } else if (std::fabs(current_s - prev_s) < CLOSE_S_DIST_THRESHOLD) { + processed_result.v.back() = 0.0; + processed_result.s.back() = current_s; + continue; + } + processed_result.t.push_back(opt_result.t.at(i)); + processed_result.s.push_back(current_s); + processed_result.v.push_back(0.0); + processed_result.a.push_back(0.0); + processed_result.j.push_back(0.0); + } + + return processed_result; +} + +void OptimizationBasedPlanner::publishDebugTrajectory( + [[maybe_unused]] const std::shared_ptr planner_data, const double offset, + const std::vector & time_vec, const SBoundaries & s_boundaries, + const VelocityOptimizer::OptimizationResult & opt_result) +{ + const auto & current_time = clock_->now(); + // Publish optimized result and boundary + Trajectory boundary_traj; + boundary_traj.header.stamp = current_time; + boundary_traj.points.resize(s_boundaries.size()); + double boundary_s_max = 0.0; + for (size_t i = 0; i < s_boundaries.size(); ++i) { + const double bound_s = s_boundaries.at(i).max_s; + const double bound_t = time_vec.at(i); + boundary_traj.points.at(i).pose.position.x = bound_s; + boundary_traj.points.at(i).pose.position.y = bound_t; + boundary_s_max = std::max(bound_s, boundary_s_max); + } + boundary_pub_->publish(boundary_traj); + + Trajectory optimized_sv_traj; + optimized_sv_traj.header.stamp = current_time; + optimized_sv_traj.points.resize(opt_result.s.size()); + for (size_t i = 0; i < opt_result.s.size(); ++i) { + const double s = opt_result.s.at(i); + const double v = opt_result.v.at(i); + optimized_sv_traj.points.at(i).pose.position.x = s + offset; + optimized_sv_traj.points.at(i).pose.position.y = v; + } + optimized_sv_pub_->publish(optimized_sv_traj); + + Trajectory optimized_st_graph; + optimized_st_graph.header.stamp = current_time; + optimized_st_graph.points.resize(opt_result.s.size()); + for (size_t i = 0; i < opt_result.s.size(); ++i) { + const double bound_s = opt_result.s.at(i); + const double bound_t = opt_result.t.at(i); + optimized_st_graph.points.at(i).pose.position.x = bound_s; + optimized_st_graph.points.at(i).pose.position.y = bound_t; + } + optimized_st_graph_pub_->publish(optimized_st_graph); +} +} // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/optimization_based_planner.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/optimization_based_planner.hpp new file mode 100644 index 0000000000000..efb924159d471 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/optimization_based_planner.hpp @@ -0,0 +1,141 @@ +// 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 OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_ // NOLINT +#define OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_ // NOLINT + +#include "../cruise_planner_interface.hpp" +#include "../type_alias.hpp" +#include "../types.hpp" +#include "s_boundary.hpp" +#include "velocity_optimizer.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class OptimizationBasedPlanner : public CruisePlannerInterface +{ +public: + OptimizationBasedPlanner( + rclcpp::Node & node, const CommonParam & common_param, + const CruisePlanningParam & cruise_planning_param); + + std::vector plan_cruise( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const std::vector & obstacles, std::shared_ptr debug_data_ptr, + std::unique_ptr & + planning_factor_interface, + std::optional & velocity_limit) override; + + void update_parameters( + [[maybe_unused]] const std::vector & parameters) override + { + } + +private: + // Member Functions + std::vector createTimeVector(); + std::tuple calcInitialMotion( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const std::vector & prev_traj_points); + + static bool checkHasReachedGoal( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points); + + std::optional getSBoundaries( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const std::vector & obstacles, const std::vector & time_vec); + + std::optional getSBoundaries( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, const CruiseObstacle & object, + const std::vector & time_vec, const double traj_length); + + std::optional getSBoundariesForOnTrajectoryObject( + const std::vector & traj_points, + const std::shared_ptr planner_data, const std::vector & time_vec, + const double safety_distance, const CruiseObstacle & object, const double traj_length) const; + + std::optional getSBoundariesForOffTrajectoryObject( + const std::vector & traj_points, + const std::shared_ptr planner_data, const std::vector & time_vec, + const double safety_distance, const CruiseObstacle & object, const double traj_length); + + bool checkOnTrajectory( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const polygon_utils::PointWithStamp & point); + + static std::optional calcTrajectoryLengthFromCurrentPose( + const std::vector & traj_points, const geometry_msgs::msg::Pose & ego_pose, + const std::shared_ptr planner_data); + + geometry_msgs::msg::Pose transformBaseLink2Center( + const geometry_msgs::msg::Pose & pose_base_link); + + static std::optional processOptimizedResult( + const double v0, const VelocityOptimizer::OptimizationResult & opt_result, const double offset); + + void publishDebugTrajectory( + [[maybe_unused]] const std::shared_ptr planner_data, const double offset, + const std::vector & time_vec, const SBoundaries & s_boundaries, + const VelocityOptimizer::OptimizationResult & opt_result); + + std::vector prev_output_; + + // Velocity Optimizer + std::shared_ptr velocity_optimizer_ptr_; + + // Publisher + rclcpp::Publisher::SharedPtr boundary_pub_; + rclcpp::Publisher::SharedPtr optimized_sv_pub_; + rclcpp::Publisher::SharedPtr optimized_st_graph_pub_; + rclcpp::Publisher::SharedPtr debug_wall_marker_pub_; + + // Subscriber + rclcpp::Subscription::SharedPtr smoothed_traj_sub_; + + Trajectory::ConstSharedPtr smoothed_trajectory_ptr_{nullptr}; + + // Resampling Parameter + double dense_resampling_time_interval_; + double sparse_resampling_time_interval_; + double dense_time_horizon_; + double max_time_horizon_; + + double t_dangerous_; + double velocity_margin_; + + double replan_vel_deviation_; + double engage_velocity_; + double engage_acceleration_; + double engage_exit_ratio_; + double stop_dist_to_prohibit_engage_; +}; +} // namespace autoware::motion_velocity_planner +// clang-format off +#endif // OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_ // NOLINT +// clang-format on diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/s_boundary.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/s_boundary.hpp new file mode 100644 index 0000000000000..66d1ddfdc2b4e --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/s_boundary.hpp @@ -0,0 +1,36 @@ +// 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 OPTIMIZATION_BASED_PLANNER__S_BOUNDARY_HPP_ +#define OPTIMIZATION_BASED_PLANNER__S_BOUNDARY_HPP_ + +#include +#include + +namespace autoware::motion_velocity_planner +{ +class SBoundary +{ +public: + SBoundary(const double _max_s, const double _min_s) : max_s(_max_s), min_s(_min_s) {} + SBoundary() : max_s(std::numeric_limits::max()), min_s(0.0) {} + + double max_s = std::numeric_limits::max(); + double min_s = 0.0; + bool is_object = false; +}; + +using SBoundaries = std::vector; +} // namespace autoware::motion_velocity_planner + +#endif // OPTIMIZATION_BASED_PLANNER__S_BOUNDARY_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/velocity_optimizer.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/velocity_optimizer.cpp new file mode 100644 index 0000000000000..d8e8c1aff5474 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/velocity_optimizer.cpp @@ -0,0 +1,287 @@ +// 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 "velocity_optimizer.hpp" + +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +VelocityOptimizer::VelocityOptimizer( + const double max_s_weight, const double max_v_weight, const double over_s_safety_weight, + const double over_s_ideal_weight, const double over_v_weight, const double over_a_weight, + const double over_j_weight) +: max_s_weight_(max_s_weight), + max_v_weight_(max_v_weight), + over_s_safety_weight_(over_s_safety_weight), + over_s_ideal_weight_(over_s_ideal_weight), + over_v_weight_(over_v_weight), + over_a_weight_(over_a_weight), + over_j_weight_(over_j_weight) +{ + qp_solver_.updateMaxIter(200000); + qp_solver_.updateRhoInterval(0); // 0 means automatic + qp_solver_.updateEpsRel(1.0e-4); // def: 1.0e-4 + qp_solver_.updateEpsAbs(1.0e-8); // def: 1.0e-4 + qp_solver_.updateVerbose(false); +} + +VelocityOptimizer::OptimizationResult VelocityOptimizer::optimize(const OptimizationData & data) +{ + const std::vector time_vec = data.time_vec; + const size_t N = time_vec.size(); + const double s0 = data.s0; + const double v0 = data.v0; + const double a0 = data.a0; + const double v_max = std::max(data.v_max, 0.1); + const double a_max = data.a_max; + const double a_min = data.a_min; + const double limit_a_max = data.limit_a_max; + const double limit_a_min = data.limit_a_min; + const double limit_j_max = data.limit_j_max; + const double limit_j_min = data.limit_j_min; + const double j_max = data.j_max; + const double j_min = data.j_min; + const double a_range = std::max(a_max - a_min, 0.1); + const double j_range = std::max(j_max - j_min, 0.1); + const double t_dangerous = data.t_dangerous; + const double t_idling = data.idling_time; + const auto s_boundary = data.s_boundary; + + // Variables: s_i, v_i, a_i, j_i, over_s_safety_i, over_s_ideal_i, over_v_i, over_a_i, over_j_i + const int IDX_S0 = 0; + const int IDX_V0 = N; + const int IDX_A0 = 2 * N; + const int IDX_J0 = 3 * N; + const int IDX_OVER_S_SAFETY0 = 4 * N; + const int IDX_OVER_S_IDEAL0 = 5 * N; + const int IDX_OVER_V0 = 6 * N; + const int IDX_OVER_A0 = 7 * N; + const int IDX_OVER_J0 = 8 * N; + const int l_variables = 9 * N; + const int l_constraints = 7 * N + 3 * (N - 1) + 3; + + // the matrix size depends on constraint numbers. + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(l_constraints, l_variables); + std::vector lower_bound(l_constraints, 0.0); + std::vector upper_bound(l_constraints, 0.0); + + // Object Variables + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(l_variables, l_variables); + std::vector q(l_variables, 0.0); + + // Object Function + // min w_s*(s_i - s_ideal_i)^2 + w_v * v0 * ((v_i - v_max)^2 / v_max^2) + // + over_s_safety^2 + over_s_ideal^2 + over_v_ideal^2 + over_a_ideal^2 + // + over_j_ideal^2 + constexpr double MINIMUM_MAX_S_BOUND = 0.01; + for (size_t i = 0; i < N; ++i) { + const double dt = + i < N - 1 ? time_vec.at(i + 1) - time_vec.at(i) : time_vec.at(N - 1) - time_vec.at(N - 2); + const double max_s = std::max(s_boundary.at(i).max_s, MINIMUM_MAX_S_BOUND); + const double v_coeff = v0 / (2 * std::fabs(a_min)) + t_idling; + if (s_boundary.at(i).is_object) { + q.at(IDX_S0 + i) += -max_s_weight_ / max_s * dt; + q.at(IDX_V0 + i) += -max_s_weight_ / max_s * v_coeff * dt; + } else { + q.at(IDX_S0 + i) += -max_s_weight_ / max_s * dt; + } + + q.at(IDX_V0 + i) += -max_v_weight_ / v_max * dt; + } + + for (size_t i = 0; i < N; ++i) { + const double dt = + i < N - 1 ? time_vec.at(i + 1) - time_vec.at(i) : time_vec.at(N - 1) - time_vec.at(N - 2); + const double max_s = std::max(s_boundary.at(i).max_s, MINIMUM_MAX_S_BOUND); + P(IDX_OVER_S_SAFETY0 + i, IDX_OVER_S_SAFETY0 + i) += + over_s_safety_weight_ / (max_s * max_s) * dt; + P(IDX_OVER_S_IDEAL0 + i, IDX_OVER_S_IDEAL0 + i) += over_s_ideal_weight_ / (max_s * max_s) * dt; + P(IDX_OVER_V0 + i, IDX_OVER_V0 + i) += over_v_weight_ / (v_max * v_max) * dt; + P(IDX_OVER_A0 + i, IDX_OVER_A0 + i) += over_a_weight_ / a_range * dt; + P(IDX_OVER_J0 + i, IDX_OVER_J0 + i) += over_j_weight_ / j_range * dt; + + if (s_boundary.at(i).is_object) { + const double v_coeff = v0 / (2 * std::fabs(a_min)) + t_idling; + P(IDX_S0 + i, IDX_S0 + i) += max_s_weight_ / (max_s * max_s) * dt; + P(IDX_V0 + i, IDX_V0 + i) += max_s_weight_ / (max_s * max_s) * v_coeff * v_coeff * dt; + P(IDX_S0 + i, IDX_V0 + i) += max_s_weight_ / (max_s * max_s) * v_coeff * dt; + P(IDX_V0 + i, IDX_S0 + i) += max_s_weight_ / (max_s * max_s) * v_coeff * dt; + } else { + P(IDX_S0 + i, IDX_S0 + i) += max_s_weight_ / (max_s * max_s) * dt; + } + + P(IDX_V0 + i, IDX_V0 + i) += max_v_weight_ / (v_max * v_max) * dt; + } + + // Constraint + size_t constr_idx = 0; + + // Safety Position Constraint: s_boundary_min < s_i + v_i*t_dangerous + v0*v_i/(2*|a_min|) - + // over_s_safety_i < s_boundary_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + const double v_coeff = v0 / (2 * std::fabs(a_min)) + t_dangerous; + A(constr_idx, IDX_S0 + i) = 1.0; // s_i + if (s_boundary.at(i).is_object) { + A(constr_idx, IDX_V0 + i) = v_coeff; // v_i * (t_dangerous + v0/(2*|a_min|)) + } + A(constr_idx, IDX_OVER_S_SAFETY0 + i) = -1.0; // over_s_safety_i + upper_bound.at(constr_idx) = s_boundary.at(i).max_s; + lower_bound.at(constr_idx) = 0.0; + } + + // Ideal Position Constraint: s_boundary_min < s_i + v_i * t_idling + v0*v_i/(2*|a_min|) - + // over_s_ideal_i < s_boundary_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + const double v_coeff = v0 / (2 * std::fabs(a_min)) + t_idling; + A(constr_idx, IDX_S0 + i) = 1.0; // s_i + if (s_boundary.at(i).is_object) { + A(constr_idx, IDX_V0 + i) = v_coeff; // v_i * (t_idling + v0/(2*|a_min|)) + } + A(constr_idx, IDX_OVER_S_IDEAL0 + i) = -1.0; // over_s_ideal_i + upper_bound.at(constr_idx) = s_boundary.at(i).max_s; + lower_bound.at(constr_idx) = 0.0; + } + + // Soft Velocity Constraint: 0 < v_i - over_v_i < v_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + A(constr_idx, IDX_V0 + i) = 1.0; // v_i + A(constr_idx, IDX_OVER_V0 + i) = -1.0; // over_v_i + upper_bound.at(constr_idx) = i == N - 1 ? 0.0 : v_max; + lower_bound.at(constr_idx) = 0.0; + } + + // Soft Acceleration Constraint: a_min < a_i - over_a_i < a_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + A(constr_idx, IDX_A0 + i) = 1.0; // a_i + A(constr_idx, IDX_OVER_A0 + i) = -1.0; // over_a_i + upper_bound.at(constr_idx) = i == N - 1 ? 0.0 : a_max; + lower_bound.at(constr_idx) = i == N - 1 ? 0.0 : a_min; + } + + // Hard Acceleration Constraint: limit_a_min < a_i < a_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + A(constr_idx, IDX_A0 + i) = 1.0; // a_i + upper_bound.at(constr_idx) = limit_a_max; + lower_bound.at(constr_idx) = limit_a_min; + } + + // Soft Jerk Constraint: j_min < j_i - over_j_i < j_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + A(constr_idx, IDX_J0 + i) = 1.0; // j_i + A(constr_idx, IDX_OVER_J0 + i) = -1.0; // over_j_i + upper_bound.at(constr_idx) = i == N - 1 ? 0.0 : j_max; + lower_bound.at(constr_idx) = i == N - 1 ? 0.0 : j_min; + } + + // Hard Jerk Constraint: limit_j_min < j_i < limit_j_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + A(constr_idx, IDX_J0 + i) = 1.0; // j_i + upper_bound.at(constr_idx) = limit_j_max; + lower_bound.at(constr_idx) = limit_j_min; + } + + // Dynamic Constraint + // s_i+1 = s_i + v_i * dt + 0.5 * a_i * dt^2 + 1/6 * j_i * dt^3 + for (size_t i = 0; i < N - 1; ++i, ++constr_idx) { + const double dt = time_vec.at(i + 1) - time_vec.at(i); + A(constr_idx, IDX_S0 + i + 1) = 1.0; // s_i+1 + A(constr_idx, IDX_S0 + i) = -1.0; // -s_i + A(constr_idx, IDX_V0 + i) = -dt; // -v_i*dt + A(constr_idx, IDX_A0 + i) = -0.5 * dt * dt; // -0.5 * a_i * dt^2 + A(constr_idx, IDX_J0 + i) = -1.0 / 6.0 * dt * dt * dt; // -1.0/6.0 * j_i * dt^3 + upper_bound.at(constr_idx) = 0.0; + lower_bound.at(constr_idx) = 0.0; + } + + // v_i+1 = v_i + a_i * dt + 0.5 * j_i * dt^2 + for (size_t i = 0; i < N - 1; ++i, ++constr_idx) { + const double dt = time_vec.at(i + 1) - time_vec.at(i); + A(constr_idx, IDX_V0 + i + 1) = 1.0; // v_i+1 + A(constr_idx, IDX_V0 + i) = -1.0; // -v_i + A(constr_idx, IDX_A0 + i) = -dt; // -a_i * dt + A(constr_idx, IDX_J0 + i) = -0.5 * dt * dt; // -0.5 * j_i * dt^2 + upper_bound.at(constr_idx) = 0.0; + lower_bound.at(constr_idx) = 0.0; + } + + // a_i+1 = a_i + j_i * dt + for (size_t i = 0; i < N - 1; ++i, ++constr_idx) { + const double dt = time_vec.at(i + 1) - time_vec.at(i); + A(constr_idx, IDX_A0 + i + 1) = 1.0; // a_i+1 + A(constr_idx, IDX_A0 + i) = -1.0; // -a_i + A(constr_idx, IDX_J0 + i) = -dt; // -j_i * dt + upper_bound.at(constr_idx) = 0.0; + lower_bound.at(constr_idx) = 0.0; + } + + // initial condition + { + A(constr_idx, IDX_S0) = 1.0; // s0 + upper_bound[constr_idx] = s0; + lower_bound[constr_idx] = s0; + ++constr_idx; + + A(constr_idx, IDX_V0) = 1.0; // v0 + upper_bound[constr_idx] = v0; + lower_bound[constr_idx] = v0; + ++constr_idx; + + A(constr_idx, IDX_A0) = 1.0; // a0 + upper_bound[constr_idx] = a0; + lower_bound[constr_idx] = a0; + } + + // execute optimization + const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound); + const std::vector optval = std::get<0>(result); + + const int status_val = std::get<3>(result); + if (status_val != 1) + std::cerr << "optimization failed : " << qp_solver_.getStatusMessage().c_str() << std::endl; + + const auto has_nan = + std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); }); + if (has_nan) std::cerr << "optimization failed : result contains NaN values\n"; + + OptimizationResult optimized_result; + const auto is_optimization_failed = status_val != 1 || has_nan; + if (!is_optimization_failed) { + std::vector opt_time = time_vec; + std::vector opt_pos(N); + std::vector opt_vel(N); + std::vector opt_acc(N); + std::vector opt_jerk(N); + for (size_t i = 0; i < N; ++i) { + opt_pos.at(i) = optval.at(IDX_S0 + i); + opt_vel.at(i) = std::max(optval.at(IDX_V0 + i), 0.0); + opt_acc.at(i) = optval.at(IDX_A0 + i); + opt_jerk.at(i) = optval.at(IDX_J0 + i); + } + opt_vel.back() = 0.0; + + optimized_result.t = opt_time; + optimized_result.s = opt_pos; + optimized_result.v = opt_vel; + optimized_result.a = opt_acc; + optimized_result.j = opt_jerk; + } + + return optimized_result; +} +} // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/velocity_optimizer.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/velocity_optimizer.hpp new file mode 100644 index 0000000000000..36f6d9f75fb99 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/velocity_optimizer.hpp @@ -0,0 +1,77 @@ +// 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 OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ +#define OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ + +#include "autoware/osqp_interface/osqp_interface.hpp" +#include "s_boundary.hpp" + +#include + +namespace autoware::motion_velocity_planner +{ +class VelocityOptimizer +{ +public: + struct OptimizationData + { + std::vector time_vec; + double s0; + double v0; + double a0; + double v_max; + double a_max; + double a_min; + double limit_a_max; + double limit_a_min; + double limit_j_max; + double limit_j_min; + double j_max; + double j_min; + double t_dangerous; + double idling_time; + SBoundaries s_boundary; + }; + + struct OptimizationResult + { + std::vector t; + std::vector s; + std::vector v; + std::vector a; + std::vector j; + }; + + VelocityOptimizer( + const double max_s_weight, const double max_v_weight, const double over_s_safety_weight, + const double over_s_ideal_weight, const double over_v_weight, const double over_a_weight, + const double over_j_weight); + + OptimizationResult optimize(const OptimizationData & data); + +private: + // Parameter + double max_s_weight_; + double max_v_weight_; + double over_s_safety_weight_; + double over_s_ideal_weight_; + double over_v_weight_; + double over_a_weight_; + double over_j_weight_; + + // QPSolver + autoware::osqp_interface::OSQPInterface qp_solver_; +}; +} // namespace autoware::motion_velocity_planner +#endif // OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/parameters.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/parameters.hpp new file mode 100644 index 0000000000000..af50fb0d4325a --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/parameters.hpp @@ -0,0 +1,159 @@ +// 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 PARAMETERS_HPP_ +#define PARAMETERS_HPP_ + +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_velocity_planner_common_universe/utils.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" +#include "type_alias.hpp" +#include "types.hpp" + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +using autoware::universe_utils::getOrDeclareParameter; + +struct CommonParam +{ + double max_accel{}; + double min_accel{}; + double max_jerk{}; + double min_jerk{}; + double limit_max_accel{}; + double limit_min_accel{}; + double limit_max_jerk{}; + double limit_min_jerk{}; + + CommonParam() = default; + explicit CommonParam(rclcpp::Node & node) + { + max_accel = getOrDeclareParameter(node, "normal.max_acc"); + min_accel = getOrDeclareParameter(node, "normal.min_acc"); + max_jerk = getOrDeclareParameter(node, "normal.max_jerk"); + min_jerk = getOrDeclareParameter(node, "normal.min_jerk"); + limit_max_accel = getOrDeclareParameter(node, "limit.max_acc"); + limit_min_accel = getOrDeclareParameter(node, "limit.min_acc"); + limit_max_jerk = getOrDeclareParameter(node, "limit.max_jerk"); + limit_min_jerk = getOrDeclareParameter(node, "limit.min_jerk"); + } +}; + +struct ObstacleFilteringParam +{ + std::vector inside_object_types{}; + std::vector outside_object_types{}; + // bool use_pointcloud{}; + + double max_lat_margin{}; + + double crossing_obstacle_velocity_threshold{}; + double crossing_obstacle_traj_angle_threshold{}; + + double outside_obstacle_velocity_threshold{}; + double ego_obstacle_overlap_time_threshold{}; + double max_prediction_time_for_collision_check{}; + double max_lateral_time_margin{}; + int num_of_predicted_paths_for_outside_cruise_obstacle{}; + + bool enable_yield{}; + double yield_lat_distance_threshold{}; + double max_lat_dist_between_obstacles{}; + double max_obstacles_collision_time{}; + double stopped_obstacle_velocity_threshold{}; + + double obstacle_velocity_threshold_from_cruise{}; + double obstacle_velocity_threshold_to_cruise{}; + + ObstacleFilteringParam() = default; + explicit ObstacleFilteringParam(rclcpp::Node & node) + { + inside_object_types = + utils::get_target_object_type(node, "obstacle_cruise.obstacle_filtering.object_type.inside."); + outside_object_types = utils::get_target_object_type( + node, "obstacle_cruise.obstacle_filtering.object_type.outside."); + // use_pointcloud = getOrDeclareParameter( + // node, "obstacle_cruise.obstacle_filtering.object_type.pointcloud"); + + max_lat_margin = + getOrDeclareParameter(node, "obstacle_cruise.obstacle_filtering.max_lat_margin"); + + crossing_obstacle_velocity_threshold = getOrDeclareParameter( + node, "obstacle_cruise.obstacle_filtering.crossing_obstacle.obstacle_velocity_threshold"); + crossing_obstacle_traj_angle_threshold = getOrDeclareParameter( + node, "obstacle_cruise.obstacle_filtering.crossing_obstacle.obstacle_traj_angle_threshold"); + + outside_obstacle_velocity_threshold = getOrDeclareParameter( + node, "obstacle_cruise.obstacle_filtering.outside_obstacle.obstacle_velocity_threshold"); + ego_obstacle_overlap_time_threshold = getOrDeclareParameter( + node, + "obstacle_cruise.obstacle_filtering.outside_obstacle.ego_obstacle_overlap_time_threshold"); + max_prediction_time_for_collision_check = getOrDeclareParameter( + node, + "obstacle_cruise.obstacle_filtering.outside_obstacle.max_prediction_time_for_collision_" + "check"); + max_lateral_time_margin = getOrDeclareParameter( + node, "obstacle_cruise.obstacle_filtering.outside_obstacle.max_lateral_time_margin"); + num_of_predicted_paths_for_outside_cruise_obstacle = getOrDeclareParameter( + node, "obstacle_cruise.obstacle_filtering.outside_obstacle.num_of_predicted_paths"); + enable_yield = + getOrDeclareParameter(node, "obstacle_cruise.obstacle_filtering.yield.enable_yield"); + yield_lat_distance_threshold = getOrDeclareParameter( + node, "obstacle_cruise.obstacle_filtering.yield.lat_distance_threshold"); + max_lat_dist_between_obstacles = getOrDeclareParameter( + node, "obstacle_cruise.obstacle_filtering.yield.max_lat_dist_between_obstacles"); + max_obstacles_collision_time = getOrDeclareParameter( + node, "obstacle_cruise.obstacle_filtering.yield.max_obstacles_collision_time"); + stopped_obstacle_velocity_threshold = getOrDeclareParameter( + node, "obstacle_cruise.obstacle_filtering.yield.stopped_obstacle_velocity_threshold"); + obstacle_velocity_threshold_from_cruise = getOrDeclareParameter( + node, "obstacle_cruise.obstacle_filtering.obstacle_velocity_threshold_from_cruise"); + obstacle_velocity_threshold_to_cruise = getOrDeclareParameter( + node, "obstacle_cruise.obstacle_filtering.obstacle_velocity_threshold_to_cruise"); + } +}; + +struct CruisePlanningParam +{ + double idling_time{}; + double min_ego_accel_for_rss{}; + double min_object_accel_for_rss{}; + double safe_distance_margin{}; + + CruisePlanningParam() = default; + explicit CruisePlanningParam(rclcpp::Node & node) + { + idling_time = + getOrDeclareParameter(node, "obstacle_cruise.cruise_planning.idling_time"); + min_ego_accel_for_rss = + getOrDeclareParameter(node, "obstacle_cruise.cruise_planning.min_ego_accel_for_rss"); + min_object_accel_for_rss = getOrDeclareParameter( + node, "obstacle_cruise.cruise_planning.min_object_accel_for_rss"); + safe_distance_margin = + getOrDeclareParameter(node, "obstacle_cruise.cruise_planning.safe_distance_margin"); + } +}; +} // namespace autoware::motion_velocity_planner + +#endif // PARAMETERS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/cruise_planning_debug_info.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/cruise_planning_debug_info.hpp new file mode 100644 index 0000000000000..7d74b602d85dc --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/cruise_planning_debug_info.hpp @@ -0,0 +1,100 @@ +// 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 PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ +#define PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ + +#include + +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" + +#include + +namespace autoware::motion_velocity_planner +{ +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; + +class CruisePlanningDebugInfo +{ +public: + enum class TYPE { + // ego + EGO_VELOCITY = 0, // index: 0 + EGO_ACCELERATION, + EGO_JERK, // no data + + // cruise planning + CRUISE_CURRENT_OBSTACLE_DISTANCE_RAW, // index: 3 + CRUISE_CURRENT_OBSTACLE_DISTANCE_FILTERED, + CRUISE_CURRENT_OBSTACLE_VELOCITY_RAW, + CRUISE_CURRENT_OBSTACLE_VELOCITY_FILTERED, + CRUISE_TARGET_OBSTACLE_DISTANCE, + CRUISE_ERROR_DISTANCE_RAW, + CRUISE_ERROR_DISTANCE_FILTERED, + + CRUISE_RELATIVE_EGO_VELOCITY, // index: 10 + CRUISE_TIME_TO_COLLISION, + + CRUISE_TARGET_VELOCITY, // index: 12 + CRUISE_TARGET_ACCELERATION, + CRUISE_TARGET_JERK_RATIO, + + SIZE + }; + + /** + * @brief get the index corresponding to the given value TYPE + * @param [in] type the TYPE enum for which to get the index + * @return index of the type + */ + static int getIndex(const TYPE type) { return static_cast(type); } + + /** + * @brief get all the debug values as an std::array + * @return array of all debug values + */ + std::array(TYPE::SIZE)> get() const { return info_; } + + /** + * @brief set the given type to the given value + * @param [in] type TYPE of the value + * @param [in] value value to set + */ + void set(const TYPE type, const double val) { info_.at(static_cast(type)) = val; } + + /** + * @brief set the given type to the given value + * @param [in] type index of the type + * @param [in] value value to set + */ + void set(const int type, const double val) { info_.at(type) = val; } + + void reset() { info_.fill(0.0); } + + Float32MultiArrayStamped convert_to_message(const rclcpp::Time & current_time) const + { + Float32MultiArrayStamped msg{}; + msg.stamp = current_time; + for (const auto & v : get()) { + msg.data.push_back(v); + } + return msg; + } + +private: + std::array(TYPE::SIZE)> info_; +}; +} // namespace autoware::motion_velocity_planner + +#endif // PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/pid_based_planner.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/pid_based_planner.cpp new file mode 100644 index 0000000000000..6bada506b977b --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/pid_based_planner.cpp @@ -0,0 +1,774 @@ +// 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 "pid_based_planner.hpp" + +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_velocity_planner_common_universe/utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" + +#include "tier4_planning_msgs/msg/velocity_limit.hpp" + +#include +#include +#include +#include + +using autoware::signal_processing::LowpassFilter1d; +namespace autoware::motion_velocity_planner +{ + +namespace +{ +VelocityLimit create_velocity_limit_message( + const rclcpp::Time & current_time, const double vel, const double acc, const double max_jerk, + const double min_jerk) +{ + VelocityLimit msg; + msg.stamp = current_time; + msg.sender = "obstacle_cruise"; + msg.use_constraints = true; + + msg.max_velocity = vel; + if (acc < 0) { + msg.constraints.min_acceleration = acc; + } + msg.constraints.max_jerk = max_jerk; + msg.constraints.min_jerk = min_jerk; + + return msg; +} + +template +T getSign(const T val) +{ + if (0 < val) { + return static_cast(1); + } else if (val < 0) { + return static_cast(-1); + } + return static_cast(0); +} +} // namespace + +PIDBasedPlanner::PIDBasedPlanner( + rclcpp::Node & node, const CommonParam & common_param, + const CruisePlanningParam & cruise_planning_param) +: CruisePlannerInterface(node, common_param, cruise_planning_param) +{ + min_accel_during_cruise_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.min_accel_during_cruise"); + + use_velocity_limit_based_planner_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.use_velocity_limit_based_planner"); + + { // velocity limit based planner + const double kp = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.kp"); + const double ki = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.ki"); + const double kd = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.kd"); + velocity_limit_based_planner_param_.pid_vel_controller = + std::make_unique(kp, ki, kd); + + velocity_limit_based_planner_param_.output_ratio_during_accel = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.output_ratio_" + "during_accel"); + velocity_limit_based_planner_param_.vel_to_acc_weight = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.vel_to_acc_" + "weight"); + + velocity_limit_based_planner_param_.enable_jerk_limit_to_output_acc = + node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.enable_" + "jerk_limit_to_output_acc"); + + velocity_limit_based_planner_param_.disable_target_acceleration = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.disable_" + "target_acceleration"); + } + + { // velocity insertion based planner + // pid controller for acc + const double kp_acc = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.kp_acc"); + const double ki_acc = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.ki_acc"); + const double kd_acc = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.kd_acc"); + velocity_insertion_based_planner_param_.pid_acc_controller = + std::make_unique(kp_acc, ki_acc, kd_acc); + + // pid controller for jerk + const double kp_jerk = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.kp_jerk"); + const double ki_jerk = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.ki_jerk"); + const double kd_jerk = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.kd_jerk"); + velocity_insertion_based_planner_param_.pid_jerk_controller = + std::make_unique(kp_jerk, ki_jerk, kd_jerk); + + velocity_insertion_based_planner_param_.output_acc_ratio_during_accel = + node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.output_" + "acc_ratio_during_accel"); + velocity_insertion_based_planner_param_.output_jerk_ratio_during_accel = + node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.output_" + "jerk_ratio_during_accel"); + + velocity_insertion_based_planner_param_.enable_jerk_limit_to_output_acc = + node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.enable_" + "jerk_limit_to_output_acc"); + } + + min_cruise_target_vel_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.min_cruise_target_vel"); + time_to_evaluate_rss_ = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.time_to_evaluate_rss"); + const auto error_function_type = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.error_function_type"); + error_func_ = [&]() -> std::function { + if (error_function_type == "linear") { + return [](const double val) { return val; }; + } else if (error_function_type == "quadratic") { + return [](const double val) { return getSign(val) * std::pow(val, 2); }; + } + throw std::domain_error("error function type is not supported"); + }(); + + // low pass filter + const double lpf_normalized_error_cruise_dist_gain = node.declare_parameter( + "obstacle_cruise.cruise_planning.pid_based_planner.lpf_normalized_error_cruise_dist_gain"); + lpf_normalized_error_cruise_dist_ptr_ = + std::make_shared(lpf_normalized_error_cruise_dist_gain); + lpf_dist_to_obstacle_ptr_ = std::make_shared(0.5); + lpf_obstacle_vel_ptr_ = std::make_shared(0.5); + lpf_error_cruise_dist_ptr_ = std::make_shared(0.5); + lpf_output_vel_ptr_ = std::make_shared(0.5); + lpf_output_acc_ptr_ = std::make_shared(0.5); + lpf_output_jerk_ptr_ = std::make_shared(0.5); +} + +std::vector PIDBasedPlanner::plan_cruise( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const std::vector & obstacles, std::shared_ptr debug_data_ptr, + std::unique_ptr & + planning_factor_interface, + std::optional & velocity_limit) +{ + cruise_planning_debug_info_.reset(); + + // calc obstacles to cruise + const auto cruise_obstacle_info = + calc_obstacle_to_cruise(stop_traj_points, planner_data, obstacles); + + // plan cruise + const auto cruise_traj_points = plan_cruise_trajectory( + planner_data, stop_traj_points, debug_data_ptr, planning_factor_interface, velocity_limit, + cruise_obstacle_info); + + prev_traj_points_ = cruise_traj_points; + return cruise_traj_points; +} + +std::optional PIDBasedPlanner::calc_obstacle_to_cruise( + const std::vector & stop_traj_points, + const std::shared_ptr planner_data, + const std::vector & obstacles) +{ + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::EGO_VELOCITY, + planner_data->current_odometry.twist.twist.linear.x); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::EGO_ACCELERATION, + planner_data->current_acceleration.accel.accel.linear.x); + + auto modified_target_obstacles = obstacles; // TODO(murooka) what is this variable? + + // search highest probability obstacle for cruise + std::optional cruise_obstacle_info; + for (size_t o_idx = 0; o_idx < obstacles.size(); ++o_idx) { + const auto & obstacle = obstacles.at(o_idx); + + if (obstacle.collision_points.empty()) { + continue; + } + + // NOTE: from ego's front to obstacle's back + // TODO(murooka) this is not dist to obstacle + const double dist_to_obstacle = + calc_distance_to_collisionPoint( + stop_traj_points, planner_data, obstacle.collision_points.front().point) + + (obstacle.velocity - planner_data->current_odometry.twist.twist.linear.x) * + time_to_evaluate_rss_; + + // calculate distance between ego and obstacle based on RSS + const double target_dist_to_obstacle = calc_rss_distance( + planner_data->current_odometry.twist.twist.linear.x, obstacle.velocity, + cruise_planning_param_.safe_distance_margin); + + // calculate error distance and normalized one + const double error_cruise_dist = dist_to_obstacle - target_dist_to_obstacle; + if (cruise_obstacle_info) { + if (error_cruise_dist > cruise_obstacle_info->error_cruise_dist) { + continue; + } + } + cruise_obstacle_info = + CruiseObstacleInfo(obstacle, error_cruise_dist, dist_to_obstacle, target_dist_to_obstacle); + } + + if (!cruise_obstacle_info) { // if obstacle to cruise was not found + lpf_dist_to_obstacle_ptr_->reset(); + lpf_obstacle_vel_ptr_->reset(); + lpf_error_cruise_dist_ptr_->reset(); + return {}; + } + + // if obstacle to cruise was found + { // debug data + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_RELATIVE_EGO_VELOCITY, + planner_data->current_odometry.twist.twist.linear.x - + cruise_obstacle_info->obstacle.velocity); + const double non_zero_relative_vel = [&]() { + const double relative_vel = planner_data->current_odometry.twist.twist.linear.x - + cruise_obstacle_info->obstacle.velocity; + constexpr double epsilon = 0.001; + if (epsilon < std::abs(relative_vel)) { + return relative_vel; + } + return getSign(relative_vel) * epsilon; + }(); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TIME_TO_COLLISION, + cruise_obstacle_info->dist_to_obstacle / non_zero_relative_vel); + } + + { // dist to obstacle + const double raw_dist_to_obstacle = cruise_obstacle_info->dist_to_obstacle; + const double filtered_dist_to_obstacle = + lpf_dist_to_obstacle_ptr_->filter(cruise_obstacle_info->dist_to_obstacle); + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_DISTANCE_RAW, raw_dist_to_obstacle); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_DISTANCE_FILTERED, + filtered_dist_to_obstacle); + + cruise_obstacle_info->dist_to_obstacle = filtered_dist_to_obstacle; + } + + { // obstacle velocity + const double raw_obstacle_velocity = cruise_obstacle_info->obstacle.velocity; + const double filtered_obstacle_velocity = lpf_obstacle_vel_ptr_->filter(raw_obstacle_velocity); + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_VELOCITY_RAW, raw_obstacle_velocity); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_VELOCITY_FILTERED, + filtered_obstacle_velocity); + + cruise_obstacle_info->obstacle.velocity = filtered_obstacle_velocity; + } + + { // error distance for cruising + const double raw_error_cruise_dist = cruise_obstacle_info->error_cruise_dist; + const double filtered_error_cruise_dist = + lpf_error_cruise_dist_ptr_->filter(cruise_obstacle_info->error_cruise_dist); + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_ERROR_DISTANCE_RAW, raw_error_cruise_dist); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_ERROR_DISTANCE_FILTERED, filtered_error_cruise_dist); + + cruise_obstacle_info->error_cruise_dist = filtered_error_cruise_dist; + } + + { // target dist for cruising + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_OBSTACLE_DISTANCE, + cruise_obstacle_info->target_dist_to_obstacle); + } + + return cruise_obstacle_info; +} + +std::vector PIDBasedPlanner::plan_cruise_trajectory( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, std::shared_ptr debug_data_ptr, + std::unique_ptr & + planning_factor_interface, + std::optional & velocity_limit, + const std::optional & cruise_obstacle_info) +{ + // do cruise + if (cruise_obstacle_info) { + RCLCPP_DEBUG(logger_, "cruise planning"); + + { // update debug marker + // virtual wall marker for cruise + const double error_cruise_dist = cruise_obstacle_info->error_cruise_dist; + const double dist_to_obstacle = cruise_obstacle_info->dist_to_obstacle; + const size_t ego_idx = + planner_data->find_index(stop_traj_points, planner_data->current_odometry.pose.pose); + const double abs_ego_offset = + planner_data->is_driving_forward + ? std::abs(planner_data->vehicle_info_.max_longitudinal_offset_m) + : std::abs(planner_data->vehicle_info_.min_longitudinal_offset_m); + const double dist_to_rss_wall = + std::min(error_cruise_dist + abs_ego_offset, dist_to_obstacle + abs_ego_offset); + const size_t wall_idx = + utils::get_index_with_longitudinal_offset(stop_traj_points, dist_to_rss_wall, ego_idx); + + const std::string wall_reason_string = cruise_obstacle_info->obstacle.is_yield_obstacle + ? "obstacle cruise (yield)" + : "obstacle cruise"; + auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( + stop_traj_points.at(wall_idx).pose, wall_reason_string, clock_->now(), 0); + // NOTE: use a different color from slow down one to visualize cruise and slow down + // separately. + markers.markers.front().color = + autoware::universe_utils::createMarkerColor(1.0, 0.6, 0.1, 0.5); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr->cruise_wall_marker); + + // cruise obstacle + debug_data_ptr->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle); + + planning_factor_interface->add( + stop_traj_points, planner_data->current_odometry.pose.pose, + stop_traj_points.at(wall_idx).pose, PlanningFactor::NONE, SafetyFactorArray{}); + } + + // do cruise planning + if (!use_velocity_limit_based_planner_) { + const auto cruise_traj = + plan_cruise_with_trajectory(planner_data, stop_traj_points, *cruise_obstacle_info); + return cruise_traj; + } + + velocity_limit = plan_cruise_with_velocity_limit(planner_data, *cruise_obstacle_info); + return stop_traj_points; + } + + // reset previous cruise data if adaptive cruise is not enabled + prev_target_acc_ = {}; + lpf_normalized_error_cruise_dist_ptr_->reset(); + lpf_output_acc_ptr_->reset(); + lpf_output_vel_ptr_->reset(); + lpf_output_jerk_ptr_->reset(); + + // delete marker + const auto markers = + autoware::motion_utils::createDeletedSlowDownVirtualWallMarker(clock_->now(), 0); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr->cruise_wall_marker); + + return stop_traj_points; +} + +VelocityLimit PIDBasedPlanner::plan_cruise_with_velocity_limit( + const std::shared_ptr planner_data, + const CruiseObstacleInfo & cruise_obstacle_info) +{ + const auto & p = velocity_limit_based_planner_param_; + + const double filtered_normalized_error_cruise_dist = [&]() { + const double normalized_error_cruise_dist = + cruise_obstacle_info.error_cruise_dist / cruise_obstacle_info.dist_to_obstacle; + return lpf_normalized_error_cruise_dist_ptr_->filter(normalized_error_cruise_dist); + }(); + + const double modified_error_cruise_dist = error_func_(filtered_normalized_error_cruise_dist); + + // calculate target velocity with acceleration limit by PID controller + const double pid_output_vel = p.pid_vel_controller->calc(modified_error_cruise_dist); + const double additional_vel = [&]() { + if (modified_error_cruise_dist > 0) { + return pid_output_vel * p.output_ratio_during_accel; + } + return pid_output_vel; + }(); + + const double positive_target_vel = lpf_output_vel_ptr_->filter(std::max( + min_cruise_target_vel_, planner_data->current_odometry.twist.twist.linear.x + additional_vel)); + + // calculate target acceleration + const double target_acc = [&]() { + if (p.disable_target_acceleration) { + return min_accel_during_cruise_; + } + + double target_acc = p.vel_to_acc_weight * additional_vel; + + // apply acc limit + target_acc = + std::clamp(target_acc, min_accel_during_cruise_, common_param_.max_accel); // apply acc limit + + if (!prev_target_acc_) { + return lpf_output_acc_ptr_->filter(target_acc); + } + + if (p.enable_jerk_limit_to_output_acc) { // apply jerk limit + const double jerk = (target_acc - *prev_target_acc_) / 0.1; // TODO(murooka) 0.1 + const double limited_jerk = std::clamp(jerk, common_param_.min_jerk, common_param_.max_jerk); + target_acc = *prev_target_acc_ + limited_jerk * 0.1; + } + + return lpf_output_acc_ptr_->filter(target_acc); + }(); + prev_target_acc_ = target_acc; + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_VELOCITY, positive_target_vel); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_ACCELERATION, target_acc); + + RCLCPP_DEBUG(logger_, "target_velocity %f", positive_target_vel); + + // set target longitudinal motion + const auto velocity_limit = create_velocity_limit_message( + clock_->now(), positive_target_vel, target_acc, common_param_.max_jerk, common_param_.min_jerk); + + return velocity_limit; +} + +std::vector PIDBasedPlanner::plan_cruise_with_trajectory( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const CruiseObstacleInfo & cruise_obstacle_info) +{ + const auto & p = velocity_insertion_based_planner_param_; + + const double filtered_normalized_error_cruise_dist = [&]() { + const double normalized_error_cruise_dist = + cruise_obstacle_info.error_cruise_dist / cruise_obstacle_info.dist_to_obstacle; + return lpf_normalized_error_cruise_dist_ptr_->filter(normalized_error_cruise_dist); + }(); + + const double modified_error_cruise_dist = error_func_(filtered_normalized_error_cruise_dist); + + // calculate target velocity with acceleration limit by PID controller + // calculate target acceleration + const double target_acc = [&]() { + double target_acc = p.pid_acc_controller->calc(modified_error_cruise_dist); + + if (0 < filtered_normalized_error_cruise_dist) { + target_acc *= p.output_acc_ratio_during_accel; + } + + target_acc = + std::clamp(target_acc, min_accel_during_cruise_, common_param_.max_accel); // apply acc limit + + if (!prev_target_acc_) { + return target_acc; + } + + if (p.enable_jerk_limit_to_output_acc) { + const double jerk = (target_acc - *prev_target_acc_) / 0.1; // TODO(murooka) 0.1 + const double limited_jerk = std::clamp(jerk, common_param_.min_jerk, common_param_.max_jerk); + target_acc = *prev_target_acc_ + limited_jerk * 0.1; + } + + return target_acc; + }(); + prev_target_acc_ = target_acc; + + const double target_jerk_ratio = [&]() { + double target_jerk_ratio = p.pid_jerk_controller->calc(modified_error_cruise_dist); + + if (0 < filtered_normalized_error_cruise_dist) { + target_jerk_ratio *= p.output_jerk_ratio_during_accel; + } + + target_jerk_ratio = std::clamp(std::abs(target_jerk_ratio), 0.0, 1.0); + return target_jerk_ratio; + }(); + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_ACCELERATION, target_acc); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_JERK_RATIO, target_jerk_ratio); + + // set target longitudinal motion + const auto prev_traj_closest_point = [&]() -> TrajectoryPoint { + // if (smoothed_trajectory_ptr_) { + // return autoware::motion_utils::calcInterpolatedPoint( + // *smoothed_trajectory_ptr_, planner_data->current_odometry.pose.pose, + // nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); + // } + return autoware::motion_utils::calcInterpolatedPoint( + autoware::motion_utils::convertToTrajectory(prev_traj_points_), + planner_data->current_odometry.pose.pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); + }(); + const double v0 = prev_traj_closest_point.longitudinal_velocity_mps; + const double a0 = prev_traj_closest_point.acceleration_mps2; + + auto cruise_traj_points = get_acceleration_limited_trajectory( + stop_traj_points, planner_data->current_odometry.pose.pose, v0, a0, target_acc, + target_jerk_ratio, planner_data); + + const auto zero_vel_idx_opt = autoware::motion_utils::searchZeroVelocityIndex(cruise_traj_points); + if (!zero_vel_idx_opt) { + return cruise_traj_points; + } + + for (size_t i = zero_vel_idx_opt.value(); i < cruise_traj_points.size(); ++i) { + cruise_traj_points.at(i).longitudinal_velocity_mps = 0.0; + } + + return cruise_traj_points; +} + +std::vector PIDBasedPlanner::get_acceleration_limited_trajectory( + const std::vector & traj_points, const geometry_msgs::msg::Pose & start_pose, + const double v0, const double a0, const double target_acc, const double target_jerk_ratio, + const std::shared_ptr planner_data) const +{ + // calculate vt function + const auto vt = [&]( + const double v0, const double a0, const double jerk, const double t, + const double target_acc) { + const double t1 = (target_acc - a0) / jerk; + + const double v = [&]() { + if (t < t1) { + return v0 + a0 * t + jerk * t * t / 2.0; + } + + const double v1 = v0 + a0 * t1 + jerk * t1 * t1 / 2.0; + return v1 + target_acc * (t - t1); + }(); + + constexpr double max_vel = 100.0; + return std::clamp(v, 0.0, max_vel); + }; + + // calculate sv graph + const double s_traj = autoware::motion_utils::calcArcLength(traj_points); + // const double t_max = 10.0; + const double s_max = 50.0; + const double max_sampling_num = 100.0; + + const double t_delta_min = 0.1; + const double s_delta_min = 0.1; + + // NOTE: v0 of motion_velocity_planner_common may be smaller than v0 of motion_velocity_smoother + // Therefore, we calculate v1 (velocity at next step) and use it for initial velocity. + const double v1 = v0; // + 0.1; // a0 * 0.1; // + jerk * 0.1 * 0.1 / 2.0; + const double a1 = a0; // + jerk * 0.1; + const double jerk = target_acc > a1 ? common_param_.max_jerk * target_jerk_ratio + : common_param_.min_jerk * target_jerk_ratio; + + double t_current = 0.0; + std::vector s_vec{0.0}; + std::vector v_vec{v1}; + for (double tmp_idx = 0.0; tmp_idx < max_sampling_num; ++tmp_idx) { + if (v_vec.back() <= 0.0) { + s_vec.push_back(s_vec.back() + s_delta_min); + v_vec.push_back(0.0); + } else { + const double v_current = vt( + v1, a1, jerk, t_current + t_delta_min, + target_acc); // TODO(murooka) + t_delta_min is not required + + const double s_delta = std::max(s_delta_min, v_current * t_delta_min); + const double s_current = s_vec.back() + s_delta; + + s_vec.push_back(s_current); + v_vec.push_back(v_current); + + const double t_delta = [&]() { + if (v_current <= 0) { + return 0.0; + } + + constexpr double t_delta_max = 100.0; // NOTE: to avoid computation explosion + return std::min(t_delta_max, s_delta / v_current); + }(); + + t_current += t_delta; + } + + if (s_traj < s_vec.back() /*|| t_max < t_current*/ || s_max < s_vec.back()) { + break; + } + } + + std::vector unique_s_vec{s_vec.front()}; + std::vector unique_v_vec{v_vec.front()}; + for (size_t i = 0; i < s_vec.size(); ++i) { + if (s_vec.at(i) == unique_s_vec.back()) { + continue; + } + + unique_s_vec.push_back(s_vec.at(i)); + unique_v_vec.push_back(v_vec.at(i)); + } + + if (unique_s_vec.size() < 2) { + return traj_points; + } + + auto acc_limited_traj_points = traj_points; + const size_t ego_seg_idx = planner_data->find_index(acc_limited_traj_points, start_pose); + double sum_dist = 0.0; + for (size_t i = ego_seg_idx; i < acc_limited_traj_points.size(); ++i) { + if (i != ego_seg_idx) { + sum_dist += autoware::universe_utils::calcDistance2d( + acc_limited_traj_points.at(i - 1), acc_limited_traj_points.at(i)); + } + + const double vel = [&]() { + if (unique_s_vec.back() < sum_dist) { + return unique_v_vec.back(); + } + return autoware::interpolation::spline(unique_s_vec, unique_v_vec, {sum_dist}).front(); + }(); + + acc_limited_traj_points.at(i).longitudinal_velocity_mps = std::clamp( + vel, 0.0, + static_cast( + acc_limited_traj_points.at(i) + .longitudinal_velocity_mps)); // TODO(murooka) this assumes forward driving + } + + return acc_limited_traj_points; +} + +void PIDBasedPlanner::update_parameters(const std::vector & parameters) +{ + autoware::universe_utils::updateParam( + parameters, "obstacle_cruise.cruise_planning.pid_based_planner.min_accel_during_cruise", + min_accel_during_cruise_); + + { // velocity limit based planner + auto & p = velocity_limit_based_planner_param_; + + double kp = p.pid_vel_controller->getKp(); + double ki = p.pid_vel_controller->getKi(); + double kd = p.pid_vel_controller->getKd(); + + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.kp", kp); + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.ki", ki); + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.kd", kd); + p.pid_vel_controller->updateParam(kp, ki, kd); + + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.output_ratio_" + "during_accel", + p.output_ratio_during_accel); + autoware::universe_utils::updateParam( + parameters, "obstacle_cruise.cruise_planning.pid_based_planner.vel_to_acc_weight", + p.vel_to_acc_weight); + + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.enable_jerk_" + "limit_to_output_acc", + p.enable_jerk_limit_to_output_acc); + + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_limit_based_planner.disable_" + "target_acceleration", + p.disable_target_acceleration); + } + + { // velocity insertion based planner + auto & p = velocity_insertion_based_planner_param_; + + // pid controller for acc + double kp_acc = p.pid_acc_controller->getKp(); + double ki_acc = p.pid_acc_controller->getKi(); + double kd_acc = p.pid_acc_controller->getKd(); + + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.kp_acc", + kp_acc); + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.ki_acc", + ki_acc); + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.kd_acc", + kd_acc); + p.pid_acc_controller->updateParam(kp_acc, ki_acc, kd_acc); + + // pid controller for jerk + double kp_jerk = p.pid_jerk_controller->getKp(); + double ki_jerk = p.pid_jerk_controller->getKi(); + double kd_jerk = p.pid_jerk_controller->getKd(); + + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.kp_jerk", + kp_jerk); + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.ki_jerk", + ki_jerk); + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.kd_jerk", + kd_jerk); + p.pid_jerk_controller->updateParam(kp_jerk, ki_jerk, kd_jerk); + + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.output_" + "acc_ratio_during_accel", + p.output_acc_ratio_during_accel); + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.output_" + "jerk_ratio_during_accel", + p.output_jerk_ratio_during_accel); + + autoware::universe_utils::updateParam( + parameters, + "obstacle_cruise.cruise_planning.pid_based_planner.velocity_insertion_based_planner.enable_" + "jerk_limit_to_output_acc", + p.enable_jerk_limit_to_output_acc); + } + + // min_cruise_target_vel + autoware::universe_utils::updateParam( + parameters, "obstacle_cruise.cruise_planning.pid_based_planner.min_cruise_target_vel", + min_cruise_target_vel_); + autoware::universe_utils::updateParam( + parameters, "obstacle_cruise.cruise_planning.pid_based_planner.time_to_evaluate_rss", + time_to_evaluate_rss_); +} +} // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/pid_based_planner.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/pid_based_planner.hpp new file mode 100644 index 0000000000000..54836bde8d96f --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/pid_based_planner.hpp @@ -0,0 +1,158 @@ +// 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 PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ +#define PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ + +#include "../cruise_planner_interface.hpp" +#include "../type_alias.hpp" +#include "../types.hpp" +#include "autoware/signal_processing/lowpass_filter_1d.hpp" +#include "cruise_planning_debug_info.hpp" +#include "pid_controller.hpp" + +#include "visualization_msgs/msg/marker_array.hpp" + +#include +#include +#include + +using autoware::signal_processing::LowpassFilter1d; + +namespace autoware::motion_velocity_planner +{ + +class PIDBasedPlanner : public CruisePlannerInterface +{ +public: + struct CruiseObstacleInfo + { + CruiseObstacleInfo( + const CruiseObstacle & obstacle_arg, const double error_cruise_dist_arg, + const double dist_to_obstacle_arg, const double target_dist_to_obstacle_arg) + : obstacle(obstacle_arg), + error_cruise_dist(error_cruise_dist_arg), + dist_to_obstacle(dist_to_obstacle_arg), + target_dist_to_obstacle(target_dist_to_obstacle_arg) + { + } + + CruiseObstacle obstacle; + double error_cruise_dist; + double dist_to_obstacle; + double target_dist_to_obstacle; + }; + + PIDBasedPlanner( + rclcpp::Node & node, const CommonParam & common_param, + const CruisePlanningParam & cruise_planning_param); + + std::vector plan_cruise( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const std::vector & obstacles, std::shared_ptr debug_data_ptr, + std::unique_ptr & + planning_factor_interface, + std::optional & velocity_limit) override; + + void update_parameters(const std::vector & parameters) override; + +private: + Float32MultiArrayStamped get_cruise_planning_debug_message( + const rclcpp::Time & current_time) const override + { + return cruise_planning_debug_info_.convert_to_message(current_time); + } + + std::optional calc_obstacle_to_cruise( + const std::vector & stop_traj_points, + const std::shared_ptr planner_data, + const std::vector & obstacles); + + std::vector plan_cruise_trajectory( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + std::shared_ptr debug_data_ptr, + std::unique_ptr & + planning_factor_interface, + std::optional & velocity_limit, + const std::optional & cruise_obstacle_info); + + // velocity limit based planner + VelocityLimit plan_cruise_with_velocity_limit( + const std::shared_ptr planner_data, + const CruiseObstacleInfo & cruise_obstacle_info); + + // velocity insertion based planner + std::vector plan_cruise_with_trajectory( + const std::shared_ptr planner_data, + const std::vector & stop_traj_points, + const CruiseObstacleInfo & cruise_obstacle_info); + std::vector get_acceleration_limited_trajectory( + const std::vector & traj_points, const geometry_msgs::msg::Pose & start_pose, + const double v0, const double a0, const double target_acc, const double target_jerk_ratio, + const std::shared_ptr planner_data) const; + + // ROS parameters + double min_accel_during_cruise_; + double min_cruise_target_vel_; + + CruisePlanningDebugInfo cruise_planning_debug_info_; + + struct VelocityLimitBasedPlannerParam + { + std::unique_ptr pid_vel_controller; + double output_ratio_during_accel; + double vel_to_acc_weight; + bool enable_jerk_limit_to_output_acc{false}; + bool disable_target_acceleration{false}; + }; + VelocityLimitBasedPlannerParam velocity_limit_based_planner_param_; + + struct VelocityInsertionBasedPlannerParam + { + std::unique_ptr pid_acc_controller; + std::unique_ptr pid_jerk_controller; + double output_acc_ratio_during_accel; + double output_jerk_ratio_during_accel; + bool enable_jerk_limit_to_output_acc{false}; + }; + VelocityInsertionBasedPlannerParam velocity_insertion_based_planner_param_; + + std::optional prev_target_acc_; + + // lpf for nodes's input + std::shared_ptr lpf_dist_to_obstacle_ptr_; + std::shared_ptr lpf_error_cruise_dist_ptr_; + std::shared_ptr lpf_obstacle_vel_ptr_; + + // lpf for planner's input + std::shared_ptr lpf_normalized_error_cruise_dist_ptr_; + + // lpf for output + std::shared_ptr lpf_output_vel_ptr_; + std::shared_ptr lpf_output_acc_ptr_; + std::shared_ptr lpf_output_jerk_ptr_; + + std::vector prev_traj_points_; + + bool use_velocity_limit_based_planner_{true}; + + double time_to_evaluate_rss_; + + std::function error_func_; +}; +} // namespace autoware::motion_velocity_planner + +#endif // PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/pid_controller.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/pid_controller.hpp new file mode 100644 index 0000000000000..adcae7784103d --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/pid_based_planner/pid_controller.hpp @@ -0,0 +1,65 @@ +// 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 PID_BASED_PLANNER__PID_CONTROLLER_HPP_ +#define PID_BASED_PLANNER__PID_CONTROLLER_HPP_ + +#include + +namespace autoware::motion_velocity_planner +{ +class PIDController +{ +public: + PIDController(const double kp, const double ki, const double kd) + : kp_(kp), ki_(ki), kd_(kd), error_sum_(0.0) + { + } + + double calc(const double error) + { + error_sum_ += error; + + // TODO(murooka) use time for d gain calculation + const double output = + kp_ * error + ki_ * error_sum_ + (prev_error_ ? kd_ * (error - *prev_error_) : 0.0); + prev_error_ = error; + return output; + } + + double getKp() const { return kp_; } + double getKi() const { return ki_; } + double getKd() const { return kd_; } + + void updateParam(const double kp, const double ki, const double kd) + { + kp_ = kp; + ki_ = ki; + kd_ = kd; + + error_sum_ = 0.0; + prev_error_ = {}; + } + +private: + double kp_; + double ki_; + double kd_; + + double error_sum_; + std::optional prev_error_; +}; +} // namespace autoware::motion_velocity_planner + +#endif // PID_BASED_PLANNER__PID_CONTROLLER_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/type_alias.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/type_alias.hpp new file mode 100644 index 0000000000000..673574c24a914 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/type_alias.hpp @@ -0,0 +1,70 @@ +// 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 TYPE_ALIAS_HPP_ +#define TYPE_ALIAS_HPP_ + +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" + +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp" +#include "autoware_perception_msgs/msg/predicted_object.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "tier4_planning_msgs/msg/velocity_limit.hpp" +#include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include +#include +#include +#include + +#include +#include + +namespace autoware::motion_velocity_planner +{ +using autoware::vehicle_info_utils::VehicleInfo; +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_internal_debug_msgs::msg::Float32Stamped; +using autoware_internal_debug_msgs::msg::Float64Stamped; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::Shape; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Twist; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::PointCloud2; +using tier4_planning_msgs::msg::VelocityLimit; +using tier4_planning_msgs::msg::VelocityLimitClearCommand; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; +namespace bg = boost::geometry; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; +using Metric = tier4_metric_msgs::msg::Metric; +using MetricArray = tier4_metric_msgs::msg::MetricArray; +using PointCloud = pcl::PointCloud; +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::SafetyFactorArray; +} // namespace autoware::motion_velocity_planner + +#endif // TYPE_ALIAS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/types.hpp new file mode 100644 index 0000000000000..7f4fcfc7e3bb1 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/types.hpp @@ -0,0 +1,67 @@ +// 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 TYPES_HPP_ +#define TYPES_HPP_ + +#include "autoware/motion_velocity_planner_common_universe/planner_data.hpp" +#include "autoware/motion_velocity_planner_common_universe/polygon_utils.hpp" +#include "type_alias.hpp" + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +struct CruiseObstacle +{ + CruiseObstacle( + const std::string & arg_uuid, const rclcpp::Time & arg_stamp, + const geometry_msgs::msg::Pose & arg_pose, const double arg_lon_velocity, + const double arg_lat_velocity, + const std::vector & arg_collision_points, + bool arg_is_yield_obstacle = false) + : uuid(arg_uuid), + stamp(arg_stamp), + pose(arg_pose), + velocity(arg_lon_velocity), + lat_velocity(arg_lat_velocity), + collision_points(arg_collision_points), + is_yield_obstacle(arg_is_yield_obstacle) + { + } + std::string uuid{}; + rclcpp::Time stamp{}; + geometry_msgs::msg::Pose pose{}; // interpolated with the current stamp + double velocity{}; // longitudinal velocity against ego's trajectory + double lat_velocity{}; // lateral velocity against ego's trajectory + + std::vector collision_points{}; // time-series collision points + bool is_yield_obstacle{}; +}; + +struct DebugData +{ + DebugData() = default; + std::vector> intentionally_ignored_obstacles{}; + std::vector obstacles_to_cruise{}; + std::vector decimated_traj_polys{}; + MarkerArray cruise_wall_marker{}; +}; + +} // namespace autoware::motion_velocity_planner + +#endif // TYPES_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/CMakeLists.txt new file mode 100644 index 0000000000000..d74b292785fa6 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_motion_velocity_obstacle_slow_down_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node_universe plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/README.md new file mode 100644 index 0000000000000..25c15b59075c1 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/README.md @@ -0,0 +1,52 @@ +# Obstacle Slow Down + +## Role + +The `obstacle_slow_down` module does the slow down planning when there is a static/dynamic obstacle near the trajectory. + +## Activation + +This module is activated if the launch parameter `launch_obstacle_slow_down_module` is set to true. + +## Inner-workings / Algorithms + +### Obstacle Filtering + +Among obstacles which are not for cruising and stopping, the obstacles meeting the following condition are determined as obstacles for slowing down. + +- The object type is for slowing down according to `obstacle_filtering.object_type.*`. +- The lateral distance from the object to the ego's trajectory is smaller than `obstacle_filtering.max_lat_margin`. + +### Slow Down Planning + +The role of the slow down planning is inserting slow down velocity in the trajectory where the trajectory points are close to the obstacles. The parameters can be customized depending on the obstacle type, making it possible to adjust the slow down behavior depending if the obstacle is a pedestrian, bicycle, car, etc. Each obstacle type has a `static` and a `moving` parameter set, so it is possible to customize the slow down response of the ego vehicle according to the obstacle type and if it is moving or not. If an obstacle is determined to be moving, the corresponding `moving` set of parameters will be used to compute the vehicle slow down, otherwise, the `static` parameters will be used. The `static` and `moving` separation is useful for customizing the ego vehicle slow down behavior to, for example, slow down more significantly when passing stopped vehicles that might cause occlusion or that might suddenly open its doors. + +An obstacle is classified as `static` if its total speed is less than the `moving_object_speed_threshold` parameter. Furthermore, a hysteresis based approach is used to avoid chattering, it uses the `moving_object_hysteresis_range` parameter range and the obstacle's previous state (`moving` or `static`) to determine if the obstacle is moving or not. In other words, if an obstacle was previously classified as `static`, it will not change its classification to `moving` unless its total speed is greater than `moving_object_speed_threshold` + `moving_object_hysteresis_range`. Likewise, an obstacle previously classified as `moving`, will only change to `static` if its speed is lower than `moving_object_speed_threshold` - `moving_object_hysteresis_range`. + +The closest point on the obstacle to the ego's trajectory is calculated. +Then, the slow down velocity is calculated by linear interpolation with the distance between the point and trajectory as follows. + +![slow_down_velocity_calculation](./docs/slow_down_velocity_calculation.svg) + +| Variable | Description | +| ---------- | ----------------------------------- | +| `v_{out}` | calculated velocity for slow down | +| `v_{min}` | `min_lat_velocity` | +| `v_{max}` | `max_lat_velocity` | +| `l_{min}` | `min_lat_margin` | +| `l_{max}` | `max_lat_margin` | +| `l'_{max}` | `obstacle_filtering.max_lat_margin` | + +The calculated velocity is inserted in the trajectory where the obstacle is inside the area with `obstacle_filtering.max_lat_margin`. + +![slow_down_planning](./docs/slow_down_planning.drawio.svg) + +## Debugging + +### Obstacle for slow down + +Yellow sphere which is an obstacle for slow_down is visualized by `obstacles_to_slow_down` in the `~/debug/marker` topic. + +Yellow wall which means a safe distance to slow_down if the ego's front meets the wall is visualized in the `~/debug/slow_down/virtual_wall` topic. + +![slow_down_visualization](./docs/slow_down_visualization.png) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/config/obstacle_slow_down.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/config/obstacle_slow_down.param.yaml new file mode 100644 index 0000000000000..f826e4ccd6618 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/config/obstacle_slow_down.param.yaml @@ -0,0 +1,50 @@ +/**: + ros__parameters: + obstacle_slow_down: + slow_down_planning: + slow_down_min_acc: -1.0 # slow down min deceleration [m/ss] + slow_down_min_jerk: -1.0 # slow down min jerk [m/sss] + + lpf_gain_slow_down_vel: 0.99 # low-pass filter gain for slow down velocity + lpf_gain_lat_dist: 0.999 # low-pass filter gain for lateral distance from obstacle to ego's path + lpf_gain_dist_to_slow_down: 0.7 # low-pass filter gain for distance to slow down start + + time_margin_on_target_velocity: 1.5 # [s] + + # parameters to calculate slow down velocity by linear interpolation + object_type_specified_params: + types: + - "default" + default: + moving: + min_lat_margin: 0.2 + max_lat_margin: 1.0 + min_ego_velocity: 2.0 + max_ego_velocity: 8.0 + static: + min_lat_margin: 0.2 + max_lat_margin: 1.0 + min_ego_velocity: 4.0 + max_ego_velocity: 8.0 + + moving_object_speed_threshold: 0.5 # [m/s] how fast the object needs to move to be considered as "moving" + moving_object_hysteresis_range: 0.1 # [m/s] hysteresis range used to prevent chattering when obstacle moves close to moving_object_speed_threshold + + obstacle_filtering: + object_type: + unknown: false + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: true + pedestrian: true + pointcloud: false + + min_lat_margin: 0.0 # lateral margin between obstacle and trajectory band with ego's width to avoid the conflict with the obstacle_stop + max_lat_margin: 1.1 # lateral margin between obstacle and trajectory band with ego's width + lat_hysteresis_margin: 0.2 + + successive_num_to_entry_slow_down_condition: 5 + successive_num_to_exit_slow_down_condition: 5 diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/docs/slow_down_planning.drawio.svg b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/docs/slow_down_planning.drawio.svg new file mode 100644 index 0000000000000..603e6b37a653f --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/docs/slow_down_planning.drawio.svg @@ -0,0 +1,133 @@ + + + + + + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + + + + + + + +
+
+
+ trajectory velocity +
+
+
+ + trajecto... + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ max_slow_down_lat_margin +
+
+
+ + max_slow_d... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/docs/slow_down_velocity_calculation.svg b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/docs/slow_down_velocity_calculation.svg new file mode 100644 index 0000000000000..ee53e6e60a0c0 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/docs/slow_down_velocity_calculation.svg @@ -0,0 +1,534 @@ + + + + + + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + + + + + + + + + + + + + + + + + +
+
+
+ slow down +
+ velocity +
+
+
+ + slow down... + + + + + + +
+
+
+ lateral distance +
+
+
+ + lateral distance + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `v_{max}` + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `v_{min}` + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `l_{min}` + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `l_{max}` + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + +
+
+
+ linear interpolation of slow down velocity +
+
+
+ + linear interpolation of slow down velocity + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `v_{out}` + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `l_{max}... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/docs/slow_down_visualization.png b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/docs/slow_down_visualization.png new file mode 100644 index 0000000000000..472b3bb6c982a Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/docs/slow_down_visualization.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/package.xml new file mode 100644 index 0000000000000..aa1ec002ce5cc --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/package.xml @@ -0,0 +1,40 @@ + + + + autoware_motion_velocity_obstacle_slow_down_module + 0.40.0 + obstacle slow down feature in motion_velocity_planner + + Takayuki Murooka + Yuki Takagi + Maxime Clement + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + autoware_motion_utils + autoware_motion_velocity_planner_common_universe + autoware_perception_msgs + autoware_planning_msgs + autoware_route_handler + autoware_signal_processing + autoware_universe_utils + autoware_vehicle_info_utils + geometry_msgs + libboost-dev + pluginlib + rclcpp + tf2 + tier4_metric_msgs + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/plugins.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/plugins.xml new file mode 100644 index 0000000000000..0cfaadbfc9796 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/metrics_manager.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/metrics_manager.hpp new file mode 100644 index 0000000000000..11554121a0882 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/metrics_manager.hpp @@ -0,0 +1,60 @@ +// 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 METRICS_MANAGER_HPP_ +#define METRICS_MANAGER_HPP_ + +#include "type_alias.hpp" +#include "types.hpp" + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class MetricsManager +{ +public: + void init() { metrics_.clear(); } + + void calculate_metrics(const std::string & module_name, const std::string & reason) + { + // Create status + { + // Decision + Metric decision_metric; + decision_metric.name = module_name + "/decision"; + decision_metric.unit = "string"; + decision_metric.value = reason; + metrics_.push_back(decision_metric); + } + } + + MetricArray create_metric_array(const rclcpp::Time & current_time) + { + MetricArray metrics_msg; + metrics_msg.stamp = current_time; + metrics_msg.metric_array.insert( + metrics_msg.metric_array.end(), metrics_.begin(), metrics_.end()); + return metrics_msg; + } + +private: + std::vector metrics_; +}; + +} // namespace autoware::motion_velocity_planner + +#endif // METRICS_MANAGER_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp new file mode 100644 index 0000000000000..286c10c0c20d1 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp @@ -0,0 +1,907 @@ +// Copyright 2025 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 "obstacle_slow_down_module.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +using autoware::universe_utils::getOrDeclareParameter; + +namespace +{ +bool is_lower_considering_hysteresis( + const double current_val, const bool was_low, const double high_val, const double low_val) +{ + if (was_low) { + if (high_val < current_val) { + return false; + } + return true; + } + if (current_val < low_val) { + return true; + } + return false; +} + +geometry_msgs::msg::Point to_geom_point(const autoware::universe_utils::Point2d & point) +{ + geometry_msgs::msg::Point geom_point; + geom_point.x = point.x(); + geom_point.y = point.y(); + return geom_point; +} + +template +std::optional get_object_from_uuid( + const std::vector & objects, const std::string & target_uuid) +{ + const auto itr = std::find_if(objects.begin(), objects.end(), [&](const auto & object) { + return object.uuid == target_uuid; + }); + + if (itr == objects.end()) { + return std::nullopt; + } + return *itr; +} + +// TODO(murooka) following two functions are copied from behavior_velocity_planner. +// These should be refactored. +double find_reach_time( + const double jerk, const double accel, const double velocity, const double distance, + const double t_min, const double t_max) +{ + const double j = jerk; + const double a = accel; + const double v = velocity; + const double d = distance; + const double min = t_min; + const double max = t_max; + auto f = [](const double t, const double j, const double a, const double v, const double d) { + return j * t * t * t / 6.0 + a * t * t / 2.0 + v * t - d; + }; + if (f(min, j, a, v, d) > 0 || f(max, j, a, v, d) < 0) { + throw std::logic_error( + "[motion_velocity_planner_common](find_reach_time): search range is invalid"); + } + const double eps = 1e-5; + const int warn_iter = 100; + double lower = min; + double upper = max; + double t; + int iter = 0; + for (int i = 0;; i++) { + t = 0.5 * (lower + upper); + const double fx = f(t, j, a, v, d); + // std::cout<<"fx: "< 0.0) { + upper = t; + } else { + lower = t; + } + iter++; + if (iter > warn_iter) + std::cerr << "[motion_velocity_planner_common](find_reach_time): current iter is over warning" + << std::endl; + } + return t; +} + +double calc_deceleration_velocity_from_distance_to_target( + const double max_slowdown_jerk, const double max_slowdown_accel, const double current_accel, + const double current_velocity, const double distance_to_target) +{ + if (max_slowdown_jerk > 0 || max_slowdown_accel > 0) { + throw std::logic_error("max_slowdown_jerk and max_slowdown_accel should be negative"); + } + // case0: distance to target is behind ego + if (distance_to_target <= 0) return current_velocity; + auto ft = [](const double t, const double j, const double a, const double v, const double d) { + return j * t * t * t / 6.0 + a * t * t / 2.0 + v * t - d; + }; + auto vt = [](const double t, const double j, const double a, const double v) { + return j * t * t / 2.0 + a * t + v; + }; + const double j_max = max_slowdown_jerk; + const double a0 = current_accel; + const double a_max = max_slowdown_accel; + const double v0 = current_velocity; + const double l = distance_to_target; + const double t_const_jerk = (a_max - a0) / j_max; + const double d_const_jerk_stop = ft(t_const_jerk, j_max, a0, v0, 0.0); + const double d_const_acc_stop = l - d_const_jerk_stop; + + if (d_const_acc_stop < 0) { + // case0: distance to target is within constant jerk deceleration + // use binary search instead of solving cubic equation + const double t_jerk = find_reach_time(j_max, a0, v0, l, 0, t_const_jerk); + const double velocity = vt(t_jerk, j_max, a0, v0); + return velocity; + } else { + const double v1 = vt(t_const_jerk, j_max, a0, v0); + const double discriminant_of_stop = 2.0 * a_max * d_const_acc_stop + v1 * v1; + // case3: distance to target is farther than distance to stop + if (discriminant_of_stop <= 0) { + return 0.0; + } + // case2: distance to target is within constant accel deceleration + // solve d = 0.5*a^2+v*t by t + const double t_acc = (-v1 + std::sqrt(discriminant_of_stop)) / a_max; + return vt(t_acc, 0.0, a_max, v1); + } + return current_velocity; +} + +VelocityLimitClearCommand create_velocity_limit_clear_command( + const rclcpp::Time & current_time, [[maybe_unused]] const std::string & module_name) +{ + VelocityLimitClearCommand msg; + msg.stamp = current_time; + msg.sender = "obstacle_slow_down"; + msg.command = true; + return msg; +} + +Float64Stamped create_float64_stamped(const rclcpp::Time & now, const float & data) +{ + Float64Stamped msg; + msg.stamp = now; + msg.data = data; + return msg; +} +} // namespace + +void ObstacleSlowDownModule::init(rclcpp::Node & node, const std::string & module_name) +{ + module_name_ = module_name; + clock_ = node.get_clock(); + logger_ = node.get_logger(); + + // ros parameters + common_param_ = CommonParam(node); + slow_down_planning_param_ = SlowDownPlanningParam(node); + obstacle_filtering_param_ = ObstacleFilteringParam(node); + + objects_of_interest_marker_interface_ = std::make_unique< + autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface>( + &node, "motion_velocity_planner_common"); + + // common publisher + processing_time_publisher_ = + node.create_publisher("~/debug/obstacle_slow_down/processing_time_ms", 1); + virtual_wall_publisher_ = + node.create_publisher("~/obstacle_slow_down/virtual_walls", 1); + debug_publisher_ = node.create_publisher("~/obstacle_slow_down/debug_markers", 1); + + // module publisher + metrics_pub_ = node.create_publisher("~/slow_down/metrics", 10); + debug_slow_down_planning_info_pub_ = + node.create_publisher("~/debug/slow_down_planning_info", 1); + processing_time_detail_pub_ = node.create_publisher( + "~/debug/processing_time_detail_ms/obstacle_slow_down", 1); + + // interface publisher + objects_of_interest_marker_interface_ = std::make_unique< + autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface>( + &node, "obstacle_slow_down"); + planning_factor_interface_ = + std::make_unique( + &node, "obstacle_slow_down"); + + // time keeper + time_keeper_ = std::make_shared(processing_time_detail_pub_); +} + +void ObstacleSlowDownModule::update_parameters( + [[maybe_unused]] const std::vector & parameters) +{ +} + +VelocityPlanningResult ObstacleSlowDownModule::plan( + const std::vector & raw_trajectory_points, + [[maybe_unused]] const std::vector & + smoothed_trajectory_points, + const std::shared_ptr planner_data) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + stop_watch_.tic(); + debug_data_ptr_ = std::make_shared(); + metrics_manager_.init(); + decimated_traj_polys_ = std::nullopt; + + const auto decimated_traj_points = utils::decimate_trajectory_points_from_ego( + raw_trajectory_points, planner_data->current_odometry.pose.pose, + planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold, + planner_data->trajectory_polygon_collision_check.decimate_trajectory_step_length, 0.0); + + const auto slow_down_obstacles = filter_slow_down_obstacle_for_predicted_object( + planner_data->current_odometry, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold, raw_trajectory_points, decimated_traj_points, + planner_data->objects, rclcpp::Time(planner_data->predicted_objects_header.stamp), + planner_data->vehicle_info_, planner_data->trajectory_polygon_collision_check); + + VelocityPlanningResult result; + result.slowdown_intervals = plan_slow_down( + planner_data, raw_trajectory_points, slow_down_obstacles, result.velocity_limit, + planner_data->vehicle_info_); + metrics_manager_.calculate_metrics("PlannerInterface", "cruise"); + + // clear velocity limit if necessary + if (result.velocity_limit) { + need_to_clear_velocity_limit_ = true; + } else { + if (need_to_clear_velocity_limit_) { + // clear velocity limit + result.velocity_limit_clear_command = + create_velocity_limit_clear_command(clock_->now(), module_name_); + need_to_clear_velocity_limit_ = false; + } + } + + publish_debug_info(); + + return result; +} + +std::string ObstacleSlowDownModule::get_module_name() const +{ + return module_name_; +} + +std::vector +ObstacleSlowDownModule::filter_slow_down_obstacle_for_predicted_object( + const Odometry & odometry, const double ego_nearest_dist_threshold, + const double ego_nearest_yaw_threshold, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector> & objects, + const rclcpp::Time & predicted_objects_stamp, const VehicleInfo & vehicle_info, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const auto & current_pose = odometry.pose.pose; + + // calculate collision points with trajectory with lateral stop margin + // NOTE: For additional margin, hysteresis is not divided by two. + const auto & p = obstacle_filtering_param_; + const auto & tp = trajectory_polygon_collision_check; + const auto decimated_traj_polys_with_lat_margin = polygon_utils::create_one_step_polygons( + decimated_traj_points, vehicle_info, odometry.pose.pose, + p.max_lat_margin + p.lat_hysteresis_margin, tp.enable_to_consider_current_pose, + tp.time_to_convergence, tp.decimate_trajectory_step_length); + debug_data_ptr_->decimated_traj_polys = decimated_traj_polys_with_lat_margin; + + // slow down + slow_down_condition_counter_.reset_current_uuids(); + std::vector slow_down_obstacles; + for (const auto & object : objects) { + // 1. rough filtering + // 1.1. Check if the obstacle is in front of the ego. + const double lon_dist_from_ego_to_obj = + object->get_dist_from_ego_longitudinal(traj_points, current_pose.position); + if (lon_dist_from_ego_to_obj < 0.0) { + continue; + } + + // 1.2. Check if the rough lateral distance is smaller than the threshold. + const double min_lat_dist_to_traj_poly = + utils::calc_possible_min_dist_from_obj_to_traj_poly(object, traj_points, vehicle_info); + if (obstacle_filtering_param_.max_lat_margin < min_lat_dist_to_traj_poly) { + continue; + } + + // 2. precise filtering + const auto & decimated_traj_polys = get_decimated_traj_polys( + traj_points, current_pose, vehicle_info, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold, trajectory_polygon_collision_check); + const double dist_from_obj_poly_to_traj_poly = + object->get_dist_to_traj_poly(decimated_traj_polys); + const auto slow_down_obstacle = create_slow_down_obstacle_for_predicted_object( + traj_points, decimated_traj_polys_with_lat_margin, object, predicted_objects_stamp, + dist_from_obj_poly_to_traj_poly); + if (slow_down_obstacle) { + slow_down_obstacles.push_back(*slow_down_obstacle); + continue; + } + } + slow_down_condition_counter_.remove_counter_unless_updated(); + prev_slow_down_object_obstacles_ = slow_down_obstacles; + + RCLCPP_DEBUG( + logger_, "The number of output obstacles of filter_slow_down_obstacles is %ld", + slow_down_obstacles.size()); + return slow_down_obstacles; +} + +std::optional +ObstacleSlowDownModule::create_slow_down_obstacle_for_predicted_object( + const std::vector & traj_points, + const std::vector & decimated_traj_polys_with_lat_margin, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const double dist_from_obj_poly_to_traj_poly) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const auto & p = obstacle_filtering_param_; + + const auto & obj_uuid = object->predicted_object.object_id; + const auto & obj_uuid_str = autoware::universe_utils::toHexString(obj_uuid); + const auto & obj_label = object->predicted_object.classification.at(0).label; + slow_down_condition_counter_.add_current_uuid(obj_uuid_str); + + const bool is_prev_obstacle_slow_down = + utils::get_obstacle_from_uuid(prev_slow_down_object_obstacles_, obj_uuid_str).has_value(); + + if (!is_slow_down_obstacle(obj_label)) { + return std::nullopt; + } + + if (dist_from_obj_poly_to_traj_poly <= p.min_lat_margin) { + return std::nullopt; + } + + // check lateral distance considering hysteresis + const bool is_lat_dist_low = is_lower_considering_hysteresis( + dist_from_obj_poly_to_traj_poly, is_prev_obstacle_slow_down, + p.max_lat_margin + p.lat_hysteresis_margin / 2.0, + p.max_lat_margin - p.lat_hysteresis_margin / 2.0); + + const bool is_slow_down_required = [&]() { + if (is_prev_obstacle_slow_down) { + // check if exiting slow down + if (!is_lat_dist_low) { + const int count = slow_down_condition_counter_.decrease_counter(obj_uuid_str); + if (count <= -p.successive_num_to_exit_slow_down_condition) { + slow_down_condition_counter_.reset(obj_uuid_str); + return false; + } + } + return true; + } + // check if entering slow down + if (is_lat_dist_low) { + const int count = slow_down_condition_counter_.increase_counter(obj_uuid_str); + if (p.successive_num_to_entry_slow_down_condition <= count) { + slow_down_condition_counter_.reset(obj_uuid_str); + return true; + } + } + return false; + }(); + if (!is_slow_down_required) { + RCLCPP_DEBUG( + logger_, "[SlowDown] Ignore obstacle (%s) since it's far from trajectory. (%f [m])", + obj_uuid_str.substr(0, 4).c_str(), dist_from_obj_poly_to_traj_poly); + return std::nullopt; + } + + const auto obstacle_poly = autoware::universe_utils::toPolygon2d( + object->predicted_object.kinematics.initial_pose_with_covariance.pose, + object->predicted_object.shape); + + std::vector front_collision_polygons; + size_t front_seg_idx = 0; + std::vector back_collision_polygons; + size_t back_seg_idx = 0; + for (size_t i = 0; i < decimated_traj_polys_with_lat_margin.size(); ++i) { + std::vector collision_polygons; + bg::intersection(decimated_traj_polys_with_lat_margin.at(i), obstacle_poly, collision_polygons); + + if (!collision_polygons.empty()) { + if (front_collision_polygons.empty()) { + front_collision_polygons = collision_polygons; + front_seg_idx = i == 0 ? i : i - 1; + } + back_collision_polygons = collision_polygons; + back_seg_idx = i == 0 ? i : i - 1; + } else { + if (!back_collision_polygons.empty()) { + break; // for efficient calculation + } + } + } + + if (front_collision_polygons.empty() || back_collision_polygons.empty()) { + RCLCPP_DEBUG( + logger_, "[SlowDown] Ignore obstacle (%s) since there is no collision point", + obj_uuid_str.substr(0, 4).c_str()); + return std::nullopt; + } + + // calculate front collision point + double front_min_dist = std::numeric_limits::max(); + geometry_msgs::msg::Point front_collision_point; + for (const auto & collision_poly : front_collision_polygons) { + for (const auto & collision_point : collision_poly.outer()) { + const auto collision_geom_point = to_geom_point(collision_point); + const double dist = autoware::motion_utils::calcLongitudinalOffsetToSegment( + traj_points, front_seg_idx, collision_geom_point); + if (dist < front_min_dist) { + front_min_dist = dist; + front_collision_point = collision_geom_point; + } + } + } + + // calculate back collision point + double back_max_dist = -std::numeric_limits::max(); + geometry_msgs::msg::Point back_collision_point = front_collision_point; + for (const auto & collision_poly : back_collision_polygons) { + for (const auto & collision_point : collision_poly.outer()) { + const auto collision_geom_point = to_geom_point(collision_point); + const double dist = autoware::motion_utils::calcLongitudinalOffsetToSegment( + traj_points, back_seg_idx, collision_geom_point); + if (back_max_dist < dist) { + back_max_dist = dist; + back_collision_point = collision_geom_point; + } + } + } + + return SlowDownObstacle{ + obj_uuid_str, + predicted_objects_stamp, + object->predicted_object.classification.at(0), + object->get_predicted_pose(clock_->now(), predicted_objects_stamp), + object->get_lon_vel_relative_to_traj(traj_points), + object->get_lat_vel_relative_to_traj(traj_points), + dist_from_obj_poly_to_traj_poly, + front_collision_point, + back_collision_point}; +} + +std::vector ObstacleSlowDownModule::plan_slow_down( + const std::shared_ptr planner_data, + const std::vector & traj_points, const std::vector & obstacles, + [[maybe_unused]] std::optional & velocity_limit, const VehicleInfo & vehicle_info) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + auto slow_down_traj_points = traj_points; + slow_down_debug_multi_array_ = Float32MultiArrayStamped(); + + const double dist_to_ego = [&]() { + const size_t ego_seg_idx = planner_data->find_segment_index( + slow_down_traj_points, planner_data->current_odometry.pose.pose); + return autoware::motion_utils::calcSignedArcLength( + slow_down_traj_points, 0, planner_data->current_odometry.pose.pose.position, ego_seg_idx); + }(); + const double abs_ego_offset = planner_data->is_driving_forward + ? std::abs(vehicle_info.max_longitudinal_offset_m) + : std::abs(vehicle_info.min_longitudinal_offset_m); + + // define function to insert slow down velocity to trajectory + const auto insert_point_in_trajectory = [&](const double lon_dist) -> std::optional { + const auto inserted_idx = + autoware::motion_utils::insertTargetPoint(0, lon_dist, slow_down_traj_points); + if (inserted_idx) { + if (inserted_idx.value() + 1 <= slow_down_traj_points.size() - 1) { + // zero-order hold for velocity interpolation + slow_down_traj_points.at(inserted_idx.value()).longitudinal_velocity_mps = + slow_down_traj_points.at(inserted_idx.value() + 1).longitudinal_velocity_mps; + } + return inserted_idx.value(); + } + return std::nullopt; + }; + + std::vector slowdown_intervals; + std::vector new_prev_slow_down_output; + for (size_t i = 0; i < obstacles.size(); ++i) { + const auto & obstacle = obstacles.at(i); + const auto prev_output = get_object_from_uuid(prev_slow_down_output_, obstacle.uuid); + + const bool is_obstacle_moving = [&]() -> bool { + const auto & p = slow_down_planning_param_; + const auto object_vel_norm = std::hypot(obstacle.velocity, obstacle.lat_velocity); + if (!prev_output) { + return object_vel_norm > p.moving_object_speed_threshold; + } + if (prev_output->is_obstacle_moving) { + return object_vel_norm > p.moving_object_speed_threshold - p.moving_object_hysteresis_range; + } + return object_vel_norm > p.moving_object_speed_threshold + p.moving_object_hysteresis_range; + }(); + + // calculate slow down start distance, and insert slow down velocity + const auto dist_vec_to_slow_down = calculate_distance_to_slow_down_with_constraints( + planner_data, slow_down_traj_points, obstacle, prev_output, dist_to_ego, vehicle_info, + is_obstacle_moving); + if (!dist_vec_to_slow_down) { + RCLCPP_DEBUG( + logger_, "[SlowDown] Ignore obstacle (%s) since distance to slow down is not valid", + obstacle.uuid.c_str()); + continue; + } + const auto dist_to_slow_down_start = std::get<0>(*dist_vec_to_slow_down); + const auto dist_to_slow_down_end = std::get<1>(*dist_vec_to_slow_down); + const auto feasible_slow_down_vel = std::get<2>(*dist_vec_to_slow_down); + + // calculate slow down end distance, and insert slow down velocity + // NOTE: slow_down_start_idx will not be wrong since inserted back point is after inserted + // front point. + const auto slow_down_start_idx = insert_point_in_trajectory(dist_to_slow_down_start); + const auto slow_down_end_idx = dist_to_slow_down_start < dist_to_slow_down_end + ? insert_point_in_trajectory(dist_to_slow_down_end) + : std::nullopt; + if (!slow_down_end_idx) { + continue; + } + + // calculate slow down velocity + const double stable_slow_down_vel = [&]() { + if (prev_output) { + return autoware::signal_processing::lowpassFilter( + feasible_slow_down_vel, prev_output->target_vel, + slow_down_planning_param_.lpf_gain_slow_down_vel); + } + return feasible_slow_down_vel; + }(); + + // insert slow down velocity between slow start and end + slowdown_intervals.push_back(SlowdownInterval{ + slow_down_traj_points.at(slow_down_start_idx ? *slow_down_start_idx : 0).pose.position, + slow_down_traj_points.at(*slow_down_end_idx).pose.position, stable_slow_down_vel}); + + // add debug data + slow_down_debug_multi_array_.data.push_back(obstacle.dist_to_traj_poly); + slow_down_debug_multi_array_.data.push_back(dist_to_slow_down_start); + slow_down_debug_multi_array_.data.push_back(dist_to_slow_down_end); + slow_down_debug_multi_array_.data.push_back(feasible_slow_down_vel); + slow_down_debug_multi_array_.data.push_back(stable_slow_down_vel); + slow_down_debug_multi_array_.data.push_back(slow_down_start_idx ? *slow_down_start_idx : -1.0); + slow_down_debug_multi_array_.data.push_back(*slow_down_end_idx); + + // add virtual wall + if (slow_down_start_idx && slow_down_end_idx) { + const size_t ego_idx = + planner_data->find_index(slow_down_traj_points, planner_data->current_odometry.pose.pose); + const size_t slow_down_wall_idx = [&]() { + if (ego_idx < *slow_down_start_idx) return *slow_down_start_idx; + if (ego_idx < *slow_down_end_idx) return ego_idx; + return *slow_down_end_idx; + }(); + + const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( + slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down", clock_->now(), i, + abs_ego_offset, "", planner_data->is_driving_forward); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); + + // update planning factor + planning_factor_interface_->add( + slow_down_traj_points, planner_data->current_odometry.pose.pose, + slow_down_traj_points.at(*slow_down_start_idx).pose, + slow_down_traj_points.at(*slow_down_end_idx).pose, PlanningFactor::SLOW_DOWN, + SafetyFactorArray{}, planner_data->is_driving_forward, stable_slow_down_vel); + } + + // add debug virtual wall + if (slow_down_start_idx) { + const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( + slow_down_traj_points.at(*slow_down_start_idx).pose, "obstacle slow down start", + clock_->now(), i * 2, abs_ego_offset, "", planner_data->is_driving_forward); + autoware::universe_utils::appendMarkerArray( + markers, &debug_data_ptr_->slow_down_debug_wall_marker); + } + if (slow_down_end_idx) { + const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( + slow_down_traj_points.at(*slow_down_end_idx).pose, "obstacle slow down end", clock_->now(), + i * 2 + 1, abs_ego_offset, "", planner_data->is_driving_forward); + autoware::universe_utils::appendMarkerArray( + markers, &debug_data_ptr_->slow_down_debug_wall_marker); + } + + // Add debug data + debug_data_ptr_->obstacles_to_slow_down.push_back(obstacle); + + // update prev_slow_down_output_ + new_prev_slow_down_output.push_back(SlowDownOutput{ + obstacle.uuid, slow_down_traj_points, slow_down_start_idx, slow_down_end_idx, + stable_slow_down_vel, feasible_slow_down_vel, obstacle.dist_to_traj_poly, + is_obstacle_moving}); + } + + // update prev_slow_down_output_ + prev_slow_down_output_ = new_prev_slow_down_output; + + return slowdown_intervals; +} + +Float32MultiArrayStamped ObstacleSlowDownModule::get_slow_down_planning_debug_message( + const rclcpp::Time & current_time) +{ + slow_down_debug_multi_array_.stamp = current_time; + return slow_down_debug_multi_array_; +} + +void ObstacleSlowDownModule::publish_debug_info() +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + // 1. debug marker + MarkerArray debug_marker; + + // 1.1. obstacles + for (size_t i = 0; i < debug_data_ptr_->obstacles_to_slow_down.size(); ++i) { + // obstacle + const auto obstacle_marker = utils::get_object_marker( + debug_data_ptr_->obstacles_to_slow_down.at(i).pose, i, "obstacles", 0.7, 0.7, 0.0); + debug_marker.markers.push_back(obstacle_marker); + + // collision points + auto front_collision_point_marker = autoware::universe_utils::createDefaultMarker( + "map", clock_->now(), "collision_points", i * 2 + 0, Marker::SPHERE, + autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + front_collision_point_marker.pose.position = + debug_data_ptr_->obstacles_to_slow_down.at(i).front_collision_point; + auto back_collision_point_marker = autoware::universe_utils::createDefaultMarker( + "map", clock_->now(), "collision_points", i * 2 + 1, Marker::SPHERE, + autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + back_collision_point_marker.pose.position = + debug_data_ptr_->obstacles_to_slow_down.at(i).back_collision_point; + + debug_marker.markers.push_back(front_collision_point_marker); + debug_marker.markers.push_back(back_collision_point_marker); + } + + // 1.2. slow down debug wall marker + autoware::universe_utils::appendMarkerArray( + debug_data_ptr_->slow_down_debug_wall_marker, &debug_marker); + + // 1.3. detection area + auto decimated_traj_polys_marker = autoware::universe_utils::createDefaultMarker( + "map", clock_->now(), "detection_area", 0, Marker::LINE_LIST, + autoware::universe_utils::createMarkerScale(0.01, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); + for (const auto & decimated_traj_poly : debug_data_ptr_->decimated_traj_polys) { + for (size_t dp_idx = 0; dp_idx < decimated_traj_poly.outer().size(); ++dp_idx) { + const auto & current_point = decimated_traj_poly.outer().at(dp_idx); + const auto & next_point = + decimated_traj_poly.outer().at((dp_idx + 1) % decimated_traj_poly.outer().size()); + + decimated_traj_polys_marker.points.push_back( + autoware::universe_utils::createPoint(current_point.x(), current_point.y(), 0.0)); + decimated_traj_polys_marker.points.push_back( + autoware::universe_utils::createPoint(next_point.x(), next_point.y(), 0.0)); + } + } + debug_marker.markers.push_back(decimated_traj_polys_marker); + + debug_publisher_->publish(debug_marker); + + // 2. virtual wall + virtual_wall_publisher_->publish(debug_data_ptr_->slow_down_wall_marker); + + // 3. slow down planning info + const auto slow_down_debug_msg = get_slow_down_planning_debug_message(clock_->now()); + debug_slow_down_planning_info_pub_->publish(slow_down_debug_msg); + + // 4. objects of interest + objects_of_interest_marker_interface_->publishMarkerArray(); + + // 5. metrics + const auto metrics_msg = metrics_manager_.create_metric_array(clock_->now()); + metrics_pub_->publish(metrics_msg); + + // 6. processing time + processing_time_publisher_->publish(create_float64_stamped(clock_->now(), stop_watch_.toc())); + + // 7. planning factor + planning_factor_interface_->publish(); +} + +bool ObstacleSlowDownModule::is_slow_down_obstacle(const uint8_t label) const +{ + const auto & types = obstacle_filtering_param_.object_types; + return std::find(types.begin(), types.end(), label) != types.end(); +} + +std::optional> +ObstacleSlowDownModule::calculate_distance_to_slow_down_with_constraints( + const std::shared_ptr planner_data, + const std::vector & traj_points, const SlowDownObstacle & obstacle, + const std::optional & prev_output, const double dist_to_ego, + const VehicleInfo & vehicle_info, const bool is_obstacle_moving) const +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const double abs_ego_offset = planner_data->is_driving_forward + ? std::abs(vehicle_info.max_longitudinal_offset_m) + : std::abs(vehicle_info.min_longitudinal_offset_m); + const double obstacle_vel = obstacle.velocity; + // calculate slow down velocity + const double slow_down_vel = + calculate_slow_down_velocity(obstacle, prev_output, is_obstacle_moving); + + // calculate distance to collision points + const double dist_to_front_collision = + autoware::motion_utils::calcSignedArcLength(traj_points, 0, obstacle.front_collision_point); + const double dist_to_back_collision = + autoware::motion_utils::calcSignedArcLength(traj_points, 0, obstacle.back_collision_point); + + // calculate offset distance to first collision considering relative velocity + const double relative_vel = + planner_data->current_odometry.twist.twist.linear.x - obstacle.velocity; + const double offset_dist_to_collision = [&]() { + if (dist_to_front_collision < dist_to_ego + abs_ego_offset) { + return 0.0; + } + + // NOTE: This min_relative_vel forces the relative velocity positive if the ego velocity is + // lower than the obstacle velocity. Without this, the slow down feature will flicker where + // the ego velocity is very close to the obstacle velocity. + constexpr double min_relative_vel = 1.0; + const double time_to_collision = (dist_to_front_collision - dist_to_ego - abs_ego_offset) / + std::max(min_relative_vel, relative_vel); + + constexpr double time_to_collision_margin = 1.0; + const double cropped_time_to_collision = + std::max(0.0, time_to_collision - time_to_collision_margin); + return obstacle_vel * cropped_time_to_collision; + }(); + + // calculate distance during deceleration, slow down preparation, and slow down + const double min_slow_down_prepare_dist = 3.0; + const double slow_down_prepare_dist = std::max( + min_slow_down_prepare_dist, + slow_down_vel * slow_down_planning_param_.time_margin_on_target_velocity); + const double deceleration_dist = offset_dist_to_collision + dist_to_front_collision - + abs_ego_offset - dist_to_ego - slow_down_prepare_dist; + const double slow_down_dist = + dist_to_back_collision - dist_to_front_collision + slow_down_prepare_dist; + + // calculate distance to start/end slow down + const double dist_to_slow_down_start = dist_to_ego + deceleration_dist; + const double dist_to_slow_down_end = dist_to_ego + deceleration_dist + slow_down_dist; + if (100.0 < dist_to_slow_down_start) { + // NOTE: distance to slow down is too far. + return std::nullopt; + } + + // apply low-pass filter to distance to start/end slow down + const auto apply_lowpass_filter = [&](const double dist_to_slow_down, const auto prev_point) { + if (prev_output && prev_point) { + const size_t seg_idx = + autoware::motion_utils::findNearestSegmentIndex(traj_points, prev_point->position); + const double prev_dist_to_slow_down = + autoware::motion_utils::calcSignedArcLength(traj_points, 0, prev_point->position, seg_idx); + return autoware::signal_processing::lowpassFilter( + dist_to_slow_down, prev_dist_to_slow_down, + slow_down_planning_param_.lpf_gain_dist_to_slow_down); + } + return dist_to_slow_down; + }; + const double filtered_dist_to_slow_down_start = + apply_lowpass_filter(dist_to_slow_down_start, prev_output->start_point); + const double filtered_dist_to_slow_down_end = + apply_lowpass_filter(dist_to_slow_down_end, prev_output->end_point); + + // calculate velocity considering constraints + const double feasible_slow_down_vel = [&]() { + if (deceleration_dist < 0) { + if (prev_output) { + return prev_output->target_vel; + } + return std::max(planner_data->current_odometry.twist.twist.linear.x, slow_down_vel); + } + if (planner_data->current_odometry.twist.twist.linear.x < slow_down_vel) { + return slow_down_vel; + } + + const double one_shot_feasible_slow_down_vel = [&]() { + if (planner_data->current_acceleration.accel.accel.linear.x < common_param_.min_accel) { + const double squared_vel = + std::pow(planner_data->current_odometry.twist.twist.linear.x, 2) + + 2 * deceleration_dist * common_param_.min_accel; + if (squared_vel < 0) { + return slow_down_vel; + } + return std::max(std::sqrt(squared_vel), slow_down_vel); + } + // TODO(murooka) Calculate more precisely. Final acceleration should be zero. + const double min_feasible_slow_down_vel = calc_deceleration_velocity_from_distance_to_target( + slow_down_planning_param_.slow_down_min_jerk, slow_down_planning_param_.slow_down_min_acc, + planner_data->current_acceleration.accel.accel.linear.x, + planner_data->current_odometry.twist.twist.linear.x, deceleration_dist); + return min_feasible_slow_down_vel; + }(); + if (prev_output) { + // NOTE: If longitudinal controllability is not good, one_shot_slow_down_vel may be getting + // larger since we use actual ego's velocity and acceleration for its calculation. + // Suppress one_shot_slow_down_vel getting larger here. + const double feasible_slow_down_vel = + std::min(one_shot_feasible_slow_down_vel, prev_output->feasible_target_vel); + return std::max(slow_down_vel, feasible_slow_down_vel); + } + return std::max(slow_down_vel, one_shot_feasible_slow_down_vel); + }(); + + return std::make_tuple( + filtered_dist_to_slow_down_start, filtered_dist_to_slow_down_end, feasible_slow_down_vel); +} + +double ObstacleSlowDownModule::calculate_slow_down_velocity( + const SlowDownObstacle & obstacle, const std::optional & prev_output, + const bool is_obstacle_moving) const +{ + const auto & p = slow_down_planning_param_.get_object_param_by_label( + obstacle.classification, is_obstacle_moving); + const double stable_dist_from_obj_poly_to_traj_poly = [&]() { + if (prev_output) { + return autoware::signal_processing::lowpassFilter( + obstacle.dist_to_traj_poly, prev_output->dist_from_obj_poly_to_traj_poly, + slow_down_planning_param_.lpf_gain_lat_dist); + } + return obstacle.dist_to_traj_poly; + }(); + + const double ratio = std::clamp( + (std::abs(stable_dist_from_obj_poly_to_traj_poly) - p.min_lat_margin) / + (p.max_lat_margin - p.min_lat_margin), + 0.0, 1.0); + const double slow_down_vel = + p.min_ego_velocity + ratio * (p.max_ego_velocity - p.min_ego_velocity); + + return slow_down_vel; +} + +std::vector ObstacleSlowDownModule::get_decimated_traj_polys( + const std::vector & traj_points, const geometry_msgs::msg::Pose & current_pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const +{ + if (!decimated_traj_polys_) { + const auto & p = trajectory_polygon_collision_check; + const auto decimated_traj_points = utils::decimate_trajectory_points_from_ego( + traj_points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold, + p.decimate_trajectory_step_length, p.goal_extended_trajectory_length); + decimated_traj_polys_ = polygon_utils::create_one_step_polygons( + decimated_traj_points, vehicle_info, current_pose, 0.0, p.enable_to_consider_current_pose, + p.time_to_convergence, p.decimate_trajectory_step_length); + } + return *decimated_traj_polys_; +} + +} // namespace autoware::motion_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::motion_velocity_planner::ObstacleSlowDownModule, + autoware::motion_velocity_planner::PluginModuleInterface) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.hpp new file mode 100644 index 0000000000000..1d367adfb6f4e --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.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 OBSTACLE_SLOW_DOWN_MODULE_HPP_ +#define OBSTACLE_SLOW_DOWN_MODULE_HPP_ + +#include "autoware/motion_velocity_planner_common_universe/polygon_utils.hpp" +#include "autoware/motion_velocity_planner_common_universe/utils.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" +#include "metrics_manager.hpp" +#include "parameters.hpp" +#include "type_alias.hpp" +#include "types.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class ObstacleSlowDownModule : 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 & raw_trajectory_points, + const std::vector & smoothed_trajectory_points, + const std::shared_ptr planner_data) override; + std::string get_module_name() const override; + +private: + std::string module_name_; + rclcpp::Clock::SharedPtr clock_{}; + + // ros parameters + CommonParam common_param_; + SlowDownPlanningParam slow_down_planning_param_; + ObstacleFilteringParam obstacle_filtering_param_; + + // module publisher + rclcpp::Publisher::SharedPtr metrics_pub_; + rclcpp::Publisher::SharedPtr debug_slow_down_planning_info_pub_; + rclcpp::Publisher::SharedPtr + processing_time_detail_pub_; + + // interface publisher + std::unique_ptr + objects_of_interest_marker_interface_; + + // internal variables + std::vector prev_slow_down_object_obstacles_; + std::vector prev_slow_down_output_; + SlowDownConditionCounter slow_down_condition_counter_; + Float32MultiArrayStamped slow_down_debug_multi_array_; + autoware::universe_utils::StopWatch stop_watch_; + mutable std::shared_ptr debug_data_ptr_; + MetricsManager metrics_manager_; + bool need_to_clear_velocity_limit_{false}; + mutable std::shared_ptr time_keeper_; + mutable std::optional> decimated_traj_polys_{std::nullopt}; + + std::vector filter_slow_down_obstacle_for_predicted_object( + const Odometry & odometry, const double ego_nearest_dist_threshold, + const double ego_nearest_yaw_threshold, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector> & objects, + const rclcpp::Time & predicted_objects_stamp, const VehicleInfo & vehicle_info, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check); + std::optional create_slow_down_obstacle_for_predicted_object( + const std::vector & traj_points, + const std::vector & decimated_traj_polys_with_lat_margin, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const double dist_from_obj_poly_to_traj_poly); + std::vector plan_slow_down( + const std::shared_ptr planner_data, + const std::vector & traj_points, + const std::vector & obstacles, + [[maybe_unused]] std::optional & velocity_limit, + const VehicleInfo & vehicle_info); + Float32MultiArrayStamped get_slow_down_planning_debug_message(const rclcpp::Time & current_time); + void publish_debug_info(); + bool is_slow_down_obstacle(const uint8_t label) const; + std::optional> + calculate_distance_to_slow_down_with_constraints( + const std::shared_ptr planner_data, + const std::vector & traj_points, const SlowDownObstacle & obstacle, + const std::optional & prev_output, const double dist_to_ego, + const VehicleInfo & vehicle_info, const bool is_obstacle_moving) const; + double calculate_slow_down_velocity( + const SlowDownObstacle & obstacle, const std::optional & prev_output, + const bool is_obstacle_moving) const; + std::vector get_decimated_traj_polys( + const std::vector & traj_points, const geometry_msgs::msg::Pose & current_pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const; +}; +} // namespace autoware::motion_velocity_planner + +#endif // OBSTACLE_SLOW_DOWN_MODULE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/parameters.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/parameters.hpp new file mode 100644 index 0000000000000..b2159def25224 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/parameters.hpp @@ -0,0 +1,176 @@ +// 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 PARAMETERS_HPP_ +#define PARAMETERS_HPP_ + +#include "type_alias.hpp" +#include "types.hpp" + +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +using autoware::universe_utils::getOrDeclareParameter; + +struct CommonParam +{ + double max_accel{}; + double min_accel{}; + double max_jerk{}; + double min_jerk{}; + double limit_max_accel{}; + double limit_min_accel{}; + double limit_max_jerk{}; + double limit_min_jerk{}; + + CommonParam() = default; + explicit CommonParam(rclcpp::Node & node) + { + max_accel = getOrDeclareParameter(node, "normal.max_acc"); + min_accel = getOrDeclareParameter(node, "normal.min_acc"); + max_jerk = getOrDeclareParameter(node, "normal.max_jerk"); + min_jerk = getOrDeclareParameter(node, "normal.min_jerk"); + limit_max_accel = getOrDeclareParameter(node, "limit.max_acc"); + limit_min_accel = getOrDeclareParameter(node, "limit.min_acc"); + limit_max_jerk = getOrDeclareParameter(node, "limit.max_jerk"); + limit_min_jerk = getOrDeclareParameter(node, "limit.min_jerk"); + } +}; + +struct ObstacleFilteringParam +{ + std::vector object_types{}; + + double min_lat_margin{}; + double max_lat_margin{}; + double lat_hysteresis_margin{}; + + int successive_num_to_entry_slow_down_condition{}; + int successive_num_to_exit_slow_down_condition{}; + + ObstacleFilteringParam() = default; + explicit ObstacleFilteringParam(rclcpp::Node & node) + { + object_types = + utils::get_target_object_type(node, "obstacle_slow_down.obstacle_filtering.object_type."); + min_lat_margin = + getOrDeclareParameter(node, "obstacle_slow_down.obstacle_filtering.min_lat_margin"); + max_lat_margin = + getOrDeclareParameter(node, "obstacle_slow_down.obstacle_filtering.max_lat_margin"); + lat_hysteresis_margin = getOrDeclareParameter( + node, "obstacle_slow_down.obstacle_filtering.lat_hysteresis_margin"); + successive_num_to_entry_slow_down_condition = getOrDeclareParameter( + node, "obstacle_slow_down.obstacle_filtering.successive_num_to_entry_slow_down_condition"); + successive_num_to_exit_slow_down_condition = getOrDeclareParameter( + node, "obstacle_slow_down.obstacle_filtering.successive_num_to_exit_slow_down_condition"); + } +}; + +struct SlowDownPlanningParam +{ + double slow_down_min_acc{}; + double slow_down_min_jerk{}; + + double lpf_gain_slow_down_vel{}; + double lpf_gain_lat_dist{}; + double lpf_gain_dist_to_slow_down{}; + + double time_margin_on_target_velocity{}; + + double moving_object_speed_threshold{}; + double moving_object_hysteresis_range{}; + + std::vector obstacle_labels{"default"}; + std::vector obstacle_moving_classification{"static", "moving"}; + struct ObjectTypeSpecificParams + { + double min_lat_margin; + double max_lat_margin; + double min_ego_velocity; + double max_ego_velocity; + }; + std::unordered_map object_types_maps = { + {ObjectClassification::UNKNOWN, "unknown"}, {ObjectClassification::CAR, "car"}, + {ObjectClassification::TRUCK, "truck"}, {ObjectClassification::BUS, "bus"}, + {ObjectClassification::TRAILER, "trailer"}, {ObjectClassification::MOTORCYCLE, "motorcycle"}, + {ObjectClassification::BICYCLE, "bicycle"}, {ObjectClassification::PEDESTRIAN, "pedestrian"}}; + std::unordered_map object_type_specific_param_map; + + SlowDownPlanningParam() = default; + explicit SlowDownPlanningParam(rclcpp::Node & node) + { + slow_down_min_acc = getOrDeclareParameter( + node, "obstacle_slow_down.slow_down_planning.slow_down_min_acc"); + slow_down_min_jerk = getOrDeclareParameter( + node, "obstacle_slow_down.slow_down_planning.slow_down_min_jerk"); + + lpf_gain_slow_down_vel = getOrDeclareParameter( + node, "obstacle_slow_down.slow_down_planning.lpf_gain_slow_down_vel"); + lpf_gain_lat_dist = getOrDeclareParameter( + node, "obstacle_slow_down.slow_down_planning.lpf_gain_lat_dist"); + lpf_gain_dist_to_slow_down = getOrDeclareParameter( + node, "obstacle_slow_down.slow_down_planning.lpf_gain_dist_to_slow_down"); + time_margin_on_target_velocity = getOrDeclareParameter( + node, "obstacle_slow_down.slow_down_planning.time_margin_on_target_velocity"); + + moving_object_speed_threshold = getOrDeclareParameter( + node, "obstacle_slow_down.slow_down_planning.moving_object_speed_threshold"); + moving_object_hysteresis_range = getOrDeclareParameter( + node, "obstacle_slow_down.slow_down_planning.moving_object_hysteresis_range"); + + const std::string param_prefix = + "obstacle_slow_down.slow_down_planning.object_type_specified_params."; + const auto object_types = + getOrDeclareParameter>(node, param_prefix + "types"); + + for (const auto & type_str : object_types) { + for (const auto & movement_type : std::vector{"moving", "static"}) { + ObjectTypeSpecificParams param{ + getOrDeclareParameter( + node, param_prefix + type_str + "." + movement_type + ".min_lat_margin"), + getOrDeclareParameter( + node, param_prefix + type_str + "." + movement_type + ".max_lat_margin"), + getOrDeclareParameter( + node, param_prefix + type_str + "." + movement_type + ".min_ego_velocity"), + getOrDeclareParameter( + node, param_prefix + type_str + "." + movement_type + ".max_ego_velocity")}; + + object_type_specific_param_map.emplace(type_str + "." + movement_type, param); + } + } + } + + std::string get_param_type(const ObjectClassification label) const + { + const auto type_str = object_types_maps.at(label.label); + if (object_type_specific_param_map.count(type_str) == 0) { + return "default"; + } + return type_str; + } + ObjectTypeSpecificParams get_object_param_by_label( + const ObjectClassification label, const bool is_obstacle_moving) const + { + const std::string movement_type = is_obstacle_moving ? "moving" : "static"; + return object_type_specific_param_map.at(get_param_type(label) + "." + movement_type); + } +}; +} // namespace autoware::motion_velocity_planner + +#endif // PARAMETERS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/type_alias.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/type_alias.hpp new file mode 100644 index 0000000000000..673574c24a914 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/type_alias.hpp @@ -0,0 +1,70 @@ +// 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 TYPE_ALIAS_HPP_ +#define TYPE_ALIAS_HPP_ + +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" + +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp" +#include "autoware_perception_msgs/msg/predicted_object.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "tier4_planning_msgs/msg/velocity_limit.hpp" +#include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include +#include +#include +#include + +#include +#include + +namespace autoware::motion_velocity_planner +{ +using autoware::vehicle_info_utils::VehicleInfo; +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_internal_debug_msgs::msg::Float32Stamped; +using autoware_internal_debug_msgs::msg::Float64Stamped; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::Shape; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Twist; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::PointCloud2; +using tier4_planning_msgs::msg::VelocityLimit; +using tier4_planning_msgs::msg::VelocityLimitClearCommand; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; +namespace bg = boost::geometry; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; +using Metric = tier4_metric_msgs::msg::Metric; +using MetricArray = tier4_metric_msgs::msg::MetricArray; +using PointCloud = pcl::PointCloud; +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::SafetyFactorArray; +} // namespace autoware::motion_velocity_planner + +#endif // TYPE_ALIAS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/types.hpp new file mode 100644 index 0000000000000..c21c6fd9103a2 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/types.hpp @@ -0,0 +1,144 @@ +// 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 TYPES_HPP_ +#define TYPES_HPP_ + +#include "type_alias.hpp" + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +struct SlowDownObstacle +{ + SlowDownObstacle( + const std::string & arg_uuid, const rclcpp::Time & arg_stamp, + const ObjectClassification & object_classification, const geometry_msgs::msg::Pose & arg_pose, + const double arg_lon_velocity, const double arg_lat_velocity, + const double arg_dist_to_traj_poly, const geometry_msgs::msg::Point & arg_front_collision_point, + const geometry_msgs::msg::Point & arg_back_collision_point) + : uuid(arg_uuid), + stamp(arg_stamp), + pose(arg_pose), + velocity(arg_lon_velocity), + lat_velocity(arg_lat_velocity), + dist_to_traj_poly(arg_dist_to_traj_poly), + front_collision_point(arg_front_collision_point), + back_collision_point(arg_back_collision_point), + classification(object_classification) + { + } + std::string uuid{}; + rclcpp::Time stamp{}; + geometry_msgs::msg::Pose pose{}; // interpolated with the current stamp + double velocity{}; // longitudinal velocity against ego's trajectory + double lat_velocity{}; // lateral velocity against ego's trajectory + + double dist_to_traj_poly{}; // for efficient calculation + geometry_msgs::msg::Point front_collision_point{}; + geometry_msgs::msg::Point back_collision_point{}; + ObjectClassification classification{}; +}; + +struct SlowDownOutput +{ + SlowDownOutput() = default; + SlowDownOutput( + const std::string & arg_uuid, const std::vector & traj_points, + const std::optional & start_idx, const std::optional & end_idx, + const double arg_target_vel, const double arg_feasible_target_vel, + const double arg_dist_from_obj_poly_to_traj_poly, const bool is_obstacle_moving) + : uuid(arg_uuid), + target_vel(arg_target_vel), + feasible_target_vel(arg_feasible_target_vel), + dist_from_obj_poly_to_traj_poly(arg_dist_from_obj_poly_to_traj_poly), + is_obstacle_moving(is_obstacle_moving) + { + if (start_idx) { + start_point = traj_points.at(*start_idx).pose; + } + if (end_idx) { + end_point = traj_points.at(*end_idx).pose; + } + } + + std::string uuid{}; + double target_vel{}; + double feasible_target_vel{}; + double dist_from_obj_poly_to_traj_poly{}; + std::optional start_point{std::nullopt}; + std::optional end_point{std::nullopt}; + bool is_obstacle_moving{}; +}; + +struct SlowDownConditionCounter +{ + void reset_current_uuids() { current_uuids_.clear(); } + void add_current_uuid(const std::string & uuid) { current_uuids_.push_back(uuid); } + void remove_counter_unless_updated() + { + std::vector obsolete_uuids; + for (const auto & key_and_value : counter_) { + if ( + std::find(current_uuids_.begin(), current_uuids_.end(), key_and_value.first) == + current_uuids_.end()) { + obsolete_uuids.push_back(key_and_value.first); + } + } + + for (const auto & obsolete_uuid : obsolete_uuids) { + counter_.erase(obsolete_uuid); + } + } + + int increase_counter(const std::string & uuid) + { + if (counter_.count(uuid) != 0) { + counter_.at(uuid) = std::max(1, counter_.at(uuid) + 1); + } else { + counter_.emplace(uuid, 1); + } + return counter_.at(uuid); + } + int decrease_counter(const std::string & uuid) + { + if (counter_.count(uuid) != 0) { + counter_.at(uuid) = std::min(-1, counter_.at(uuid) - 1); + } else { + counter_.emplace(uuid, -1); + } + return counter_.at(uuid); + } + void reset(const std::string & uuid) { counter_.emplace(uuid, 0); } + + // NOTE: positive is for meeting entering condition, and negative is for exiting. + std::unordered_map counter_{}; + std::vector current_uuids_{}; +}; + +struct DebugData +{ + DebugData() = default; + std::vector obstacles_to_slow_down{}; + std::vector decimated_traj_polys{}; + MarkerArray slow_down_debug_wall_marker{}; + MarkerArray slow_down_wall_marker{}; +}; +} // namespace autoware::motion_velocity_planner + +#endif // TYPES_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/CMakeLists.txt new file mode 100644 index 0000000000000..4bcd50db64cb8 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_motion_velocity_obstacle_stop_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node_universe plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/README.md new file mode 100644 index 0000000000000..412a4e28f0580 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/README.md @@ -0,0 +1,82 @@ +# Obstacle Stop + +## Role + +The `obstacle_stop` module does the stop planning when there is a static obstacle near the trajectory. + +## Activation + +This module is activated if the launch parameter `launch_obstacle_stop_module` is set to true. + +## Inner-workings / Algorithms + +### Obstacle Filtering + +The obstacles meeting the following condition are determined as obstacles for stopping. + +- The object type is for stopping according to `obstacle_filtering.object_type.*`. +- The lateral distance from the object to the ego's trajectory is smaller than `obstacle_filtering.max_lat_margin`. +- The object velocity along the ego's trajectory is smaller than `obstacle_filtering.obstacle_velocity_threshold_from_stop`. +- The object + - does not cross the ego's trajectory (\*1) + - and its collision time margin is large enough (\*2). + +#### NOTE + +##### \*1: Crossing obstacles + +Crossing obstacle is the object whose orientation's yaw angle against the ego's trajectory is smaller than `obstacle_filtering.crossing_obstacle.obstacle_traj_angle_threshold`. + +##### \*2: Enough collision time margin + +We predict the collision area and its time by the ego with a constant velocity motion and the obstacle with its predicted path. +Then, we calculate a collision time margin which is the difference of the time when the ego will be inside the collision area and the obstacle will be inside the collision area. +When this time margin is smaller than `obstacle_filtering.crossing_obstacle.collision_time_margin`, the margin is not enough. + +### Stop Planning + +The role of the stop planning is keeping a safe distance with static vehicle objects or dynamic/static non vehicle objects. + +The stop planning just inserts the stop point in the trajectory to keep a distance with obstacles. +The safe distance is parameterized as `stop_planning.stop_margin`. +When it stops at the end of the trajectory, and obstacle is on the same point, the safe distance becomes `stop_planning.terminal_stop_margin`. + +When inserting the stop point, the required acceleration for the ego to stop in front of the stop point is calculated. +If the acceleration is less than `common.min_strong_accel`, the stop planning will be cancelled since this package does not assume a strong sudden brake for emergency. + +### Minor functions + +#### Prioritization of behavior module's stop point + +When stopping for a pedestrian walking on the crosswalk, the behavior module inserts the zero velocity in the trajectory in front of the crosswalk. +Also `autoware_obstacle_cruise_planner`'s stop planning also works, and the ego may not reach the behavior module's stop point since the safe distance defined in `autoware_obstacle_cruise_planner` may be longer than the behavior module's safe distance. +To resolve this non-alignment of the stop point between the behavior module and this module, `stop_planning.min_behavior_stop_margin` is defined. +In the case of the crosswalk described above, this module inserts the stop point with a distance `stop_planning.min_behavior_stop_margin` at minimum between the ego and obstacle. + +#### A function to keep the closest stop obstacle in target obstacles + +In order to keep the closest stop obstacle in the target obstacles, we check whether it is disappeared or not from the target obstacles in the `check_consistency` function. +If the previous closest stop obstacle is remove from the lists, we keep it in the lists for `obstacle_filtering.stop_obstacle_hold_time_threshold` seconds. +Note that if a new stop obstacle appears and the previous closest obstacle removes from the lists, we do not add it to the target obstacles again. + +## Debugging + +### Detection area + +Green polygons which is a detection area is visualized by `detection_polygons` in the `~/debug/marker` topic. + +![detection_area](./docs/detection_area.png) + +### Collision points + +Red points which are collision points with obstacle are visualized by `*_collision_points` for each behavior in the `~/debug/marker` topic. + +![collision_point](./docs/collision_point.png) + +### Obstacle for stop + +Red sphere which is an obstacle for stop is visualized by `obstacles_to_stop` in the `~/debug/marker` topic. + +Red wall which means a safe distance to stop if the ego's front meets the wall is visualized in the `~/virtual_wall` topic. + +![stop_visualization](./docs/stop_visualization.png) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/config/obstacle_stop.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/config/obstacle_stop.param.yaml new file mode 100644 index 0000000000000..21dbf5f5bc611 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/config/obstacle_stop.param.yaml @@ -0,0 +1,74 @@ +/**: + ros__parameters: + obstacle_stop: + option: + ignore_crossing_obstacle: true + suppress_sudden_stop: true + + stop_planning: + stop_margin : 5.0 # longitudinal margin to obstacle [m] + terminal_stop_margin : 3.0 # Stop margin at the goal. This value cannot exceed stop margin. [m] + min_behavior_stop_margin: 3.0 # [m] + + hold_stop_velocity_threshold: 0.01 # The maximum ego velocity to hold stopping [m/s] + hold_stop_distance_threshold: 0.3 # The ego keeps stopping if the distance to stop changes within the threshold [m] + + stop_on_curve: + enable_approaching: false + additional_stop_margin: 3.0 # [m] + min_stop_margin: 3.0 # [m] + + object_type_specified_params: + types: # For the listed types, the node try to read the following type specified values + - "default" + - "unknown" + # default: For the default type, which denotes the other types listed above, the following param is defined implicitly, and the other type-specified parameters are not defined. + # limit_min_acc: common_param.yaml/limit.min_acc + unknown: + limit_min_acc: -1.2 # overwrite the deceleration limit, in usually, common_param.yaml/limit.min_acc is referred. + sudden_object_acc_threshold: -1.1 # If a stop can be achieved by a deceleration smaller than this value, it is not considered as "sudden stop". + sudden_object_dist_threshold: 1000.0 # If a stop distance is longer than this value, it is not considered as "sudden stop". + abandon_to_stop: false # If true, the planner gives up to stop when it cannot avoid to run over while maintaining the deceleration limit. + + obstacle_filtering: + object_type: + pointcloud: false + + inside: + unknown: true + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: true + pedestrian: true + + outside: + unknown: false + car: false + truck: false + bus: false + trailer: false + motorcycle: false + bicycle: false + pedestrian: false + + # hysteresis for velocity + obstacle_velocity_threshold_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] + obstacle_velocity_threshold_from_stop : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] + + max_lat_margin: 0.3 # lateral margin between the obstacles except for unknown and ego's footprint + max_lat_margin_against_unknown: 0.3 # lateral margin between the unknown obstacles and ego's footprint + + min_velocity_to_reach_collision_point: 2.0 # minimum velocity margin to calculate time to reach collision [m/s] + stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle + + outside_obstacle: + max_lateral_time_margin: 4.5 # time threshold of lateral margin between the obstacles and ego's footprint [s] + num_of_predicted_paths: 1 # number of the highest confidence predicted path to check collision between obstacle and ego + pedestrian_deceleration_rate: 0.5 # planner perceives pedestrians will stop with this rate to avoid unnecessary stops [m/ss] + bicycle_deceleration_rate: 0.5 # planner perceives bicycles will stop with this rate to avoid unnecessary stops [m/ss] + + crossing_obstacle: + collision_time_margin : 1.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/docs/collision_point.png b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/docs/collision_point.png new file mode 100644 index 0000000000000..29a4f913891b3 Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/docs/collision_point.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/docs/detection_area.png b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/docs/detection_area.png new file mode 100644 index 0000000000000..6e1a831979fc0 Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/docs/detection_area.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/docs/stop_visualization.png b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/docs/stop_visualization.png new file mode 100644 index 0000000000000..f3cec33fd754b Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/docs/stop_visualization.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/package.xml new file mode 100644 index 0000000000000..1425e72cdd886 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/package.xml @@ -0,0 +1,40 @@ + + + + autoware_motion_velocity_obstacle_stop_module + 0.40.0 + obstacle stop feature in motion_velocity_planner + + Takayuki Murooka + Yuki Takagi + Maxime Clement + Berkay Karaman + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + autoware_motion_utils + autoware_motion_velocity_planner_common_universe + autoware_perception_msgs + autoware_planning_msgs + autoware_route_handler + autoware_universe_utils + autoware_vehicle_info_utils + geometry_msgs + libboost-dev + pluginlib + rclcpp + tf2 + tier4_metric_msgs + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/plugins.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/plugins.xml new file mode 100644 index 0000000000000..c51cab7b7c2c8 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/metrics_manager.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/metrics_manager.hpp new file mode 100644 index 0000000000000..444799aeef615 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/metrics_manager.hpp @@ -0,0 +1,102 @@ +// 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 METRICS_MANAGER_HPP_ +#define METRICS_MANAGER_HPP_ + +#include "type_alias.hpp" +#include "types.hpp" + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class MetricsManager +{ +public: + void init() { metrics_.clear(); } + + void calculate_metrics( + const std::string & module_name, const std::string & reason, + const std::shared_ptr planner_data, + const std::vector & traj_points, + const std::optional & stop_pose, + const std::optional & stop_obstacle) + { + // Create status + { + // Decision + Metric decision_metric; + decision_metric.name = module_name + "/decision"; + decision_metric.unit = "string"; + decision_metric.value = reason; + metrics_.push_back(decision_metric); + } + + if (stop_pose.has_value() && planner_data) { // Stop info + Metric stop_position_metric; + stop_position_metric.name = module_name + "/stop_position"; + stop_position_metric.unit = "string"; + const auto & p = stop_pose.value().position; + stop_position_metric.value = + "{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}"; + metrics_.push_back(stop_position_metric); + + Metric stop_orientation_metric; + stop_orientation_metric.name = module_name + "/stop_orientation"; + stop_orientation_metric.unit = "string"; + const auto & o = stop_pose.value().orientation; + stop_orientation_metric.value = "{" + std::to_string(o.w) + ", " + std::to_string(o.x) + + ", " + std::to_string(o.y) + ", " + std::to_string(o.z) + "}"; + metrics_.push_back(stop_orientation_metric); + + const auto dist_to_stop_pose = autoware::motion_utils::calcSignedArcLength( + traj_points, planner_data->current_odometry.pose.pose.position, stop_pose.value().position); + + Metric dist_to_stop_pose_metric; + dist_to_stop_pose_metric.name = module_name + "/distance_to_stop_pose"; + dist_to_stop_pose_metric.unit = "double"; + dist_to_stop_pose_metric.value = std::to_string(dist_to_stop_pose); + metrics_.push_back(dist_to_stop_pose_metric); + } + + if (stop_obstacle.has_value()) { + // Obstacle info + Metric collision_point_metric; + const auto & p = stop_obstacle.value().collision_point; + collision_point_metric.name = module_name + "/collision_point"; + collision_point_metric.unit = "string"; + collision_point_metric.value = + "{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}"; + metrics_.push_back(collision_point_metric); + } + } + + MetricArray create_metric_array(const rclcpp::Time & current_time) + { + MetricArray metrics_msg; + metrics_msg.stamp = current_time; + metrics_msg.metric_array.insert( + metrics_msg.metric_array.end(), metrics_.begin(), metrics_.end()); + return metrics_msg; + } + +private: + std::vector metrics_; +}; +} // namespace autoware::motion_velocity_planner + +#endif // METRICS_MANAGER_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp new file mode 100644 index 0000000000000..d45ca3894587d --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp @@ -0,0 +1,1180 @@ +// Copyright 2025 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 "obstacle_stop_module.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +using autoware::universe_utils::getOrDeclareParameter; + +namespace +{ +template +bool is_in_vector(const T variable, const std::vector & vec) +{ + return std::find(vec.begin(), vec.end(), variable) != vec.end(); +} + +double calc_minimum_distance_to_stop( + const double initial_vel, const double max_acc, const double min_acc) +{ + if (initial_vel < 0.0) { + return -std::pow(initial_vel, 2) / 2.0 / max_acc; + } + + return -std::pow(initial_vel, 2) / 2.0 / min_acc; +} + +autoware::universe_utils::Point2d convert_point(const geometry_msgs::msg::Point & p) +{ + return autoware::universe_utils::Point2d{p.x, p.y}; +} + +std::vector resample_trajectory_points( + const std::vector & traj_points, const double interval) +{ + const auto traj = autoware::motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = autoware::motion_utils::resampleTrajectory(traj, interval); + return autoware::motion_utils::convertToTrajectoryPointArray(resampled_traj); +} + +std::vector resample_highest_confidence_predicted_paths( + const std::vector & predicted_paths, const double time_interval, + const double time_horizon, const size_t num_paths) +{ + std::vector sorted_paths = predicted_paths; + + // Sort paths by descending confidence + std::sort( + sorted_paths.begin(), sorted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence > b.confidence; }); + + std::vector selected_paths; + size_t path_count = 0; + + // Select paths that meet the confidence thresholds + for (const auto & path : sorted_paths) { + if (path_count < num_paths) { + selected_paths.push_back(path); + ++path_count; + } + } + + // Resample each selected path + std::vector resampled_paths; + for (const auto & path : selected_paths) { + if (path.path.size() < 2) { + continue; + } + resampled_paths.push_back( + autoware::object_recognition_utils::resamplePredictedPath(path, time_interval, time_horizon)); + } + + return resampled_paths; +} + +template +std::vector concat_vectors( + const std::vector & first_vector, const std::vector & second_vector) +{ + std::vector concatenated_vector; + concatenated_vector.insert(concatenated_vector.end(), first_vector.begin(), first_vector.end()); + concatenated_vector.insert(concatenated_vector.end(), second_vector.begin(), second_vector.end()); + return concatenated_vector; +} + +double calc_dist_to_bumper(const bool is_driving_forward, const VehicleInfo & vehicle_info) +{ + if (is_driving_forward) { + return std::abs(vehicle_info.max_longitudinal_offset_m); + } + return std::abs(vehicle_info.min_longitudinal_offset_m); +} + +Float64Stamped create_float64_stamped(const rclcpp::Time & now, const float & data) +{ + Float64Stamped msg; + msg.stamp = now; + msg.data = data; + return msg; +} + +double calc_time_to_reach_collision_point( + const Odometry & odometry, const geometry_msgs::msg::Point & collision_point, + const std::vector & traj_points, const double dist_to_bumper, + const double min_velocity_to_reach_collision_point) +{ + const double dist_from_ego_to_obstacle = + std::abs(autoware::motion_utils::calcSignedArcLength( + traj_points, odometry.pose.pose.position, collision_point)) - + dist_to_bumper; + return dist_from_ego_to_obstacle / + std::max(min_velocity_to_reach_collision_point, std::abs(odometry.twist.twist.linear.x)); +} +} // namespace + +void ObstacleStopModule::init(rclcpp::Node & node, const std::string & module_name) +{ + module_name_ = module_name; + clock_ = node.get_clock(); + logger_ = node.get_logger(); + + // ros parameters + ignore_crossing_obstacle_ = + getOrDeclareParameter(node, "obstacle_stop.option.ignore_crossing_obstacle"); + suppress_sudden_stop_ = + getOrDeclareParameter(node, "obstacle_stop.option.suppress_sudden_stop"); + + common_param_ = CommonParam(node); + stop_planning_param_ = StopPlanningParam(node, common_param_); + obstacle_filtering_param_ = ObstacleFilteringParam(node); + + // common publisher + processing_time_publisher_ = + node.create_publisher("~/debug/obstacle_stop/processing_time_ms", 1); + virtual_wall_publisher_ = + node.create_publisher("~/obstacle_stop/virtual_walls", 1); + debug_publisher_ = + node.create_publisher("~/obstacle_stop/debug_markers", 1); + + // module publisher + debug_stop_planning_info_pub_ = + node.create_publisher("~/debug/stop_planning_info", 1); + metrics_pub_ = node.create_publisher("~/stop/metrics", 10); + processing_time_detail_pub_ = node.create_publisher( + "~/debug/processing_time_detail_ms/obstacle_stop", 1); + // interface publisher + objects_of_interest_marker_interface_ = std::make_unique< + autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface>( + &node, "obstacle_stop"); + planning_factor_interface_ = + std::make_unique( + &node, "obstacle_stop"); + + // time keeper + time_keeper_ = std::make_shared(processing_time_detail_pub_); +} + +void ObstacleStopModule::update_parameters(const std::vector & parameters) +{ + using universe_utils::updateParam; + + updateParam( + parameters, "obstacle_stop.option.ignore_crossing_obstacle", ignore_crossing_obstacle_); + updateParam(parameters, "obstacle_stop.option.suppress_sudden_stop", suppress_sudden_stop_); +} + +VelocityPlanningResult ObstacleStopModule::plan( + const std::vector & raw_trajectory_points, + [[maybe_unused]] const std::vector & + smoothed_trajectory_points, + const std::shared_ptr planner_data) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + // 1. init variables + stop_watch_.tic(); + debug_data_ptr_ = std::make_shared(); + metrics_manager_.init(); + const double dist_to_bumper = + calc_dist_to_bumper(planner_data->is_driving_forward, planner_data->vehicle_info_); + stop_planning_debug_info_.reset(); + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::EGO_VELOCITY, planner_data->current_odometry.twist.twist.linear.x); + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::EGO_ACCELERATION, + planner_data->current_acceleration.accel.accel.linear.x); + trajectory_polygon_for_inside_map_.clear(); + trajectory_polygon_for_outside_ = std::nullopt; + decimated_traj_polys_ = std::nullopt; + + // 2. pre-process + const auto decimated_traj_points = utils::decimate_trajectory_points_from_ego( + raw_trajectory_points, planner_data->current_odometry.pose.pose, + planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold, + planner_data->trajectory_polygon_collision_check.decimate_trajectory_step_length, + stop_planning_param_.stop_margin); + + // 3. filter obstacles of predicted objects + const auto stop_obstacles_for_predicted_object = filter_stop_obstacle_for_predicted_object( + planner_data->current_odometry, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold, + rclcpp::Time(planner_data->predicted_objects_header.stamp), raw_trajectory_points, + decimated_traj_points, planner_data->objects, planner_data->is_driving_forward, + planner_data->vehicle_info_, dist_to_bumper, planner_data->trajectory_polygon_collision_check); + + // 4. filter obstacles of point cloud + const auto stop_obstacles_for_point_cloud = + std::vector{}; // filter_stop_obstacle_for_point_cloud(); + + // 5. concat stop obstacles by predicted objects and point cloud + const auto stop_obstacles = stop_obstacles_for_predicted_object; + concat_vectors(stop_obstacles_for_predicted_object, stop_obstacles_for_point_cloud); + + // 6. plan stop + const auto stop_point = + plan_stop(planner_data, raw_trajectory_points, stop_obstacles, dist_to_bumper); + + // 7. publish messages for debugging + publish_debug_info(); + + // 8. generate VelocityPlanningResult + VelocityPlanningResult result; + if (stop_point) { + result.stop_points.push_back(*stop_point); + } + + return result; +} + +std::vector ObstacleStopModule::filter_stop_obstacle_for_predicted_object( + const Odometry & odometry, const double ego_nearest_dist_threshold, + const double ego_nearest_yaw_threshold, const rclcpp::Time & predicted_objects_stamp, + const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector> & objects, const bool is_driving_forward, + const VehicleInfo & vehicle_info, const double dist_to_bumper, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const auto & current_pose = odometry.pose.pose; + + std::vector stop_obstacles; + for (const auto & object : objects) { + autoware::universe_utils::ScopedTimeTrack st_for_each_object("for_each_object", *time_keeper_); + + // 1. rough filtering + // 1.1. Check if the obstacle is in front of the ego. + const double lon_dist_from_ego_to_obj = + object->get_dist_from_ego_longitudinal(traj_points, current_pose.position); + if (lon_dist_from_ego_to_obj < 0.0) { + continue; + } + + // 1.2. Check if the rough lateral distance is smaller than the threshold. + // TODO(murooka) outside obstacle stop was removed. + const double min_lat_dist_to_traj_poly = + utils::calc_possible_min_dist_from_obj_to_traj_poly(object, traj_points, vehicle_info); + const uint8_t obj_label = object->predicted_object.classification.at(0).label; + if (get_max_lat_margin(obj_label) < min_lat_dist_to_traj_poly) { + continue; + } + + // 2. precise filtering + const auto & decimated_traj_polys = [&]() { + autoware::universe_utils::ScopedTimeTrack st_get_decimated_traj_polys( + "get_decimated_traj_polys", *time_keeper_); + return get_decimated_traj_polys( + traj_points, current_pose, vehicle_info, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold, trajectory_polygon_collision_check); + }(); + const double dist_from_obj_to_traj_poly = [&]() { + autoware::universe_utils::ScopedTimeTrack st_get_dist_to_traj_poly( + "get_dist_to_traj_poly", *time_keeper_); + return object->get_dist_to_traj_poly(decimated_traj_polys); + }(); + + // 2.1. filter target object inside trajectory + const auto inside_stop_obstacle = filter_inside_stop_obstacle_for_predicted_object( + odometry, traj_points, decimated_traj_points, object, predicted_objects_stamp, + dist_from_obj_to_traj_poly, vehicle_info, dist_to_bumper, trajectory_polygon_collision_check); + if (inside_stop_obstacle) { + stop_obstacles.push_back(*inside_stop_obstacle); + continue; + } + + // 2.2 filter target object outside trajectory + const auto outside_stop_obstacle = filter_outside_stop_obstacle_for_predicted_object( + odometry, traj_points, decimated_traj_points, predicted_objects_stamp, object, + dist_from_obj_to_traj_poly, is_driving_forward, vehicle_info, dist_to_bumper, + trajectory_polygon_collision_check); + if (outside_stop_obstacle) { + stop_obstacles.push_back(*outside_stop_obstacle); + } + } + + // Check target obstacles' consistency + check_consistency(predicted_objects_stamp, objects, stop_obstacles); + + prev_stop_obstacles_ = stop_obstacles; + + return stop_obstacles; +} + +std::optional ObstacleStopModule::filter_inside_stop_obstacle_for_predicted_object( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const double dist_from_obj_poly_to_traj_poly, const VehicleInfo & vehicle_info, + const double dist_to_bumper, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const auto & predicted_object = object->predicted_object; + const auto & obj_pose = object->get_predicted_pose(clock_->now(), predicted_objects_stamp); + + // 1. filter by label + const uint8_t obj_label = predicted_object.classification.at(0).label; + if (!is_in_vector(obj_label, obstacle_filtering_param_.inside_stop_object_types)) { + return std::nullopt; + } + + // 2. filter by lateral distance + const double max_lat_margin = get_max_lat_margin(obj_label); + // NOTE: max_lat_margin can be negative, so apply std::max with 1e-3. + if (std::max(max_lat_margin, 1e-3) <= dist_from_obj_poly_to_traj_poly) { + return std::nullopt; + } + + // 3. filter by velocity + if (!is_inside_stop_obstacle_velocity(object, traj_points)) { + return std::nullopt; + } + + // calculate collision points with trajectory with lateral stop margin + const auto & p = trajectory_polygon_collision_check; + const auto decimated_traj_polys_with_lat_margin = get_trajectory_polygon_for_inside( + decimated_traj_points, vehicle_info, odometry.pose.pose, max_lat_margin, + p.enable_to_consider_current_pose, p.time_to_convergence, p.decimate_trajectory_step_length); + debug_data_ptr_->decimated_traj_polys = decimated_traj_polys_with_lat_margin; + + // 4. check if the obstacle really collides with the trajectory + const auto collision_point = polygon_utils::get_collision_point( + decimated_traj_points, decimated_traj_polys_with_lat_margin, obj_pose, clock_->now(), + predicted_object.shape, dist_to_bumper); + if (!collision_point) { + return std::nullopt; + } + + // 5. filter if the obstacle will cross and go out of trajectory soon + if ( + ignore_crossing_obstacle_ && + is_crossing_transient_obstacle( + odometry, traj_points, decimated_traj_points, object, dist_to_bumper, + decimated_traj_polys_with_lat_margin, collision_point)) { + return std::nullopt; + } + + return StopObstacle{ + autoware::universe_utils::toHexString(predicted_object.object_id), + predicted_objects_stamp, + predicted_object.classification.at(0), + obj_pose, + predicted_object.shape, + object->get_lon_vel_relative_to_traj(traj_points), + collision_point->first, + collision_point->second}; +} + +bool ObstacleStopModule::is_inside_stop_obstacle_velocity( + const std::shared_ptr object, + const std::vector & traj_points) const +{ + const bool is_prev_object_stop = + utils::get_obstacle_from_uuid( + prev_stop_obstacles_, + autoware::universe_utils::toHexString(object->predicted_object.object_id)) + .has_value(); + + if (is_prev_object_stop) { + if ( + obstacle_filtering_param_.obstacle_velocity_threshold_from_stop < + object->get_lon_vel_relative_to_traj(traj_points)) { + return false; + } + return true; + } + if ( + object->get_lon_vel_relative_to_traj(traj_points) < + obstacle_filtering_param_.obstacle_velocity_threshold_to_stop) { + return true; + } + return false; +} + +bool ObstacleStopModule::is_crossing_transient_obstacle( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::shared_ptr object, const double dist_to_bumper, + const std::vector & decimated_traj_polys_with_lat_margin, + const std::optional> & collision_point) const +{ + // calculate the time to reach the collision point + const double time_to_reach_stop_point = calc_time_to_reach_collision_point( + odometry, collision_point->first, traj_points, + stop_planning_param_.min_behavior_stop_margin + dist_to_bumper, + obstacle_filtering_param_.min_velocity_to_reach_collision_point); + if ( + time_to_reach_stop_point <= obstacle_filtering_param_.crossing_obstacle_collision_time_margin) { + return false; + } + + // get the highest confident predicted paths + std::vector predicted_paths; + for (const auto & path : object->predicted_object.kinematics.predicted_paths) { + predicted_paths.push_back(path); + } + constexpr double prediction_resampling_time_interval = 0.1; + constexpr double prediction_resampling_time_horizon = 10.0; + const auto resampled_predicted_paths = resample_highest_confidence_predicted_paths( + predicted_paths, prediction_resampling_time_interval, prediction_resampling_time_horizon, 1); + if (resampled_predicted_paths.empty() || resampled_predicted_paths.front().path.empty()) { + return false; + } + + // predict object pose when the ego reaches the collision point + const auto future_obj_pose = [&]() { + const auto opt_future_obj_pose = autoware::object_recognition_utils::calcInterpolatedPose( + resampled_predicted_paths.front(), + time_to_reach_stop_point - obstacle_filtering_param_.crossing_obstacle_collision_time_margin); + if (opt_future_obj_pose) { + return *opt_future_obj_pose; + } + return resampled_predicted_paths.front().path.back(); + }(); + + // check if the ego will collide with the obstacle + auto future_predicted_object = object->predicted_object; + future_predicted_object.kinematics.initial_pose_with_covariance.pose = future_obj_pose; + const auto future_collision_point = polygon_utils::get_collision_point( + decimated_traj_points, decimated_traj_polys_with_lat_margin, + future_predicted_object.kinematics.initial_pose_with_covariance.pose, clock_->now(), + future_predicted_object.shape, dist_to_bumper); + const bool no_collision = !future_collision_point; + + return no_collision; +} + +std::optional ObstacleStopModule::filter_outside_stop_obstacle_for_predicted_object( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const rclcpp::Time & predicted_objects_stamp, const std::shared_ptr object, + const double dist_from_obj_poly_to_traj_poly, const bool is_driving_forward, + const VehicleInfo & vehicle_info, const double dist_to_bumper, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const auto & object_id = + autoware::universe_utils::toHexString(object->predicted_object.object_id); + + // 1. filter by label + const uint8_t obj_label = object->predicted_object.classification.at(0).label; + if (!is_in_vector(obj_label, obstacle_filtering_param_.outside_stop_object_types)) { + return std::nullopt; + } + + // 2. filter by lateral distance + const double max_lat_margin = obj_label == ObjectClassification::UNKNOWN + ? obstacle_filtering_param_.max_lat_margin_against_unknown + : obstacle_filtering_param_.max_lat_margin; + if (dist_from_obj_poly_to_traj_poly < std::max(max_lat_margin, 1e-3)) { + // Obstacle that is not inside of trajectory + return std::nullopt; + } + + const auto time_to_traj = dist_from_obj_poly_to_traj_poly / + std::max(1e-6, object->get_lat_vel_relative_to_traj(traj_points)); + if (time_to_traj > obstacle_filtering_param_.outside_max_lat_time_margin) { + RCLCPP_DEBUG( + logger_, "[Stop] Ignore outside obstacle (%s) since it's far from trajectory.", + object_id.substr(0, 4).c_str()); + return std::nullopt; + } + + // brkay54: For the pedestrians and bicycles, we need to check the collision point by thinking + // they will stop with a predefined deceleration rate to avoid unnecessary stops. + double resample_time_horizon = 10.0; + if (obj_label == ObjectClassification::PEDESTRIAN) { + resample_time_horizon = + std::sqrt( + std::pow( + object->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, 2) + + std::pow( + object->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y, 2)) / + (2.0 * obstacle_filtering_param_.outside_pedestrian_deceleration_rate); + } else if (obj_label == ObjectClassification::BICYCLE) { + resample_time_horizon = + std::sqrt( + std::pow( + object->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, 2) + + std::pow( + object->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y, 2)) / + (2.0 * obstacle_filtering_param_.outside_bicycle_deceleration_rate); + } + + // Get the highest confidence predicted path + std::vector predicted_paths; + for (const auto & path : object->predicted_object.kinematics.predicted_paths) { + predicted_paths.push_back(path); + } + constexpr double prediction_resampling_time_interval = 0.1; + const auto resampled_predicted_paths = resample_highest_confidence_predicted_paths( + predicted_paths, prediction_resampling_time_interval, resample_time_horizon, + obstacle_filtering_param_.outside_num_of_predicted_paths); + if (resampled_predicted_paths.empty()) { + return std::nullopt; + } + + const auto & p = trajectory_polygon_collision_check; + const auto decimated_traj_polys_with_lat_margin = get_trajectory_polygon_for_outside( + decimated_traj_points, vehicle_info, odometry.pose.pose, 0.0, p.enable_to_consider_current_pose, + p.time_to_convergence, p.decimate_trajectory_step_length); + + const auto get_collision_point = + [&]() -> std::optional> { + for (const auto & predicted_path : resampled_predicted_paths) { + const auto collision_point = create_collision_point_for_outside_stop_obstacle( + odometry, traj_points, decimated_traj_points, decimated_traj_polys_with_lat_margin, object, + predicted_objects_stamp, predicted_path, max_lat_margin, is_driving_forward, vehicle_info, + dist_to_bumper, trajectory_polygon_collision_check.decimate_trajectory_step_length); + if (collision_point) { + return collision_point; + } + } + return std::nullopt; + }; + + const auto collision_point = get_collision_point(); + + if (collision_point) { + return StopObstacle{ + object_id, + predicted_objects_stamp, + object->predicted_object.classification.at(0), + object->get_predicted_pose(clock_->now(), predicted_objects_stamp), + object->predicted_object.shape, + object->get_lon_vel_relative_to_traj(traj_points), + collision_point->first, + collision_point->second}; + } + return std::nullopt; +} + +std::optional> +ObstacleStopModule::create_collision_point_for_outside_stop_obstacle( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector & decimated_traj_polys, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const PredictedPath & resampled_predicted_path, double max_lat_margin, + const bool is_driving_forward, const VehicleInfo & vehicle_info, const double dist_to_bumper, + const double decimate_trajectory_step_length) const +{ + const auto & object_id = + autoware::universe_utils::toHexString(object->predicted_object.object_id); + + std::vector collision_index; + const auto collision_points = polygon_utils::get_collision_points( + decimated_traj_points, decimated_traj_polys, predicted_objects_stamp, resampled_predicted_path, + object->predicted_object.shape, clock_->now(), is_driving_forward, collision_index, + utils::calc_object_possible_max_dist_from_center(object->predicted_object.shape) + + decimate_trajectory_step_length + + std::hypot( + vehicle_info.vehicle_length_m, vehicle_info.vehicle_width_m * 0.5 + max_lat_margin)); + if (collision_points.empty()) { + RCLCPP_DEBUG( + logger_, + "[Stop] Ignore outside obstacle (%s) since there is no collision point between the " + "predicted path " + "and the ego.", + object_id.substr(0, 4).c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(object); + return std::nullopt; + } + + const double collision_time_margin = + calc_collision_time_margin(odometry, collision_points, traj_points, dist_to_bumper); + if (obstacle_filtering_param_.crossing_obstacle_collision_time_margin < collision_time_margin) { + RCLCPP_DEBUG( + logger_, "[Stop] Ignore outside obstacle (%s) since it will not collide with the ego.", + object_id.substr(0, 4).c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(object); + return std::nullopt; + } + + return polygon_utils::get_collision_point( + decimated_traj_points, collision_index.front(), collision_points, dist_to_bumper); +} + +std::optional ObstacleStopModule::plan_stop( + const std::shared_ptr planner_data, + const std::vector & traj_points, + const std::vector & stop_obstacles, const double dist_to_bumper) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + if (stop_obstacles.empty()) { + const auto markers = + autoware::motion_utils::createDeletedStopVirtualWallMarker(clock_->now(), 0); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + + prev_stop_distance_info_ = std::nullopt; + return std::nullopt; + } + + std::optional determined_stop_obstacle{}; + std::optional determined_zero_vel_dist{}; + std::optional determined_desired_stop_margin{}; + + const auto closest_stop_obstacles = get_closest_stop_obstacles(stop_obstacles); + for (const auto & stop_obstacle : closest_stop_obstacles) { + const auto ego_segment_idx = + planner_data->find_segment_index(traj_points, planner_data->current_odometry.pose.pose); + + // calculate dist to collide + const double dist_to_collide_on_ref_traj = + autoware::motion_utils::calcSignedArcLength(traj_points, 0, ego_segment_idx) + + stop_obstacle.dist_to_collide_on_decimated_traj; + + // calculate desired stop margin + const double desired_stop_margin = calc_desired_stop_margin( + planner_data, traj_points, stop_obstacle, dist_to_bumper, ego_segment_idx, + dist_to_collide_on_ref_traj); + + // calculate stop point against the obstacle + const auto candidate_zero_vel_dist = calc_candidate_zero_vel_dist( + planner_data, traj_points, stop_obstacle, dist_to_collide_on_ref_traj, desired_stop_margin); + if (!candidate_zero_vel_dist) { + continue; + } + + if (determined_stop_obstacle) { + const bool is_same_param_types = + (stop_obstacle.classification.label == determined_stop_obstacle->classification.label); + if ( + (is_same_param_types && stop_obstacle.dist_to_collide_on_decimated_traj > + determined_stop_obstacle->dist_to_collide_on_decimated_traj) || + (!is_same_param_types && *candidate_zero_vel_dist > determined_zero_vel_dist)) { + continue; + } + } + determined_zero_vel_dist = *candidate_zero_vel_dist; + determined_stop_obstacle = stop_obstacle; + determined_desired_stop_margin = desired_stop_margin; + } + + if (!determined_zero_vel_dist) { + // delete marker + const auto markers = + autoware::motion_utils::createDeletedStopVirtualWallMarker(clock_->now(), 0); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + + prev_stop_distance_info_ = std::nullopt; + return std::nullopt; + } + + // Hold previous stop distance if necessary + hold_previous_stop_if_necessary(planner_data, traj_points, determined_zero_vel_dist); + + // Insert stop point + const auto stop_point = calc_stop_point( + planner_data, traj_points, dist_to_bumper, determined_stop_obstacle, determined_zero_vel_dist); + + // set stop_planning_debug_info + set_stop_planning_debug_info(determined_stop_obstacle, determined_desired_stop_margin); + + return stop_point; +} + +double ObstacleStopModule::calc_desired_stop_margin( + const std::shared_ptr planner_data, + const std::vector & traj_points, const StopObstacle & stop_obstacle, + const double dist_to_bumper, const size_t ego_segment_idx, + const double dist_to_collide_on_ref_traj) +{ + // calculate default stop margin + const double default_stop_margin = [&]() { + const auto ref_traj_length = + autoware::motion_utils::calcSignedArcLength(traj_points, 0, traj_points.size() - 1); + if (dist_to_collide_on_ref_traj > ref_traj_length) { + // Use terminal margin (terminal_stop_margin) for obstacle stop + return stop_planning_param_.terminal_stop_margin; + } + return stop_planning_param_.stop_margin; + }(); + + // calculate stop margin on curve + const double stop_margin_on_curve = calc_margin_from_obstacle_on_curve( + planner_data, traj_points, stop_obstacle, dist_to_bumper, default_stop_margin); + + // calculate stop margin considering behavior's stop point + // NOTE: If behavior stop point is ahead of the closest_obstacle_stop point within a certain + // margin we set closest_obstacle_stop_distance to closest_behavior_stop_distance + const auto closest_behavior_stop_idx = + autoware::motion_utils::searchZeroVelocityIndex(traj_points, ego_segment_idx + 1); + if (closest_behavior_stop_idx) { + const double closest_behavior_stop_dist_on_ref_traj = + autoware::motion_utils::calcSignedArcLength(traj_points, 0, *closest_behavior_stop_idx); + const double stop_dist_diff = + closest_behavior_stop_dist_on_ref_traj - (dist_to_collide_on_ref_traj - stop_margin_on_curve); + if (0.0 < stop_dist_diff && stop_dist_diff < stop_margin_on_curve) { + return stop_planning_param_.min_behavior_stop_margin; + } + } + return stop_margin_on_curve; +} + +std::optional ObstacleStopModule::calc_candidate_zero_vel_dist( + const std::shared_ptr planner_data, + const std::vector & traj_points, const StopObstacle & stop_obstacle, + const double dist_to_collide_on_ref_traj, const double desired_stop_margin) +{ + double candidate_zero_vel_dist = std::max(0.0, dist_to_collide_on_ref_traj - desired_stop_margin); + if (suppress_sudden_stop_) { + const auto acceptable_stop_acc = [&]() -> std::optional { + if (stop_planning_param_.get_param_type(stop_obstacle.classification) == "default") { + return common_param_.limit_min_accel; + } + const double distance_to_judge_suddenness = std::min( + calc_minimum_distance_to_stop( + planner_data->current_odometry.twist.twist.linear.x, common_param_.limit_max_accel, + stop_planning_param_.get_param(stop_obstacle.classification).sudden_object_acc_threshold), + stop_planning_param_.get_param(stop_obstacle.classification).sudden_object_dist_threshold); + if (candidate_zero_vel_dist > distance_to_judge_suddenness) { + return common_param_.limit_min_accel; + } + if (stop_planning_param_.get_param(stop_obstacle.classification).abandon_to_stop) { + RCLCPP_WARN( + rclcpp::get_logger("ObstacleCruisePlanner::StopPlanner"), + "[Cruise] abandon to stop against %s object", + stop_planning_param_.object_types_maps.at(stop_obstacle.classification.label).c_str()); + return std::nullopt; + } else { + return stop_planning_param_.get_param(stop_obstacle.classification).limit_min_acc; + } + }(); + if (!acceptable_stop_acc) { + return std::nullopt; + } + + const double acceptable_stop_pos = + autoware::motion_utils::calcSignedArcLength( + traj_points, 0, planner_data->current_odometry.pose.pose.position) + + calc_minimum_distance_to_stop( + planner_data->current_odometry.twist.twist.linear.x, common_param_.limit_max_accel, + acceptable_stop_acc.value()); + if (acceptable_stop_pos > candidate_zero_vel_dist) { + candidate_zero_vel_dist = acceptable_stop_pos; + } + } + return candidate_zero_vel_dist; +} + +void ObstacleStopModule::hold_previous_stop_if_necessary( + const std::shared_ptr planner_data, + const std::vector & traj_points, + std::optional & determined_zero_vel_dist) +{ + if ( + std::abs(planner_data->current_odometry.twist.twist.linear.x) < + stop_planning_param_.hold_stop_velocity_threshold && + prev_stop_distance_info_) { + // NOTE: We assume that the current trajectory's front point is ahead of the previous + // trajectory's front point. + const size_t traj_front_point_prev_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_stop_distance_info_->first, traj_points.front().pose); + const double diff_dist_front_points = autoware::motion_utils::calcSignedArcLength( + prev_stop_distance_info_->first, 0, traj_points.front().pose.position, + traj_front_point_prev_seg_idx); + + const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points; + if ( + std::abs(prev_zero_vel_dist - determined_zero_vel_dist.value()) < + stop_planning_param_.hold_stop_distance_threshold) { + determined_zero_vel_dist.value() = prev_zero_vel_dist; + } + } +} + +std::optional ObstacleStopModule::calc_stop_point( + const std::shared_ptr planner_data, + const std::vector & traj_points, const double dist_to_bumper, + const std::optional & determined_stop_obstacle, + const std::optional & determined_zero_vel_dist) +{ + auto output_traj_points = traj_points; + + // insert stop point + const auto zero_vel_idx = + autoware::motion_utils::insertStopPoint(0, *determined_zero_vel_dist, output_traj_points); + if (!zero_vel_idx) { + return std::nullopt; + } + + // virtual wall marker for stop obstacle + const auto markers = autoware::motion_utils::createStopVirtualWallMarker( + output_traj_points.at(*zero_vel_idx).pose, "obstacle stop", clock_->now(), 0, dist_to_bumper, + "", planner_data->is_driving_forward); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + debug_data_ptr_->obstacles_to_stop.push_back(*determined_stop_obstacle); + + // update planning factor + const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose; + planning_factor_interface_->add( + output_traj_points, planner_data->current_odometry.pose.pose, stop_pose, PlanningFactor::STOP, + SafetyFactorArray{}); + + // Store stop reason debug data + metrics_manager_.calculate_metrics( + "PlannerInterface", "stop", planner_data, traj_points, stop_pose, *determined_stop_obstacle); + + prev_stop_distance_info_ = std::make_pair(output_traj_points, determined_zero_vel_dist.value()); + + return stop_pose.position; +} + +void ObstacleStopModule::set_stop_planning_debug_info( + const std::optional & determined_stop_obstacle, + const std::optional & determined_desired_stop_margin) const +{ + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_DISTANCE, + determined_stop_obstacle->dist_to_collide_on_decimated_traj); + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY, + determined_stop_obstacle->velocity); + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, + determined_desired_stop_margin.value()); + stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_VELOCITY, 0.0); + stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_ACCELERATION, 0.0); +} + +void ObstacleStopModule::publish_debug_info() +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + // 1. debug marker + MarkerArray debug_marker; + + // 1.1. obstacles + for (size_t i = 0; i < debug_data_ptr_->obstacles_to_stop.size(); ++i) { + // obstacle + const auto obstacle_marker = utils::get_object_marker( + debug_data_ptr_->obstacles_to_stop.at(i).pose, i, "obstacles", 1.0, 0.0, 0.0); + debug_marker.markers.push_back(obstacle_marker); + + // collision point + auto collision_point_marker = autoware::universe_utils::createDefaultMarker( + "map", clock_->now(), "collision_points", 0, Marker::SPHERE, + autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + collision_point_marker.pose.position = debug_data_ptr_->obstacles_to_stop.at(i).collision_point; + debug_marker.markers.push_back(collision_point_marker); + } + + // 1.2. intentionally ignored obstacles + for (size_t i = 0; i < debug_data_ptr_->intentionally_ignored_obstacles.size(); ++i) { + const auto marker = utils::get_object_marker( + debug_data_ptr_->intentionally_ignored_obstacles.at(i) + ->predicted_object.kinematics.initial_pose_with_covariance.pose, + i, "intentionally_ignored_obstacles", 0.0, 1.0, 0.0); + debug_marker.markers.push_back(marker); + } + + // 1.3. detection area + auto decimated_traj_polys_marker = autoware::universe_utils::createDefaultMarker( + "map", clock_->now(), "detection_area", 0, Marker::LINE_LIST, + autoware::universe_utils::createMarkerScale(0.01, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); + for (const auto & decimated_traj_poly : debug_data_ptr_->decimated_traj_polys) { + for (size_t dp_idx = 0; dp_idx < decimated_traj_poly.outer().size(); ++dp_idx) { + const auto & current_point = decimated_traj_poly.outer().at(dp_idx); + const auto & next_point = + decimated_traj_poly.outer().at((dp_idx + 1) % decimated_traj_poly.outer().size()); + + decimated_traj_polys_marker.points.push_back( + autoware::universe_utils::createPoint(current_point.x(), current_point.y(), 0.0)); + decimated_traj_polys_marker.points.push_back( + autoware::universe_utils::createPoint(next_point.x(), next_point.y(), 0.0)); + } + } + debug_marker.markers.push_back(decimated_traj_polys_marker); + + debug_publisher_->publish(debug_marker); + + // 2. virtual wall + virtual_wall_publisher_->publish(debug_data_ptr_->stop_wall_marker); + + // 3. stop planning info + const auto stop_debug_msg = stop_planning_debug_info_.convert_to_message(clock_->now()); + debug_stop_planning_info_pub_->publish(stop_debug_msg); + + // 4. objects of interest + objects_of_interest_marker_interface_->publishMarkerArray(); + + // 5. metrics + const auto metrics_msg = metrics_manager_.create_metric_array(clock_->now()); + metrics_pub_->publish(metrics_msg); + + // 6. processing time + processing_time_publisher_->publish(create_float64_stamped(clock_->now(), stop_watch_.toc())); + + // 7. planning factor + planning_factor_interface_->publish(); +} + +double ObstacleStopModule::calc_collision_time_margin( + const Odometry & odometry, const std::vector & collision_points, + const std::vector & traj_points, const double dist_to_bumper) const +{ + const double time_to_reach_stop_point = calc_time_to_reach_collision_point( + odometry, collision_points.front().point, traj_points, + stop_planning_param_.min_behavior_stop_margin + dist_to_bumper, + obstacle_filtering_param_.min_velocity_to_reach_collision_point); + + const double time_to_leave_collision_point = + time_to_reach_stop_point + + dist_to_bumper / std::max( + obstacle_filtering_param_.min_velocity_to_reach_collision_point, + odometry.twist.twist.linear.x); + + const double time_to_start_cross = + (rclcpp::Time(collision_points.front().stamp) - clock_->now()).seconds(); + const double time_to_end_cross = + (rclcpp::Time(collision_points.back().stamp) - clock_->now()).seconds(); + + if (time_to_leave_collision_point < time_to_start_cross) { // Ego goes first. + return time_to_start_cross - time_to_reach_stop_point; + } + if (time_to_end_cross < time_to_reach_stop_point) { // Obstacle goes first. + return time_to_reach_stop_point - time_to_end_cross; + } + return 0.0; // Ego and obstacle will collide. +} + +std::vector ObstacleStopModule::get_trajectory_polygon_for_inside( + const std::vector & decimated_traj_points, const VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin, + const bool enable_to_consider_current_pose, const double time_to_convergence, + const double decimate_trajectory_step_length) const +{ + if (trajectory_polygon_for_inside_map_.count(lat_margin) == 0) { + const auto traj_polys = polygon_utils::create_one_step_polygons( + decimated_traj_points, vehicle_info, current_ego_pose, lat_margin, + enable_to_consider_current_pose, time_to_convergence, decimate_trajectory_step_length); + trajectory_polygon_for_inside_map_.emplace(lat_margin, traj_polys); + } + return trajectory_polygon_for_inside_map_.at(lat_margin); +} + +std::vector ObstacleStopModule::get_trajectory_polygon_for_outside( + const std::vector & decimated_traj_points, const VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin, + const bool enable_to_consider_current_pose, const double time_to_convergence, + const double decimate_trajectory_step_length) const +{ + if (!trajectory_polygon_for_outside_) { + trajectory_polygon_for_outside_ = polygon_utils::create_one_step_polygons( + decimated_traj_points, vehicle_info, current_ego_pose, lat_margin, + enable_to_consider_current_pose, time_to_convergence, decimate_trajectory_step_length); + } + return *trajectory_polygon_for_outside_; +} + +void ObstacleStopModule::check_consistency( + const rclcpp::Time & current_time, + const std::vector> & objects, + std::vector & stop_obstacles) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + for (const auto & prev_closest_stop_obstacle : prev_closest_stop_obstacles_) { + const auto object_itr = std::find_if( + objects.begin(), objects.end(), + [&prev_closest_stop_obstacle](const std::shared_ptr & o) { + return autoware::universe_utils::toHexString(o->predicted_object.object_id) == + prev_closest_stop_obstacle.uuid; + }); + // If previous closest obstacle disappear from the perception result, do nothing anymore. + if (object_itr == objects.end()) { + continue; + } + + const auto is_disappeared_from_stop_obstacle = std::none_of( + stop_obstacles.begin(), stop_obstacles.end(), + [&prev_closest_stop_obstacle](const StopObstacle & so) { + return so.uuid == prev_closest_stop_obstacle.uuid; + }); + if (is_disappeared_from_stop_obstacle) { + // re-evaluate as a stop candidate, and overwrite the current decision if "maintain stop" + // condition is satisfied + const double elapsed_time = (current_time - prev_closest_stop_obstacle.stamp).seconds(); + if ( + (*object_itr)->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x < + obstacle_filtering_param_.obstacle_velocity_threshold_from_stop && + elapsed_time < obstacle_filtering_param_.stop_obstacle_hold_time_threshold) { + stop_obstacles.push_back(prev_closest_stop_obstacle); + } + } + } + + prev_closest_stop_obstacles_ = get_closest_stop_obstacles(stop_obstacles); +} + +double ObstacleStopModule::calc_margin_from_obstacle_on_curve( + const std::shared_ptr planner_data, + const std::vector & traj_points, const StopObstacle & stop_obstacle, + const double dist_to_bumper, const double default_stop_margin) const +{ + if ( + !stop_planning_param_.enable_approaching_on_curve || obstacle_filtering_param_.use_pointcloud) { + return default_stop_margin; + } + + // calculate short trajectory points towards obstacle + const size_t obj_segment_idx = + autoware::motion_utils::findNearestSegmentIndex(traj_points, stop_obstacle.collision_point); + std::vector short_traj_points{traj_points.at(obj_segment_idx + 1)}; + double sum_short_traj_length{0.0}; + for (int i = obj_segment_idx; 0 <= i; --i) { + short_traj_points.push_back(traj_points.at(i)); + + if ( + 1 < short_traj_points.size() && + stop_planning_param_.stop_margin + dist_to_bumper < sum_short_traj_length) { + break; + } + sum_short_traj_length += + autoware::universe_utils::calcDistance2d(traj_points.at(i), traj_points.at(i + 1)); + } + std::reverse(short_traj_points.begin(), short_traj_points.end()); + if (short_traj_points.size() < 2) { + return default_stop_margin; + } + + // calculate collision index between straight line from ego pose and object + const auto calculate_distance_from_straight_ego_path = + [&](const auto & ego_pose, const auto & object_polygon) { + const auto forward_ego_pose = autoware::universe_utils::calcOffsetPose( + ego_pose, stop_planning_param_.stop_margin + 3.0, 0.0, 0.0); + const auto ego_straight_segment = autoware::universe_utils::Segment2d{ + convert_point(ego_pose.position), convert_point(forward_ego_pose.position)}; + return boost::geometry::distance(ego_straight_segment, object_polygon); + }; + const auto resampled_short_traj_points = resample_trajectory_points(short_traj_points, 0.5); + const auto object_polygon = + autoware::universe_utils::toPolygon2d(stop_obstacle.pose, stop_obstacle.shape); + const auto collision_idx = [&]() -> std::optional { + for (size_t i = 0; i < resampled_short_traj_points.size(); ++i) { + const double dist_to_obj = calculate_distance_from_straight_ego_path( + resampled_short_traj_points.at(i).pose, object_polygon); + if (dist_to_obj < planner_data->vehicle_info_.vehicle_width_m / 2.0) { + return i; + } + } + return std::nullopt; + }(); + if (!collision_idx) { + return stop_planning_param_.min_stop_margin_on_curve; + } + if (*collision_idx == 0) { + return default_stop_margin; + } + + // calculate margin from obstacle + const double partial_segment_length = [&]() { + const double collision_segment_length = autoware::universe_utils::calcDistance2d( + resampled_short_traj_points.at(*collision_idx - 1), + resampled_short_traj_points.at(*collision_idx)); + const double prev_dist = calculate_distance_from_straight_ego_path( + resampled_short_traj_points.at(*collision_idx - 1).pose, object_polygon); + const double next_dist = calculate_distance_from_straight_ego_path( + resampled_short_traj_points.at(*collision_idx).pose, object_polygon); + return (next_dist - planner_data->vehicle_info_.vehicle_width_m / 2.0) / + (next_dist - prev_dist) * collision_segment_length; + }(); + + const double short_margin_from_obstacle = + partial_segment_length + + autoware::motion_utils::calcSignedArcLength( + resampled_short_traj_points, *collision_idx, stop_obstacle.collision_point) - + dist_to_bumper + stop_planning_param_.additional_stop_margin_on_curve; + + return std::min( + default_stop_margin, + std::max(stop_planning_param_.min_stop_margin_on_curve, short_margin_from_obstacle)); +} + +std::vector ObstacleStopModule::get_closest_stop_obstacles( + const std::vector & stop_obstacles) +{ + std::vector candidates{}; + for (const auto & stop_obstacle : stop_obstacles) { + const auto itr = + std::find_if(candidates.begin(), candidates.end(), [&stop_obstacle](const StopObstacle & co) { + return co.classification.label == stop_obstacle.classification.label; + }); + if (itr == candidates.end()) { + candidates.emplace_back(stop_obstacle); + } else if ( + stop_obstacle.dist_to_collide_on_decimated_traj < itr->dist_to_collide_on_decimated_traj) { + *itr = stop_obstacle; + } + } + return candidates; +} + +double ObstacleStopModule::get_max_lat_margin(const uint8_t obj_label) const +{ + if (obj_label == ObjectClassification::UNKNOWN) { + return obstacle_filtering_param_.max_lat_margin_against_unknown; + } + return obstacle_filtering_param_.max_lat_margin; +} + +std::vector ObstacleStopModule::get_decimated_traj_polys( + const std::vector & traj_points, const geometry_msgs::msg::Pose & current_pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const +{ + if (!decimated_traj_polys_) { + const auto & p = trajectory_polygon_collision_check; + const auto decimated_traj_points = utils::decimate_trajectory_points_from_ego( + traj_points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold, + p.decimate_trajectory_step_length, p.goal_extended_trajectory_length); + decimated_traj_polys_ = polygon_utils::create_one_step_polygons( + decimated_traj_points, vehicle_info, current_pose, 0.0, p.enable_to_consider_current_pose, + p.time_to_convergence, p.decimate_trajectory_step_length); + } + return *decimated_traj_polys_; +} + +} // namespace autoware::motion_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::motion_velocity_planner::ObstacleStopModule, + autoware::motion_velocity_planner::PluginModuleInterface) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.hpp new file mode 100644 index 0000000000000..e1f94eb243814 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.hpp @@ -0,0 +1,196 @@ +// 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 OBSTACLE_STOP_MODULE_HPP_ +#define OBSTACLE_STOP_MODULE_HPP_ + +#include "autoware/motion_velocity_planner_common_universe/polygon_utils.hpp" +#include "autoware/motion_velocity_planner_common_universe/utils.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" +#include "metrics_manager.hpp" +#include "parameters.hpp" +#include "stop_planning_debug_info.hpp" +#include "type_alias.hpp" +#include "types.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class ObstacleStopModule : public PluginModuleInterface +{ +public: + void init(rclcpp::Node & node, const std::string & module_name) override; + void update_parameters(const std::vector & parameters) override; + std::string get_module_name() const override { return module_name_; } + + VelocityPlanningResult plan( + const std::vector & raw_trajectory_points, + const std::vector & smoothed_trajectory_points, + const std::shared_ptr planner_data) override; + +private: + std::string module_name_{}; + rclcpp::Clock::SharedPtr clock_{}; + + // ros parameters + bool ignore_crossing_obstacle_{}; + bool suppress_sudden_stop_{}; + CommonParam common_param_{}; + StopPlanningParam stop_planning_param_{}; + ObstacleFilteringParam obstacle_filtering_param_{}; + + // module publisher + rclcpp::Publisher::SharedPtr debug_stop_planning_info_pub_{}; + rclcpp::Publisher::SharedPtr metrics_pub_{}; + rclcpp::Publisher::SharedPtr + processing_time_detail_pub_{}; + + // interface publisher + std::unique_ptr + objects_of_interest_marker_interface_{}; + + // internal variables + mutable StopPlanningDebugInfo stop_planning_debug_info_{}; + mutable std::shared_ptr debug_data_ptr_{}; + std::vector prev_closest_stop_obstacles_{}; + std::vector prev_stop_obstacles_{}; + MetricsManager metrics_manager_{}; + // previous trajectory and distance to stop + // NOTE: Previous trajectory is memorized to deal with nearest index search for overlapping or + // crossing lanes. + std::optional, double>> prev_stop_distance_info_{ + std::nullopt}; + autoware::universe_utils::StopWatch stop_watch_{}; + mutable std::unordered_map> trajectory_polygon_for_inside_map_{}; + mutable std::optional> trajectory_polygon_for_outside_{std::nullopt}; + mutable std::optional> decimated_traj_polys_{std::nullopt}; + mutable std::shared_ptr time_keeper_{}; + + std::vector get_trajectory_polygon_for_inside( + const std::vector & decimated_traj_points, const VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin, + const bool enable_to_consider_current_pose, const double time_to_convergence, + const double decimate_trajectory_step_length) const; + + std::vector get_trajectory_polygon_for_outside( + const std::vector & decimated_traj_points, const VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin, + const bool enable_to_consider_current_pose, const double time_to_convergence, + const double decimate_trajectory_step_length) const; + + std::vector filter_stop_obstacle_for_predicted_object( + const Odometry & odometry, const double ego_nearest_dist_threshold, + const double ego_nearest_yaw_threshold, const rclcpp::Time & predicted_objects_stamp, + const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector> & objects, + const bool is_driving_forward, const VehicleInfo & vehicle_info, const double dist_to_bumper, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check); + + std::optional plan_stop( + const std::shared_ptr planner_data, + const std::vector & traj_points, + const std::vector & stop_obstacles, const double dist_to_bumper); + double calc_desired_stop_margin( + const std::shared_ptr planner_data, + const std::vector & traj_points, const StopObstacle & stop_obstacle, + const double dist_to_bumper, const size_t ego_segment_idx, + const double dist_to_collide_on_ref_traj); + std::optional calc_candidate_zero_vel_dist( + const std::shared_ptr planner_data, + const std::vector & traj_points, const StopObstacle & stop_obstacle, + const double dist_to_collide_on_ref_traj, const double desired_stop_margin); + void hold_previous_stop_if_necessary( + const std::shared_ptr planner_data, + const std::vector & traj_points, + std::optional & determined_zero_vel_dist); + std::optional calc_stop_point( + const std::shared_ptr planner_data, + const std::vector & traj_points, const double dist_to_bumper, + const std::optional & determined_stop_obstacle, + const std::optional & determined_zero_vel_dist); + void set_stop_planning_debug_info( + const std::optional & determined_stop_obstacle, + const std::optional & determined_desired_stop_margin) const; + void publish_debug_info(); + + std::optional filter_inside_stop_obstacle_for_predicted_object( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const double dist_from_obj_poly_to_traj_poly, const VehicleInfo & vehicle_info, + const double dist_to_bumper, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const; + bool is_inside_stop_obstacle_velocity( + const std::shared_ptr object, + const std::vector & traj_points) const; + bool is_crossing_transient_obstacle( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::shared_ptr object, const double dist_to_bumper, + const std::vector & decimated_traj_polys_with_lat_margin, + const std::optional> & collision_point) const; + + std::optional filter_outside_stop_obstacle_for_predicted_object( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const rclcpp::Time & predicted_objects_stamp, const std::shared_ptr object, + const double dist_from_obj_poly_to_traj_poly, const bool is_driving_forward, + const VehicleInfo & vehicle_info, const double dist_to_bumper, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const; + std::optional> + create_collision_point_for_outside_stop_obstacle( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & decimated_traj_points, + const std::vector & decimated_traj_polys, + const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, + const PredictedPath & resampled_predicted_path, double max_lat_margin, + const bool is_driving_forward, const VehicleInfo & vehicle_info, const double dist_to_bumper, + const double decimate_trajectory_step_length) const; + double calc_collision_time_margin( + const Odometry & odometry, const std::vector & collision_points, + const std::vector & traj_points, const double dist_to_bumper) const; + void check_consistency( + const rclcpp::Time & current_time, + const std::vector> & objects, + std::vector & stop_obstacles); + double calc_margin_from_obstacle_on_curve( + const std::shared_ptr planner_data, + const std::vector & traj_points, const StopObstacle & stop_obstacle, + const double dist_to_bumper, const double default_stop_margin) const; + std::vector get_closest_stop_obstacles( + const std::vector & stop_obstacles); + double get_max_lat_margin(const uint8_t obj_label) const; + std::vector get_decimated_traj_polys( + const std::vector & traj_points, const geometry_msgs::msg::Pose & current_pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold, + const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const; +}; +} // namespace autoware::motion_velocity_planner + +#endif // OBSTACLE_STOP_MODULE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp new file mode 100644 index 0000000000000..ed17f60b6f417 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp @@ -0,0 +1,210 @@ +// 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 PARAMETERS_HPP_ +#define PARAMETERS_HPP_ + +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_velocity_planner_common_universe/utils.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" +#include "autoware/universe_utils/ros/parameter.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" +#include "type_alias.hpp" +#include "types.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +using autoware::universe_utils::getOrDeclareParameter; + +struct CommonParam +{ + double max_accel{}; + double min_accel{}; + double max_jerk{}; + double min_jerk{}; + double limit_max_accel{}; + double limit_min_accel{}; + double limit_max_jerk{}; + double limit_min_jerk{}; + + CommonParam() = default; + explicit CommonParam(rclcpp::Node & node) + { + max_accel = getOrDeclareParameter(node, "normal.max_acc"); + min_accel = getOrDeclareParameter(node, "normal.min_acc"); + max_jerk = getOrDeclareParameter(node, "normal.max_jerk"); + min_jerk = getOrDeclareParameter(node, "normal.min_jerk"); + limit_max_accel = getOrDeclareParameter(node, "limit.max_acc"); + limit_min_accel = getOrDeclareParameter(node, "limit.min_acc"); + limit_max_jerk = getOrDeclareParameter(node, "limit.max_jerk"); + limit_min_jerk = getOrDeclareParameter(node, "limit.min_jerk"); + } +}; + +struct ObstacleFilteringParam +{ + bool use_pointcloud{}; + std::vector inside_stop_object_types{}; + std::vector outside_stop_object_types{}; + + double obstacle_velocity_threshold_to_stop{}; + double obstacle_velocity_threshold_from_stop{}; + + double max_lat_margin{}; + double max_lat_margin_against_unknown{}; + + double min_velocity_to_reach_collision_point{}; + double stop_obstacle_hold_time_threshold{}; + + double outside_max_lat_time_margin{}; + int outside_num_of_predicted_paths{}; + double outside_pedestrian_deceleration_rate{}; + double outside_bicycle_deceleration_rate{}; + + double crossing_obstacle_collision_time_margin{}; + + ObstacleFilteringParam() = default; + explicit ObstacleFilteringParam(rclcpp::Node & node) + { + inside_stop_object_types = + utils::get_target_object_type(node, "obstacle_stop.obstacle_filtering.object_type.inside."); + outside_stop_object_types = + utils::get_target_object_type(node, "obstacle_stop.obstacle_filtering.object_type.outside."); + use_pointcloud = + getOrDeclareParameter(node, "obstacle_stop.obstacle_filtering.object_type.pointcloud"); + + obstacle_velocity_threshold_to_stop = getOrDeclareParameter( + node, "obstacle_stop.obstacle_filtering.obstacle_velocity_threshold_to_stop"); + obstacle_velocity_threshold_from_stop = getOrDeclareParameter( + node, "obstacle_stop.obstacle_filtering.obstacle_velocity_threshold_from_stop"); + + max_lat_margin = + getOrDeclareParameter(node, "obstacle_stop.obstacle_filtering.max_lat_margin"); + max_lat_margin_against_unknown = getOrDeclareParameter( + node, "obstacle_stop.obstacle_filtering.max_lat_margin_against_unknown"); + + min_velocity_to_reach_collision_point = getOrDeclareParameter( + node, "obstacle_stop.obstacle_filtering.min_velocity_to_reach_collision_point"); + stop_obstacle_hold_time_threshold = getOrDeclareParameter( + node, "obstacle_stop.obstacle_filtering.stop_obstacle_hold_time_threshold"); + + outside_max_lat_time_margin = getOrDeclareParameter( + node, "obstacle_stop.obstacle_filtering.outside_obstacle.max_lateral_time_margin"); + outside_num_of_predicted_paths = getOrDeclareParameter( + node, "obstacle_stop.obstacle_filtering.outside_obstacle.num_of_predicted_paths"); + outside_pedestrian_deceleration_rate = getOrDeclareParameter( + node, "obstacle_stop.obstacle_filtering.outside_obstacle.pedestrian_deceleration_rate"); + outside_bicycle_deceleration_rate = getOrDeclareParameter( + node, "obstacle_stop.obstacle_filtering.outside_obstacle.bicycle_deceleration_rate"); + + crossing_obstacle_collision_time_margin = getOrDeclareParameter( + node, "obstacle_stop.obstacle_filtering.crossing_obstacle.collision_time_margin"); + } +}; + +struct StopPlanningParam +{ + double stop_margin{}; + double terminal_stop_margin{}; + double min_behavior_stop_margin{}; + double hold_stop_velocity_threshold{}; + double hold_stop_distance_threshold{}; + bool enable_approaching_on_curve{}; + double additional_stop_margin_on_curve{}; + double min_stop_margin_on_curve{}; + + struct ObjectTypeSpecificParams + { + double limit_min_acc{}; + double sudden_object_acc_threshold{}; + double sudden_object_dist_threshold{}; + bool abandon_to_stop{}; + }; + std::unordered_map object_types_maps = { + {ObjectClassification::UNKNOWN, "unknown"}, {ObjectClassification::CAR, "car"}, + {ObjectClassification::TRUCK, "truck"}, {ObjectClassification::BUS, "bus"}, + {ObjectClassification::TRAILER, "trailer"}, {ObjectClassification::MOTORCYCLE, "motorcycle"}, + {ObjectClassification::BICYCLE, "bicycle"}, {ObjectClassification::PEDESTRIAN, "pedestrian"}}; + std::unordered_map object_type_specific_param_map; + + StopPlanningParam() = default; + StopPlanningParam(rclcpp::Node & node, const CommonParam & common_param) + { + stop_margin = getOrDeclareParameter(node, "obstacle_stop.stop_planning.stop_margin"); + terminal_stop_margin = + getOrDeclareParameter(node, "obstacle_stop.stop_planning.terminal_stop_margin"); + min_behavior_stop_margin = + getOrDeclareParameter(node, "obstacle_stop.stop_planning.min_behavior_stop_margin"); + hold_stop_velocity_threshold = getOrDeclareParameter( + node, "obstacle_stop.stop_planning.hold_stop_velocity_threshold"); + hold_stop_distance_threshold = getOrDeclareParameter( + node, "obstacle_stop.stop_planning.hold_stop_distance_threshold"); + enable_approaching_on_curve = getOrDeclareParameter( + node, "obstacle_stop.stop_planning.stop_on_curve.enable_approaching"); + additional_stop_margin_on_curve = getOrDeclareParameter( + node, "obstacle_stop.stop_planning.stop_on_curve.additional_stop_margin"); + min_stop_margin_on_curve = getOrDeclareParameter( + node, "obstacle_stop.stop_planning.stop_on_curve.min_stop_margin"); + + const std::string param_prefix = "obstacle_stop.stop_planning.object_type_specified_params."; + const auto object_types = + getOrDeclareParameter>(node, param_prefix + "types"); + + for (const auto & type_str : object_types) { + if (type_str != "default") { + ObjectTypeSpecificParams param{ + getOrDeclareParameter(node, param_prefix + type_str + ".limit_min_acc"), + getOrDeclareParameter( + node, param_prefix + type_str + ".sudden_object_acc_threshold"), + getOrDeclareParameter( + node, param_prefix + type_str + ".sudden_object_dist_threshold"), + getOrDeclareParameter(node, param_prefix + type_str + ".abandon_to_stop")}; + + param.sudden_object_acc_threshold = + std::min(param.sudden_object_acc_threshold, common_param.limit_min_accel); + param.limit_min_acc = std::min(param.limit_min_acc, param.sudden_object_acc_threshold); + + object_type_specific_param_map.emplace(type_str, param); + } + } + } + + std::string get_param_type(const ObjectClassification label) + { + const auto type_str = object_types_maps.at(label.label); + if (object_type_specific_param_map.count(type_str) == 0) { + return "default"; + } + return type_str; + } + ObjectTypeSpecificParams get_param(const ObjectClassification label) + { + return object_type_specific_param_map.at(get_param_type(label)); + } +}; +} // namespace autoware::motion_velocity_planner + +#endif // PARAMETERS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/stop_planning_debug_info.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/stop_planning_debug_info.hpp new file mode 100644 index 0000000000000..2028a1123e956 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/stop_planning_debug_info.hpp @@ -0,0 +1,90 @@ +// 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 STOP_PLANNING_DEBUG_INFO_HPP_ +#define STOP_PLANNING_DEBUG_INFO_HPP_ + +#include + +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" + +#include + +namespace autoware::motion_velocity_planner +{ +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; + +class StopPlanningDebugInfo +{ +public: + enum class TYPE { + // ego + EGO_VELOCITY = 0, // index: 0 + EGO_ACCELERATION, + EGO_JERK, // no data + + // stop planning + STOP_CURRENT_OBSTACLE_DISTANCE, // index: 3 + STOP_CURRENT_OBSTACLE_VELOCITY, + STOP_TARGET_OBSTACLE_DISTANCE, + STOP_TARGET_VELOCITY, + STOP_TARGET_ACCELERATION, + + SIZE + }; + + /** + * @brief get the index corresponding to the given value TYPE + * @param [in] type the TYPE enum for which to get the index + * @return index of the type + */ + static int getIndex(const TYPE type) { return static_cast(type); } + + /** + * @brief get all the debug values as an std::array + * @return array of all debug values + */ + std::array(TYPE::SIZE)> get() const { return info_; } + + /** + * @brief set the given type to the given value + * @param [in] type TYPE of the value + * @param [in] value value to set + */ + void set(const TYPE type, const double val) { info_.at(static_cast(type)) = val; } + + /** + * @brief set the given type to the given value + * @param [in] type index of the type + * @param [in] value value to set + */ + void set(const int type, const double val) { info_.at(type) = val; } + + void reset() { info_.fill(0.0); } + + Float32MultiArrayStamped convert_to_message(const rclcpp::Time & current_time) const + { + Float32MultiArrayStamped msg{}; + msg.stamp = current_time; + for (const auto & v : get()) { + msg.data.push_back(v); + } + return msg; + } + +private: + std::array(TYPE::SIZE)> info_; +}; +} // namespace autoware::motion_velocity_planner +#endif // STOP_PLANNING_DEBUG_INFO_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/type_alias.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/type_alias.hpp new file mode 100644 index 0000000000000..b3c278fcafa50 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/type_alias.hpp @@ -0,0 +1,63 @@ +// 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 TYPE_ALIAS_HPP_ +#define TYPE_ALIAS_HPP_ + +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" + +#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp" +#include "autoware_perception_msgs/msg/predicted_object.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include +#include +#include +#include + +#include +#include + +namespace autoware::motion_velocity_planner +{ +using autoware::vehicle_info_utils::VehicleInfo; +using autoware_internal_debug_msgs::msg::Float32Stamped; +using autoware_internal_debug_msgs::msg::Float64Stamped; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::Shape; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Twist; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::PointCloud2; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; +namespace bg = boost::geometry; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; +using Metric = tier4_metric_msgs::msg::Metric; +using MetricArray = tier4_metric_msgs::msg::MetricArray; +using PointCloud = pcl::PointCloud; +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::SafetyFactorArray; +} // namespace autoware::motion_velocity_planner +#endif // TYPE_ALIAS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/types.hpp new file mode 100644 index 0000000000000..23b0cb701afab --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/types.hpp @@ -0,0 +1,73 @@ +// 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 TYPES_HPP_ +#define TYPES_HPP_ + +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "type_alias.hpp" + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +struct StopObstacle +{ + StopObstacle( + const std::string & arg_uuid, const rclcpp::Time & arg_stamp, + const ObjectClassification & object_classification, const geometry_msgs::msg::Pose & arg_pose, + const Shape & arg_shape, const double arg_lon_velocity, + const geometry_msgs::msg::Point arg_collision_point, + const double arg_dist_to_collide_on_decimated_traj) + : uuid(arg_uuid), + stamp(arg_stamp), + pose(arg_pose), + velocity(arg_lon_velocity), + shape(arg_shape), + collision_point(arg_collision_point), + dist_to_collide_on_decimated_traj(arg_dist_to_collide_on_decimated_traj), + classification(object_classification) + { + } + std::string uuid; + rclcpp::Time stamp; + geometry_msgs::msg::Pose pose; // interpolated with the current stamp + double velocity; // longitudinal velocity against ego's trajectory + + Shape shape; + geometry_msgs::msg::Point + collision_point; // TODO(yuki_takagi): this member variable still used in + // calculateMarginFromObstacleOnCurve() and should be removed as it can be + // replaced by ”dist_to_collide_on_decimated_traj” + double dist_to_collide_on_decimated_traj; + ObjectClassification classification; +}; + +struct DebugData +{ + DebugData() = default; + std::vector> intentionally_ignored_obstacles; + std::vector obstacles_to_stop; + std::vector decimated_traj_polys; + MarkerArray stop_wall_marker; +}; + +} // namespace autoware::motion_velocity_planner + +#endif // TYPES_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp index 9be7f52bca99a..1e63713a61a4d 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp @@ -29,7 +29,7 @@ namespace autoware::motion_velocity_planner::obstacle_velocity_limiter { multi_polygon_t createPolygonMasks( - const std::vector & dynamic_obstacles, const double buffer, + const std::vector> & dynamic_obstacles, const double buffer, const double min_vel) { return createObjectPolygons(dynamic_obstacles, buffer, min_vel); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp index ad746fb2a7b7e..d7a944514cd26 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp @@ -56,7 +56,7 @@ void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_b /// @param[in] min_vel minimum velocity for an object to be masked /// @return polygon masks around dynamic objects multi_polygon_t createPolygonMasks( - const std::vector & dynamic_obstacles, const double buffer, + const std::vector> & dynamic_obstacles, const double buffer, const double min_vel); /// @brief create footprint polygons from projection lines diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp index eb89c027a3b48..baa98ea1f79b8 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp @@ -135,7 +135,9 @@ void ObstacleVelocityLimiterModule::update_parameters( } VelocityPlanningResult ObstacleVelocityLimiterModule::plan( - const std::vector & ego_trajectory_points, + [[maybe_unused]] const std::vector & + raw_trajectory_points, + const std::vector & smoothed_trajectory_points, const std::shared_ptr planner_data) { autoware::universe_utils::StopWatch stopwatch; @@ -143,14 +145,14 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( VelocityPlanningResult result; stopwatch.tic("preprocessing"); const auto ego_idx = autoware::motion_utils::findNearestIndex( - ego_trajectory_points, planner_data->current_odometry.pose.pose); + smoothed_trajectory_points, planner_data->current_odometry.pose.pose); if (!ego_idx) { RCLCPP_WARN_THROTTLE( logger_, *clock_, rcutils_duration_value_t(1000), "Cannot calculate ego index on the trajectory"); return result; } - auto original_traj_points = ego_trajectory_points; + auto original_traj_points = smoothed_trajectory_points; if (preprocessing_params_.calculate_steering_angles) obstacle_velocity_limiter::calculateSteeringAngles( original_traj_points, projection_params_.wheel_base); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp index 54f9c5c12e59e..f89a8d43e9e93 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp @@ -49,7 +49,8 @@ class ObstacleVelocityLimiterModule : public PluginModuleInterface 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::vector & raw_trajectory_points, + const std::vector & smoothed_trajectory_points, const std::shared_ptr planner_data) override; std::string get_module_name() const override { return module_name_; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp index 187f028dc5969..be38ac92f92ec 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp @@ -59,11 +59,12 @@ polygon_t createObjectPolygon( } multi_polygon_t createObjectPolygons( - const std::vector & objects, const double buffer, const double min_velocity) + const std::vector> & objects, const double buffer, + const double min_velocity) { multi_polygon_t polygons; for (const auto & object : objects) { - const auto & predicted_object = object.predicted_object; + const auto & predicted_object = object->predicted_object; const double obj_vel_norm = std::hypot( predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp index aff99140744e4..ddcb1161b6f59 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp @@ -164,7 +164,8 @@ polygon_t createObjectPolygon( /// @param [in] min_velocity objects with velocity lower will be ignored /// @return polygons of the objects multi_polygon_t createObjectPolygons( - const std::vector & objects, const double buffer, const double min_velocity); + const std::vector> & objects, const double buffer, + const double min_velocity); /// @brief add obstacles obtained from sensors to the given Obstacles object /// @param[out] obstacles Obstacles object in which to add the sensor obstacles diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp index 926b730dcc3dd..a538c7f6610a0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp @@ -394,7 +394,7 @@ TEST(TestCollisionDistance, createObjPolygons) using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; - std::vector objects; + std::vector> objects; auto polygons = createObjectPolygons(objects, 0.0, 0.0); EXPECT_TRUE(polygons.empty()); @@ -407,7 +407,7 @@ TEST(TestCollisionDistance, createObjPolygons) object1.kinematics.initial_twist_with_covariance.twist.linear.x = 0.0; object1.shape.dimensions.x = 1.0; object1.shape.dimensions.y = 1.0; - objects.push_back(PlannerData::Object(object1)); + objects.push_back(std::make_shared(object1)); polygons = createObjectPolygons(objects, 0.0, 1.0); EXPECT_TRUE(polygons.empty()); @@ -434,7 +434,7 @@ TEST(TestCollisionDistance, createObjPolygons) object2.kinematics.initial_twist_with_covariance.twist.linear.x = 2.0; object2.shape.dimensions.x = 2.0; object2.shape.dimensions.y = 1.0; - objects.push_back(PlannerData::Object(object2)); + objects.push_back(std::make_shared(object2)); polygons = createObjectPolygons(objects, 0.0, 2.0); ASSERT_EQ(polygons.size(), 1ul); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp index a6c8368c20571..baf481a56bbff 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -108,7 +108,7 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( autoware_perception_msgs::msg::PredictedObjects filtered_objects; filtered_objects.header = planner_data.predicted_objects_header; for (const auto & object : planner_data.objects) { - const auto & predicted_object = object.predicted_object; + const auto & predicted_object = object->predicted_object; const auto is_pedestrian = std::find_if( predicted_object.classification.begin(), predicted_object.classification.end(), diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index 814af37aaa800..c215eacfa5c7e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -148,22 +148,23 @@ void OutOfLaneModule::update_parameters(const std::vector & p void OutOfLaneModule::limit_trajectory_size( out_of_lane::EgoData & ego_data, - const std::vector & ego_trajectory_points, + const std::vector & smoothed_trajectory_points, const double max_arc_length) { ego_data.first_trajectory_idx = - motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position); + motion_utils::findNearestSegmentIndex(smoothed_trajectory_points, ego_data.pose.position); ego_data.longitudinal_offset_to_first_trajectory_index = motion_utils::calcLongitudinalOffsetToSegment( - ego_trajectory_points, ego_data.first_trajectory_idx, ego_data.pose.position); + smoothed_trajectory_points, ego_data.first_trajectory_idx, ego_data.pose.position); auto l = -ego_data.longitudinal_offset_to_first_trajectory_index; - ego_data.trajectory_points.push_back(ego_trajectory_points[ego_data.first_trajectory_idx]); - for (auto i = ego_data.first_trajectory_idx + 1; i < ego_trajectory_points.size(); ++i) { - l += universe_utils::calcDistance2d(ego_trajectory_points[i - 1], ego_trajectory_points[i]); + ego_data.trajectory_points.push_back(smoothed_trajectory_points[ego_data.first_trajectory_idx]); + for (auto i = ego_data.first_trajectory_idx + 1; i < smoothed_trajectory_points.size(); ++i) { + l += universe_utils::calcDistance2d( + smoothed_trajectory_points[i - 1], smoothed_trajectory_points[i]); if (l >= max_arc_length) { break; } - ego_data.trajectory_points.push_back(ego_trajectory_points[i]); + ego_data.trajectory_points.push_back(smoothed_trajectory_points[i]); } } @@ -213,7 +214,9 @@ out_of_lane::OutOfLaneData prepare_out_of_lane_data(const out_of_lane::EgoData & } VelocityPlanningResult OutOfLaneModule::plan( - const std::vector & ego_trajectory_points, + [[maybe_unused]] const std::vector & + raw_trajectory_points, + const std::vector & smoothed_trajectory_points, const std::shared_ptr planner_data) { VelocityPlanningResult result; @@ -223,7 +226,7 @@ VelocityPlanningResult OutOfLaneModule::plan( stopwatch.tic("preprocessing"); out_of_lane::EgoData ego_data; ego_data.pose = planner_data->current_odometry.pose.pose; - limit_trajectory_size(ego_data, ego_trajectory_points, params_.max_arc_length); + limit_trajectory_size(ego_data, smoothed_trajectory_points, params_.max_arc_length); out_of_lane::calculate_min_stop_and_slowdown_distances( ego_data, *planner_data, previous_slowdown_pose_); prepare_stop_lines_rtree(ego_data, *planner_data, params_.max_arc_length); @@ -314,7 +317,7 @@ VelocityPlanningResult OutOfLaneModule::plan( } planning_factor_interface_->add( - ego_trajectory_points, ego_data.pose, *slowdown_pose, PlanningFactor::SLOW_DOWN, + smoothed_trajectory_points, ego_data.pose, *slowdown_pose, PlanningFactor::SLOW_DOWN, SafetyFactorArray{}); virtual_wall_marker_creator.add_virtual_walls( out_of_lane::debug::create_virtual_walls(*slowdown_pose, slowdown_velocity == 0.0, params_)); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp index b6eb03f5f0469..1e011022ff99e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp @@ -41,7 +41,8 @@ class OutOfLaneModule : public PluginModuleInterface void publish_planning_factor() override { planning_factor_interface_->publish(); }; void update_parameters(const std::vector & parameters) override; VelocityPlanningResult plan( - const std::vector & ego_trajectory_points, + const std::vector & raw_trajectory_points, + const std::vector & smoothed_trajectory_points, const std::shared_ptr planner_data) override; std::string get_module_name() const override { return module_name_; } @@ -51,7 +52,7 @@ class OutOfLaneModule : public PluginModuleInterface /// given length static void limit_trajectory_size( out_of_lane::EgoData & ego_data, - const std::vector & ego_trajectory_points, + const std::vector & smoothed_trajectory_points, const double max_arc_length); out_of_lane::PlannerParam params_{}; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/planner_data.hpp index 4d4917f23496c..54af835171d14 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/planner_data.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/planner_data.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__PLANNER_DATA_HPP_ #include +#include #include #include #include @@ -26,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -43,11 +45,22 @@ namespace autoware::motion_velocity_planner { +using autoware_planning_msgs::msg::TrajectoryPoint; + struct TrafficSignalStamped { builtin_interfaces::msg::Time stamp; autoware_perception_msgs::msg::TrafficLightGroup signal; }; + +struct TrajectoryPolygonCollisionCheck +{ + double decimate_trajectory_step_length; + double goal_extended_trajectory_length; + bool enable_to_consider_current_pose; + double time_to_convergence; +}; + struct PlannerData { explicit PlannerData(rclcpp::Node & node) @@ -65,28 +78,27 @@ struct PlannerData } autoware_perception_msgs::msg::PredictedObject predicted_object; - // double get_lon_vel_relative_to_traj() - // { - // if (!lon_vel_relative_to_traj) { - // lon_vel_relative_to_traj = 0.0; - // } - // return *lon_vel_relative_to_traj; - // } - // double get_lat_vel_relative_to_traj() - // { - // if (!lat_vel_relative_to_traj) { - // lat_vel_relative_to_traj = 0.0; - // } - // return *lat_vel_relative_to_traj; - // } + + double get_dist_to_traj_poly( + const std::vector & decimated_traj_polys) const; + double get_dist_to_traj_lateral(const std::vector & traj_points) const; + double get_dist_from_ego_longitudinal( + const std::vector & traj_points, + const geometry_msgs::msg::Point & ego_pos) const; + double get_lon_vel_relative_to_traj(const std::vector & traj_points) const; + double get_lat_vel_relative_to_traj(const std::vector & traj_points) const; + geometry_msgs::msg::Pose get_predicted_pose( + const rclcpp::Time & current_stamp, const rclcpp::Time & predicted_object_stamp) const; private: - // TODO(murooka) implement the following variables and their functions. - // std::optional dist_to_traj_poly{std::nullopt}; - // std::optional dist_to_traj_lateral{std::nullopt}; - // std::optional dist_from_ego_longitudinal{std::nullopt}; - // std::optional lon_vel_relative_to_traj{std::nullopt}; - // std::optional lat_vel_relative_to_traj{std::nullopt}; + void calc_vel_relative_to_traj(const std::vector & traj_points) const; + + mutable std::optional dist_to_traj_poly{std::nullopt}; + mutable std::optional dist_to_traj_lateral{std::nullopt}; + mutable std::optional dist_from_ego_longitudinal{std::nullopt}; + mutable std::optional lon_vel_relative_to_traj{std::nullopt}; + mutable std::optional lat_vel_relative_to_traj{std::nullopt}; + mutable std::optional predicted_pose; }; struct Pointcloud @@ -105,21 +117,13 @@ struct PlannerData }; void process_predicted_objects( - const autoware_perception_msgs::msg::PredictedObjects & predicted_objects) - { - predicted_objects_header = predicted_objects.header; - - objects.clear(); - for (const auto & predicted_object : predicted_objects.objects) { - objects.push_back(Object(predicted_object)); - } - } + const autoware_perception_msgs::msg::PredictedObjects & predicted_objects); // msgs from callbacks that are used for data-ready nav_msgs::msg::Odometry current_odometry; geometry_msgs::msg::AccelWithCovarianceStamped current_acceleration; std_msgs::msg::Header predicted_objects_header; - std::vector objects; + std::vector> objects; Pointcloud no_ground_pointcloud; nav_msgs::msg::OccupancyGrid occupancy_grid; std::shared_ptr route_handler; @@ -128,6 +132,8 @@ struct PlannerData double ego_nearest_dist_threshold{}; double ego_nearest_yaw_threshold{}; + TrajectoryPolygonCollisionCheck trajectory_polygon_collision_check{}; + // other internal data // traffic_light_id_map_raw is the raw observation, while traffic_light_id_map_keep_last keeps the // last observed infomation for UNKNOWN @@ -139,6 +145,8 @@ struct PlannerData // parameters autoware::vehicle_info_utils::VehicleInfo vehicle_info_; + bool is_driving_forward{true}; + /** *@fn *@brief queries the traffic signal information of given Id. if keep_last_observation = true, @@ -163,6 +171,20 @@ struct PlannerData current_acceleration.accel.accel.linear.x, velocity_smoother_->getMinDecel(), std::abs(velocity_smoother_->getMinJerk()), velocity_smoother_->getMinJerk()); } + + size_t find_index( + const std::vector & traj_points, const geometry_msgs::msg::Pose & pose) const + { + return autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( + traj_points, pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); + } + + size_t find_segment_index( + const std::vector & traj_points, const geometry_msgs::msg::Pose & pose) const + { + return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + traj_points, pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); + } }; } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/plugin_module_interface.hpp index a01b39646e6e2..d8c32f93d7c15 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/plugin_module_interface.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/plugin_module_interface.hpp @@ -42,7 +42,8 @@ class PluginModuleInterface virtual void init(rclcpp::Node & node, const std::string & module_name) = 0; virtual void update_parameters(const std::vector & parameters) = 0; virtual VelocityPlanningResult plan( - const std::vector & ego_trajectory_points, + const std::vector & raw_trajectory_points, + const std::vector & smoothed_trajectory_points, const std::shared_ptr planner_data) = 0; virtual std::string get_module_name() const = 0; virtual void publish_planning_factor() {} diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/polygon_utils.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/polygon_utils.hpp new file mode 100644 index 0000000000000..8f7b0920a4372 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/polygon_utils.hpp @@ -0,0 +1,82 @@ +// 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 AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__POLYGON_UTILS_HPP_ +#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__POLYGON_UTILS_HPP_ + +#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" + +#include + +#include "autoware_perception_msgs/msg/predicted_path.hpp" +#include "autoware_perception_msgs/msg/shape.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" + +#include + +#include +#include + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +namespace polygon_utils +{ +namespace bg = boost::geometry; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; + +using autoware_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::Shape; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +namespace bg = boost::geometry; +using autoware::vehicle_info_utils::VehicleInfo; + +struct PointWithStamp +{ + rclcpp::Time stamp; + geometry_msgs::msg::Point point; +}; + +std::optional> get_collision_point( + const std::vector & traj_points, const std::vector & traj_polygons, + const geometry_msgs::msg::Pose obj_pose, const rclcpp::Time obj_stamp, const Shape & obj_shape, + const double dist_to_bumper); + +std::optional> get_collision_point( + const std::vector & traj_points, const size_t collision_idx, + const std::vector & collision_points, const double dist_to_bumper); + +std::vector get_collision_points( + const std::vector & traj_points, const std::vector & traj_polygons, + const rclcpp::Time & obstacle_stamp, const PredictedPath & predicted_path, const Shape & shape, + const rclcpp::Time & current_time, const bool is_driving_forward, + std::vector & collision_index, const double max_dist = std::numeric_limits::max(), + const double max_prediction_time_for_collision_check = std::numeric_limits::max()); + +std::vector create_one_step_polygons( + const std::vector & traj_points, const VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin, + const bool enable_to_consider_current_pose, const double time_to_convergence, + const double decimate_trajectory_step_length); +} // namespace polygon_utils +} // namespace autoware::motion_velocity_planner + +#endif // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__POLYGON_UTILS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/utils.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/utils.hpp new file mode 100644 index 0000000000000..9f3baf6d0710a --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/utils.hpp @@ -0,0 +1,134 @@ +// 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 AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__UTILS_HPP_ +#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__UTILS_HPP_ + +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" +#include "planner_data.hpp" + +#include + +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner::utils +{ +using autoware::universe_utils::Polygon2d; +using autoware::vehicle_info_utils::VehicleInfo; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::Shape; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using nav_msgs::msg::Odometry; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +std::vector decimate_trajectory_points_from_ego( + const std::vector & traj_points, const geometry_msgs::msg::Pose & current_pose, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold, + const double decimate_trajectory_step_length, const double goal_extended_trajectory_length); + +template +std::optional get_obstacle_from_uuid( + const std::vector & obstacles, const std::string & target_uuid) +{ + const auto itr = std::find_if(obstacles.begin(), obstacles.end(), [&](const auto & obstacle) { + return obstacle.uuid == target_uuid; + }); + + if (itr == obstacles.end()) { + return std::nullopt; + } + return *itr; +} + +std::vector get_target_object_type(rclcpp::Node & node, const std::string & param_prefix); + +double calc_object_possible_max_dist_from_center(const Shape & shape); + +Marker get_object_marker( + const geometry_msgs::msg::Pose & obj_pose, size_t idx, const std::string & ns, const double r, + const double g, const double b); + +template +size_t get_index_with_longitudinal_offset( + const T & points, const double longitudinal_offset, std::optional start_idx) +{ + if (points.empty()) { + throw std::logic_error("points is empty."); + } + + if (start_idx) { + if (/*start_idx.get() < 0 || */ points.size() <= *start_idx) { + throw std::out_of_range("start_idx is out of range."); + } + } else { + if (longitudinal_offset > 0) { + start_idx = 0; + } else { + start_idx = points.size() - 1; + } + } + + double sum_length = 0.0; + if (longitudinal_offset > 0) { + for (size_t i = *start_idx; i < points.size() - 1; ++i) { + const double segment_length = + autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); + sum_length += segment_length; + if (sum_length >= longitudinal_offset) { + const double back_length = sum_length - longitudinal_offset; + const double front_length = segment_length - back_length; + if (front_length < back_length) { + return i; + } else { + return i + 1; + } + } + } + return points.size() - 1; + } + + for (size_t i = *start_idx; 0 < i; --i) { + const double segment_length = + autoware::universe_utils::calcDistance2d(points.at(i - 1), points.at(i)); + sum_length += segment_length; + if (sum_length >= -longitudinal_offset) { + const double back_length = sum_length + longitudinal_offset; + const double front_length = segment_length - back_length; + if (front_length < back_length) { + return i; + } else { + return i - 1; + } + } + } + return 0; +} + +double calc_possible_min_dist_from_obj_to_traj_poly( + const std::shared_ptr object, + const std::vector & traj_points, const VehicleInfo & vehicle_info); +} // namespace autoware::motion_velocity_planner::utils + +#endif // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__UTILS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/velocity_planning_result.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/velocity_planning_result.hpp index d3dfa7a270a16..4faeb698e2bcd 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/velocity_planning_result.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/velocity_planning_result.hpp @@ -18,6 +18,8 @@ #include #include +#include +#include #include #include @@ -42,6 +44,9 @@ struct VelocityPlanningResult { std::vector stop_points{}; std::vector slowdown_intervals{}; + std::optional velocity_limit{std::nullopt}; + std::optional velocity_limit_clear_command{ + std::nullopt}; }; } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/package.xml index 91d569dc3f8cc..8c33cfc862271 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/package.xml @@ -19,6 +19,7 @@ autoware_behavior_velocity_planner_common autoware_internal_debug_msgs autoware_motion_utils + autoware_object_recognition_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/planner_data.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/planner_data.cpp new file mode 100644 index 0000000000000..a83c9d51aab96 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/planner_data.cpp @@ -0,0 +1,189 @@ +// 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 "autoware/motion_velocity_planner_common_universe/planner_data.hpp" + +#include "autoware/motion_velocity_planner_common_universe/polygon_utils.hpp" +#include "autoware/motion_velocity_planner_common_universe/utils.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" + +#include "autoware_perception_msgs/msg/predicted_path.hpp" + +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +using autoware_perception_msgs::msg::PredictedPath; +namespace bg = boost::geometry; + +namespace +{ +std::optional get_predicted_object_pose_from_predicted_path( + const PredictedPath & predicted_path, const rclcpp::Time & obj_stamp, + const rclcpp::Time & current_stamp) +{ + const double rel_time = (current_stamp - obj_stamp).seconds(); + if (rel_time < 0.0) { + return std::nullopt; + } + + const auto pose = + autoware::object_recognition_utils::calcInterpolatedPose(predicted_path, rel_time); + if (!pose) { + return std::nullopt; + } + return pose.get(); +} + +std::optional get_predicted_object_pose_from_predicted_paths( + const std::vector & predicted_paths, const rclcpp::Time & obj_stamp, + const rclcpp::Time & current_stamp) +{ + if (predicted_paths.empty()) { + return std::nullopt; + } + + // Get the most reliable path + const auto predicted_path = std::max_element( + predicted_paths.begin(), predicted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); + + return get_predicted_object_pose_from_predicted_path(*predicted_path, obj_stamp, current_stamp); +} +} // namespace + +double PlannerData::Object::get_dist_to_traj_poly( + const std::vector & decimated_traj_polys) const +{ + if (!dist_to_traj_poly) { + const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; + const auto obj_poly = autoware::universe_utils::toPolygon2d(obj_pose, predicted_object.shape); + dist_to_traj_poly = std::numeric_limits::max(); + for (const auto & traj_poly : decimated_traj_polys) { + const double current_dist_to_traj_poly = bg::distance(traj_poly, obj_poly); + dist_to_traj_poly = std::min(*dist_to_traj_poly, current_dist_to_traj_poly); + } + } + return *dist_to_traj_poly; +} + +double PlannerData::Object::get_dist_to_traj_lateral( + const std::vector & traj_points) const +{ + if (!dist_to_traj_lateral) { + const auto & obj_pos = predicted_object.kinematics.initial_pose_with_covariance.pose.position; + dist_to_traj_lateral = autoware::motion_utils::calcLateralOffset(traj_points, obj_pos); + } + return *dist_to_traj_lateral; +} + +double PlannerData::Object::get_dist_from_ego_longitudinal( + const std::vector & traj_points, const geometry_msgs::msg::Point & ego_pos) const +{ + if (!dist_from_ego_longitudinal) { + const auto & obj_pos = predicted_object.kinematics.initial_pose_with_covariance.pose.position; + dist_from_ego_longitudinal = + autoware::motion_utils::calcSignedArcLength(traj_points, ego_pos, obj_pos); + } + return *dist_from_ego_longitudinal; +} + +double PlannerData::Object::get_lon_vel_relative_to_traj( + const std::vector & traj_points) const +{ + if (!lon_vel_relative_to_traj) { + calc_vel_relative_to_traj(traj_points); + } + return *lon_vel_relative_to_traj; +} + +double PlannerData::Object::get_lat_vel_relative_to_traj( + const std::vector & traj_points) const +{ + if (!lat_vel_relative_to_traj) { + calc_vel_relative_to_traj(traj_points); + } + return *lat_vel_relative_to_traj; +} + +void PlannerData::Object::calc_vel_relative_to_traj( + const std::vector & traj_points) const +{ + const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; + const auto & obj_twist = predicted_object.kinematics.initial_twist_with_covariance.twist; + + const size_t object_idx = + autoware::motion_utils::findNearestIndex(traj_points, obj_pose.position); + const auto & nearest_traj_point = traj_points.at(object_idx); + + const double traj_yaw = tf2::getYaw(nearest_traj_point.pose.orientation); + const double obj_yaw = tf2::getYaw(obj_pose.orientation); + const Eigen::Rotation2Dd R_ego_to_obstacle( + autoware::universe_utils::normalizeRadian(obj_yaw - traj_yaw)); + + // Calculate the trajectory direction and the vector from the trajectory to the obstacle + const Eigen::Vector2d traj_direction(std::cos(traj_yaw), std::sin(traj_yaw)); + const Eigen::Vector2d traj_to_obstacle( + obj_pose.position.x - nearest_traj_point.pose.position.x, + obj_pose.position.y - nearest_traj_point.pose.position.y); + + // Determine if the obstacle is to the left or right of the trajectory using the cross product + const double cross_product = + traj_direction.x() * traj_to_obstacle.y() - traj_direction.y() * traj_to_obstacle.x(); + const int sign = (cross_product > 0) ? -1 : 1; + + const Eigen::Vector2d obstacle_velocity(obj_twist.linear.x, obj_twist.linear.y); + const Eigen::Vector2d projected_velocity = R_ego_to_obstacle * obstacle_velocity; + + lon_vel_relative_to_traj = projected_velocity[0]; + lat_vel_relative_to_traj = sign * projected_velocity[1]; +} + +geometry_msgs::msg::Pose PlannerData::Object::get_predicted_pose( + const rclcpp::Time & current_stamp, const rclcpp::Time & predicted_objects_stamp) const +{ + if (!predicted_pose) { + const auto obj_stamp = predicted_objects_stamp; + const auto predicted_pose_opt = get_predicted_object_pose_from_predicted_paths( + predicted_object.kinematics.predicted_paths, obj_stamp, current_stamp); + + if (predicted_pose_opt) { + predicted_pose = *predicted_pose_opt; + } else { + predicted_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; + RCLCPP_WARN( + rclcpp::get_logger("motion_velocity_planner_common"), + "Failed to calculate the predicted object pose."); + } + } + + return *predicted_pose; +} + +void PlannerData::process_predicted_objects( + const autoware_perception_msgs::msg::PredictedObjects & predicted_objects) +{ + predicted_objects_header = predicted_objects.header; + + objects.clear(); + for (const auto & predicted_object : predicted_objects.objects) { + objects.push_back(std::make_shared(predicted_object)); + } +} +} // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/polygon_utils.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/polygon_utils.cpp new file mode 100644 index 0000000000000..21deaec2c0c02 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/polygon_utils.cpp @@ -0,0 +1,276 @@ +// 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 "autoware/motion_velocity_planner_common_universe/polygon_utils.hpp" + +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner::polygon_utils +{ +namespace +{ +PointWithStamp calc_nearest_collision_point( + const size_t first_within_idx, const std::vector & collision_points, + const std::vector & decimated_traj_points, const bool is_driving_forward) +{ + const size_t prev_idx = first_within_idx == 0 ? first_within_idx : first_within_idx - 1; + const size_t next_idx = first_within_idx == 0 ? first_within_idx + 1 : first_within_idx; + + std::vector segment_points{ + decimated_traj_points.at(prev_idx).pose, decimated_traj_points.at(next_idx).pose}; + if (!is_driving_forward) { + std::reverse(segment_points.begin(), segment_points.end()); + } + + std::vector dist_vec; + for (const auto & collision_point : collision_points) { + const double dist = autoware::motion_utils::calcLongitudinalOffsetToSegment( + segment_points, 0, collision_point.point); + dist_vec.push_back(dist); + } + + const size_t min_idx = std::min_element(dist_vec.begin(), dist_vec.end()) - dist_vec.begin(); + return collision_points.at(min_idx); +} + +// NOTE: max_dist is used for efficient calculation to suppress boost::geometry's polygon +// calculation. +std::optional>> get_collision_index( + const std::vector & traj_points, const std::vector & traj_polygons, + const geometry_msgs::msg::Pose & object_pose, const rclcpp::Time & object_time, + const Shape & object_shape, const double max_dist = std::numeric_limits::max()) +{ + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object_pose, object_shape); + for (size_t i = 0; i < traj_polygons.size(); ++i) { + const double approximated_dist = + autoware::universe_utils::calcDistance2d(traj_points.at(i).pose, object_pose); + if (approximated_dist > max_dist) { + continue; + } + + std::vector collision_polygons; + boost::geometry::intersection(traj_polygons.at(i), obj_polygon, collision_polygons); + + std::vector collision_geom_points; + bool has_collision = false; + for (const auto & collision_polygon : collision_polygons) { + if (boost::geometry::area(collision_polygon) > 0.0) { + has_collision = true; + + for (const auto & collision_point : collision_polygon.outer()) { + PointWithStamp collision_geom_point; + collision_geom_point.stamp = object_time; + collision_geom_point.point.x = collision_point.x(); + collision_geom_point.point.y = collision_point.y(); + collision_geom_point.point.z = traj_points.at(i).pose.position.z; + collision_geom_points.push_back(collision_geom_point); + } + } + } + + if (has_collision) { + const auto collision_info = + std::pair>{i, collision_geom_points}; + return collision_info; + } + } + + return std::nullopt; +} +} // namespace + +std::optional> get_collision_point( + const std::vector & traj_points, const std::vector & traj_polygons, + const geometry_msgs::msg::Pose obj_pose, const rclcpp::Time obj_stamp, const Shape & obj_shape, + const double dist_to_bumper) +{ + const auto collision_info = + get_collision_index(traj_points, traj_polygons, obj_pose, obj_stamp, obj_shape); + if (!collision_info) { + return std::nullopt; + } + + const auto bumper_pose = autoware::universe_utils::calcOffsetPose( + traj_points.at(collision_info->first).pose, dist_to_bumper, 0.0, 0.0); + + std::optional max_collision_length = std::nullopt; + std::optional max_collision_point = std::nullopt; + for (const auto & poly_vertex : collision_info->second) { + const double dist_from_bumper = + std::abs(autoware::universe_utils::inverseTransformPoint(poly_vertex.point, bumper_pose).x); + + if (!max_collision_length.has_value() || dist_from_bumper > *max_collision_length) { + max_collision_length = dist_from_bumper; + max_collision_point = poly_vertex.point; + } + } + return std::make_pair( + *max_collision_point, + autoware::motion_utils::calcSignedArcLength(traj_points, 0, collision_info->first) - + *max_collision_length); +} + +std::optional> get_collision_point( + const std::vector & traj_points, const size_t collision_idx, + const std::vector & collision_points, const double dist_to_bumper) +{ + std::pair> collision_info; + collision_info.first = collision_idx; + collision_info.second = collision_points; + + const auto bumper_pose = autoware::universe_utils::calcOffsetPose( + traj_points.at(collision_info.first).pose, dist_to_bumper, 0.0, 0.0); + + std::optional max_collision_length = std::nullopt; + std::optional max_collision_point = std::nullopt; + for (const auto & poly_vertex : collision_info.second) { + const double dist_from_bumper = + std::abs(autoware::universe_utils::inverseTransformPoint(poly_vertex.point, bumper_pose).x); + + if (!max_collision_length.has_value() || dist_from_bumper > *max_collision_length) { + max_collision_length = dist_from_bumper; + max_collision_point = poly_vertex.point; + } + } + return std::make_pair( + *max_collision_point, + autoware::motion_utils::calcSignedArcLength(traj_points, 0, collision_info.first) - + *max_collision_length); +} + +// NOTE: max_lat_dist is used for efficient calculation to suppress boost::geometry's polygon +// calculation. +std::vector get_collision_points( + const std::vector & traj_points, const std::vector & traj_polygons, + const rclcpp::Time & obstacle_stamp, const PredictedPath & predicted_path, const Shape & shape, + const rclcpp::Time & current_time, const bool is_driving_forward, + std::vector & collision_index, const double max_lat_dist, + const double max_prediction_time_for_collision_check) +{ + std::vector collision_points; + for (size_t i = 0; i < predicted_path.path.size(); ++i) { + if ( + max_prediction_time_for_collision_check < + rclcpp::Duration(predicted_path.time_step).seconds() * static_cast(i)) { + break; + } + + const auto object_time = + rclcpp::Time(obstacle_stamp) + rclcpp::Duration(predicted_path.time_step) * i; + // Ignore past position + if ((object_time - current_time).seconds() < 0.0) { + continue; + } + + const auto collision_info = get_collision_index( + traj_points, traj_polygons, predicted_path.path.at(i), object_time, shape, max_lat_dist); + if (collision_info) { + const auto nearest_collision_point = calc_nearest_collision_point( + collision_info->first, collision_info->second, traj_points, is_driving_forward); + collision_points.push_back(nearest_collision_point); + collision_index.push_back(collision_info->first); + } + } + + return collision_points; +} + +std::vector create_one_step_polygons( + const std::vector & traj_points, const VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin, + const bool enable_to_consider_current_pose, const double time_to_convergence, + const double decimate_trajectory_step_length) +{ + const double front_length = vehicle_info.max_longitudinal_offset_m; + const double rear_length = vehicle_info.rear_overhang_m; + const double vehicle_width = vehicle_info.vehicle_width_m; + + const size_t nearest_idx = + autoware::motion_utils::findNearestSegmentIndex(traj_points, current_ego_pose.position); + const auto nearest_pose = traj_points.at(nearest_idx).pose; + const auto current_ego_pose_error = + autoware::universe_utils::inverseTransformPose(current_ego_pose, nearest_pose); + const double current_ego_lat_error = current_ego_pose_error.position.y; + const double current_ego_yaw_error = tf2::getYaw(current_ego_pose_error.orientation); + double time_elapsed{0.0}; + + std::vector output_polygons; + Polygon2d tmp_polys{}; + for (size_t i = 0; i < traj_points.size(); ++i) { + std::vector current_poses = {traj_points.at(i).pose}; + + // estimate the future ego pose with assuming that the pose error against the reference path + // will decrease to zero by the time_to_convergence + if (enable_to_consider_current_pose && time_elapsed < time_to_convergence) { + const double rem_ratio = (time_to_convergence - time_elapsed) / time_to_convergence; + geometry_msgs::msg::Pose indexed_pose_err; + indexed_pose_err.set__orientation( + autoware::universe_utils::createQuaternionFromYaw(current_ego_yaw_error * rem_ratio)); + indexed_pose_err.set__position( + autoware::universe_utils::createPoint(0.0, current_ego_lat_error * rem_ratio, 0.0)); + current_poses.push_back( + autoware::universe_utils::transformPose(indexed_pose_err, traj_points.at(i).pose)); + if (traj_points.at(i).longitudinal_velocity_mps != 0.0) { + time_elapsed += + decimate_trajectory_step_length / std::abs(traj_points.at(i).longitudinal_velocity_mps); + } else { + time_elapsed = std::numeric_limits::max(); + } + } + + Polygon2d idx_poly{}; + for (const auto & pose : current_poses) { + if (i == 0 && traj_points.at(i).longitudinal_velocity_mps > 1e-3) { + boost::geometry::append( + idx_poly, + autoware::universe_utils::toFootprint(pose, front_length, rear_length, vehicle_width) + .outer()); + boost::geometry::append( + idx_poly, autoware::universe_utils::fromMsg( + autoware::universe_utils::calcOffsetPose( + pose, front_length, vehicle_width * 0.5 + lat_margin, 0.0) + .position) + .to_2d()); + boost::geometry::append( + idx_poly, autoware::universe_utils::fromMsg( + autoware::universe_utils::calcOffsetPose( + pose, front_length, -vehicle_width * 0.5 - lat_margin, 0.0) + .position) + .to_2d()); + } else { + boost::geometry::append( + idx_poly, autoware::universe_utils::toFootprint( + pose, front_length, rear_length, vehicle_width + lat_margin * 2.0) + .outer()); + } + } + + boost::geometry::append(tmp_polys, idx_poly.outer()); + Polygon2d hull_polygon; + boost::geometry::convex_hull(tmp_polys, hull_polygon); + boost::geometry::correct(hull_polygon); + + output_polygons.push_back(hull_polygon); + tmp_polys = std::move(idx_poly); + } + return output_polygons; +} +} // namespace autoware::motion_velocity_planner::polygon_utils diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/utils.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/utils.cpp new file mode 100644 index 0000000000000..de86f64fa7965 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/utils.cpp @@ -0,0 +1,176 @@ +// 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 "autoware/motion_velocity_planner_common_universe/utils.hpp" + +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_velocity_planner_common_universe/planner_data.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner::utils +{ +namespace +{ +TrajectoryPoint extend_trajectory_point( + const double extend_distance, const TrajectoryPoint & goal_point, const bool is_driving_forward) +{ + TrajectoryPoint extended_trajectory_point; + extended_trajectory_point.pose = autoware::universe_utils::calcOffsetPose( + goal_point.pose, extend_distance * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); + extended_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps; + extended_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps; + extended_trajectory_point.acceleration_mps2 = goal_point.acceleration_mps2; + return extended_trajectory_point; +} + +std::vector get_extended_trajectory_points( + const std::vector & input_points, const double extend_distance, + const double step_length) +{ + auto output_points = input_points; + const auto is_driving_forward_opt = + autoware::motion_utils::isDrivingForwardWithTwist(input_points); + const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true; + + if (extend_distance < std::numeric_limits::epsilon()) { + return output_points; + } + + const auto goal_point = input_points.back(); + + double extend_sum = 0.0; + while (extend_sum <= (extend_distance - step_length)) { + const auto extended_trajectory_point = + extend_trajectory_point(extend_sum, goal_point, is_driving_forward); + output_points.push_back(extended_trajectory_point); + extend_sum += step_length; + } + const auto extended_trajectory_point = + extend_trajectory_point(extend_distance, goal_point, is_driving_forward); + output_points.push_back(extended_trajectory_point); + + return output_points; +} + +std::vector resample_trajectory_points( + const std::vector & traj_points, const double interval) +{ + const auto traj = autoware::motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = autoware::motion_utils::resampleTrajectory(traj, interval); + return autoware::motion_utils::convertToTrajectoryPointArray(resampled_traj); +} + +} // namespace + +std::vector decimate_trajectory_points_from_ego( + const std::vector & traj_points, const geometry_msgs::msg::Pose & current_pose, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold, + const double decimate_trajectory_step_length, const double goal_extended_trajectory_length) +{ + // trim trajectory points from ego pose + const size_t traj_ego_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + traj_points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); + const auto traj_points_from_ego = + std::vector(traj_points.begin() + traj_ego_seg_idx, traj_points.end()); + + // decimate trajectory + const auto decimated_traj_points_from_ego = + resample_trajectory_points(traj_points_from_ego, decimate_trajectory_step_length); + + // extend trajectory + const auto extended_traj_points_from_ego = get_extended_trajectory_points( + decimated_traj_points_from_ego, goal_extended_trajectory_length, + decimate_trajectory_step_length); + if (extended_traj_points_from_ego.size() < 2) { + return traj_points; + } + return extended_traj_points_from_ego; +} + +std::vector get_target_object_type(rclcpp::Node & node, const std::string & param_prefix) +{ + std::unordered_map types_map{ + {"unknown", ObjectClassification::UNKNOWN}, {"car", ObjectClassification::CAR}, + {"truck", ObjectClassification::TRUCK}, {"bus", ObjectClassification::BUS}, + {"trailer", ObjectClassification::TRAILER}, {"motorcycle", ObjectClassification::MOTORCYCLE}, + {"bicycle", ObjectClassification::BICYCLE}, {"pedestrian", ObjectClassification::PEDESTRIAN}}; + + std::vector types; + for (const auto & type : types_map) { + if (node.declare_parameter(param_prefix + type.first)) { + types.push_back(type.second); + } + } + return types; +} + +double calc_object_possible_max_dist_from_center(const Shape & shape) +{ + if (shape.type == Shape::BOUNDING_BOX) { + return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); + } else if (shape.type == Shape::CYLINDER) { + return shape.dimensions.x / 2.0; + } else if (shape.type == Shape::POLYGON) { + double max_length_to_point = 0.0; + for (const auto rel_point : shape.footprint.points) { + const double length_to_point = std::hypot(rel_point.x, rel_point.y); + if (max_length_to_point < length_to_point) { + max_length_to_point = length_to_point; + } + } + return max_length_to_point; + } + + throw std::logic_error("The shape type is not supported in motion_velocity_planner_common."); +} +visualization_msgs::msg::Marker get_object_marker( + const geometry_msgs::msg::Pose & obj_pose, size_t idx, const std::string & ns, const double r, + const double g, const double b) +{ + const auto current_time = rclcpp::Clock().now(); + + auto marker = autoware::universe_utils::createDefaultMarker( + "map", current_time, ns, idx, visualization_msgs::msg::Marker::SPHERE, + autoware::universe_utils::createMarkerScale(2.0, 2.0, 2.0), + autoware::universe_utils::createMarkerColor(r, g, b, 0.8)); + + marker.pose = obj_pose; + + return marker; +} + +double calc_possible_min_dist_from_obj_to_traj_poly( + const std::shared_ptr object, + const std::vector & traj_points, const VehicleInfo & vehicle_info) +{ + const double object_possible_max_dist = + calc_object_possible_max_dist_from_center(object->predicted_object.shape); + const double possible_min_dist_to_traj_poly = + std::abs(object->get_dist_to_traj_lateral(traj_points)) - vehicle_info.vehicle_width_m - + object_possible_max_dist; + return possible_min_dist_to_traj_poly; +} +} // namespace autoware::motion_velocity_planner::utils diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/config/motion_velocity_planner.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/config/motion_velocity_planner.param.yaml index 5b2fea537d4f7..4d5409cef23c2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/config/motion_velocity_planner.param.yaml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/config/motion_velocity_planner.param.yaml @@ -1,3 +1,14 @@ /**: ros__parameters: smooth_velocity_before_planning: true # [-] if true, smooth the velocity profile of the input trajectory before planning + + trajectory_polygon_collision_check: + decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking + goal_extended_trajectory_length: 6.0 + + # consider the current ego pose (it is not the nearest pose on the reference trajectory) + # Both the lateral error and the yaw error are assumed to decrease to zero by the time duration "time_to_convergence" + # The both errors decrease with constant rates against the time. + consider_current_pose: + enable_to_consider_current_pose: true + time_to_convergence: 1.5 #[s] diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/schema/motion_velocity_planner.schema.json b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/schema/motion_velocity_planner.schema.json index 7db22e5e39d17..9b5fcf0c91eb1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/schema/motion_velocity_planner.schema.json +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/schema/motion_velocity_planner.schema.json @@ -10,6 +10,38 @@ "type": "boolean", "default": true, "description": "if true, smooth the velocity profile of the input trajectory before planning" + }, + "trajectory_polygon_collision_check": { + "type": "object", + "properties": { + "decimate_trajectory_step_length": { + "type": "number", + "default": 2.0, + "description": "trajectory's step length to decimate for the polygon-based collision check" + }, + "goal_extended_trajectory_length": { + "type": "number", + "default": 6.0, + "description": "trajectory's extended length on the goal for the polygon-based collision check" + }, + "consider_current_pose": { + "type": "object", + "properties": { + "enable_to_consider_current_pose": { + "type": "boolean", + "default": true, + "description": "whether to consider the current pose to create the trajectory polygon for the polygon-based collision check" + }, + "time_to_convergence": { + "type": "number", + "default": 1.5, + "description": "time for the ego to converge to the trajectory for the polygon-based collision check" + } + }, + "required": ["enable_to_consider_current_pose", "time_to_convergence"] + } + }, + "required": ["decimate_trajectory_step_length", "goal_extended_trajectory_length"] } }, "required": ["smooth_velocity_before_planning"], diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.cpp index 8f5aa761573b7..32c7877b1dccd 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.cpp @@ -82,6 +82,10 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & // Publishers trajectory_pub_ = this->create_publisher("~/output/trajectory", 1); + velocity_limit_pub_ = this->create_publisher( + "~/output/velocity_limit", rclcpp::QoS{1}.transient_local()); + clear_velocity_limit_pub_ = this->create_publisher( + "~/output/clear_velocity_limit", rclcpp::QoS{1}.transient_local()); processing_time_publisher_ = this->create_publisher( "~/debug/processing_time_ms", 1); @@ -95,6 +99,17 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & planner_data_.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); planner_data_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); + + planner_data_.trajectory_polygon_collision_check.decimate_trajectory_step_length = + declare_parameter("trajectory_polygon_collision_check.decimate_trajectory_step_length"); + planner_data_.trajectory_polygon_collision_check.goal_extended_trajectory_length = + declare_parameter("trajectory_polygon_collision_check.goal_extended_trajectory_length"); + planner_data_.trajectory_polygon_collision_check.enable_to_consider_current_pose = + declare_parameter( + "trajectory_polygon_collision_check.consider_current_pose.enable_to_consider_current_pose"); + planner_data_.trajectory_polygon_collision_check.time_to_convergence = declare_parameter( + "trajectory_polygon_collision_check.consider_current_pose.time_to_convergence"); + // set velocity smoother param set_velocity_smoother_params(); @@ -131,7 +146,8 @@ void MotionVelocityPlannerNode::on_unload_plugin( // NOTE: argument planner_data must not be referenced for multithreading bool MotionVelocityPlannerNode::update_planner_data( - std::map & processing_times) + std::map & processing_times, + const std::vector & input_traj_points) { auto clock = *get_clock(); auto is_ready = true; @@ -181,6 +197,14 @@ bool MotionVelocityPlannerNode::update_planner_data( planner_data_.velocity_smoother_, "Waiting for the initialization of the velocity smoother"); processing_times["update_planner_data.smoother"] = sw.toc(true); + // is driving forward + const auto is_driving_forward = + autoware::motion_utils::isDrivingForwardWithTwist(input_traj_points); + if (is_driving_forward) { + planner_data_.is_driving_forward = is_driving_forward.value(); + } + processing_times["update_planner_data.is_driving_forward"] = sw.toc(true); + // optional data const auto traffic_signals_ptr = sub_traffic_signals_.takeData(); if (traffic_signals_ptr) process_traffic_signals(traffic_signals_ptr); @@ -269,7 +293,7 @@ void MotionVelocityPlannerNode::on_trajectory( std::map processing_times; stop_watch.tic("Total"); - if (!update_planner_data(processing_times)) { + if (!update_planner_data(processing_times, input_trajectory_msg->points)) { return; } processing_times["update_planner_data"] = stop_watch.toc(true); @@ -383,40 +407,46 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s } autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_trajectory( - autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points, + const autoware::motion_velocity_planner::TrajectoryPoints & input_trajectory_points, std::map & processing_times) { universe_utils::StopWatch stop_watch; autoware_planning_msgs::msg::Trajectory output_trajectory_msg; output_trajectory_msg.points = {input_trajectory_points.begin(), input_trajectory_points.end()}; - if (smooth_velocity_before_planning_) { - stop_watch.tic("smooth"); - input_trajectory_points = smooth_trajectory(input_trajectory_points, planner_data_); - processing_times["velocity_smoothing"] = stop_watch.toc("smooth"); - } + + stop_watch.tic("smooth"); + const auto smoothed_trajectory_points = [&]() { + if (smooth_velocity_before_planning_) { + return smooth_trajectory(input_trajectory_points, planner_data_); + } + return input_trajectory_points; + }(); + processing_times["velocity_smoothing"] = stop_watch.toc("smooth"); + stop_watch.tic("resample"); - TrajectoryPoints resampled_trajectory; + TrajectoryPoints resampled_smoothed_trajectory_points; // skip points that are too close together to make computation easier - if (!input_trajectory_points.empty()) { - resampled_trajectory.push_back(input_trajectory_points.front()); + if (!smoothed_trajectory_points.empty()) { + resampled_smoothed_trajectory_points.push_back(smoothed_trajectory_points.front()); constexpr auto min_interval_squared = 0.5 * 0.5; // TODO(Maxime): change to a parameter - for (auto i = 1UL; i < input_trajectory_points.size(); ++i) { - const auto & p = input_trajectory_points[i]; + for (auto i = 1UL; i < smoothed_trajectory_points.size(); ++i) { + const auto & p = smoothed_trajectory_points[i]; const auto dist_to_prev_point = - universe_utils::calcSquaredDistance2d(resampled_trajectory.back(), p); + universe_utils::calcSquaredDistance2d(resampled_smoothed_trajectory_points.back(), p); if (dist_to_prev_point > min_interval_squared) { - resampled_trajectory.push_back(p); + resampled_smoothed_trajectory_points.push_back(p); } } } processing_times["resample"] = stop_watch.toc("resample"); stop_watch.tic("calculate_time_from_start"); motion_utils::calculate_time_from_start( - resampled_trajectory, planner_data_.current_odometry.pose.pose.position); + resampled_smoothed_trajectory_points, planner_data_.current_odometry.pose.pose.position); processing_times["calculate_time_from_start"] = stop_watch.toc("calculate_time_from_start"); stop_watch.tic("plan_velocities"); const auto planning_results = planner_manager_.plan_velocities( - resampled_trajectory, std::make_shared(planner_data_)); + input_trajectory_points, resampled_smoothed_trajectory_points, + std::make_shared(planner_data_)); processing_times["plan_velocities"] = stop_watch.toc("plan_velocities"); for (const auto & planning_result : planning_results) { @@ -424,6 +454,12 @@ autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_traj insert_stop(output_trajectory_msg, stop_point); for (const auto & slowdown_interval : planning_result.slowdown_intervals) insert_slowdown(output_trajectory_msg, slowdown_interval); + if (planning_result.velocity_limit) { + velocity_limit_pub_->publish(*planning_result.velocity_limit); + } + if (planning_result.velocity_limit_clear_command) { + clear_velocity_limit_pub_->publish(*planning_result.velocity_limit_clear_command); + } } return output_trajectory_msg; @@ -442,6 +478,19 @@ rcl_interfaces::msg::SetParametersResult MotionVelocityPlannerNode::on_set_param updateParam(parameters, "smooth_velocity_before_planning", smooth_velocity_before_planning_); updateParam(parameters, "ego_nearest_dist_threshold", planner_data_.ego_nearest_dist_threshold); updateParam(parameters, "ego_nearest_yaw_threshold", planner_data_.ego_nearest_yaw_threshold); + updateParam( + parameters, "trajectory_polygon_collision_check.decimate_trajectory_step_length", + planner_data_.trajectory_polygon_collision_check.decimate_trajectory_step_length); + updateParam( + parameters, "trajectory_polygon_collision_check.goal_extended_trajectory_length", + planner_data_.trajectory_polygon_collision_check.goal_extended_trajectory_length); + updateParam( + parameters, + "trajectory_polygon_collision_check.consider_current_pose.enable_to_consider_current_pose", + planner_data_.trajectory_polygon_collision_check.enable_to_consider_current_pose); + updateParam( + parameters, "trajectory_polygon_collision_check.consider_current_pose.time_to_convergence", + planner_data_.trajectory_polygon_collision_check.time_to_convergence); // set_velocity_smoother_params(); TODO(Maxime): fix update parameters of the velocity smoother diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.hpp index 826c740d85b76..dee10e94ee64f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.hpp @@ -33,6 +33,8 @@ #include #include #include +#include +#include #include #include @@ -50,6 +52,8 @@ using autoware_map_msgs::msg::LaneletMapBin; using autoware_motion_velocity_planner_node_universe::srv::LoadPlugin; using autoware_motion_velocity_planner_node_universe::srv::UnloadPlugin; using autoware_planning_msgs::msg::Trajectory; +using tier4_planning_msgs::msg::VelocityLimit; +using tier4_planning_msgs::msg::VelocityLimitClearCommand; using TrajectoryPoints = std::vector; class MotionVelocityPlannerNode : public rclcpp::Node @@ -92,6 +96,8 @@ class MotionVelocityPlannerNode : public rclcpp::Node // publishers rclcpp::Publisher::SharedPtr trajectory_pub_; + rclcpp::Publisher::SharedPtr velocity_limit_pub_; + rclcpp::Publisher::SharedPtr clear_velocity_limit_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{ this, "~/debug/processing_time_ms_diag"}; @@ -128,7 +134,9 @@ class MotionVelocityPlannerNode : public rclcpp::Node // function /// @brief update the PlannerData instance with the latest messages received /// @return false if some data is not available - bool update_planner_data(std::map & processing_times); + bool update_planner_data( + std::map & processing_times, + const std::vector & input_traj_points); void insert_stop( autoware_planning_msgs::msg::Trajectory & trajectory, const geometry_msgs::msg::Point & stop_point) const; @@ -139,7 +147,7 @@ class MotionVelocityPlannerNode : public rclcpp::Node const autoware::motion_velocity_planner::TrajectoryPoints & trajectory_points, const autoware::motion_velocity_planner::PlannerData & planner_data) const; autoware_planning_msgs::msg::Trajectory generate_trajectory( - autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points, + const autoware::motion_velocity_planner::TrajectoryPoints & input_trajectory_points, std::map & processing_times); std::unique_ptr logger_configure_; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/planner_manager.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/planner_manager.cpp index 6d889343641a9..d6b7e5760dccb 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/planner_manager.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/planner_manager.cpp @@ -94,12 +94,14 @@ std::shared_ptr MotionVelocityPlannerManager::get_metrics( } std::vector MotionVelocityPlannerManager::plan_velocities( - const std::vector & ego_trajectory_points, + const std::vector & raw_trajectory_points, + const std::vector & smoothed_trajectory_points, const std::shared_ptr planner_data) { std::vector results; for (auto & plugin : loaded_plugins_) { - VelocityPlanningResult res = plugin->plan(ego_trajectory_points, planner_data); + VelocityPlanningResult res = + plugin->plan(raw_trajectory_points, smoothed_trajectory_points, planner_data); results.push_back(res); plugin->publish_planning_factor(); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/planner_manager.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/planner_manager.hpp index 48f6a246c6456..3e1cb429992c6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/planner_manager.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/planner_manager.hpp @@ -49,7 +49,8 @@ class MotionVelocityPlannerManager void unload_module_plugin(rclcpp::Node & node, const std::string & name); void update_module_parameters(const std::vector & parameters); std::vector plan_velocities( - const std::vector & ego_trajectory_points, + const std::vector & raw_trajectory_points, + const std::vector & smoothed_trajectory_points, const std::shared_ptr planner_data); // Metrics