diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml new file mode 100644 index 0000000000000..32af994e49938 --- /dev/null +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -0,0 +1,123 @@ +/**: + ros__parameters: + common: + planning_algorithm: "pid_base" # currently supported algorithm is "pid_base" + + is_showing_debug_info: true + + # longitudinal info + idling_time: 4.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 : 6.0 # This is also used as a stop margin [m] + min_strong_accel: -3.0 # obstacle stop requiring deceleration less than this value will be ignored [m/ss] + + lpf_gain_for_accel: 0.2 # gain of low pass filter for ego acceleration [-] + + nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index + nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index + min_behavior_stop_margin: 3.0 # [m] + + cruise_obstacle_type: + unknown: true + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: false + pedestrian: false + + stop_obstacle_type: + unknown: true + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: true + pedestrian: true + + obstacle_filtering: + rough_detection_area_expand_width : 5.0 # rough lateral margin for rough detection area expansion [m] + detection_area_expand_width : 0.0 # lateral margin for precise detection area expansion + decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking + + # if crossing vehicle is decided as target obstacles or not + crossing_obstacle_velocity_threshold : 3.0 # velocity threshold for crossing obstacle for cruise or stop [m/s] + crossing_obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop + collision_time_margin : 8.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] + + ego_obstacle_overlap_time_threshold : 1.0 # time threshold to decide cut-in obstacle for cruise or stop [s] + max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego + + ignored_outside_obstacle_type: + unknown: false + car: false + truck: false + bus: false + trailer: false + motorcycle: false + bicycle: true + pedestrian: true + + pid_based_planner: + # use_predicted_obstacle_pose: false + + # PID gains to keep safe distance with the front vehicle + kp: 0.6 + ki: 0.0 + kd: 0.5 + + min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss] + + output_ratio_during_accel: 2.5 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + vel_to_acc_weight: 8.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] + + min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s] + + obstacle_velocity_threshold_from_cruise_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] + + optimization_based_planner: + limit_min_accel: -3.0 + resampling_s_interval: 2.0 + dense_resampling_time_interval: 0.1 + sparse_resampling_time_interval: 1.0 + dense_time_horizon: 5.0 + max_time_horizon: 25.0 + max_trajectory_length: 200.0 + velocity_margin: 0.1 #[m/s] + + # Parameters for safe distance + safe_distance_margin: 5.0 + t_dangerous: 0.5 + + # Parameters for collision detection + collision_time_threshold: 10.0 + object_zero_velocity_threshold: 3.0 #[m/s] + object_low_velocity_threshold: 3.0 #[m/s] + external_velocity_limit: 20.0 #[m/s] + delta_yaw_threshold_of_object_and_ego: 90.0 # [deg] + delta_yaw_threshold_of_nearest_index: 60.0 # [deg] + collision_duration_threshold_of_object_and_ego: 1.0 # [s] + + # Switch for each functions + enable_adaptive_cruise: true + use_object_acceleration: false + use_hd_map: true + + # 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 diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py index 89d04bc5b3a00..0aad10cb6812c 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py @@ -21,6 +21,7 @@ from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration from launch.conditions import IfCondition +from launch.conditions import LaunchConfigurationEquals from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer @@ -172,6 +173,52 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + # obstacle cruise planner + obstacle_cruise_planner_param_path = os.path.join( + get_package_share_directory("tier4_planning_launch"), + "config", + "scenario_planning", + "lane_driving", + "motion_planning", + "obstacle_cruise_planner", + "obstacle_cruise_planner.param.yaml", + ) + with open(obstacle_cruise_planner_param_path, "r") as f: + obstacle_cruise_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] + obstacle_cruise_planner_component = ComposableNode( + package="obstacle_cruise_planner", + plugin="motion_planning::ObstacleCruisePlannerNode", + name="obstacle_cruise_planner", + namespace="", + remappings=[ + ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"), + ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/objects", "/perception/object_recognition/objects"), + ("~/output/trajectory", "/planning/scenario_planning/lane_driving/trajectory"), + ("~/output/velocity_limit", "/planning/scenario_planning/max_velocity_candidates"), + ("~/output/clear_velocity_limit", "/planning/scenario_planning/clear_velocity_limit"), + ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"), + ], + parameters=[ + common_param, + obstacle_cruise_planner_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + obstacle_cruise_planner_relay_component = ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="obstacle_cruise_planner_relay", + namespace="", + parameters=[ + {"input_topic": "obstacle_avoidance_planner/trajectory"}, + {"output_topic": "/planning/scenario_planning/lane_driving/trajectory"}, + {"type": "autoware_auto_planning_msgs/msg/Trajectory"}, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + container = ComposableNodeContainer( name="motion_planning_container", namespace="", @@ -179,18 +226,42 @@ def launch_setup(context, *args, **kwargs): executable=LaunchConfiguration("container_executable"), composable_node_descriptions=[ obstacle_avoidance_planner_component, - obstacle_stop_planner_component, ], ) + obstacle_stop_planner_loader = LoadComposableNodes( + composable_node_descriptions=[obstacle_stop_planner_component], + target_container=container, + condition=LaunchConfigurationEquals("cruise_planner", "obstacle_stop_planner"), + ) + + obstacle_cruise_planner_loader = LoadComposableNodes( + composable_node_descriptions=[obstacle_cruise_planner_component], + target_container=container, + condition=LaunchConfigurationEquals("cruise_planner", "obstacle_cruise_planner"), + ) + + obstacle_cruise_planner_relay_loader = LoadComposableNodes( + composable_node_descriptions=[obstacle_cruise_planner_relay_component], + target_container=container, + condition=LaunchConfigurationEquals("cruise_planner", "none"), + ) + surround_obstacle_checker_loader = LoadComposableNodes( composable_node_descriptions=[surround_obstacle_checker_component], target_container=container, condition=IfCondition(LaunchConfiguration("use_surround_obstacle_check")), ) - group = GroupAction([container, surround_obstacle_checker_loader]) - + group = GroupAction( + [ + container, + obstacle_stop_planner_loader, + obstacle_cruise_planner_loader, + obstacle_cruise_planner_relay_loader, + surround_obstacle_checker_loader, + ] + ) return [group] @@ -221,6 +292,9 @@ def add_launch_arg(name: str, default_value=None, description=None): # surround obstacle checker add_launch_arg("use_surround_obstacle_check", "true", "launch surround_obstacle_checker or not") + add_launch_arg( + "cruise_planner", "obstacle_stop_planner", "cruise planner type" + ) # select from "obstacle_stop_planner", "obstacle_cruise_planner", "none" add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") add_launch_arg("use_multithread", "false", "use multithread") diff --git a/planning/obstacle_cruise_planner/CMakeLists.txt b/planning/obstacle_cruise_planner/CMakeLists.txt new file mode 100644 index 0000000000000..7ba8bd9c000ed --- /dev/null +++ b/planning/obstacle_cruise_planner/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.14) +project(obstacle_cruise_planner) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Eigen3 REQUIRED) + +ament_auto_add_library(obstacle_cruise_planner_core SHARED + src/node.cpp + src/utils.cpp + src/polygon_utils.cpp + src/optimization_based_planner/resample.cpp + src/optimization_based_planner/box2d.cpp + src/optimization_based_planner/velocity_optimizer.cpp + src/optimization_based_planner/optimization_based_planner.cpp + src/pid_based_planner/pid_based_planner.cpp +) + +rclcpp_components_register_node(obstacle_cruise_planner_core + PLUGIN "motion_planning::ObstacleCruisePlannerNode" + EXECUTABLE obstacle_cruise_planner +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) + +install(PROGRAMS + scripts/trajectory_visualizer.py + DESTINATION lib/${PROJECT_NAME} +) diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml new file mode 100644 index 0000000000000..32af994e49938 --- /dev/null +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -0,0 +1,123 @@ +/**: + ros__parameters: + common: + planning_algorithm: "pid_base" # currently supported algorithm is "pid_base" + + is_showing_debug_info: true + + # longitudinal info + idling_time: 4.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 : 6.0 # This is also used as a stop margin [m] + min_strong_accel: -3.0 # obstacle stop requiring deceleration less than this value will be ignored [m/ss] + + lpf_gain_for_accel: 0.2 # gain of low pass filter for ego acceleration [-] + + nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index + nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index + min_behavior_stop_margin: 3.0 # [m] + + cruise_obstacle_type: + unknown: true + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: false + pedestrian: false + + stop_obstacle_type: + unknown: true + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: true + pedestrian: true + + obstacle_filtering: + rough_detection_area_expand_width : 5.0 # rough lateral margin for rough detection area expansion [m] + detection_area_expand_width : 0.0 # lateral margin for precise detection area expansion + decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking + + # if crossing vehicle is decided as target obstacles or not + crossing_obstacle_velocity_threshold : 3.0 # velocity threshold for crossing obstacle for cruise or stop [m/s] + crossing_obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop + collision_time_margin : 8.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] + + ego_obstacle_overlap_time_threshold : 1.0 # time threshold to decide cut-in obstacle for cruise or stop [s] + max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego + + ignored_outside_obstacle_type: + unknown: false + car: false + truck: false + bus: false + trailer: false + motorcycle: false + bicycle: true + pedestrian: true + + pid_based_planner: + # use_predicted_obstacle_pose: false + + # PID gains to keep safe distance with the front vehicle + kp: 0.6 + ki: 0.0 + kd: 0.5 + + min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss] + + output_ratio_during_accel: 2.5 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + vel_to_acc_weight: 8.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] + + min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s] + + obstacle_velocity_threshold_from_cruise_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] + + optimization_based_planner: + limit_min_accel: -3.0 + resampling_s_interval: 2.0 + dense_resampling_time_interval: 0.1 + sparse_resampling_time_interval: 1.0 + dense_time_horizon: 5.0 + max_time_horizon: 25.0 + max_trajectory_length: 200.0 + velocity_margin: 0.1 #[m/s] + + # Parameters for safe distance + safe_distance_margin: 5.0 + t_dangerous: 0.5 + + # Parameters for collision detection + collision_time_threshold: 10.0 + object_zero_velocity_threshold: 3.0 #[m/s] + object_low_velocity_threshold: 3.0 #[m/s] + external_velocity_limit: 20.0 #[m/s] + delta_yaw_threshold_of_object_and_ego: 90.0 # [deg] + delta_yaw_threshold_of_nearest_index: 60.0 # [deg] + collision_duration_threshold_of_object_and_ego: 1.0 # [s] + + # Switch for each functions + enable_adaptive_cruise: true + use_object_acceleration: false + use_hd_map: true + + # 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 diff --git a/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_velocity_planner.xml b/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_velocity_planner.xml new file mode 100644 index 0000000000000..634268b93f2de --- /dev/null +++ b/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_velocity_planner.xml @@ -0,0 +1,150 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/obstacle_cruise_planner/image/collision_point.png b/planning/obstacle_cruise_planner/image/collision_point.png new file mode 100644 index 0000000000000..29a4f913891b3 Binary files /dev/null and b/planning/obstacle_cruise_planner/image/collision_point.png differ diff --git a/planning/obstacle_cruise_planner/image/detection_area.png b/planning/obstacle_cruise_planner/image/detection_area.png new file mode 100644 index 0000000000000..6e1a831979fc0 Binary files /dev/null and b/planning/obstacle_cruise_planner/image/detection_area.png differ diff --git a/planning/obstacle_cruise_planner/image/obstacle_to_cruise.png b/planning/obstacle_cruise_planner/image/obstacle_to_cruise.png new file mode 100644 index 0000000000000..c5f56f59a6d7b Binary files /dev/null and b/planning/obstacle_cruise_planner/image/obstacle_to_cruise.png differ diff --git a/planning/obstacle_cruise_planner/image/obstacle_to_stop.png b/planning/obstacle_cruise_planner/image/obstacle_to_stop.png new file mode 100644 index 0000000000000..aee61797a6d6c Binary files /dev/null and b/planning/obstacle_cruise_planner/image/obstacle_to_stop.png differ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp new file mode 100644 index 0000000000000..675b708b3914b --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -0,0 +1,120 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ +#define OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ + +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include + +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include + +#include + +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_auto_perception_msgs::msg::Shape; + +struct TargetObstacle +{ + TargetObstacle( + const rclcpp::Time & arg_time_stamp, const PredictedObject & object, + const double aligned_velocity, const geometry_msgs::msg::Point & arg_collision_point) + { + time_stamp = arg_time_stamp; + orientation_reliable = true; + pose = object.kinematics.initial_pose_with_covariance.pose; + velocity_reliable = true; + velocity = aligned_velocity; + is_classified = true; + classification = object.classification.at(0); + shape = object.shape; + + predicted_paths.clear(); + for (const auto & path : object.kinematics.predicted_paths) { + predicted_paths.push_back(path); + } + + collision_point = arg_collision_point; + } + + rclcpp::Time time_stamp; + bool orientation_reliable; + geometry_msgs::msg::Pose pose; + bool velocity_reliable; + float velocity; + bool is_classified; + ObjectClassification classification; + Shape shape; + std::vector predicted_paths; + geometry_msgs::msg::Point collision_point; +}; + +struct ObstacleCruisePlannerData +{ + rclcpp::Time current_time; + autoware_auto_planning_msgs::msg::Trajectory traj; + geometry_msgs::msg::Pose current_pose; + double current_vel; + double current_acc; + std::vector target_obstacles; +}; + +struct LongitudinalInfo +{ + LongitudinalInfo( + const double arg_max_accel, const double arg_min_accel, const double arg_max_jerk, + const double arg_min_jerk, const double arg_min_strong_accel, const double arg_idling_time, + const double arg_min_ego_accel_for_rss, const double arg_min_object_accel_for_rss, + const double arg_safe_distance_margin) + : max_accel(arg_max_accel), + min_accel(arg_min_accel), + max_jerk(arg_max_jerk), + min_jerk(arg_min_jerk), + min_strong_accel(arg_min_strong_accel), + idling_time(arg_idling_time), + min_ego_accel_for_rss(arg_min_ego_accel_for_rss), + min_object_accel_for_rss(arg_min_object_accel_for_rss), + safe_distance_margin(arg_safe_distance_margin) + { + } + double max_accel; + double min_accel; + double max_jerk; + double min_jerk; + double min_strong_accel; + double idling_time; + double min_ego_accel_for_rss; + double min_object_accel_for_rss; + double safe_distance_margin; +}; + +struct DebugData +{ + std::vector intentionally_ignored_obstacles; + std::vector obstacles_to_stop; + std::vector obstacles_to_cruise; + visualization_msgs::msg::MarkerArray stop_wall_marker; + visualization_msgs::msg::MarkerArray cruise_wall_marker; + std::vector detection_polygons; + std::vector collision_points; +}; + +#endif // OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp new file mode 100644 index 0000000000000..3b9db69162d21 --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -0,0 +1,165 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_CRUISE_PLANNER__NODE_HPP_ +#define OBSTACLE_CRUISE_PLANNER__NODE_HPP_ + +#include "obstacle_cruise_planner/common_structs.hpp" +#include "obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp" +#include "obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" +#include "signal_processing/lowpass_filter_1d.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" + +#include + +#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tier4_debug_msgs/msg/float32_stamped.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 + +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using nav_msgs::msg::Odometry; +using tier4_debug_msgs::msg::Float32Stamped; +using tier4_planning_msgs::msg::StopReasonArray; +using tier4_planning_msgs::msg::VelocityLimit; +using tier4_planning_msgs::msg::VelocityLimitClearCommand; +using vehicle_info_util::VehicleInfo; + +namespace motion_planning +{ +class ObstacleCruisePlannerNode : public rclcpp::Node +{ +public: + explicit ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options); + +private: + // callback functions + rcl_interfaces::msg::SetParametersResult onParam( + const std::vector & parameters); + void onObjects(const PredictedObjects::ConstSharedPtr msg); + void onOdometry(const Odometry::ConstSharedPtr); + void onTrajectory(const Trajectory::ConstSharedPtr msg); + void onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg); + + // member Functions + ObstacleCruisePlannerData createPlannerData( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose, + DebugData & debug_data); + double calcCurrentAccel() const; + std::vector filterObstacles( + const PredictedObjects & predicted_objects, const Trajectory & traj, + const geometry_msgs::msg::Pose & current_pose, const double current_vel, + DebugData & debug_data); + geometry_msgs::msg::Point calcNearestCollisionPoint( + const size_t & first_within_idx, + const std::vector & collision_points, + const Trajectory & decimated_traj); + double calcCollisionTimeMargin( + const geometry_msgs::msg::Pose & current_pose, const double current_vel, + const geometry_msgs::msg::Point & nearest_collision_point, + const PredictedObject & predicted_object, const size_t first_within_idx, + const Trajectory & decimated_traj, + const std::vector & decimated_traj_polygons); + void publishVelocityLimit(const boost::optional & vel_limit); + void publishDebugData(const DebugData & debug_data) const; + void publishCalculationTime(const double calculation_time) const; + + bool is_showing_debug_info_; + double min_behavior_stop_margin_; + double nearest_dist_deviation_threshold_; + double nearest_yaw_deviation_threshold_; + + // parameter callback result + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + // publisher + rclcpp::Publisher::SharedPtr trajectory_pub_; + rclcpp::Publisher::SharedPtr vel_limit_pub_; + rclcpp::Publisher::SharedPtr clear_vel_limit_pub_; + rclcpp::Publisher::SharedPtr debug_marker_pub_; + rclcpp::Publisher::SharedPtr debug_cruise_wall_marker_pub_; + rclcpp::Publisher::SharedPtr debug_stop_wall_marker_pub_; + rclcpp::Publisher::SharedPtr debug_calculation_time_pub_; + + // subscriber + rclcpp::Subscription::SharedPtr trajectory_sub_; + rclcpp::Subscription::SharedPtr smoothed_trajectory_sub_; + rclcpp::Subscription::SharedPtr objects_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + + // self pose listener + tier4_autoware_utils::SelfPoseListener self_pose_listener_; + + // data for callback functions + PredictedObjects::ConstSharedPtr in_objects_ptr_; + geometry_msgs::msg::TwistStamped::SharedPtr current_twist_ptr_; + geometry_msgs::msg::TwistStamped::SharedPtr prev_twist_ptr_; + + // low pass filter of acceleration + std::shared_ptr lpf_acc_ptr_; + + // Vehicle Parameters + VehicleInfo vehicle_info_; + + // planning algorithm + enum class PlanningAlgorithm { OPTIMIZATION_BASE, PID_BASE, INVALID }; + PlanningAlgorithm getPlanningAlgorithmType(const std::string & param) const; + PlanningAlgorithm planning_algorithm_; + + // stop watch + mutable tier4_autoware_utils::StopWatch< + std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> + stop_watch_; + + // planner + std::unique_ptr planner_ptr_; + + // obstacle filtering parameter + struct ObstacleFilteringParam + { + double rough_detection_area_expand_width; + double detection_area_expand_width; + double decimate_trajectory_step_length; + double crossing_obstacle_velocity_threshold; + double collision_time_margin; + double ego_obstacle_overlap_time_threshold; + double max_prediction_time_for_collision_check; + double crossing_obstacle_traj_angle_threshold; + std::vector ignored_outside_obstacle_types; + }; + ObstacleFilteringParam obstacle_filtering_param_; + + bool need_to_clear_vel_limit_{false}; +}; +} // namespace motion_planning + +#endif // OBSTACLE_CRUISE_PLANNER__NODE_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/box2d.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/box2d.hpp new file mode 100644 index 0000000000000..84349eca57dec --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/box2d.hpp @@ -0,0 +1,135 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__BOX2D_HPP_ +#define OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__BOX2D_HPP_ + +#include +#include + +#include +#include +#include + +class Box2d +{ +public: + Box2d(const geometry_msgs::msg::Pose & center_pose, const double length, const double width); + + bool hasOverlap(const Box2d & box) const; + + void initCorners(); + + /** + * @brief Getter of the center of the box + * @return The center of the box + */ + const geometry_msgs::msg::Point & center() const { return center_; } + + /** + * @brief Getter of the x-coordinate of the center of the box + * @return The x-coordinate of the center of the box + */ + double getCenterX() const { return center_.x; } + + /** + * @brief Getter of the y-coordinate of the center of the box + * @return The y-coordinate of the center of the box + */ + double getCenterY() const { return center_.y; } + + /** + * @brief Getter of the length + * @return The length of the heading-axis + */ + double length() const { return length_; } + + /** + * @brief Getter of the width + * @return The width of the box taken perpendicularly to the heading + */ + double width() const { return width_; } + + /** + * @brief Getter of half the length + * @return Half the length of the heading-axis + */ + double getHalfLength() const { return half_length_; } + + /** + * @brief Getter of half the width + * @return Half the width of the box taken perpendicularly to the heading + */ + double getHalfWidth() const { return half_width_; } + + /** + * @brief Getter of the heading + * @return The counter-clockwise angle between the x-axis and the heading-axis + */ + double heading() const { return heading_; } + + /** + * @brief Getter of the cosine of the heading + * @return The cosine of the heading + */ + double getCosHeading() const { return cos_heading_; } + + /** + * @brief Getter of the sine of the heading + * @return The sine of the heading + */ + double getSinHeading() const { return sin_heading_; } + + /** + * @brief Getter of the area of the box + * @return The product of its length and width + */ + double area() const { return length_ * width_; } + + /** + * @brief Getter of the size of the diagonal of the box + * @return The diagonal size of the box + */ + double diagonal() const { return std::hypot(length_, width_); } + + /** + * @brief Getter of the corners of the box + * @param corners The vector where the corners are listed + */ + std::vector getAllCorners() const { return corners_; } + + double getMaxX() const { return max_x_; } + double getMinX() const { return min_x_; } + double getMaxY() const { return max_y_; } + double getMinY() const { return min_y_; } + +private: + geometry_msgs::msg::Point center_; + double length_ = 0.0; + double width_ = 0.0; + double half_length_ = 0.0; + double half_width_ = 0.0; + double heading_ = 0.0; + double cos_heading_ = 1.0; + double sin_heading_ = 0.0; + + std::vector corners_; + + double max_x_ = std::numeric_limits::lowest(); + double min_x_ = std::numeric_limits::max(); + double max_y_ = std::numeric_limits::lowest(); + double min_y_ = std::numeric_limits::max(); +}; + +#endif // OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__BOX2D_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp new file mode 100644 index 0000000000000..c8d6b294afd59 --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp @@ -0,0 +1,189 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_ +#define OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_ + +#include "obstacle_cruise_planner/optimization_based_planner/box2d.hpp" +#include "obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp" +#include "obstacle_cruise_planner/optimization_based_planner/st_point.hpp" +#include "obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp" +#include "obstacle_cruise_planner/planner_interface.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include + +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using tier4_debug_msgs::msg::Float32Stamped; + +class OptimizationBasedPlanner : public PlannerInterface +{ +public: + OptimizationBasedPlanner( + rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, + const vehicle_info_util::VehicleInfo & vehicle_info); + + Trajectory generateTrajectory( + const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, + DebugData & debug_data) override; + +private: + struct TrajectoryData + { + TrajectoryData() {} + + Trajectory traj; + std::vector s; + }; + + struct ObjectData + { + geometry_msgs::msg::Pose pose; + double length; + double width; + double time; + }; + + // Member Functions + std::vector createTimeVector(); + + double getClosestStopDistance( + const ObstacleCruisePlannerData & planner_data, const TrajectoryData & ego_traj_data, + const std::vector & resolutions); + + std::tuple calcInitialMotion( + const double current_vel, const Trajectory & input_traj, const size_t input_closest, + const Trajectory & prev_traj, const double closest_stop_dist); + + TrajectoryPoint calcInterpolatedTrajectoryPoint( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & target_pose); + + bool checkHasReachedGoal(const Trajectory & traj, const size_t closest_idx, const double v0); + + TrajectoryData getTrajectoryData( + const Trajectory & traj, const geometry_msgs::msg::Pose & current_pose); + + TrajectoryData resampleTrajectoryData( + const TrajectoryData & base_traj_data, const double resampling_s_interval, + const double max_traj_length, const double stop_dist); + + Trajectory resampleTrajectory( + const std::vector & base_index, const Trajectory & base_trajectory, + const std::vector & query_index, const bool use_spline_for_pose = false); + + boost::optional getSBoundaries( + const ObstacleCruisePlannerData & planner_data, const TrajectoryData & ego_traj_data, + const std::vector & time_vec); + + boost::optional getSBoundaries( + const rclcpp::Time & current_time, const TrajectoryData & ego_traj_data, + const TargetObstacle & object, const rclcpp::Time & obj_base_time, + const std::vector & time_vec); + + boost::optional getSBoundaries( + const TrajectoryData & ego_traj_data, const std::vector & time_vec, + const double safety_distance, const TargetObstacle & object, + const double dist_to_collision_point); + + boost::optional getSBoundaries( + const rclcpp::Time & current_time, const TrajectoryData & ego_traj_data, + const std::vector & time_vec, const double safety_distance, + const TargetObstacle & object, const rclcpp::Time & obj_base_time, + const PredictedPath & predicted_path); + + bool checkIsFrontObject(const TargetObstacle & object, const Trajectory & traj); + + boost::optional resampledPredictedPath( + const TargetObstacle & object, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time, const std::vector & resolutions, + const double horizon); + + boost::optional getDistanceToCollisionPoint( + const TrajectoryData & ego_traj_data, const ObjectData & obj_data, + const double delta_yaw_threshold); + + boost::optional getCollisionIdx( + const TrajectoryData & ego_traj, const Box2d & obj_box, const size_t start_idx, + const size_t end_idx); + + geometry_msgs::msg::Pose transformBaseLink2Center( + const geometry_msgs::msg::Pose & pose_base_link); + + boost::optional processOptimizedResult( + const double v0, const VelocityOptimizer::OptimizationResult & opt_result); + + void publishDebugTrajectory( + const rclcpp::Time & current_time, const Trajectory & traj, const size_t closest_idx, + const std::vector & time_vec, const SBoundaries & s_boundaries, + const VelocityOptimizer::OptimizationResult & opt_result); + + // Calculation time watcher + tier4_autoware_utils::StopWatch stop_watch_; + + Trajectory 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 distance_to_closest_obj_pub_; + rclcpp::Publisher::SharedPtr debug_calculation_time_; + rclcpp::Publisher::SharedPtr debug_wall_marker_pub_; + + // Resampling Parameter + double resampling_s_interval_; + double max_trajectory_length_; + double dense_resampling_time_interval_; + double sparse_resampling_time_interval_; + double dense_time_horizon_; + double max_time_horizon_; + double limit_min_accel_; + + double delta_yaw_threshold_of_nearest_index_; + double delta_yaw_threshold_of_object_and_ego_; + double object_zero_velocity_threshold_; + double object_low_velocity_threshold_; + double external_velocity_limit_; + double collision_time_threshold_; + double safe_distance_margin_; + double t_dangerous_; + double velocity_margin_; + bool enable_adaptive_cruise_; + bool use_object_acceleration_; + + double replan_vel_deviation_; + double engage_velocity_; + double engage_acceleration_; + double engage_exit_ratio_; + double stop_dist_to_prohibit_engage_; +}; + +#endif // OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/resample.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/resample.hpp new file mode 100644 index 0000000000000..dffe7f181fec5 --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/resample.hpp @@ -0,0 +1,50 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__RESAMPLE_HPP_ +#define OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__RESAMPLE_HPP_ + +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include "autoware_auto_perception_msgs/msg/predicted_path.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" + +#include "boost/optional.hpp" + +#include + +namespace resampling +{ +std::vector resampledValidRelativeTimeVector( + const rclcpp::Time & start_time, const rclcpp::Time & obj_base_time, + const std::vector & rel_time_vec, const double duration); + +autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_auto_perception_msgs::msg::PredictedPath & input_path, + const std::vector & rel_time_vec); + +geometry_msgs::msg::Pose lerpByPose( + const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t); + +boost::optional lerpByTimeStamp( + const autoware_auto_perception_msgs::msg::PredictedPath & path, + const rclcpp::Duration & rel_time); + +autoware_auto_planning_msgs::msg::Trajectory applyLinearInterpolation( + const std::vector & base_index, + const autoware_auto_planning_msgs::msg::Trajectory & base_trajectory, + const std::vector & out_index, const bool use_spline_for_pose = false); +} // namespace resampling + +#endif // OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__RESAMPLE_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp new file mode 100644 index 0000000000000..1665434ad5969 --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp @@ -0,0 +1,33 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__S_BOUNDARY_HPP_ +#define OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__S_BOUNDARY_HPP_ + +#include +#include + +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; + +#endif // OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__S_BOUNDARY_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/st_point.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/st_point.hpp new file mode 100644 index 0000000000000..8ef958c741414 --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/st_point.hpp @@ -0,0 +1,31 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__ST_POINT_HPP_ +#define OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__ST_POINT_HPP_ + +#include + +class STPoint +{ +public: + STPoint(const double _s, const double _t) : s(_s), t(_t) {} + STPoint() : s(0.0), t(0.0) {} + + double s; + double t; +}; + +using STPoints = std::vector; + +#endif // OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__ST_POINT_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp new file mode 100644 index 0000000000000..cb7d1baab79b8 --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp @@ -0,0 +1,73 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ +#define OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ + +#include "obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp" +#include "osqp_interface/osqp_interface.hpp" + +#include + +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_min; + double j_max; + double j_min; + double t_dangerous; + double idling_time; + double v_margin; + 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::common::osqp::OSQPInterface qp_solver_; +}; + +#endif // OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/debug_values.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/debug_values.hpp new file mode 100644 index 0000000000000..ae8a909d3bb51 --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/debug_values.hpp @@ -0,0 +1,79 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__DEBUG_VALUES_HPP_ +#define OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__DEBUG_VALUES_HPP_ + +#include + +class DebugValues +{ +public: + enum class TYPE { + // current + CURRENT_VELOCITY = 0, + CURRENT_ACCELERATION, + CURRENT_JERK, // ignored + // stop + STOP_CURRENT_OBJECT_DISTANCE = 3, + STOP_CURRENT_OBJECT_VELOCITY, + STOP_TARGET_OBJECT_DISTANCE, + STOP_TARGET_VELOCITY, // ignored + STOP_TARGET_ACCELERATION, + STOP_TARGET_JERK, // ignored + STOP_ERROR_OBJECT_DISTANCE, + // cruise + CRUISE_CURRENT_OBJECT_DISTANCE = 10, + CRUISE_CURRENT_OBJECT_VELOCITY, + CRUISE_TARGET_OBJECT_DISTANCE, + CRUISE_TARGET_VELOCITY, + CRUISE_TARGET_ACCELERATION, + CRUISE_TARGET_JERK, + CRUISE_ERROR_OBJECT_DISTANCE, + + 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 + */ + int getValuesIdx(const TYPE type) const { return static_cast(type); } + /** + * @brief get all the debug values as an std::array + * @return array of all debug values + */ + std::array(TYPE::SIZE)> getValues() const { return values_; } + /** + * @brief set the given type to the given value + * @param [in] type TYPE of the value + * @param [in] value value to set + */ + void setValues(const TYPE type, const double val) { values_.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 setValues(const int type, const double val) { values_.at(type) = val; } + + void resetValues() { values_.fill(0.0); } + +private: + static constexpr int num_debug_values_ = static_cast(TYPE::SIZE); + std::array(TYPE::SIZE)> values_; +}; + +#endif // OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__DEBUG_VALUES_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp new file mode 100644 index 0000000000000..7697fd54b1d1a --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp @@ -0,0 +1,138 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ +#define OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ + +#include "obstacle_cruise_planner/pid_based_planner/debug_values.hpp" +#include "obstacle_cruise_planner/pid_based_planner/pid_controller.hpp" +#include "obstacle_cruise_planner/planner_interface.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" + +#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" +#include "tier4_planning_msgs/msg/stop_reason_array.hpp" +#include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include + +#include +#include + +using tier4_debug_msgs::msg::Float32MultiArrayStamped; +using tier4_planning_msgs::msg::StopSpeedExceeded; + +class PIDBasedPlanner : public PlannerInterface +{ +public: + struct CruiseObstacleInfo + { + CruiseObstacleInfo( + const TargetObstacle & obstacle_arg, const double dist_to_cruise_arg, + const double normalized_dist_to_cruise_arg) + : obstacle(obstacle_arg), + dist_to_cruise(dist_to_cruise_arg), + normalized_dist_to_cruise(normalized_dist_to_cruise_arg) + { + } + TargetObstacle obstacle; + double dist_to_cruise; + double normalized_dist_to_cruise; + }; + + struct StopObstacleInfo + { + StopObstacleInfo(const TargetObstacle & obstacle_arg, const double dist_to_stop_arg) + : obstacle(obstacle_arg), dist_to_stop(dist_to_stop_arg) + { + } + TargetObstacle obstacle; + double dist_to_stop; + }; + + PIDBasedPlanner( + rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, + const vehicle_info_util::VehicleInfo & vehicle_info); + + Trajectory generateTrajectory( + const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, + DebugData & debug_data) override; + + void updateParam(const std::vector & parameters) override; + +private: + void calcObstaclesToCruiseAndStop( + const ObstacleCruisePlannerData & planner_data, + boost::optional & stop_obstacle_info, + boost::optional & cruise_obstacle_info); + double calcDistanceToObstacle( + const ObstacleCruisePlannerData & planner_data, const TargetObstacle & obstacle); + bool isStopRequired(const TargetObstacle & obstacle); + + void planCruise( + const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, + const boost::optional & cruise_obstacle_info, DebugData & debug_data); + VelocityLimit doCruise( + const ObstacleCruisePlannerData & planner_data, const CruiseObstacleInfo & cruise_obstacle_info, + std::vector & debug_obstacles_to_cruise, + visualization_msgs::msg::MarkerArray & debug_walls_marker); + + Trajectory planStop( + const ObstacleCruisePlannerData & planner_data, + const boost::optional & stop_obstacle_info, DebugData & debug_data); + boost::optional doStop( + const ObstacleCruisePlannerData & planner_data, const StopObstacleInfo & stop_obstacle_info, + std::vector & debug_obstacles_to_stop, + visualization_msgs::msg::MarkerArray & debug_walls_marker) const; + + void publishDebugValues(const ObstacleCruisePlannerData & planner_data) const; + + size_t findExtendedNearestIndex( + const Trajectory traj, const geometry_msgs::msg::Pose & pose) const + { + const auto nearest_idx = tier4_autoware_utils::findNearestIndex( + traj.points, pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); + if (nearest_idx) { + return nearest_idx.get(); + } + return tier4_autoware_utils::findNearestIndex(traj.points, pose.position); + } + + // ROS parameters + double min_accel_during_cruise_; + double vel_to_acc_weight_; + double min_cruise_target_vel_; + double obstacle_velocity_threshold_from_cruise_to_stop_; + // bool use_predicted_obstacle_pose_; + + // pid controller + std::unique_ptr pid_controller_; + double output_ratio_during_accel_; + + // stop watch + tier4_autoware_utils::StopWatch< + std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> + stop_watch_; + + // publisher + rclcpp::Publisher::SharedPtr stop_reasons_pub_; + rclcpp::Publisher::SharedPtr stop_speed_exceeded_pub_; + rclcpp::Publisher::SharedPtr debug_values_pub_; + + boost::optional prev_target_vel_; + + DebugValues debug_values_; +}; + +#endif // OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_controller.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_controller.hpp new file mode 100644 index 0000000000000..593fd1f4336d6 --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_controller.hpp @@ -0,0 +1,62 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_CONTROLLER_HPP_ +#define OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_CONTROLLER_HPP_ + +#include + +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_.get()) : 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_; + boost::optional prev_error_; +}; + +#endif // OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_CONTROLLER_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp new file mode 100644 index 0000000000000..af31e1d979e4b --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -0,0 +1,192 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ +#define OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ + +#include "obstacle_cruise_planner/common_structs.hpp" +#include "obstacle_cruise_planner/utils.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "tier4_planning_msgs/msg/velocity_limit.hpp" + +#include + +#include +#include + +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_planning_msgs::msg::Trajectory; +using tier4_planning_msgs::msg::VelocityLimit; + +class PlannerInterface +{ +public: + PlannerInterface( + rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, + const vehicle_info_util::VehicleInfo & vehicle_info) + : longitudinal_info_(longitudinal_info), vehicle_info_(vehicle_info) + { + { // cruise obstacle type + if (node.declare_parameter("common.cruise_obstacle_type.unknown")) { + cruise_obstacle_types_.push_back(ObjectClassification::UNKNOWN); + } + if (node.declare_parameter("common.cruise_obstacle_type.car")) { + cruise_obstacle_types_.push_back(ObjectClassification::CAR); + } + if (node.declare_parameter("common.cruise_obstacle_type.truck")) { + cruise_obstacle_types_.push_back(ObjectClassification::TRUCK); + } + if (node.declare_parameter("common.cruise_obstacle_type.bus")) { + cruise_obstacle_types_.push_back(ObjectClassification::BUS); + } + if (node.declare_parameter("common.cruise_obstacle_type.trailer")) { + cruise_obstacle_types_.push_back(ObjectClassification::TRAILER); + } + if (node.declare_parameter("common.cruise_obstacle_type.motorcycle")) { + cruise_obstacle_types_.push_back(ObjectClassification::MOTORCYCLE); + } + if (node.declare_parameter("common.cruise_obstacle_type.bicycle")) { + cruise_obstacle_types_.push_back(ObjectClassification::BICYCLE); + } + if (node.declare_parameter("common.cruise_obstacle_type.pedestrian")) { + cruise_obstacle_types_.push_back(ObjectClassification::PEDESTRIAN); + } + } + + { // stop obstacle type + if (node.declare_parameter("common.stop_obstacle_type.unknown")) { + stop_obstacle_types_.push_back(ObjectClassification::UNKNOWN); + } + if (node.declare_parameter("common.stop_obstacle_type.car")) { + stop_obstacle_types_.push_back(ObjectClassification::CAR); + } + if (node.declare_parameter("common.stop_obstacle_type.truck")) { + stop_obstacle_types_.push_back(ObjectClassification::TRUCK); + } + if (node.declare_parameter("common.stop_obstacle_type.bus")) { + stop_obstacle_types_.push_back(ObjectClassification::BUS); + } + if (node.declare_parameter("common.stop_obstacle_type.trailer")) { + stop_obstacle_types_.push_back(ObjectClassification::TRAILER); + } + if (node.declare_parameter("common.stop_obstacle_type.motorcycle")) { + stop_obstacle_types_.push_back(ObjectClassification::MOTORCYCLE); + } + if (node.declare_parameter("common.stop_obstacle_type.bicycle")) { + stop_obstacle_types_.push_back(ObjectClassification::BICYCLE); + } + if (node.declare_parameter("common.stop_obstacle_type.pedestrian")) { + stop_obstacle_types_.push_back(ObjectClassification::PEDESTRIAN); + } + } + } + + PlannerInterface() = default; + + void setParams( + const bool is_showing_debug_info, const double min_behavior_stop_margin, + const double nearest_dist_deviation_threshold, const double nearest_yaw_deviation_threshold) + { + is_showing_debug_info_ = is_showing_debug_info; + min_behavior_stop_margin_ = min_behavior_stop_margin; + nearest_dist_deviation_threshold_ = nearest_dist_deviation_threshold; + nearest_yaw_deviation_threshold_ = nearest_yaw_deviation_threshold; + } + + /* + // two kinds of velocity planning is supported. + // 1. getZeroVelocityIndexWithVelocityLimit + // returns zero velocity index and velocity limit + // 2. generateTrajectory + // returns trajectory with planned velocity + virtual boost::optional getZeroVelocityIndexWithVelocityLimit( + [[maybe_unused]] const ObstacleCruisePlannerData & planner_data, + [[maybe_unused]] boost::optional & vel_limit) + { + return {}; + }; + */ + + virtual Trajectory generateTrajectory( + const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, + DebugData & debug_data) = 0; + + void updateCommonParam(const std::vector & parameters) + { + auto & i = longitudinal_info_; + + tier4_autoware_utils::updateParam(parameters, "common.max_accel", i.max_accel); + tier4_autoware_utils::updateParam(parameters, "common.min_accel", i.min_accel); + tier4_autoware_utils::updateParam(parameters, "common.max_jerk", i.max_jerk); + tier4_autoware_utils::updateParam(parameters, "common.min_jerk", i.min_jerk); + tier4_autoware_utils::updateParam( + parameters, "common.min_ego_accel_for_rss", i.min_ego_accel_for_rss); + tier4_autoware_utils::updateParam( + parameters, "common.min_object_accel_for_rss", i.min_object_accel_for_rss); + tier4_autoware_utils::updateParam(parameters, "common.idling_time", i.idling_time); + } + + virtual void updateParam([[maybe_unused]] const std::vector & parameters) {} + + // TODO(shimizu) remove this function + void setSmoothedTrajectory(const Trajectory::ConstSharedPtr traj) + { + smoothed_trajectory_ptr_ = traj; + } + + bool isCruiseObstacle(const uint8_t label) + { + const auto & types = cruise_obstacle_types_; + return std::find(types.begin(), types.end(), label) != types.end(); + } + + bool isStopObstacle(const uint8_t label) + { + const auto & types = stop_obstacle_types_; + return std::find(types.begin(), types.end(), label) != types.end(); + } + +protected: + // Parameters + bool is_showing_debug_info_{false}; + LongitudinalInfo longitudinal_info_; + double min_behavior_stop_margin_; + double nearest_dist_deviation_threshold_; + double nearest_yaw_deviation_threshold_; + + // Vehicle Parameters + vehicle_info_util::VehicleInfo vehicle_info_; + + // TODO(shimizu) remove these parameters + Trajectory::ConstSharedPtr smoothed_trajectory_ptr_; + + double calcRSSDistance( + const double ego_vel, const double obj_vel, const double margin = 0.0) const + { + const auto & i = longitudinal_info_; + const double rss_dist_with_margin = + ego_vel * i.idling_time + std::pow(ego_vel, 2) * 0.5 / std::abs(i.min_accel) - + std::pow(obj_vel, 2) * 0.5 / std::abs(i.min_object_accel_for_rss) + margin; + return rss_dist_with_margin; + } + +private: + std::vector cruise_obstacle_types_; + std::vector stop_obstacle_types_; +}; + +#endif // OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp new file mode 100644 index 0000000000000..8a34f49172cd2 --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp @@ -0,0 +1,62 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ +#define OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ + +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/point32.hpp" +#include "geometry_msgs/msg/pose.hpp" + +#include +#include + +#include + +namespace polygon_utils +{ +namespace bg = boost::geometry; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; + +Polygon2d convertObstacleToPolygon( + const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape); + +boost::optional getFirstCollisionIndex( + const std::vector & traj_polygons, const Polygon2d & obj_polygon, + std::vector & collision_points); + +boost::optional getFirstNonCollisionIndex( + const std::vector & base_polygons, + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_auto_perception_msgs::msg::Shape & shape, const size_t start_idx); + +bool willCollideWithSurroundObstacle( + const autoware_auto_planning_msgs::msg::Trajectory & traj, + const std::vector & traj_polygons, + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_auto_perception_msgs::msg::Shape & shape, const double max_dist, + const double ego_obstacle_overlap_time_threshold, + const double max_prediction_time_for_collision_check); + +std::vector createOneStepPolygons( + const autoware_auto_planning_msgs::msg::Trajectory & traj, + const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width); +} // namespace polygon_utils + +#endif // OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp new file mode 100644 index 0000000000000..d1e6a0b86e715 --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp @@ -0,0 +1,49 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ +#define OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ + +#include + +#include "autoware_auto_perception_msgs/msg/object_classification.hpp" +#include "autoware_auto_perception_msgs/msg/predicted_path.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include + +#include + +namespace obstacle_cruise_utils +{ +using autoware_auto_perception_msgs::msg::ObjectClassification; + +bool isVehicle(const uint8_t label); + +visualization_msgs::msg::Marker getObjectMarker( + const geometry_msgs::msg::Pose & obstacle_pose, size_t idx, const std::string & ns, + const double r, const double g, const double b); + +boost::optional calcForwardPose( + const autoware_auto_planning_msgs::msg::Trajectory & traj, const size_t nearest_idx, + const double target_length); + +boost::optional getCurrentObjectPoseFromPredictedPath( + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time); +} // namespace obstacle_cruise_utils + +#endif // OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ diff --git a/planning/obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml b/planning/obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml new file mode 100644 index 0000000000000..a6c95d65acc6a --- /dev/null +++ b/planning/obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/obstacle_cruise_planner/obstacle_cruise_planner-design.md b/planning/obstacle_cruise_planner/obstacle_cruise_planner-design.md new file mode 100644 index 0000000000000..3a78af3827b2e --- /dev/null +++ b/planning/obstacle_cruise_planner/obstacle_cruise_planner-design.md @@ -0,0 +1,326 @@ +# Obstacle Velocity Planner + +## Overview + +The `obstacle_cruise_planner` package has following modules. + +- obstacle stop planning + - inserting a stop point in the trajectory when there is a static obstacle on the trajectory. +- adaptive cruise planning + - sending an external velocity limit to `motion_velocity_smoother` when there is a dynamic obstacle to cruise on the trajectory + +## Interfaces + +### Input topics + +| Name | Type | Description | +| ----------------------------- | ----------------------------------------------- | --------------------------------- | +| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | input trajectory | +| `~/input/smoothed_trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory with smoothed velocity | +| `~/input/objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects | +| `~/input/odometry` | nav_msgs::msg::Odometry | ego odometry | + +### Output topics + +| Name | Type | Description | +| ------------------------------ | ---------------------------------------------- | ------------------------------------- | +| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | output trajectory | +| `~output/velocity_limit` | tier4_planning_msgs::VelocityLimit | velocity limit for cruising | +| `~output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit | +| `~output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that make the vehicle to stop | + +## Design + +Design for the following functions is defined here. + +- Obstacle candidates selection +- Obstacle stop planning +- Adaptive cruise planning + +A data structure for cruise and stop planning is as follows. +This planner data is created first, and then sent to the planning algorithm. + +```cpp +struct ObstacleCruisePlannerData +{ + rclcpp::Time current_time; + autoware_auto_planning_msgs::msg::Trajectory traj; + geometry_msgs::msg::Pose current_pose; + double current_vel; + double current_acc; + std::vector target_obstacles; +}; +``` + +```cpp +struct TargetObstacle +{ + rclcpp::Time time_stamp; + bool orientation_reliable; + geometry_msgs::msg::Pose pose; + bool velocity_reliable; + float velocity; + bool is_classified; + ObjectClassification classification; + Shape shape; + std::vector predicted_paths; + geometry_msgs::msg::Point collision_point; +}; +``` + +### Obstacle candidates selection + +In this function, target obstacles for stopping or cruising are selected based on their pose and velocity. + +By default, objects that realize one of the following conditions are considered to be the target obstacle candidates. +Some terms will be defined in the following subsections. + +- Vehicle objects "inside the detection area" other than "far crossing vehicles". +- non vehicle objects "inside the detection area" +- "Near cut-in vehicles" outside the detection area + +Note that currently the obstacle candidates selection algorithm is for autonomous driving. +However, we have following parameters as well for stop and cruise respectively so that we can extend the obstacles candidates selection algorithm for non vehicle robots. +By default, unknown and vehicles are obstacles to cruise and stop, and non vehicles are obstacles just to stop. + +| Parameter | Type | Description | +| ------------------------------ | ---- | ------------------------------------------------- | +| `cruise_obstacle_type.unknown` | bool | flag to consider unknown objects as being cruised | +| `cruise_obstacle_type.car` | bool | flag to consider unknown objects as being cruised | +| `cruise_obstacle_type.truck` | bool | flag to consider unknown objects as being cruised | +| ... | bool | ... | +| `stop_obstacle_type.unknown` | bool | flag to consider unknown objects as being stopped | +| ... | bool | ... | + +#### Inside the detection area + +To calculate obstacles inside the detection area, firstly, obstacles whose distance to the trajectory is less than `rough_detection_area_expand_width` are selected. +Then, the detection area, which is a trajectory with some lateral margin, is calculated as shown in the figure. +The detection area width is a vehicle's width + `detection_area_expand_width`, and it is represented as a polygon resampled with `decimate_trajectory_step_length` longitudinally. +The roughly selected obstacles inside the detection area are considered as inside the detection area. + +![detection_area](./image/detection_area.png) + +This two-step detection is used for calculation efficiency since collision checking of polygons is heavy. +Boost.Geometry is used as a library to check collision among polygons. + +In the `obstacle_filtering` namespace, + +| Parameter | Type | Description | +| ----------------------------------- | ------ | ----------------------------------------------------------------------------------- | +| `rough_detection_area_expand_width` | double | rough lateral margin for rough detection area expansion [m] | +| `detection_area_expand_width` | double | lateral margin for precise detection area expansion [m] | +| `decimate_trajectory_step_length` | double | longitudinal step length to calculate trajectory polygon for collision checking [m] | + +#### Far crossing vehicles + +Near crossing vehicles (= not far crossing vehicles) are defined as vehicle objects realizing either of following conditions. + +- whose yaw angle against the nearest trajectory point is greater than `crossing_obstacle_traj_angle_threshold` +- whose velocity is less than `crossing_obstacle_velocity_threshold`. + +Assuming `t_1` to be the time for the ego to reach the current crossing obstacle position with the constant velocity motion, and `t_2` to be the time for the crossing obstacle to go outside the detection area, if the following condition is realized, the crossing vehicle will be ignored. + +$$ +t_1 - t_2 > \mathrm{margin\_for\_collision\_time} +$$ + +In the `obstacle_filtering` namespace, + +| Parameter | Type | Description | +| ---------------------------------------- | ------ | ----------------------------------------------------------------------------- | +| `crossing_obstacle_velocity_threshold` | double | velocity threshold to decide crossing obstacle [m/s] | +| `crossing_obstacle_traj_angle_threshold` | double | yaw threshold of crossing obstacle against the nearest trajectory point [rad] | +| `collision_time_margin` | double | time threshold of collision between obstacle and ego [s] | + +#### Near Cut-in vehicles + +Near Cut-in vehicles are defined as vehicle objects + +- whose predicted path's footprints from the current time to `max_prediction_time_for_collision_check` overlap with the detection area longer than `ego_obstacle_overlap_time_threshold`. + +In the `obstacle_filtering` namespace, + +| Parameter | Type | Description | +| ----------------------------------------- | ------ | --------------------------------------------------------------- | +| `ego_obstacle_overlap_time_threshold` | double | time threshold to decide cut-in obstacle for cruise or stop [s] | +| `max_prediction_time_for_collision_check` | double | prediction time to check collision between obstacle and ego [s] | + +### Stop planning + +| Parameter | Type | Description | +| ----------------------------- | ------ | ----------------------------------------- | +| `common.min_strong_accel` | double | ego's minimum acceleration to stop [m/ss] | +| `common.safe_distance_margin` | double | distance with obstacles for stop [m] | + +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 inside the detection area. +The safe distance is parameterized as `common.safe_distance_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. + +### Adaptive cruise planning + +| Parameter | Type | Description | +| ----------------------------- | ------ | ---------------------------------------------- | +| `common.safe_distance_margin` | double | minimum distance with obstacles for cruise [m] | + +The role of the adaptive 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 = 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$ 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 | +| --------------------------------- | ------ | ----------------------------------------------------------------------------- | +| `common.idling_time` | double | idling time for the ego to detect the front vehicle starting deceleration [s] | +| `common.min_object_accel_for_rss` | double | front obstacle's acceleration [m/ss] | + +## Implementation + +### Flowchart + +Successive functions consist of `obstacle_cruise_planner` as follows. + +Various algorithms for stop and cruise planning will be implemented, and one of them is designated depending on the use cases. +The core algorithm implementation `generateTrajectory` depends on the designated algorithm. + +```plantuml +@startuml +title trajectoryCallback +start + +group createPlannerData + :filterObstacles; +end group + +:generateTrajectory; + +:publishVelocityLimit; + +:publish trajectory; + +:publishDebugData; + +:publish and print calculation time; + +stop +@enduml +``` + +### Algorithm selection + +Currently, only a PID-based planner is supported. +Each planner will be explained in the following. + +| Parameter | Type | Description | +| ------------------------ | ------ | ------------------------------------------------------------ | +| `common.planning_method` | string | cruise and stop planning algorithm, selected from "pid_base" | + +### PID-based planner + +#### Stop planning + +In the `pid_based_planner` namespace, + +| Parameter | Type | Description | +| ------------------------------------------------- | ------ | ------------------------------------------------------------ | +| `obstacle_velocity_threshold_from_cruise_to_stop` | double | obstacle velocity threshold to be stopped from cruised [m/s] | + +Only one obstacle is targeted for the stop planning. +It is the obstacle among obstacle candidates whose velocity is less than `obstacle_velocity_threshold_from_cruise_to_stop`, and which is the nearest to the ego along the trajectory. A stop point is inserted keeping`common.safe_distance_margin` distance between the ego and obstacle. + +Note that, as explained in the stop planning design, a stop planning which requires a strong acceleration (less than `common.min_strong_accel`) will be canceled. + +#### Adaptive cruise planning + +In the `pid_based_planner` namespace, + +| Parameter | Type | Description | +| --------------------------- | ------ | -------------------------------------------------------------------------------------------------------- | +| `kp` | double | p gain for pid control [-] | +| `ki` | double | i gain for pid control [-] | +| `kd` | double | d gain for pid control [-] | +| `output_ratio_during_accel` | double | The output velocity will be multiplied by the ratio during acceleration to follow the front vehicle. [-] | +| `vel_to_acc_weight` | double | target acceleration is target velocity \* `vel_to_acc_weight` [-] | +| `min_cruise_target_vel` | double | minimum target velocity during cruise [m/s] | + +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 (`motion_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 + +## 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 `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 `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 `obstacle_cruise_planner`, `common.min_behavior_stop_margin` is defined. +In the case of the crosswalk described above, `obstacle_cruise_planner` inserts the stop point with a distance `common.min_behavior_stop_margin` at minimum between the ego and obstacle. + +| Parameter | Type | Description | +| --------------------------------- | ------ | ---------------------------------------------------------------------- | +| `common.min_behavior_stop_margin` | double | minimum stop margin when stopping with the behavior module enabled [m] | + +## Visualization for debugging + +### Detection area + +Green polygons which is a detection area is visualized by `detection_polygons` in the `~/debug/marker` topic. + +![detection_area](./image/detection_area.png) + +### Collision point + +Red point which is a collision point with obstacle is visualized by `collision_points` in the `~/debug/marker` topic. + +![collision_point](./image/collision_point.png) + +### Obstacle for cruise + +Yellow sphere which is a obstacle for cruise is visualized by `obstacles_to_cruise` in the `~/debug/marker` topic. + +![obstacle_to_cruise](./image/obstacle_to_cruise.png) + +### Obstacle for stop + +Red sphere which is a obstacle for stop is visualized by `obstacles_to_stop` in the `~/debug/marker` topic. + +![obstacle_to_stop](./image/obstacle_to_stop.png) + + + + + + + +### Obstacle cruise wall + +Yellow wall which means a safe distance to cruise if the ego's front meets the wall is visualized in the `~/debug/cruise_wall_marker` topic. + +![obstacle_to_cruise](./image/obstacle_to_cruise.png) + +### Obstacle stop wall + +Red wall which means a safe distance to stop if the ego's front meets the wall is visualized in the `~/debug/stop_wall_marker` topic. + +![obstacle_to_stop](./image/obstacle_to_stop.png) + +## Known Limits + +- Common + - When the obstacle pose or velocity estimation has a delay, the ego sometimes will go close to the front vehicle keeping deceleration. + - Current implementation only uses predicted objects message for static/dynamic obstacles and does not use pointcloud. Therefore, if object recognition is lost, the ego cannot deal with the lost obstacle. +- PID-based planner + - The algorithm strongly depends on the velocity smoothing package (`motion_velocity_smoother` by default) whether or not the ego realizes the designated target speed. If the velocity smoothing package is updated, please take care of the vehicle's behavior as much as possible. diff --git a/planning/obstacle_cruise_planner/package.xml b/planning/obstacle_cruise_planner/package.xml new file mode 100644 index 0000000000000..ec261d99a229f --- /dev/null +++ b/planning/obstacle_cruise_planner/package.xml @@ -0,0 +1,40 @@ + + + obstacle_cruise_planner + 0.1.0 + The Stop/Slow down planner for dynamic obstacles + + Takayuki Murooka + + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + geometry_msgs + interpolation + lanelet2_extension + nav_msgs + osqp_interface + rclcpp + rclcpp_components + signal_processing + std_msgs + tf2 + tf2_ros + tier4_autoware_utils + tier4_debug_msgs + tier4_planning_msgs + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py b/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py new file mode 100755 index 0000000000000..00833d4e84e83 --- /dev/null +++ b/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py @@ -0,0 +1,384 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +# Copyright 2022 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. + +from autoware_auto_planning_msgs.msg import Trajectory +from geometry_msgs.msg import Pose +from geometry_msgs.msg import Twist +from geometry_msgs.msg import TwistStamped +from matplotlib import animation +import matplotlib.pyplot as plt +import message_filters +from nav_msgs.msg import Odometry +import numpy as np +import rclpy +from rclpy.node import Node +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + +PLOT_MAX_ARCLENGTH = 200 +PATH_ORIGIN_FRAME = "map" +SELF_POSE_FRAME = "base_link" + + +class TrajectoryVisualizer(Node): + def __init__(self): + + super().__init__("trajectory_visualizer") + + self.fig = plt.figure() + + self.max_vel = 0.0 + self.min_vel = 0.0 + + # update flag + self.update_sv_traj = False + self.update_traj = False + self.update_max_traj = False + self.update_boundary = False + self.update_optimized_st_graph = False + + self.tf_buffer = Buffer(node=self) + self.tf_listener = TransformListener( + self.tf_buffer, self, spin_thread=True + ) # For get self-position + self.self_pose = Pose() + self.self_pose_received = False + self.localization_twist = Twist() + self.vehicle_twist = Twist() + + self.sv_trajectory = Trajectory() + self.trajectory = Trajectory() + self.max_trajectory = Trajectory() + self.boundary = Trajectory() + self.optimized_st_graph = Trajectory() + + self.sub_localization_twist = self.create_subscription( + Odometry, "/localization/kinematic_state", self.CallbackLocalizationOdom, 1 + ) + self.sub_vehicle_twist = self.create_subscription( + TwistStamped, "/vehicle/status/twist", self.CallbackVehicleTwist, 1 + ) + + topic_header = "/planning/scenario_planning/lane_driving/motion_planning/" + traj0 = "obstacle_cruise_planner/optimized_sv_trajectory" + self.sub_status0 = message_filters.Subscriber(self, Trajectory, topic_header + traj0) + traj1 = "/planning/scenario_planning/lane_driving/trajectory" + self.sub_status1 = message_filters.Subscriber(self, Trajectory, traj1) + traj2 = "surround_obstacle_checker/trajectory" + self.sub_status2 = message_filters.Subscriber(self, Trajectory, topic_header + traj2) + traj3 = "obstacle_cruise_planner/boundary" + self.sub_status3 = message_filters.Subscriber(self, Trajectory, topic_header + traj3) + traj4 = "obstacle_cruise_planner/optimized_st_graph" + self.sub_status4 = message_filters.Subscriber(self, Trajectory, topic_header + traj4) + + self.ts1 = message_filters.ApproximateTimeSynchronizer( + [self.sub_status0, self.sub_status1, self.sub_status2], 30, 0.5 + ) + self.ts1.registerCallback(self.CallbackMotionVelOptTraj) + self.ts2 = message_filters.ApproximateTimeSynchronizer( + [self.sub_status3, self.sub_status4], 30, 0.5 + ) + self.ts2.registerCallback(self.CallbackMotionVelObsTraj) + + # Main Process + self.ani = animation.FuncAnimation( + self.fig, self.plotTrajectoryVelocity, interval=100, blit=True + ) + self.setPlotTrajectoryVelocity() + + plt.show() + return + + def CallbackLocalizationOdom(self, cmd): + self.localization_twist = cmd.twist.twist + + def CallbackVehicleTwist(self, cmd): + self.vehicle_twist = cmd.twist + + def CallbackMotionVelOptTraj(self, cmd0, cmd1, cmd2): + self.CallbackSVTraj(cmd0) + self.CallbackTraj(cmd1) + self.CallbackMaxTraj(cmd2) + + def CallbackMotionVelObsTraj(self, cmd1, cmd2): + self.CallbackBoundary(cmd1) + self.CallbackOptimizedSTGraph(cmd2) + + def CallbackSVTraj(self, cmd): + self.sv_trajectory = cmd + self.update_sv_traj = True + + def CallbackTraj(self, cmd): + self.trajectory = cmd + self.update_traj = True + + def CallbackMaxTraj(self, cmd): + self.max_trajectory = cmd + self.update_max_traj = True + + def CallbackBoundary(self, cmd): + self.boundary = cmd + self.update_boundary = True + + def CallbackOptimizedSTGraph(self, cmd): + self.optimized_st_graph = cmd + self.update_optimized_st_graph = True + + def setPlotTrajectoryVelocity(self): + self.ax1 = plt.subplot(2, 1, 1) # row, col, index(= 0: + x_closest = x[opt_closest] + self.im3.set_data(x_closest, self.localization_twist.linear.x) + self.im4.set_data(x_closest, self.vehicle_twist.linear.x) + + opt_zero_vel_id = -1 + for i in range(opt_closest, len(trajectory.points)): + if trajectory.points[i].longitudinal_velocity_mps < 1e-3: + opt_zero_vel_id = i + break + else: + opt_zero_vel_id = len(trajectory.points) - 1 + + opt_pos = self.CalcPartArcLength(trajectory, opt_closest, opt_zero_vel_id + 1) + opt_time = self.CalcTime(trajectory, opt_closest, opt_zero_vel_id + 1) + self.im6.set_data(opt_time, opt_pos) + + if self.update_max_traj: + x = self.CalcArcLength(max_trajectory) + y = self.ToVelList(max_trajectory) + self.im1.set_data(x, y) + self.update_max_traj = False + + max_closest = self.calcClosestTrajectory(max_trajectory) + if max_closest >= 0: + max_zero_vel_id = -1 + for i in range(max_closest, len(max_trajectory.points)): + if max_trajectory.points[i].longitudinal_velocity_mps < 1e-3: + max_zero_vel_id = i + break + else: + max_zero_vel_id = len(max_trajectory.points) - 1 + + max_pos = self.CalcPartArcLength(max_trajectory, max_closest, max_zero_vel_id + 1) + max_time = self.CalcTime(max_trajectory, max_closest, max_zero_vel_id + 1) + self.im5.set_data(max_time, max_pos) + + if self.update_boundary: + self.update_boundary = False + s_list = [] + t_list = [] + for p in self.boundary.points: + s_list.append(p.pose.position.x) + t_list.append(p.pose.position.y) + self.im7.set_data(t_list, s_list) + + if self.update_optimized_st_graph: + self.update_optimized_st_graph = False + s_list = [] + t_list = [] + for p in self.optimized_st_graph.points: + s_list.append(p.pose.position.x) + t_list.append(p.pose.position.y) + self.im8.set_data(t_list, s_list) + + # change y-range + self.ax1.set_ylim([-1.0, 50.0]) + + return ( + self.im0, + self.im1, + self.im2, + self.im3, + self.im4, + self.im5, + self.im6, + self.im7, + self.im8, + ) + + def CalcArcLength(self, traj): + return self.CalcPartArcLength(traj, 0, len(traj.points)) + + def CalcPartArcLength(self, traj, start, end): + assert start <= end + s_arr = [] + ds = 0.0 + s_sum = 0.0 + + if len(traj.points) > 0: + s_arr.append(s_sum) + + for i in range(start + 1, end): + p0 = traj.points[i - 1] + p1 = traj.points[i] + dx = p1.pose.position.x - p0.pose.position.x + dy = p1.pose.position.y - p0.pose.position.y + ds = np.sqrt(dx**2 + dy**2) + s_sum += ds + s_arr.append(s_sum) + return s_arr + + def CalcTrajectoryInterval(self, traj, start, end): + ds_arr = [] + + for i in range(start + 1, end): + p0 = traj.points[i - 1] + p1 = traj.points[i] + dx = p1.pose.position.x - p0.pose.position.x + dy = p1.pose.position.y - p0.pose.position.y + ds = np.sqrt(dx**2 + dy**2) + ds_arr.append(ds) + return ds_arr + + def CalcTime(self, traj, start, end): + t_arr = [] + t_sum = 0.0 + ds_arr = self.CalcTrajectoryInterval(traj, start, end) + + if len(traj.points) > 0: + t_arr.append(t_sum) + + for i in range(start, end - 1): + v = traj.points[i].longitudinal_velocity_mps + ds = ds_arr[i - start] + dt = ds / max(v, 0.1) + t_sum += dt + t_arr.append(t_sum) + return t_arr + + def ToVelList(self, traj): + v_list = [] + for p in traj.points: + v_list.append(p.longitudinal_velocity_mps) + return v_list + + def updatePose(self, from_link, to_link): + try: + tf = self.tf_buffer.lookup_transform(from_link, to_link, rclpy.time.Time()) + self.self_pose.position.x = tf.transform.translation.x + self.self_pose.position.y = tf.transform.translation.y + self.self_pose.position.z = tf.transform.translation.z + self.self_pose.orientation.x = tf.transform.rotation.x + self.self_pose.orientation.y = tf.transform.rotation.y + self.self_pose.orientation.z = tf.transform.rotation.z + self.self_pose.orientation.w = tf.transform.rotation.w + self.self_pose_received = True + return + except BaseException: + self.get_logger().warn( + "lookup transform failed: from {} to {}".format(from_link, to_link) + ) + return + + def calcClosestTrajectory(self, path): + closest = -1 + min_dist_squared = 1.0e10 + for i in range(0, len(path.points)): + dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].pose) + if dist_sq < min_dist_squared: + min_dist_squared = dist_sq + closest = i + return closest + + def calcSquaredDist2d(self, p1, p2): + dx = p1.position.x - p2.position.x + dy = p1.position.y - p2.position.y + return dx * dx + dy * dy + + +def main(args=None): + try: + rclpy.init(args=args) + node = TrajectoryVisualizer() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp new file mode 100644 index 0000000000000..45bab678c98b7 --- /dev/null +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -0,0 +1,780 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_cruise_planner/node.hpp" + +#include "obstacle_cruise_planner/polygon_utils.hpp" +#include "obstacle_cruise_planner/utils.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/trajectory/tmp_conversion.hpp" + +#include + +#include +#include + +namespace +{ +VelocityLimitClearCommand createVelocityLimitClearCommandMsg(const rclcpp::Time & current_time) +{ + VelocityLimitClearCommand msg; + msg.stamp = current_time; + msg.sender = "obstacle_cruise_planner"; + msg.command = true; + return msg; +} + +// TODO(murooka) make this function common +size_t findExtendedNearestIndex( + const Trajectory traj, const geometry_msgs::msg::Pose & pose, const double max_dist, + const double max_yaw) +{ + const auto nearest_idx = + tier4_autoware_utils::findNearestIndex(traj.points, pose, max_dist, max_yaw); + if (nearest_idx) { + return nearest_idx.get(); + } + return tier4_autoware_utils::findNearestIndex(traj.points, pose.position); +} + +Trajectory trimTrajectoryFrom(const Trajectory & input, const double nearest_idx) +{ + Trajectory output{}; + + for (size_t i = nearest_idx; i < input.points.size(); ++i) { + output.points.push_back(input.points.at(i)); + } + + return output; +} + +bool isFrontObstacle( + const Trajectory & traj, const size_t ego_idx, const geometry_msgs::msg::Point & obj_pos) +{ + size_t obj_idx = tier4_autoware_utils::findNearestSegmentIndex(traj.points, obj_pos); + + const double ego_to_obj_distance = + tier4_autoware_utils::calcSignedArcLength(traj.points, ego_idx, obj_idx); + + if (obj_idx == 0 && ego_to_obj_distance < 0) { + return false; + } + + return true; +} + +TrajectoryPoint calcLinearPoint( + const TrajectoryPoint & p_from, const TrajectoryPoint & p_to, const double length) +{ + TrajectoryPoint output; + const double dx = p_to.pose.position.x - p_from.pose.position.x; + const double dy = p_to.pose.position.y - p_from.pose.position.y; + const double norm = std::hypot(dx, dy); + + output = p_to; + output.pose.position.x += length * dx / norm; + output.pose.position.y += length * dy / norm; + + return output; +} + +// TODO(murooka) replace with spline interpolation +Trajectory decimateTrajectory(const Trajectory & input, const double step_length) +{ + Trajectory output{}; + + if (input.points.empty()) { + return output; + } + + double trajectory_length_sum = 0.0; + double next_length = 0.0; + + for (int i = 0; i < static_cast(input.points.size()) - 1; ++i) { + const auto & p_front = input.points.at(i); + const auto & p_back = input.points.at(i + 1); + constexpr double epsilon = 1e-3; + + if (next_length <= trajectory_length_sum + epsilon) { + const auto p_interpolate = + calcLinearPoint(p_front, p_back, next_length - trajectory_length_sum); + output.points.push_back(p_interpolate); + next_length += step_length; + continue; + } + + trajectory_length_sum += tier4_autoware_utils::calcDistance2d(p_front, p_back); + } + + output.points.push_back(input.points.back()); + + return output; +} + +PredictedPath getHighestConfidencePredictedPath(const PredictedObject & predicted_object) +{ + const auto reliable_path = std::max_element( + predicted_object.kinematics.predicted_paths.begin(), + predicted_object.kinematics.predicted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); + return *reliable_path; +} + +bool isAngleAlignedWithTrajectory( + const Trajectory & traj, const geometry_msgs::msg::Pose & pose, const double threshold_angle) +{ + if (traj.points.empty()) { + return false; + } + + const double obj_yaw = tf2::getYaw(pose.orientation); + + const size_t nearest_idx = tier4_autoware_utils::findNearestIndex(traj.points, pose.position); + const double traj_yaw = tf2::getYaw(traj.points.at(nearest_idx).pose.orientation); + + const double diff_yaw = tier4_autoware_utils::normalizeRadian(obj_yaw - traj_yaw); + + const bool is_aligned = std::abs(diff_yaw) <= threshold_angle; + return is_aligned; +} + +double calcAlignedAdaptiveCruise( + const PredictedObject & predicted_object, const Trajectory & trajectory) +{ + const auto & object_pos = predicted_object.kinematics.initial_pose_with_covariance.pose.position; + const auto & object_vel = + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x; + + const size_t object_idx = tier4_autoware_utils::findNearestIndex(trajectory.points, object_pos); + + const double object_yaw = + tf2::getYaw(predicted_object.kinematics.initial_pose_with_covariance.pose.orientation); + const double traj_yaw = tf2::getYaw(trajectory.points.at(object_idx).pose.orientation); + + return object_vel * std::cos(object_yaw - traj_yaw); +} +} // namespace + +namespace motion_planning +{ +ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options) +: Node("obstacle_cruise_planner", node_options), + self_pose_listener_(this), + in_objects_ptr_(nullptr), + lpf_acc_ptr_(nullptr), + vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()) +{ + using std::placeholders::_1; + + // subscriber + trajectory_sub_ = create_subscription( + "~/input/trajectory", rclcpp::QoS{1}, + std::bind(&ObstacleCruisePlannerNode::onTrajectory, this, _1)); + smoothed_trajectory_sub_ = create_subscription( + "/planning/scenario_planning/trajectory", rclcpp::QoS{1}, + std::bind(&ObstacleCruisePlannerNode::onSmoothedTrajectory, this, _1)); + objects_sub_ = create_subscription( + "~/input/objects", rclcpp::QoS{1}, std::bind(&ObstacleCruisePlannerNode::onObjects, this, _1)); + odom_sub_ = create_subscription( + "~/input/odometry", rclcpp::QoS{1}, + std::bind(&ObstacleCruisePlannerNode::onOdometry, this, std::placeholders::_1)); + + // publisher + trajectory_pub_ = create_publisher("~/output/trajectory", 1); + vel_limit_pub_ = + create_publisher("~/output/velocity_limit", rclcpp::QoS{1}.transient_local()); + clear_vel_limit_pub_ = create_publisher( + "~/output/clear_velocity_limit", rclcpp::QoS{1}.transient_local()); + debug_calculation_time_pub_ = create_publisher("~/debug/calculation_time", 1); + debug_cruise_wall_marker_pub_ = + create_publisher("~/debug/cruise_wall_marker", 1); + debug_stop_wall_marker_pub_ = + create_publisher("~/virtual_wall", 1); + debug_marker_pub_ = create_publisher("~/debug/marker", 1); + + // longitudinal_info + const auto longitudinal_info = [&]() { + const double max_accel = declare_parameter("normal.max_acc"); + const double min_accel = declare_parameter("normal.min_acc"); + const double max_jerk = declare_parameter("normal.max_jerk"); + const double min_jerk = declare_parameter("normal.min_jerk"); + + const double min_strong_accel = declare_parameter("common.min_strong_accel"); + const double min_ego_accel_for_rss = declare_parameter("common.min_ego_accel_for_rss"); + const double min_object_accel_for_rss = + declare_parameter("common.min_object_accel_for_rss"); + const double idling_time = declare_parameter("common.idling_time"); + const double safe_distance_margin = declare_parameter("common.safe_distance_margin"); + + return LongitudinalInfo( + max_accel, min_accel, max_jerk, min_jerk, min_strong_accel, idling_time, + min_ego_accel_for_rss, min_object_accel_for_rss, safe_distance_margin); + }(); + + const bool is_showing_debug_info_ = declare_parameter("common.is_showing_debug_info"); + + // low pass filter for ego acceleration + const double lpf_gain_for_accel = declare_parameter("common.lpf_gain_for_accel"); + lpf_acc_ptr_ = std::make_shared(0.0, lpf_gain_for_accel); + + { // Obstacle filtering parameters + obstacle_filtering_param_.rough_detection_area_expand_width = + declare_parameter("obstacle_filtering.rough_detection_area_expand_width"); + obstacle_filtering_param_.detection_area_expand_width = + declare_parameter("obstacle_filtering.detection_area_expand_width"); + obstacle_filtering_param_.decimate_trajectory_step_length = + declare_parameter("obstacle_filtering.decimate_trajectory_step_length"); + obstacle_filtering_param_.crossing_obstacle_velocity_threshold = + declare_parameter("obstacle_filtering.crossing_obstacle_velocity_threshold"); + obstacle_filtering_param_.collision_time_margin = + declare_parameter("obstacle_filtering.collision_time_margin"); + obstacle_filtering_param_.ego_obstacle_overlap_time_threshold = + declare_parameter("obstacle_filtering.ego_obstacle_overlap_time_threshold"); + obstacle_filtering_param_.max_prediction_time_for_collision_check = + declare_parameter("obstacle_filtering.max_prediction_time_for_collision_check"); + obstacle_filtering_param_.crossing_obstacle_traj_angle_threshold = + declare_parameter("obstacle_filtering.crossing_obstacle_traj_angle_threshold"); + + { + if (declare_parameter("obstacle_filtering.ignored_outside_obstacle_type.unknown")) { + obstacle_filtering_param_.ignored_outside_obstacle_types.push_back( + ObjectClassification::UNKNOWN); + } + if (declare_parameter("obstacle_filtering.ignored_outside_obstacle_type.car")) { + obstacle_filtering_param_.ignored_outside_obstacle_types.push_back( + ObjectClassification::CAR); + } + if (declare_parameter("obstacle_filtering.ignored_outside_obstacle_type.truck")) { + obstacle_filtering_param_.ignored_outside_obstacle_types.push_back( + ObjectClassification::TRUCK); + } + if (declare_parameter("obstacle_filtering.ignored_outside_obstacle_type.bus")) { + obstacle_filtering_param_.ignored_outside_obstacle_types.push_back( + ObjectClassification::BUS); + } + if (declare_parameter("obstacle_filtering.ignored_outside_obstacle_type.trailer")) { + obstacle_filtering_param_.ignored_outside_obstacle_types.push_back( + ObjectClassification::TRAILER); + } + if (declare_parameter("obstacle_filtering.ignored_outside_obstacle_type.motorcycle")) { + obstacle_filtering_param_.ignored_outside_obstacle_types.push_back( + ObjectClassification::MOTORCYCLE); + } + if (declare_parameter("obstacle_filtering.ignored_outside_obstacle_type.bicycle")) { + obstacle_filtering_param_.ignored_outside_obstacle_types.push_back( + ObjectClassification::BICYCLE); + } + if (declare_parameter("obstacle_filtering.ignored_outside_obstacle_type.pedestrian")) { + obstacle_filtering_param_.ignored_outside_obstacle_types.push_back( + ObjectClassification::PEDESTRIAN); + } + } + } + + { // planning algorithm + const std::string planning_algorithm_param = + declare_parameter("common.planning_algorithm"); + planning_algorithm_ = getPlanningAlgorithmType(planning_algorithm_param); + + if (planning_algorithm_ == PlanningAlgorithm::OPTIMIZATION_BASE) { + planner_ptr_ = + std::make_unique(*this, longitudinal_info, vehicle_info_); + } else if (planning_algorithm_ == PlanningAlgorithm::PID_BASE) { + planner_ptr_ = std::make_unique(*this, longitudinal_info, vehicle_info_); + } else { + std::logic_error("Designated algorithm is not supported."); + } + + min_behavior_stop_margin_ = declare_parameter("common.min_behavior_stop_margin"); + nearest_dist_deviation_threshold_ = + declare_parameter("common.nearest_dist_deviation_threshold"); + nearest_yaw_deviation_threshold_ = + declare_parameter("common.nearest_yaw_deviation_threshold"); + planner_ptr_->setParams( + is_showing_debug_info_, min_behavior_stop_margin_, nearest_dist_deviation_threshold_, + nearest_yaw_deviation_threshold_); + } + + // wait for first self pose + self_pose_listener_.waitForFirstPose(); + + // set parameter callback + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&ObstacleCruisePlannerNode::onParam, this, std::placeholders::_1)); +} + +ObstacleCruisePlannerNode::PlanningAlgorithm ObstacleCruisePlannerNode::getPlanningAlgorithmType( + const std::string & param) const +{ + if (param == "pid_base") { + return PlanningAlgorithm::PID_BASE; + } else if (param == "optimization_base") { + return PlanningAlgorithm::OPTIMIZATION_BASE; + } + return PlanningAlgorithm::INVALID; +} + +rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( + const std::vector & parameters) +{ + planner_ptr_->updateCommonParam(parameters); + planner_ptr_->updateParam(parameters); + + tier4_autoware_utils::updateParam( + parameters, "common.is_showing_debug_info", is_showing_debug_info_); + planner_ptr_->setParams( + is_showing_debug_info_, min_behavior_stop_margin_, nearest_dist_deviation_threshold_, + nearest_yaw_deviation_threshold_); + + // obstacle_filtering + tier4_autoware_utils::updateParam( + parameters, "obstacle_filtering.rough_detection_area_expand_width", + obstacle_filtering_param_.rough_detection_area_expand_width); + tier4_autoware_utils::updateParam( + parameters, "obstacle_filtering.detection_area_expand_width", + obstacle_filtering_param_.detection_area_expand_width); + tier4_autoware_utils::updateParam( + parameters, "obstacle_filtering.decimate_trajectory_step_length", + obstacle_filtering_param_.decimate_trajectory_step_length); + tier4_autoware_utils::updateParam( + parameters, "obstacle_filtering.crossing_obstacle_velocity_threshold", + obstacle_filtering_param_.crossing_obstacle_velocity_threshold); + tier4_autoware_utils::updateParam( + parameters, "obstacle_filtering.collision_time_margin", + obstacle_filtering_param_.collision_time_margin); + tier4_autoware_utils::updateParam( + parameters, "obstacle_filtering.ego_obstacle_overlap_time_threshold", + obstacle_filtering_param_.ego_obstacle_overlap_time_threshold); + tier4_autoware_utils::updateParam( + parameters, "obstacle_filtering.max_prediction_time_for_collision_check", + obstacle_filtering_param_.max_prediction_time_for_collision_check); + tier4_autoware_utils::updateParam( + parameters, "obstacle_filtering.crossing_obstacle_traj_angle_threshold", + obstacle_filtering_param_.crossing_obstacle_traj_angle_threshold); + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; +} + +void ObstacleCruisePlannerNode::onObjects(const PredictedObjects::ConstSharedPtr msg) +{ + in_objects_ptr_ = msg; +} + +void ObstacleCruisePlannerNode::onOdometry(const Odometry::ConstSharedPtr msg) +{ + if (current_twist_ptr_) { + prev_twist_ptr_ = current_twist_ptr_; + } + + current_twist_ptr_ = std::make_unique(); + current_twist_ptr_->header = msg->header; + current_twist_ptr_->twist = msg->twist.twist; +} + +void ObstacleCruisePlannerNode::onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg) +{ + planner_ptr_->setSmoothedTrajectory(msg); +} + +void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg) +{ + const auto current_pose_ptr = self_pose_listener_.getCurrentPose(); + + // check if subscribed variables are ready + if ( + msg->points.empty() || !current_twist_ptr_ || !prev_twist_ptr_ || !in_objects_ptr_ || + !current_pose_ptr) { + return; + } + + stop_watch_.tic(__func__); + + // create algorithmic data + DebugData debug_data; + const auto planner_data = createPlannerData(*msg, current_pose_ptr->pose, debug_data); + + // generate Trajectory + boost::optional vel_limit; + const auto output_traj = planner_ptr_->generateTrajectory(planner_data, vel_limit, debug_data); + + // publisher external velocity limit if required + publishVelocityLimit(vel_limit); + + // Publish trajectory + trajectory_pub_->publish(output_traj); + + // publish debug data + publishDebugData(debug_data); + + // publish and print calculation time + const double calculation_time = stop_watch_.toc(__func__); + publishCalculationTime(calculation_time); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner"), is_showing_debug_info_, "%s := %f [ms]", __func__, + calculation_time); +} + +ObstacleCruisePlannerData ObstacleCruisePlannerNode::createPlannerData( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose, + DebugData & debug_data) +{ + stop_watch_.tic(__func__); + + const double current_vel = current_twist_ptr_->twist.linear.x; + const double current_accel = calcCurrentAccel(); + + // create planner_data + ObstacleCruisePlannerData planner_data; + planner_data.current_time = now(); + planner_data.traj = trajectory; + planner_data.current_pose = current_pose; + planner_data.current_vel = current_vel; + planner_data.current_acc = current_accel; + planner_data.target_obstacles = + filterObstacles(*in_objects_ptr_, trajectory, current_pose, current_vel, debug_data); + + // print calculation time + const double calculation_time = stop_watch_.toc(__func__); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner"), is_showing_debug_info_, " %s := %f [ms]", + __func__, calculation_time); + + return planner_data; +} + +double ObstacleCruisePlannerNode::calcCurrentAccel() const +{ + const double diff_vel = current_twist_ptr_->twist.linear.x - prev_twist_ptr_->twist.linear.x; + const double diff_time = std::max( + (rclcpp::Time(current_twist_ptr_->header.stamp) - rclcpp::Time(prev_twist_ptr_->header.stamp)) + .seconds(), + 1e-03); + + const double accel = diff_vel / diff_time; + + return lpf_acc_ptr_->filter(accel); +} + +std::vector ObstacleCruisePlannerNode::filterObstacles( + const PredictedObjects & predicted_objects, const Trajectory & traj, + const geometry_msgs::msg::Pose & current_pose, const double current_vel, DebugData & debug_data) +{ + const auto time_stamp = rclcpp::Time(predicted_objects.header.stamp); + + const size_t ego_idx = findExtendedNearestIndex( + traj, current_pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); + + // calculate decimated trajectory + const auto trimmed_traj = trimTrajectoryFrom(traj, ego_idx); + const auto decimated_traj = + decimateTrajectory(trimmed_traj, obstacle_filtering_param_.decimate_trajectory_step_length); + if (decimated_traj.points.size() < 2) { + return {}; + } + + // calculate decimated trajectory polygons + const auto decimated_traj_polygons = polygon_utils::createOneStepPolygons( + decimated_traj, vehicle_info_, obstacle_filtering_param_.detection_area_expand_width); + debug_data.detection_polygons = decimated_traj_polygons; + + std::vector target_obstacles; + for (const auto & predicted_object : predicted_objects.objects) { + // filter object whose label is not cruised or stopped + const bool is_target_obstacle = + planner_ptr_->isStopObstacle(predicted_object.classification.front().label) || + planner_ptr_->isCruiseObstacle(predicted_object.classification.front().label); + if (!is_target_obstacle) { + RCLCPP_INFO_EXPRESSION( + get_logger(), is_showing_debug_info_, "Ignore obstacles since its label is not target."); + continue; + } + + const auto & object_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; + const auto & object_velocity = + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x; + + const bool is_front_obstacle = isFrontObstacle(traj, ego_idx, object_pose.position); + if (!is_front_obstacle) { + RCLCPP_INFO_EXPRESSION( + get_logger(), is_showing_debug_info_, "Ignore obstacles since its not front obstacle."); + continue; + } + + // rough detection area filtering without polygons + const double dist_from_obstacle_to_traj = [&]() { + return tier4_autoware_utils::calcLateralOffset(decimated_traj.points, object_pose.position); + }(); + if (dist_from_obstacle_to_traj > obstacle_filtering_param_.rough_detection_area_expand_width) { + RCLCPP_INFO_EXPRESSION( + get_logger(), is_showing_debug_info_, + "Ignore obstacles since it is far from the trajectory."); + continue; + } + + // calculate collision points + const auto obstacle_polygon = + polygon_utils::convertObstacleToPolygon(object_pose, predicted_object.shape); + std::vector collision_points; + const auto first_within_idx = polygon_utils::getFirstCollisionIndex( + decimated_traj_polygons, obstacle_polygon, collision_points); + + // precise detection area filtering with polygons + geometry_msgs::msg::Point nearest_collision_point; + if (first_within_idx) { // obstacles inside the trajectory + // calculate nearest collision point + nearest_collision_point = + calcNearestCollisionPoint(first_within_idx.get(), collision_points, decimated_traj); + debug_data.collision_points.push_back(nearest_collision_point); + + const bool is_angle_aligned = isAngleAlignedWithTrajectory( + decimated_traj, object_pose, + obstacle_filtering_param_.crossing_obstacle_traj_angle_threshold); + const double has_high_speed = + std::abs(object_velocity) > obstacle_filtering_param_.crossing_obstacle_velocity_threshold; + + // ignore running vehicle crossing the ego trajectory with high speed with some condition + if (!is_angle_aligned && has_high_speed) { + const double collision_time_margin = calcCollisionTimeMargin( + current_pose, current_vel, nearest_collision_point, predicted_object, + first_within_idx.get(), decimated_traj, decimated_traj_polygons); + if (collision_time_margin > obstacle_filtering_param_.collision_time_margin) { + // Ignore condition 1 + // Ignore vehicle obstacles inside the trajectory, which is crossing the trajectory with + // high speed and does not collide with ego in a certain time. + RCLCPP_INFO_EXPRESSION( + get_logger(), is_showing_debug_info_, + "Ignore inside obstacles since it will not collide with the ego."); + + debug_data.intentionally_ignored_obstacles.push_back(predicted_object); + continue; + } + } + } else { // obstacles outside the trajectory + const auto & types = obstacle_filtering_param_.ignored_outside_obstacle_types; + if ( + std::find(types.begin(), types.end(), predicted_object.classification.front().label) != + types.end()) { + continue; + } + + const auto predicted_path_with_highest_confidence = + getHighestConfidencePredictedPath(predicted_object); + + const bool will_collide = polygon_utils::willCollideWithSurroundObstacle( + decimated_traj, decimated_traj_polygons, predicted_path_with_highest_confidence, + predicted_object.shape, obstacle_filtering_param_.rough_detection_area_expand_width, + obstacle_filtering_param_.ego_obstacle_overlap_time_threshold, + obstacle_filtering_param_.max_prediction_time_for_collision_check); + + // TODO(murooka) think later + nearest_collision_point = object_pose.position; + + if (!will_collide) { + // Ignore condition 2 + // Ignore vehicle obstacles outside the trajectory, whose predicted path + // overlaps the ego trajectory in a certain time. + RCLCPP_INFO_EXPRESSION( + get_logger(), is_showing_debug_info_, + "Ignore outside obstacles since it will not collide with the ego."); + + debug_data.intentionally_ignored_obstacles.push_back(predicted_object); + continue; + } + } + + // convert to obstacle type + const double trajectory_aligned_adaptive_cruise = + calcAlignedAdaptiveCruise(predicted_object, traj); + const auto target_obstacle = TargetObstacle( + time_stamp, predicted_object, trajectory_aligned_adaptive_cruise, nearest_collision_point); + target_obstacles.push_back(target_obstacle); + } + + return target_obstacles; +} + +geometry_msgs::msg::Point ObstacleCruisePlannerNode::calcNearestCollisionPoint( + const size_t & first_within_idx, const std::vector & collision_points, + const Trajectory & decimated_traj) +{ + std::array segment_points; + if (first_within_idx == 0) { + const auto & traj_front_pose = decimated_traj.points.at(0).pose; + segment_points.at(0) = traj_front_pose.position; + + const auto front_pos = tier4_autoware_utils::calcOffsetPose( + traj_front_pose, vehicle_info_.max_longitudinal_offset_m, 0.0, 0.0) + .position; + segment_points.at(1) = front_pos; + } else { + const size_t seg_idx = first_within_idx - 1; + segment_points.at(0) = decimated_traj.points.at(seg_idx).pose.position; + segment_points.at(1) = decimated_traj.points.at(seg_idx + 1).pose.position; + } + + size_t min_idx = 0; + double min_dist = std::numeric_limits::max(); + for (size_t cp_idx = 0; cp_idx < collision_points.size(); ++cp_idx) { + const auto & collision_point = collision_points.at(cp_idx); + const double dist = + tier4_autoware_utils::calcLongitudinalOffsetToSegment(segment_points, 0, collision_point); + if (dist < min_dist) { + min_dist = dist; + min_idx = cp_idx; + } + } + + return collision_points.at(min_idx); +} + +double ObstacleCruisePlannerNode::calcCollisionTimeMargin( + const geometry_msgs::msg::Pose & current_pose, const double current_vel, + const geometry_msgs::msg::Point & nearest_collision_point, + const PredictedObject & predicted_object, const size_t first_within_idx, + const Trajectory & decimated_traj, + const std::vector & decimated_traj_polygons) +{ + const auto & object_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; + const auto & object_velocity = + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x; + const auto predicted_path_with_highest_confidence = + getHighestConfidencePredictedPath(predicted_object); + + const double time_to_collision = [&]() { + const double dist_from_ego_to_obstacle = + tier4_autoware_utils::calcSignedArcLength( + decimated_traj.points, current_pose.position, nearest_collision_point) - + vehicle_info_.max_longitudinal_offset_m; + return dist_from_ego_to_obstacle / std::max(1e-6, current_vel); + }(); + + const double time_to_obstacle_getting_out = [&]() { + const auto obstacle_getting_out_idx = polygon_utils::getFirstNonCollisionIndex( + decimated_traj_polygons, predicted_path_with_highest_confidence, predicted_object.shape, + first_within_idx); + if (!obstacle_getting_out_idx) { + return std::numeric_limits::max(); + } + + const double dist_to_obstacle_getting_out = tier4_autoware_utils::calcSignedArcLength( + decimated_traj.points, object_pose.position, obstacle_getting_out_idx.get()); + + return dist_to_obstacle_getting_out / object_velocity; + }(); + + return time_to_collision - time_to_obstacle_getting_out; +} + +void ObstacleCruisePlannerNode::publishVelocityLimit( + const boost::optional & vel_limit) +{ + if (vel_limit) { + vel_limit_pub_->publish(vel_limit.get()); + need_to_clear_vel_limit_ = true; + } else { + if (need_to_clear_vel_limit_) { + const auto clear_vel_limit_msg = createVelocityLimitClearCommandMsg(now()); + clear_vel_limit_pub_->publish(clear_vel_limit_msg); + need_to_clear_vel_limit_ = false; + } + } +} + +void ObstacleCruisePlannerNode::publishDebugData(const DebugData & debug_data) const +{ + stop_watch_.tic(__func__); + + visualization_msgs::msg::MarkerArray debug_marker; + const auto current_time = now(); + + // obstacles to cruise + for (size_t i = 0; i < debug_data.obstacles_to_cruise.size(); ++i) { + const auto marker = obstacle_cruise_utils::getObjectMarker( + debug_data.obstacles_to_cruise.at(i).pose, i, "obstacles_to_cruise", 0.7, 0.7, 0.0); + debug_marker.markers.push_back(marker); + } + + // obstacles to stop + for (size_t i = 0; i < debug_data.obstacles_to_stop.size(); ++i) { + const auto marker = obstacle_cruise_utils::getObjectMarker( + debug_data.obstacles_to_stop.at(i).pose, i, "obstacles_to_stop", 1.0, 0.0, 0.0); + debug_marker.markers.push_back(marker); + } + + // intentionally ignored obstacles to cruise or stop + for (size_t i = 0; i < debug_data.intentionally_ignored_obstacles.size(); ++i) { + const auto marker = obstacle_cruise_utils::getObjectMarker( + debug_data.intentionally_ignored_obstacles.at(i).kinematics.initial_pose_with_covariance.pose, + i, "intentionally_ignored_obstacles", 0.0, 1.0, 0.0); + debug_marker.markers.push_back(marker); + } + + { // footprint polygons + auto marker = tier4_autoware_utils::createDefaultMarker( + "map", current_time, "detection_polygons", 0, visualization_msgs::msg::Marker::LINE_LIST, + tier4_autoware_utils::createMarkerScale(0.01, 0.0, 0.0), + tier4_autoware_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); + + for (const auto & detection_polygon : debug_data.detection_polygons) { + for (size_t dp_idx = 0; dp_idx < detection_polygon.outer().size(); ++dp_idx) { + const auto & current_point = detection_polygon.outer().at(dp_idx); + const auto & next_point = + detection_polygon.outer().at((dp_idx + 1) % detection_polygon.outer().size()); + + marker.points.push_back( + tier4_autoware_utils::createPoint(current_point.x(), current_point.y(), 0.0)); + marker.points.push_back( + tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), 0.0)); + } + } + debug_marker.markers.push_back(marker); + } + + { // collision points + for (size_t i = 0; i < debug_data.collision_points.size(); ++i) { + auto marker = tier4_autoware_utils::createDefaultMarker( + "map", current_time, "collision_points", i, visualization_msgs::msg::Marker::SPHERE, + tier4_autoware_utils::createMarkerScale(0.25, 0.25, 0.25), + tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + marker.pose.position = debug_data.collision_points.at(i); + debug_marker.markers.push_back(marker); + } + } + + debug_marker_pub_->publish(debug_marker); + + // wall for cruise and stop + debug_cruise_wall_marker_pub_->publish(debug_data.cruise_wall_marker); + debug_stop_wall_marker_pub_->publish(debug_data.stop_wall_marker); + + // print calculation time + const double calculation_time = stop_watch_.toc(__func__); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner"), is_showing_debug_info_, " %s := %f [ms]", + __func__, calculation_time); +} + +void ObstacleCruisePlannerNode::publishCalculationTime(const double calculation_time) const +{ + Float32Stamped calculation_time_msg; + calculation_time_msg.stamp = now(); + calculation_time_msg.data = calculation_time; + debug_calculation_time_pub_->publish(calculation_time_msg); +} +} // namespace motion_planning +#include +RCLCPP_COMPONENTS_REGISTER_NODE(motion_planning::ObstacleCruisePlannerNode) diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/box2d.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/box2d.cpp new file mode 100644 index 0000000000000..6fc5900e7ccab --- /dev/null +++ b/planning/obstacle_cruise_planner/src/optimization_based_planner/box2d.cpp @@ -0,0 +1,101 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_cruise_planner/optimization_based_planner/box2d.hpp" + +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include + +Box2d::Box2d(const geometry_msgs::msg::Pose & center_pose, const double length, const double width) +: center_(center_pose.position), + length_(length), + width_(width), + half_length_(length / 2.0), + half_width_(width / 2.0) +{ + max_x_ = std::numeric_limits::min(); + max_y_ = std::numeric_limits::min(); + min_x_ = std::numeric_limits::max(); + min_y_ = std::numeric_limits::max(); + heading_ = tf2::getYaw(center_pose.orientation); + cos_heading_ = std::cos(heading_); + sin_heading_ = std::sin(heading_); + initCorners(); +} + +void Box2d::initCorners() +{ + const double dx1 = cos_heading_ * half_length_; + const double dy1 = sin_heading_ * half_length_; + const double dx2 = sin_heading_ * half_width_; + const double dy2 = -cos_heading_ * half_width_; + + const auto p1 = + tier4_autoware_utils::createPoint(center_.x + dx1 + dx2, center_.y + dy1 + dy2, center_.z); + const auto p2 = + tier4_autoware_utils::createPoint(center_.x + dx1 - dx2, center_.y + dy1 - dy2, center_.z); + const auto p3 = + tier4_autoware_utils::createPoint(center_.x - dx1 - dx2, center_.y - dy1 - dy2, center_.z); + const auto p4 = + tier4_autoware_utils::createPoint(center_.x - dx1 + dx2, center_.y - dy1 + dy2, center_.z); + corners_.clear(); + corners_.resize(4); + corners_.at(0) = p1; + corners_.at(1) = p2; + corners_.at(2) = p3; + corners_.at(3) = p4; + + for (auto & corner : corners_) { + max_x_ = std::fmax(corner.x, max_x_); + min_x_ = std::fmin(corner.x, min_x_); + max_y_ = std::fmax(corner.y, max_y_); + min_y_ = std::fmin(corner.y, min_y_); + } +} + +bool Box2d::hasOverlap(const Box2d & box) const +{ + if ( + box.getMaxX() < getMinX() || box.getMinX() > getMaxX() || box.getMaxY() < getMinY() || + box.getMinY() > getMaxY()) { + return false; + } + + const double shift_x = box.getCenterX() - center_.x; + const double shift_y = box.getCenterY() - center_.y; + + const double dx1 = cos_heading_ * half_length_; + const double dy1 = sin_heading_ * half_length_; + const double dx2 = sin_heading_ * half_width_; + const double dy2 = -cos_heading_ * half_width_; + const double dx3 = box.getCosHeading() * box.getHalfLength(); + const double dy3 = box.getSinHeading() * box.getHalfLength(); + const double dx4 = box.getSinHeading() * box.getHalfWidth(); + const double dy4 = -box.getCosHeading() * box.getHalfWidth(); + + return std::abs(shift_x * cos_heading_ + shift_y * sin_heading_) <= + std::abs(dx3 * cos_heading_ + dy3 * sin_heading_) + + std::abs(dx4 * cos_heading_ + dy4 * sin_heading_) + half_length_ && + std::abs(shift_x * sin_heading_ - shift_y * cos_heading_) <= + std::abs(dx3 * sin_heading_ - dy3 * cos_heading_) + + std::abs(dx4 * sin_heading_ - dy4 * cos_heading_) + half_width_ && + std::abs(shift_x * box.getCosHeading() + shift_y * box.getSinHeading()) <= + std::abs(dx1 * box.getCosHeading() + dy1 * box.getSinHeading()) + + std::abs(dx2 * box.getCosHeading() + dy2 * box.getSinHeading()) + + box.getHalfLength() && + std::abs(shift_x * box.getSinHeading() - shift_y * box.getCosHeading()) <= + std::abs(dx1 * box.getSinHeading() - dy1 * box.getCosHeading()) + + std::abs(dx2 * box.getSinHeading() - dy2 * box.getCosHeading()) + box.getHalfWidth(); +} diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp new file mode 100644 index 0000000000000..08806fd0dc43a --- /dev/null +++ b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -0,0 +1,1370 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp" + +#include "interpolation/linear_interpolation.hpp" +#include "interpolation/spline_interpolation.hpp" +#include "obstacle_cruise_planner/optimization_based_planner/resample.hpp" +#include "obstacle_cruise_planner/utils.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +constexpr double ZERO_VEL_THRESHOLD = 0.01; +constexpr double CLOSE_S_DIST_THRESHOLD = 1e-3; + +// TODO(shimizu) Is is ok to use planner_data.current_time instead of get_clock()->now()? +namespace +{ +[[maybe_unused]] std::string toHexString(const unique_identifier_msgs::msg::UUID & id) +{ + std::stringstream ss; + for (auto i = 0; i < 16; ++i) { + ss << std::hex << std::setfill('0') << std::setw(2) << +id.uuid[i]; + } + return ss.str(); +} + +inline void convertEulerAngleToMonotonic(std::vector & a) +{ + for (unsigned int i = 1; i < a.size(); ++i) { + const double da = a[i] - a[i - 1]; + a[i] = a[i - 1] + tier4_autoware_utils::normalizeRadian(da); + } +} + +inline tf2::Vector3 getTransVector3( + const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to) +{ + double dx = to.position.x - from.position.x; + double dy = to.position.y - from.position.y; + double dz = to.position.z - from.position.z; + return tf2::Vector3(dx, dy, dz); +} + +// TODO(shimizu) consider dist/yaw threshold for nearest index +boost::optional calcForwardPose( + const autoware_auto_planning_msgs::msg::Trajectory & traj, + const geometry_msgs::msg::Point & point, const double target_length) +{ + if (traj.points.empty()) { + return {}; + } + + const size_t nearest_idx = tier4_autoware_utils::findNearestIndex(traj.points, point); + + size_t search_idx = nearest_idx; + double length_to_search_idx = 0.0; + for (; search_idx < traj.points.size(); ++search_idx) { + length_to_search_idx = + tier4_autoware_utils::calcSignedArcLength(traj.points, nearest_idx, search_idx); + if (length_to_search_idx > target_length) { + break; + } else if (search_idx == traj.points.size() - 1) { + return {}; + } + } + + if (search_idx == 0 && !traj.points.empty()) { + return traj.points.at(0).pose; + } + + const auto & pre_pose = traj.points.at(search_idx - 1).pose; + const auto & next_pose = traj.points.at(search_idx).pose; + + geometry_msgs::msg::Pose target_pose; + + // lerp position + const double seg_length = + tier4_autoware_utils::calcDistance2d(pre_pose.position, next_pose.position); + const double lerp_ratio = (length_to_search_idx - target_length) / seg_length; + target_pose.position.x = + pre_pose.position.x + (next_pose.position.x - pre_pose.position.x) * lerp_ratio; + target_pose.position.y = + pre_pose.position.y + (next_pose.position.y - pre_pose.position.y) * lerp_ratio; + target_pose.position.z = + pre_pose.position.z + (next_pose.position.z - pre_pose.position.z) * lerp_ratio; + + // lerp orientation + const double pre_yaw = tf2::getYaw(pre_pose.orientation); + const double next_yaw = tf2::getYaw(next_pose.orientation); + target_pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(pre_yaw + (next_yaw - pre_yaw) * lerp_ratio); + + return target_pose; +} +} // namespace + +OptimizationBasedPlanner::OptimizationBasedPlanner( + rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, + const vehicle_info_util::VehicleInfo & vehicle_info) +: PlannerInterface(node, longitudinal_info, vehicle_info) +{ + // parameter + resampling_s_interval_ = + node.declare_parameter("optimization_based_planner.resampling_s_interval"); + max_trajectory_length_ = + node.declare_parameter("optimization_based_planner.max_trajectory_length"); + dense_resampling_time_interval_ = + node.declare_parameter("optimization_based_planner.dense_resampling_time_interval"); + sparse_resampling_time_interval_ = + node.declare_parameter("optimization_based_planner.sparse_resampling_time_interval"); + dense_time_horizon_ = + node.declare_parameter("optimization_based_planner.dense_time_horizon"); + max_time_horizon_ = node.declare_parameter("optimization_based_planner.max_time_horizon"); + limit_min_accel_ = node.declare_parameter("optimization_based_planner.limit_min_accel"); + + delta_yaw_threshold_of_nearest_index_ = + tier4_autoware_utils::deg2rad(node.declare_parameter( + "optimization_based_planner.delta_yaw_threshold_of_nearest_index")); + delta_yaw_threshold_of_object_and_ego_ = + tier4_autoware_utils::deg2rad(node.declare_parameter( + "optimization_based_planner.delta_yaw_threshold_of_object_and_ego")); + object_zero_velocity_threshold_ = + node.declare_parameter("optimization_based_planner.object_zero_velocity_threshold"); + object_low_velocity_threshold_ = + node.declare_parameter("optimization_based_planner.object_low_velocity_threshold"); + external_velocity_limit_ = + node.declare_parameter("optimization_based_planner.external_velocity_limit"); + collision_time_threshold_ = + node.declare_parameter("optimization_based_planner.collision_time_threshold"); + safe_distance_margin_ = + node.declare_parameter("optimization_based_planner.safe_distance_margin"); + t_dangerous_ = node.declare_parameter("optimization_based_planner.t_dangerous"); + velocity_margin_ = node.declare_parameter("optimization_based_planner.velocity_margin"); + enable_adaptive_cruise_ = + node.declare_parameter("optimization_based_planner.enable_adaptive_cruise"); + use_object_acceleration_ = + node.declare_parameter("optimization_based_planner.use_object_acceleration"); + + replan_vel_deviation_ = + node.declare_parameter("optimization_based_planner.replan_vel_deviation"); + engage_velocity_ = node.declare_parameter("optimization_based_planner.engage_velocity"); + engage_acceleration_ = + node.declare_parameter("optimization_based_planner.engage_acceleration"); + engage_exit_ratio_ = + node.declare_parameter("optimization_based_planner.engage_exit_ratio"); + stop_dist_to_prohibit_engage_ = + node.declare_parameter("optimization_based_planner.stop_dist_to_prohibit_engage"); + + const double max_s_weight = + node.declare_parameter("optimization_based_planner.max_s_weight"); + const double max_v_weight = + node.declare_parameter("optimization_based_planner.max_v_weight"); + const double over_s_safety_weight = + node.declare_parameter("optimization_based_planner.over_s_safety_weight"); + const double over_s_ideal_weight = + node.declare_parameter("optimization_based_planner.over_s_ideal_weight"); + const double over_v_weight = + node.declare_parameter("optimization_based_planner.over_v_weight"); + const double over_a_weight = + node.declare_parameter("optimization_based_planner.over_a_weight"); + const double over_j_weight = + node.declare_parameter("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); + distance_to_closest_obj_pub_ = + node.create_publisher("~/distance_to_closest_obj", 1); + debug_calculation_time_ = node.create_publisher("~/calculation_time", 1); + debug_wall_marker_pub_ = + node.create_publisher("~/debug/wall_marker", 1); +} + +Trajectory OptimizationBasedPlanner::generateTrajectory( + const ObstacleCruisePlannerData & planner_data, + [[maybe_unused]] boost::optional & vel_limit, + [[maybe_unused]] DebugData & debug_data) +{ + // 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_ = planner_data.traj; + return planner_data.traj; + } + + // Get the nearest point on the trajectory + const auto closest_idx = tier4_autoware_utils::findNearestIndex( + planner_data.traj.points, planner_data.current_pose, delta_yaw_threshold_of_nearest_index_); + if (!closest_idx) { // Check validity of the closest index + RCLCPP_ERROR( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "Closest Index is Invalid"); + prev_output_ = planner_data.traj; + return planner_data.traj; + } + + // Transform original trajectory to TrajectoryData + const auto base_traj_data = getTrajectoryData(planner_data.traj, planner_data.current_pose); + if (base_traj_data.traj.points.size() < 2) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "The number of points on the trajectory data is too small"); + prev_output_ = planner_data.traj; + return planner_data.traj; + } + + // Get Closest Stop Point for static obstacles + double closest_stop_dist = getClosestStopDistance(planner_data, base_traj_data, time_vec); + + // Compute maximum velocity + double v_max = 0.0; + for (const auto & point : planner_data.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.current_vel, planner_data.traj, *closest_idx, prev_output_, closest_stop_dist); + a0 = std::min(longitudinal_info_.max_accel, std::max(a0, longitudinal_info_.min_accel)); + + // If closest distance is too close, return zero velocity + if (closest_stop_dist < 0.01) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "Closest Stop Point is too close"); + + auto output_traj = planner_data.traj; + output_traj.points.at(*closest_idx).longitudinal_velocity_mps = v0; + for (size_t i = *closest_idx + 1; i < output_traj.points.size(); ++i) { + output_traj.points.at(i).longitudinal_velocity_mps = 0.0; + } + prev_output_ = output_traj; + return output_traj; + } + + // Check trajectory size + if (planner_data.traj.points.size() - *closest_idx <= 2) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "The number of points on the trajectory is too small"); + prev_output_ = planner_data.traj; + return planner_data.traj; + } + + // Check if reached goal + if (checkHasReachedGoal(planner_data.traj, *closest_idx, v0) || !enable_adaptive_cruise_) { + prev_output_ = planner_data.traj; + return planner_data.traj; + } + + // Resample base trajectory data by time + const auto resampled_traj_data = resampleTrajectoryData( + base_traj_data, resampling_s_interval_, max_trajectory_length_, closest_stop_dist); + if (resampled_traj_data.traj.points.size() < 2) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "The number of points on the resampled trajectory data is too small"); + prev_output_ = planner_data.traj; + return planner_data.traj; + } + + // Get S Boundaries from the obstacle + const auto s_boundaries = getSBoundaries(planner_data, resampled_traj_data, time_vec); + if (!s_boundaries) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "No Dangerous Objects around the ego vehicle"); + prev_output_ = planner_data.traj; + return planner_data.traj; + } + + // Optimization + VelocityOptimizer::OptimizationData data; + data.time_vec = time_vec; + data.s0 = resampled_traj_data.s.front(); + data.a0 = a0; + data.v_max = v_max; + data.a_max = longitudinal_info_.max_accel; + data.a_min = longitudinal_info_.min_accel; + data.limit_a_min = limit_min_accel_; + data.j_max = longitudinal_info_.max_jerk; + data.j_min = longitudinal_info_.min_jerk; + data.t_dangerous = t_dangerous_; + data.idling_time = longitudinal_info_.idling_time; + data.v_margin = velocity_margin_; + data.s_boundary = *s_boundaries; + data.v0 = v0; + RCLCPP_DEBUG(rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), "v0 %f", v0); + + stop_watch_.tic(); + + const auto optimized_result = velocity_optimizer_ptr_->optimize(data); + + Float32Stamped calculation_time_data{}; + calculation_time_data.stamp = planner_data.current_time; + calculation_time_data.data = stop_watch_.toc(); + debug_calculation_time_->publish(calculation_time_data); + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "Optimization Time; %f[ms]", calculation_time_data.data); + + // Publish Debug trajectories + publishDebugTrajectory( + planner_data.current_time, planner_data.traj, *closest_idx, time_vec, *s_boundaries, + optimized_result); + + // Transformation from t to s + const auto processed_result = processOptimizedResult(data.v0, optimized_result); + if (!processed_result) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "Processed Result is empty"); + prev_output_ = planner_data.traj; + return planner_data.traj; + } + 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 = planner_data.traj; + output.points.at(*closest_idx).longitudinal_velocity_mps = data.v0; + for (size_t i = *closest_idx + 1; i < output.points.size(); ++i) { + output.points.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_ = planner_data.traj; + return planner_data.traj; + } + + // Get Zero Velocity Position + for (size_t i = 0; i < opt_velocity.size(); ++i) { + if (opt_velocity.at(i) < ZERO_VEL_THRESHOLD) { + const double zero_vel_s = opt_position.at(i); + closest_stop_dist = std::min(closest_stop_dist, zero_vel_s); + break; + } + } + + size_t break_id = base_traj_data.s.size(); + bool has_insert_stop_point = false; + std::vector resampled_opt_position = {base_traj_data.s.front()}; + for (size_t i = 1; i < base_traj_data.s.size(); ++i) { + const double query_s = base_traj_data.s.at(i); + if ( + !has_insert_stop_point && query_s > closest_stop_dist && + closest_stop_dist < opt_position.back()) { + const double prev_query_s = resampled_opt_position.back(); + if (closest_stop_dist - prev_query_s > CLOSE_S_DIST_THRESHOLD) { + resampled_opt_position.push_back(closest_stop_dist); + } else { + resampled_opt_position.back() = closest_stop_dist; + } + has_insert_stop_point = true; + } + + if (query_s > opt_position.back()) { + break_id = i; + break; + } + + const double prev_query_s = resampled_opt_position.back(); + if (query_s - prev_query_s > 1e-3) { + resampled_opt_position.push_back(query_s); + } + } + const auto resampled_opt_velocity = + interpolation::lerp(opt_position, opt_velocity, resampled_opt_position); + + // Push positions after the last position of the opt position + for (size_t i = break_id; i < base_traj_data.s.size(); ++i) { + const double query_s = base_traj_data.s.at(i); + const double prev_query_s = resampled_opt_position.back(); + if (query_s - prev_query_s > CLOSE_S_DIST_THRESHOLD) { + resampled_opt_position.push_back(query_s); + } + } + + auto resampled_traj = resampling::applyLinearInterpolation( + base_traj_data.s, base_traj_data.traj, resampled_opt_position); + for (size_t i = 0; i < resampled_opt_velocity.size(); ++i) { + resampled_traj.points.at(i).longitudinal_velocity_mps = std::min( + resampled_opt_velocity.at(i), + static_cast(resampled_traj.points.at(i).longitudinal_velocity_mps)); + } + for (size_t i = 0; i < resampled_opt_position.size(); ++i) { + if (resampled_opt_position.at(i) >= closest_stop_dist) { + resampled_traj.points.at(i).longitudinal_velocity_mps = 0.0; + } + } + + Trajectory output; + output.header = planner_data.traj.header; + for (size_t i = 0; i < *closest_idx; ++i) { + auto point = planner_data.traj.points.at(i); + point.longitudinal_velocity_mps = data.v0; + output.points.push_back(point); + } + for (const auto & resampled_point : resampled_traj.points) { + if (output.points.empty()) { + output.points.push_back(resampled_point); + } else { + const auto prev_point = output.points.back(); + const double dist = tier4_autoware_utils::calcDistance2d( + prev_point.pose.position, resampled_point.pose.position); + if (dist > 1e-3) { + output.points.push_back(resampled_point); + } else { + output.points.back() = resampled_point; + } + } + } + output.points.back().longitudinal_velocity_mps = 0.0; // terminal velocity is zero + + 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; +} + +double OptimizationBasedPlanner::getClosestStopDistance( + const ObstacleCruisePlannerData & planner_data, const TrajectoryData & ego_traj_data, + const std::vector & resolutions) +{ + const auto & current_time = planner_data.current_time; + double closest_stop_dist = ego_traj_data.s.back(); + const auto closest_stop_id = + tier4_autoware_utils::searchZeroVelocityIndex(ego_traj_data.traj.points); + closest_stop_dist = closest_stop_id + ? std::min(closest_stop_dist, ego_traj_data.s.at(*closest_stop_id)) + : closest_stop_dist; + + double closest_obj_distance = ego_traj_data.s.back(); + boost::optional closest_obj; + for (const auto & obj : planner_data.target_obstacles) { + const auto obj_base_time = obj.time_stamp; + + // Step1. Ignore obstacles that are not vehicles + if ( + obj.classification.label != ObjectClassification::CAR && + obj.classification.label != ObjectClassification::TRUCK && + obj.classification.label != ObjectClassification::BUS && + obj.classification.label != ObjectClassification::MOTORCYCLE) { + continue; + } + + // Step2 Check object position + if (!checkIsFrontObject(obj, ego_traj_data.traj)) { + continue; + } + + // Get the predicted path, which has the most high confidence + const auto predicted_path = + resampledPredictedPath(obj, obj_base_time, current_time, resolutions, max_time_horizon_); + if (!predicted_path) { + continue; + } + + // Get current pose from object's predicted path + const auto current_object_pose = obstacle_cruise_utils::getCurrentObjectPoseFromPredictedPath( + *predicted_path, obj_base_time, current_time); + if (!current_object_pose) { + continue; + } + + // Get current object.kinematics + ObjectData obj_data; + obj_data.pose = current_object_pose.get(); + obj_data.length = obj.shape.dimensions.x; + obj_data.width = obj.shape.dimensions.y; + obj_data.time = std::max((obj_base_time - current_time).seconds(), 0.0); + const double current_delta_yaw_threshold = 3.14; + const auto dist_to_collision_point = + getDistanceToCollisionPoint(ego_traj_data, obj_data, current_delta_yaw_threshold); + + // Calculate Safety Distance + const double ego_vehicle_offset = vehicle_info_.wheel_base_m + vehicle_info_.front_overhang_m; + const double object_offset = obj_data.length / 2.0; + const double safety_distance = ego_vehicle_offset + object_offset + safe_distance_margin_; + + // If the object is on the current ego trajectory, + // we assume the object travels along ego trajectory + const double obj_vel = std::abs(obj.velocity); + if (dist_to_collision_point && obj_vel < object_zero_velocity_threshold_) { + const double current_stop_point = std::max(*dist_to_collision_point - safety_distance, 0.0); + closest_stop_dist = std::min(current_stop_point, closest_stop_dist); + } + + // Update Distance to the closest object on the ego trajectory + if (dist_to_collision_point) { + const double current_obj_distance = std::max( + *dist_to_collision_point - safety_distance + safe_distance_margin_, -safety_distance); + closest_obj_distance = std::min(closest_obj_distance, current_obj_distance); + closest_obj = obj; + } + } + + // Publish distance from the ego vehicle to the object which is on the trajectory + if (closest_obj && closest_obj_distance < ego_traj_data.s.back()) { + Float32Stamped dist_to_obj; + dist_to_obj.stamp = planner_data.current_time; + dist_to_obj.data = closest_obj_distance; + distance_to_closest_obj_pub_->publish(dist_to_obj); + + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "Closest Object Distance %f", closest_obj_distance); + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "Closest Object Velocity %f", closest_obj.get().velocity); + } + + return closest_stop_dist; +} + +// v0, a0 +std::tuple OptimizationBasedPlanner::calcInitialMotion( + const double current_vel, const Trajectory & input_traj, const size_t input_closest, + const Trajectory & prev_traj, const double closest_stop_dist) +{ + const double vehicle_speed{std::abs(current_vel)}; + const double target_vel{std::abs(input_traj.points.at(input_closest).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 = calcInterpolatedTrajectoryPoint( + *smoothed_trajectory_ptr_, input_traj.points.at(input_closest).pose); + } else { + prev_output_closest_point = + calcInterpolatedTrajectoryPoint(prev_traj, input_traj.points.at(input_closest).pose); + } + + // 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 idx = tier4_autoware_utils::searchZeroVelocityIndex(input_traj.points); + const double zero_vel_dist = + idx ? tier4_autoware_utils::calcDistance2d( + input_traj.points.at(*idx), input_traj.points.at(input_closest)) + : 0.0; + const double stop_dist = std::min(zero_vel_dist, closest_stop_dist); + if (!idx || stop_dist > stop_dist_to_prohibit_engage_) { + initial_vel = engage_velocity_; + initial_acc = engage_acceleration_; + 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); + return std::make_tuple(initial_vel, initial_acc); + } else { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "calcInitialMotion : stop point is close (%.3f[m]). no engage.", stop_dist); + } + } 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; + /* + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "calcInitialMotion : normal update. v0 = %f, a0 = %f, vehicle_speed = %f, target_vel = %f", + initial_vel, initial_acc, vehicle_speed, target_vel); + */ + return std::make_tuple(initial_vel, initial_acc); +} + +TrajectoryPoint OptimizationBasedPlanner::calcInterpolatedTrajectoryPoint( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & target_pose) +{ + TrajectoryPoint traj_p{}; + traj_p.pose = target_pose; + + if (trajectory.points.empty()) { + traj_p.longitudinal_velocity_mps = 0.0; + traj_p.acceleration_mps2 = 0.0; + return traj_p; + } + + if (trajectory.points.size() == 1) { + traj_p.longitudinal_velocity_mps = trajectory.points.at(0).longitudinal_velocity_mps; + traj_p.acceleration_mps2 = trajectory.points.at(0).acceleration_mps2; + return traj_p; + } + + const size_t segment_idx = + tier4_autoware_utils::findNearestSegmentIndex(trajectory.points, target_pose.position); + + auto v1 = getTransVector3( + trajectory.points.at(segment_idx).pose, trajectory.points.at(segment_idx + 1).pose); + auto v2 = getTransVector3(trajectory.points.at(segment_idx).pose, target_pose); + // calc internal proportion + const double prop{std::max(0.0, std::min(1.0, v1.dot(v2) / v1.length2()))}; + + auto interpolate = [&prop](double x1, double x2) { return prop * x1 + (1.0 - prop) * x2; }; + + { + const auto & seg_pt = trajectory.points.at(segment_idx); + const auto & next_pt = trajectory.points.at(segment_idx + 1); + traj_p.longitudinal_velocity_mps = + interpolate(next_pt.longitudinal_velocity_mps, seg_pt.longitudinal_velocity_mps); + traj_p.acceleration_mps2 = interpolate(next_pt.acceleration_mps2, seg_pt.acceleration_mps2); + traj_p.pose.position.x = interpolate(next_pt.pose.position.x, seg_pt.pose.position.x); + traj_p.pose.position.y = interpolate(next_pt.pose.position.y, seg_pt.pose.position.y); + traj_p.pose.position.z = interpolate(next_pt.pose.position.z, seg_pt.pose.position.z); + } + + return traj_p; +} + +bool OptimizationBasedPlanner::checkHasReachedGoal( + const Trajectory & traj, const size_t closest_idx, const double v0) +{ + // If goal is close and current velocity is low, we don't optimize trajectory + const auto closest_stop_id = tier4_autoware_utils::searchZeroVelocityIndex(traj.points); + if (closest_stop_id) { + const double closest_stop_dist = + tier4_autoware_utils::calcSignedArcLength(traj.points, closest_idx, *closest_stop_id); + if (closest_stop_dist < 0.5 && v0 < 0.6) { + return true; + } + } + + return false; +} + +OptimizationBasedPlanner::TrajectoryData OptimizationBasedPlanner::getTrajectoryData( + const Trajectory & traj, const geometry_msgs::msg::Pose & current_pose) +{ + TrajectoryData base_traj; + const auto closest_segment_idx = + tier4_autoware_utils::findNearestSegmentIndex(traj.points, current_pose.position); + const auto interpolated_point = calcInterpolatedTrajectoryPoint(traj, current_pose); + const auto dist = tier4_autoware_utils::calcDistance2d( + interpolated_point.pose.position, traj.points.at(closest_segment_idx).pose.position); + const auto current_point = + dist > CLOSE_S_DIST_THRESHOLD ? interpolated_point : traj.points.at(closest_segment_idx); + base_traj.traj.points.push_back(current_point); + base_traj.s.push_back(0.0); + + for (size_t id = closest_segment_idx + 1; id < traj.points.size(); ++id) { + const auto prev_point = base_traj.traj.points.back(); + const double ds = tier4_autoware_utils::calcDistance2d( + prev_point.pose.position, traj.points.at(id).pose.position); + if (ds < CLOSE_S_DIST_THRESHOLD) { + continue; + } + const double current_s = base_traj.s.back() + ds; + + base_traj.traj.points.push_back(traj.points.at(id)); + base_traj.s.push_back(current_s); + } + + return base_traj; +} + +OptimizationBasedPlanner::TrajectoryData OptimizationBasedPlanner::resampleTrajectoryData( + const TrajectoryData & base_traj_data, const double resampling_s_interval, + const double max_traj_length, const double stop_dist) +{ + // Create Base Keys + std::vector base_s(base_traj_data.s.size()); + for (size_t i = 0; i < base_s.size(); ++i) { + base_s.at(i) = base_traj_data.s.at(i); + } + + // Obtain trajectory length until the velocity is zero or stop dist + const auto closest_stop_id = + tier4_autoware_utils::searchZeroVelocityIndex(base_traj_data.traj.points); + const double closest_stop_dist = + closest_stop_id ? std::min(stop_dist, base_traj_data.s.at(*closest_stop_id)) : stop_dist; + const double traj_length = std::min(closest_stop_dist, std::min(base_s.back(), max_traj_length)); + + // Create Query Keys + std::vector resampled_s; + for (double s = 0.0; s < traj_length; s += resampling_s_interval) { + resampled_s.push_back(s); + } + + if (traj_length - resampled_s.back() < CLOSE_S_DIST_THRESHOLD) { + resampled_s.back() = traj_length; + } else { + resampled_s.push_back(traj_length); + } + + if (resampled_s.empty()) { + return TrajectoryData{}; + } + + // Resample trajectory + const auto resampled_traj = resampleTrajectory(base_s, base_traj_data.traj, resampled_s); + + // Store Data + TrajectoryData resampled_traj_data; + resampled_traj_data.traj = resampled_traj; + resampled_traj_data.s = resampled_s; + + return resampled_traj_data; +} + +// TODO(shimizu) what is the difference with apply linear interpolation +Trajectory OptimizationBasedPlanner::resampleTrajectory( + const std::vector & base_index, const Trajectory & base_trajectory, + const std::vector & query_index, const bool use_spline_for_pose) +{ + std::vector px, py, pz, pyaw, tlx, taz, alx; + for (const auto & p : base_trajectory.points) { + px.push_back(p.pose.position.x); + py.push_back(p.pose.position.y); + pz.push_back(p.pose.position.z); + pyaw.push_back(tf2::getYaw(p.pose.orientation)); + tlx.push_back(p.longitudinal_velocity_mps); + taz.push_back(p.heading_rate_rps); + alx.push_back(p.acceleration_mps2); + } + + convertEulerAngleToMonotonic(pyaw); + + std::vector px_p, py_p, pz_p, pyaw_p; + if (use_spline_for_pose) { + px_p = interpolation::slerp(base_index, px, query_index); + py_p = interpolation::slerp(base_index, py, query_index); + pz_p = interpolation::slerp(base_index, pz, query_index); + pyaw_p = interpolation::slerp(base_index, pyaw, query_index); + } else { + px_p = interpolation::lerp(base_index, px, query_index); + py_p = interpolation::lerp(base_index, py, query_index); + pz_p = interpolation::lerp(base_index, pz, query_index); + pyaw_p = interpolation::lerp(base_index, pyaw, query_index); + } + const auto tlx_p = interpolation::lerp(base_index, tlx, query_index); + const auto taz_p = interpolation::lerp(base_index, taz, query_index); + const auto alx_p = interpolation::lerp(base_index, alx, query_index); + + Trajectory resampled_trajectory; + resampled_trajectory.header = base_trajectory.header; + resampled_trajectory.points.resize(query_index.size()); + + for (size_t i = 0; i < query_index.size(); ++i) { + TrajectoryPoint point; + point.pose.position.x = px_p.at(i); + point.pose.position.y = py_p.at(i); + point.pose.position.z = pz_p.at(i); + point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(pyaw_p.at(i)); + + point.longitudinal_velocity_mps = tlx_p.at(i); + point.heading_rate_rps = taz_p.at(i); + point.acceleration_mps2 = alx_p.at(i); + resampled_trajectory.points.at(i) = point; + } + return resampled_trajectory; +} + +boost::optional OptimizationBasedPlanner::getSBoundaries( + const ObstacleCruisePlannerData & planner_data, const TrajectoryData & ego_traj_data, + const std::vector & time_vec) +{ + if (ego_traj_data.traj.points.empty()) { + return boost::none; + } + + const auto & current_time = planner_data.current_time; + + SBoundaries s_boundaries(time_vec.size()); + for (size_t i = 0; i < s_boundaries.size(); ++i) { + s_boundaries.at(i).max_s = ego_traj_data.s.back(); + } + + double min_slow_down_point_length = std::numeric_limits::max(); + boost::optional min_slow_down_idx = {}; + for (size_t o_idx = 0; o_idx < planner_data.target_obstacles.size(); ++o_idx) { + const auto obj_base_time = planner_data.target_obstacles.at(o_idx).time_stamp; + + const auto & obj = planner_data.target_obstacles.at(o_idx); + // Step1 Check object position + if (!checkIsFrontObject(obj, ego_traj_data.traj)) { + continue; + } + + // Step2 Get S boundary from the obstacle + const auto obj_s_boundaries = + getSBoundaries(planner_data.current_time, ego_traj_data, obj, obj_base_time, time_vec); + if (!obj_s_boundaries) { + continue; + } + + // Step3 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); + } + } + + // Step4 add object to marker + // const auto marker = getObjectMarkerArray(obj.pose, o_idx, "objects_to_follow", 0.7, 0.7, + // 0.0); tier4_autoware_utils::appendMarkerArray(marker, &msg); + + // Step5 search nearest obstacle to follow for rviz marker + const double object_offset = obj.shape.dimensions.x / 2.0; + + const auto predicted_path = + resampledPredictedPath(obj, obj_base_time, current_time, time_vec, max_time_horizon_); + + const auto current_object_pose = obstacle_cruise_utils::getCurrentObjectPoseFromPredictedPath( + *predicted_path, obj_base_time, current_time); + + const double obj_vel = std::abs(obj.velocity); + const double rss_dist = calcRSSDistance(planner_data.current_vel, obj_vel); + + const double ego_obj_length = tier4_autoware_utils::calcSignedArcLength( + ego_traj_data.traj.points, planner_data.current_pose.position, + current_object_pose.get().position); + const double slow_down_point_length = + ego_obj_length - (rss_dist + object_offset + 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 & obj = planner_data.target_obstacles.at(min_slow_down_idx.get()); + + const auto predicted_path = + resampledPredictedPath(obj, obj.time_stamp, current_time, time_vec, max_time_horizon_); + + const auto current_object_pose = obstacle_cruise_utils::getCurrentObjectPoseFromPredictedPath( + *predicted_path, obj.time_stamp, current_time); + + const auto marker_pose = calcForwardPose( + ego_traj_data.traj, planner_data.current_pose.position, min_slow_down_point_length); + + if (marker_pose) { + visualization_msgs::msg::MarkerArray wall_msg; + + const double obj_vel = std::abs(obj.velocity); + if (obj_vel < object_zero_velocity_threshold_) { + const auto markers = tier4_autoware_utils::createStopVirtualWallMarker( + marker_pose.get(), "obstacle to follow", current_time, 0); + tier4_autoware_utils::appendMarkerArray(markers, &wall_msg); + } else { + const auto markers = tier4_autoware_utils::createSlowDownVirtualWallMarker( + marker_pose.get(), "obstacle to follow", current_time, 0); + tier4_autoware_utils::appendMarkerArray(markers, &wall_msg); + } + + // publish rviz marker + debug_wall_marker_pub_->publish(wall_msg); + } + } + + return s_boundaries; +} + +boost::optional OptimizationBasedPlanner::getSBoundaries( + const rclcpp::Time & current_time, const TrajectoryData & ego_traj_data, + const TargetObstacle & object, const rclcpp::Time & obj_base_time, + const std::vector & time_vec) +{ + // Get the predicted path, which has the most high confidence + const double max_horizon = time_vec.back(); + const auto predicted_path = + resampledPredictedPath(object, obj_base_time, current_time, time_vec, max_horizon); + if (!predicted_path) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "Closest Obstacle does not have a predicted path"); + return boost::none; + } + + // Get current pose from object's predicted path + const auto current_object_pose = obstacle_cruise_utils::getCurrentObjectPoseFromPredictedPath( + *predicted_path, obj_base_time, current_time); + if (!current_object_pose) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "Failed to get current pose from the predicted path"); + return boost::none; + } + + // Check current object.kinematics + ObjectData obj_data; + obj_data.pose = current_object_pose.get(); + obj_data.length = object.shape.dimensions.x; + obj_data.width = object.shape.dimensions.y; + obj_data.time = std::max((obj_base_time - current_time).seconds(), 0.0); + const double current_delta_yaw_threshold = 3.14; + const auto current_collision_dist = + getDistanceToCollisionPoint(ego_traj_data, obj_data, current_delta_yaw_threshold); + + // Calculate Safety Distance + const double ego_vehicle_offset = vehicle_info_.wheel_base_m + vehicle_info_.front_overhang_m; + const double object_offset = obj_data.length / 2.0; + const double safety_distance = ego_vehicle_offset + object_offset + safe_distance_margin_; + + // If the object is on the current ego trajectory, + // we assume the object travels along ego trajectory + if (current_collision_dist) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "On Trajectory Object"); + + return getSBoundaries( + ego_traj_data, time_vec, safety_distance, object, *current_collision_dist); + } + + // Ignore low velocity objects that are not on the trajectory + const double obj_vel = std::abs(object.velocity); + if (obj_vel > object_low_velocity_threshold_) { + // RCLCPP_DEBUG(rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), "Off + // Trajectory Object"); + return getSBoundaries( + current_time, ego_traj_data, time_vec, safety_distance, object, obj_base_time, + *predicted_path); + } + + return boost::none; +} + +boost::optional OptimizationBasedPlanner::getSBoundaries( + const TrajectoryData & ego_traj_data, const std::vector & time_vec, + const double safety_distance, const TargetObstacle & object, const double dist_to_collision_point) +{ + const double & min_object_accel_for_rss = longitudinal_info_.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 = ego_traj_data.s.back(); + } + + const double v_obj = std::abs(object.velocity); + const double a_obj = 0.0; + + double current_v_obj = v_obj < object_zero_velocity_threshold_ ? 0.0 : v_obj; + double current_s_obj = std::max(dist_to_collision_point - safety_distance, 0.0); + const double initial_s_obj = current_s_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); + if (i * dt <= 1.0 && use_object_acceleration_) { + current_s_obj = + std::max(current_s_obj + current_v_obj * dt + 0.5 * a_obj * dt * dt, initial_s_obj); + current_v_obj = std::max(current_v_obj + a_obj * dt, 0.0); + } else { + 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; +} + +boost::optional OptimizationBasedPlanner::getSBoundaries( + const rclcpp::Time & current_time, const TrajectoryData & ego_traj_data, + const std::vector & time_vec, const double safety_distance, const TargetObstacle & object, + const rclcpp::Time & obj_base_time, const PredictedPath & predicted_path) +{ + const double & min_object_accel_for_rss = longitudinal_info_.min_object_accel_for_rss; + + const double abs_obj_vel = std::abs(object.velocity); + const double v_obj = abs_obj_vel < object_zero_velocity_threshold_ ? 0.0 : abs_obj_vel; + + SBoundaries s_boundaries(time_vec.size()); + for (size_t i = 0; i < s_boundaries.size(); ++i) { + s_boundaries.at(i).max_s = ego_traj_data.s.back(); + } + + for (size_t predicted_path_id = 0; predicted_path_id < predicted_path.path.size(); + ++predicted_path_id) { + const auto predicted_pose = predicted_path.path.at(predicted_path_id); + const double object_time = (obj_base_time - current_time).seconds(); + if (object_time < 0) { + // Ignore Past Positions + continue; + } + + ObjectData obj_data; + obj_data.pose = predicted_pose; + obj_data.length = object.shape.dimensions.x; + obj_data.width = object.shape.dimensions.y; + obj_data.time = object_time; + + const auto dist_to_collision_point = + getDistanceToCollisionPoint(ego_traj_data, obj_data, delta_yaw_threshold_of_object_and_ego_); + 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)); + for (size_t i = 0; i < predicted_path_id; ++i) { + if (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; +} + +boost::optional OptimizationBasedPlanner::getDistanceToCollisionPoint( + const TrajectoryData & ego_traj_data, const ObjectData & obj_data, + const double delta_yaw_threshold) +{ + const auto obj_pose = obj_data.pose; + const auto obj_length = obj_data.length; + const auto obj_width = obj_data.width; + + // check diff yaw + const size_t nearest_idx = + tier4_autoware_utils::findNearestIndex(ego_traj_data.traj.points, obj_pose.position); + const double ego_yaw = tf2::getYaw(ego_traj_data.traj.points.at(nearest_idx).pose.orientation); + const double obj_yaw = tf2::getYaw(obj_pose.orientation); + const double diff_yaw = tier4_autoware_utils::normalizeRadian(obj_yaw - ego_yaw); + if (diff_yaw > delta_yaw_threshold) { + // ignore object whose yaw difference from ego is too large + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "Ignore object since the yaw difference is above the threshold"); + return boost::none; + } + + const auto object_box = Box2d(obj_pose, obj_length, obj_width); + const auto object_points = object_box.getAllCorners(); + + // Get nearest segment index for each point + size_t min_nearest_idx = ego_traj_data.s.size(); + size_t max_nearest_idx = 0; + for (const auto & obj_p : object_points) { + size_t nearest_idx = + tier4_autoware_utils::findNearestSegmentIndex(ego_traj_data.traj.points, obj_p); + min_nearest_idx = std::min(nearest_idx, min_nearest_idx); + max_nearest_idx = std::max(nearest_idx, max_nearest_idx); + } + + double min_len = 0.0; + size_t start_idx = 0; + for (size_t i = min_nearest_idx; i > 0; --i) { + min_len += (ego_traj_data.s.at(i) - ego_traj_data.s.at(i - 1)); + if (min_len > 5.0) { + start_idx = i - 1; + break; + } + } + + double max_len = 0.0; + size_t end_idx = ego_traj_data.s.size() - 1; + for (size_t i = max_nearest_idx; i < ego_traj_data.s.size() - 1; ++i) { + max_len += (ego_traj_data.s.at(i + 1) - ego_traj_data.s.at(i)); + if (max_len > 5.0) { + end_idx = i + 1; + break; + } + } + + // Check collision + const auto collision_idx = getCollisionIdx(ego_traj_data, object_box, start_idx, end_idx); + + if (collision_idx) { + // TODO(shimizu) Consider the time difference between ego vehicle and objects + return tier4_autoware_utils::calcSignedArcLength( + ego_traj_data.traj.points, ego_traj_data.traj.points.front().pose.position, + obj_pose.position); + } + + return boost::none; +} + +boost::optional OptimizationBasedPlanner::getCollisionIdx( + const TrajectoryData & ego_traj, const Box2d & obj_box, const size_t start_idx, + const size_t end_idx) +{ + for (size_t ego_idx = start_idx; ego_idx <= end_idx; ++ego_idx) { + const auto ego_center_pose = transformBaseLink2Center(ego_traj.traj.points.at(ego_idx).pose); + const auto ego_box = + Box2d(ego_center_pose, vehicle_info_.vehicle_length_m, vehicle_info_.vehicle_width_m); + if (ego_box.hasOverlap(obj_box)) { + return ego_idx; + } + } + + return boost::none; +} + +bool OptimizationBasedPlanner::checkIsFrontObject( + const TargetObstacle & object, const Trajectory & traj) +{ + const auto point = object.pose.position; + + // Get nearest segment index + size_t nearest_idx = tier4_autoware_utils::findNearestSegmentIndex(traj.points, point); + + // Calculate longitudinal length + const double l = + tier4_autoware_utils::calcLongitudinalOffsetToSegment(traj.points, nearest_idx, point); + + if (nearest_idx == 0 && l < 0) { + return false; + } + + return true; +} + +boost::optional OptimizationBasedPlanner::resampledPredictedPath( + const TargetObstacle & object, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time, const std::vector & resolutions, const double horizon) +{ + if (object.predicted_paths.empty()) { + return boost::none; + } + + // Get the most reliable path + const auto reliable_path = std::max_element( + object.predicted_paths.begin(), object.predicted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); + + // Resample Predicted Path + const double duration = std::min( + std::max( + (obj_base_time + + rclcpp::Duration(reliable_path->time_step) * + (static_cast(reliable_path->path.size()) - 1) - + current_time) + .seconds(), + 0.0), + horizon); + + // Calculate relative valid time vector + // rel_valid_time_vec is relative to obj_base_time. + const auto rel_valid_time_vec = resampling::resampledValidRelativeTimeVector( + current_time, obj_base_time, resolutions, duration); + + return resampling::resamplePredictedPath(*reliable_path, rel_valid_time_vec); +} + +geometry_msgs::msg::Pose OptimizationBasedPlanner::transformBaseLink2Center( + const geometry_msgs::msg::Pose & pose_base_link) +{ + tf2::Transform tf_map2base; + tf2::fromMsg(pose_base_link, tf_map2base); + + // set vertices at map coordinate + tf2::Vector3 base2center; + base2center.setX(std::abs(vehicle_info_.vehicle_length_m / 2.0 - vehicle_info_.rear_overhang_m)); + base2center.setY(0.0); + base2center.setZ(0.0); + base2center.setW(1.0); + + // transform vertices from map coordinate to object coordinate + const auto map2center = tf_map2base * base2center; + + geometry_msgs::msg::Pose center_pose; + center_pose.position = + tier4_autoware_utils::createPoint(map2center.x(), map2center.y(), map2center.z()); + center_pose.orientation = pose_base_link.orientation; + + return center_pose; +} + +boost::optional +OptimizationBasedPlanner::processOptimizedResult( + const double v0, const VelocityOptimizer::OptimizationResult & opt_result) +{ + if ( + opt_result.t.empty() || opt_result.s.empty() || opt_result.v.empty() || opt_result.a.empty() || + opt_result.j.empty()) { + return boost::none; + } + + size_t break_id = opt_result.s.size(); + VelocityOptimizer::OptimizationResult processed_result; + processed_result.t.push_back(0.0); + processed_result.s.push_back(0.0); + 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); + 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() = prev_s > 0.0 ? current_s : prev_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); + 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() = prev_s > 0.0 ? current_s : prev_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( + const rclcpp::Time & current_time, const Trajectory & traj, const size_t closest_idx, + const std::vector & time_vec, const SBoundaries & s_boundaries, + const VelocityOptimizer::OptimizationResult & opt_result) +{ + const std::vector time = opt_result.t; + // 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); + + const double s_before = tier4_autoware_utils::calcSignedArcLength(traj.points, 0, closest_idx); + 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 + s_before; + 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); +} diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/resample.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/resample.cpp new file mode 100644 index 0000000000000..a629abca7f0dd --- /dev/null +++ b/planning/obstacle_cruise_planner/src/optimization_based_planner/resample.cpp @@ -0,0 +1,220 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_cruise_planner/optimization_based_planner/resample.hpp" + +#include "interpolation/linear_interpolation.hpp" +#include "interpolation/spline_interpolation.hpp" + +#include + +namespace +{ +[[maybe_unused]] rclcpp::Duration safeSubtraction(const rclcpp::Time & t1, const rclcpp::Time & t2) +{ + rclcpp::Duration duration = rclcpp::Duration::from_seconds(0.0); + try { + duration = t1 - t2; + } catch (std::runtime_error & err) { + if (t1 > t2) { + duration = rclcpp::Duration::max() * -1.0; + } else { + duration = rclcpp::Duration::max(); + } + } + return duration; +} + +// tf2::toMsg does not have this type of function +geometry_msgs::msg::Point toMsg(tf2::Vector3 vec) +{ + geometry_msgs::msg::Point point; + point.x = vec.x(); + point.y = vec.y(); + point.z = vec.z(); + return point; +} +} // namespace + +namespace resampling +{ +std::vector resampledValidRelativeTimeVector( + const rclcpp::Time & start_time, const rclcpp::Time & obj_base_time, + const std::vector & rel_time_vec, const double duration) +{ + const auto prediction_duration = rclcpp::Duration::from_seconds(duration); + const auto end_time = start_time + prediction_duration; + + // NOTE: rel_time_vec is relative time to start_time. + // rel_valid_time_vec is relative to obj_base_time, which is time stamp in predicted object. + std::vector rel_valid_time_vec; + for (const auto & time : rel_time_vec) { + // absolute target time + const auto target_time = start_time + rclcpp::Duration::from_seconds(time); + if (target_time > end_time) { + break; + } + + // relative target time + const auto rel_target_time = target_time - obj_base_time; + if (rel_target_time < rclcpp::Duration::from_seconds(0.0)) { + continue; + } + + rel_valid_time_vec.push_back(rel_target_time); + } + + return rel_valid_time_vec; +} + +autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_auto_perception_msgs::msg::PredictedPath & input_path, + const std::vector & rel_time_vec) +{ + autoware_auto_perception_msgs::msg::PredictedPath resampled_path; + resampled_path.time_step = input_path.time_step; + + for (const auto & rel_time : rel_time_vec) { + const auto opt_pose = lerpByTimeStamp(input_path, rel_time); + if (!opt_pose) { + continue; + } + + resampled_path.path.push_back(opt_pose.get()); + } + + return resampled_path; +} + +geometry_msgs::msg::Pose lerpByPose( + const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t) +{ + tf2::Transform tf_transform1, tf_transform2; + tf2::fromMsg(p1, tf_transform1); + tf2::fromMsg(p2, tf_transform2); + const auto & tf_point = tf2::lerp(tf_transform1.getOrigin(), tf_transform2.getOrigin(), t); + const auto & tf_quaternion = + tf2::slerp(tf_transform1.getRotation(), tf_transform2.getRotation(), t); + + geometry_msgs::msg::Pose pose; + pose.position = ::toMsg(tf_point); + pose.orientation = tf2::toMsg(tf_quaternion); + return pose; +} + +boost::optional lerpByTimeStamp( + const autoware_auto_perception_msgs::msg::PredictedPath & path, const rclcpp::Duration & rel_time) +{ + auto clock{rclcpp::Clock{RCL_ROS_TIME}}; + if (path.path.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + rclcpp::get_logger("DynamicAvoidance.resample"), clock, 1000, + "Empty path. Failed to interpolate path by time!"); + return {}; + } + if (rel_time < rclcpp::Duration::from_seconds(0.0)) { + RCLCPP_DEBUG_STREAM( + rclcpp::get_logger("DynamicAvoidance.resample"), "failed to interpolate path by time!" + << std::endl + << "query time: " << rel_time.seconds()); + + return {}; + } + + if (rel_time > rclcpp::Duration(path.time_step) * (static_cast(path.path.size()) - 1)) { + RCLCPP_DEBUG_STREAM( + rclcpp::get_logger("DynamicAvoidance.resample"), + "failed to interpolate path by time!" + << std::endl + << "path max duration: " << path.path.size() * rclcpp::Duration(path.time_step).seconds() + << std::endl + << "query time : " << rel_time.seconds()); + + return {}; + } + + for (size_t i = 1; i < path.path.size(); ++i) { + const auto & pt = path.path.at(i); + const auto & prev_pt = path.path.at(i - 1); + if (rel_time <= rclcpp::Duration(path.time_step) * static_cast(i)) { + const auto offset = rel_time - rclcpp::Duration(path.time_step) * static_cast(i - 1); + const auto ratio = offset.seconds() / rclcpp::Duration(path.time_step).seconds(); + return lerpByPose(prev_pt, pt, ratio); + } + } + + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("DynamicAvoidance.resample"), "Something failed in function: " << __func__); + return {}; +} + +inline void convertEulerAngleToMonotonic(std::vector & a) +{ + for (unsigned int i = 1; i < a.size(); ++i) { + const double da = a[i] - a[i - 1]; + a[i] = a[i - 1] + tier4_autoware_utils::normalizeRadian(da); + } +} + +autoware_auto_planning_msgs::msg::Trajectory applyLinearInterpolation( + const std::vector & base_index, + const autoware_auto_planning_msgs::msg::Trajectory & base_trajectory, + const std::vector & out_index, const bool use_spline_for_pose) +{ + std::vector px, py, pz, pyaw, tlx, taz, alx; + for (const auto & p : base_trajectory.points) { + px.push_back(p.pose.position.x); + py.push_back(p.pose.position.y); + pz.push_back(p.pose.position.z); + pyaw.push_back(tf2::getYaw(p.pose.orientation)); + tlx.push_back(p.longitudinal_velocity_mps); + taz.push_back(p.heading_rate_rps); + alx.push_back(p.acceleration_mps2); + } + + convertEulerAngleToMonotonic(pyaw); + + std::vector px_p, py_p, pz_p, pyaw_p; + if (use_spline_for_pose) { + px_p = interpolation::slerp(base_index, px, out_index); + py_p = interpolation::slerp(base_index, py, out_index); + pz_p = interpolation::slerp(base_index, pz, out_index); + pyaw_p = interpolation::slerp(base_index, pyaw, out_index); + } else { + px_p = interpolation::lerp(base_index, px, out_index); + py_p = interpolation::lerp(base_index, py, out_index); + pz_p = interpolation::lerp(base_index, pz, out_index); + pyaw_p = interpolation::lerp(base_index, pyaw, out_index); + } + const auto tlx_p = interpolation::lerp(base_index, tlx, out_index); + const auto taz_p = interpolation::lerp(base_index, taz, out_index); + const auto alx_p = interpolation::lerp(base_index, alx, out_index); + + autoware_auto_planning_msgs::msg::Trajectory out_trajectory; + out_trajectory.header = base_trajectory.header; + autoware_auto_planning_msgs::msg::TrajectoryPoint point; + for (unsigned int i = 0; i < out_index.size(); ++i) { + point.pose.position.x = px_p.at(i); + point.pose.position.y = py_p.at(i); + point.pose.position.z = pz_p.at(i); + point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(pyaw_p.at(i)); + + point.longitudinal_velocity_mps = tlx_p.at(i); + point.heading_rate_rps = taz_p.at(i); + point.acceleration_mps2 = alx_p.at(i); + out_trajectory.points.push_back(point); + } + return out_trajectory; +} +} // namespace resampling diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp new file mode 100644 index 0000000000000..990e30842e6ea --- /dev/null +++ b/planning/obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp @@ -0,0 +1,276 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp" + +#include + +#include + +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_min = data.limit_a_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 double v_margin = data.v_margin; + 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) = 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) = 1.5; + lower_bound.at(constr_idx) = -1.5; + } + + // 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; + } + + 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_vel.at(i) = + opt_vel.at(i) > 0.01 ? std::min(opt_vel.at(i) + v_margin, v_max) : opt_vel.at(i); + opt_acc.at(i) = optval.at(IDX_A0 + i); + opt_jerk.at(i) = optval.at(IDX_J0 + i); + } + opt_vel.back() = 0.0; + + OptimizationResult optimized_result; + 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; +} diff --git a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp new file mode 100644 index 0000000000000..5d2a6ba7920af --- /dev/null +++ b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -0,0 +1,537 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" + +#include "obstacle_cruise_planner/utils.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include "tier4_planning_msgs/msg/velocity_limit.hpp" + +namespace +{ +StopSpeedExceeded createStopSpeedExceededMsg( + const rclcpp::Time & current_time, const bool stop_flag) +{ + StopSpeedExceeded msg{}; + msg.stamp = current_time; + msg.stop_speed_exceeded = stop_flag; + return msg; +} + +VelocityLimit createVelocityLimitMsg( + 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_planner"; + 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; +} + +Float32MultiArrayStamped convertDebugValuesToMsg( + const rclcpp::Time & current_time, const DebugValues & debug_values) +{ + Float32MultiArrayStamped debug_msg{}; + debug_msg.stamp = current_time; + for (const auto & v : debug_values.getValues()) { + debug_msg.data.push_back(v); + } + return debug_msg; +} + +template +size_t getIndexWithLongitudinalOffset( + const T & points, const double longitudinal_offset, boost::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.get()) { + 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.get(); i < points.size() - 1; ++i) { + const double segment_length = + tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + sum_length += segment_length; + if (sum_length >= longitudinal_offset) { + const double front_length = segment_length; + const double back_length = sum_length - longitudinal_offset; + if (front_length < back_length) { + return i; + } else { + return i + 1; + } + } + } + return points.size() - 1; + } + + for (size_t i = start_idx.get(); i > 0; --i) { + const double segment_length = + tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + sum_length += segment_length; + if (sum_length >= -longitudinal_offset) { + const double front_length = segment_length; + const double back_length = sum_length + longitudinal_offset; + if (front_length < back_length) { + return i; + } else { + return i + 1; + } + } + } + return 0; +} + +double calcMinimumDistanceToStop(const double initial_vel, const double min_acc) +{ + return -std::pow(initial_vel, 2) / 2.0 / min_acc; +} + +tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray( + const rclcpp::Time & current_time, const geometry_msgs::msg::Pose & stop_pose, + const boost::optional & stop_obstacle_info) +{ + // create header + std_msgs::msg::Header header; + header.frame_id = "map"; + header.stamp = current_time; + + // create stop factor + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = stop_pose; + if (stop_obstacle_info) { + stop_factor.stop_factor_points.emplace_back(stop_obstacle_info->obstacle.collision_point); + } + + // create stop reason stamped + tier4_planning_msgs::msg::StopReason stop_reason_msg; + stop_reason_msg.reason = tier4_planning_msgs::msg::StopReason::OBSTACLE_STOP; + stop_reason_msg.stop_factors.emplace_back(stop_factor); + + // create stop reason array + tier4_planning_msgs::msg::StopReasonArray stop_reason_array; + stop_reason_array.header = header; + stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); + return stop_reason_array; +} +} // namespace + +PIDBasedPlanner::PIDBasedPlanner( + rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, + const vehicle_info_util::VehicleInfo & vehicle_info) +: PlannerInterface(node, longitudinal_info, vehicle_info) +{ + min_accel_during_cruise_ = + node.declare_parameter("pid_based_planner.min_accel_during_cruise"); + + // pid controller + const double kp = node.declare_parameter("pid_based_planner.kp"); + const double ki = node.declare_parameter("pid_based_planner.ki"); + const double kd = node.declare_parameter("pid_based_planner.kd"); + pid_controller_ = std::make_unique(kp, ki, kd); + output_ratio_during_accel_ = + node.declare_parameter("pid_based_planner.output_ratio_during_accel"); + + // some parameters + // use_predicted_obstacle_pose_ = + // node.declare_parameter("pid_based_planner.use_predicted_obstacle_pose"); + + vel_to_acc_weight_ = node.declare_parameter("pid_based_planner.vel_to_acc_weight"); + + min_cruise_target_vel_ = + node.declare_parameter("pid_based_planner.min_cruise_target_vel"); + + obstacle_velocity_threshold_from_cruise_to_stop_ = node.declare_parameter( + "pid_based_planner.obstacle_velocity_threshold_from_cruise_to_stop"); + + // publisher + stop_reasons_pub_ = + node.create_publisher("~/output/stop_reasons", 1); + stop_speed_exceeded_pub_ = + node.create_publisher("~/output/stop_speed_exceeded", 1); + debug_values_pub_ = node.create_publisher("~/debug/values", 1); +} + +Trajectory PIDBasedPlanner::generateTrajectory( + const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, + DebugData & debug_data) +{ + stop_watch_.tic(__func__); + debug_values_.resetValues(); + + // calc obstacles to cruise and stop + boost::optional stop_obstacle_info; + boost::optional cruise_obstacle_info; + calcObstaclesToCruiseAndStop(planner_data, stop_obstacle_info, cruise_obstacle_info); + + // plan cruise + planCruise(planner_data, vel_limit, cruise_obstacle_info, debug_data); + + // plan stop + const auto output_traj = planStop(planner_data, stop_obstacle_info, debug_data); + + // publish debug values + publishDebugValues(planner_data); + + const double calculation_time = stop_watch_.toc(__func__); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), is_showing_debug_info_, + " %s := %f [ms]", __func__, calculation_time); + + return output_traj; +} + +void PIDBasedPlanner::calcObstaclesToCruiseAndStop( + const ObstacleCruisePlannerData & planner_data, + boost::optional & stop_obstacle_info, + boost::optional & cruise_obstacle_info) +{ + debug_values_.setValues(DebugValues::TYPE::CURRENT_VELOCITY, planner_data.current_vel); + debug_values_.setValues(DebugValues::TYPE::CURRENT_ACCELERATION, planner_data.current_acc); + + // search highest probability obstacle for stop and cruise + for (const auto & obstacle : planner_data.target_obstacles) { + // NOTE: from ego's front to obstacle's back + const double dist_to_obstacle = calcDistanceToObstacle(planner_data, obstacle); + + const bool is_stop_required = isStopRequired(obstacle); + if (is_stop_required) { // stop + // calculate error distance (= distance to stop) + const double error_dist = dist_to_obstacle - longitudinal_info_.safe_distance_margin; + if (stop_obstacle_info) { + if (error_dist > stop_obstacle_info->dist_to_stop) { + return; + } + } + stop_obstacle_info = StopObstacleInfo(obstacle, error_dist); + + // update debug values + debug_values_.setValues(DebugValues::TYPE::STOP_CURRENT_OBJECT_DISTANCE, dist_to_obstacle); + debug_values_.setValues(DebugValues::TYPE::STOP_CURRENT_OBJECT_VELOCITY, obstacle.velocity); + debug_values_.setValues( + DebugValues::TYPE::STOP_TARGET_OBJECT_DISTANCE, longitudinal_info_.safe_distance_margin); + debug_values_.setValues( + DebugValues::TYPE::STOP_TARGET_ACCELERATION, longitudinal_info_.min_strong_accel); + debug_values_.setValues(DebugValues::TYPE::STOP_ERROR_OBJECT_DISTANCE, error_dist); + } else { // cruise + // calculate distance between ego and obstacle based on RSS + const double rss_dist = calcRSSDistance( + planner_data.current_vel, obstacle.velocity, longitudinal_info_.safe_distance_margin); + + // calculate error distance and normalized one + const double error_dist = dist_to_obstacle - rss_dist; + if (cruise_obstacle_info) { + if (error_dist > cruise_obstacle_info->dist_to_cruise) { + return; + } + } + const double normalized_dist_to_cruise = error_dist / dist_to_obstacle; + cruise_obstacle_info = CruiseObstacleInfo(obstacle, error_dist, normalized_dist_to_cruise); + + // update debug values + debug_values_.setValues(DebugValues::TYPE::CRUISE_CURRENT_OBJECT_VELOCITY, obstacle.velocity); + debug_values_.setValues(DebugValues::TYPE::CRUISE_CURRENT_OBJECT_DISTANCE, dist_to_obstacle); + debug_values_.setValues(DebugValues::TYPE::CRUISE_TARGET_OBJECT_DISTANCE, rss_dist); + debug_values_.setValues(DebugValues::TYPE::CRUISE_ERROR_OBJECT_DISTANCE, error_dist); + } + } +} + +double PIDBasedPlanner::calcDistanceToObstacle( + const ObstacleCruisePlannerData & planner_data, const TargetObstacle & obstacle) +{ + const double offset = vehicle_info_.max_longitudinal_offset_m; + + // TODO(murooka) enable this option considering collision_point (precise obstacle point to measure + // distance) if (use_predicted_obstacle_pose_) { + // // interpolate current obstacle pose from predicted path + // const auto current_interpolated_obstacle_pose = + // obstacle_cruise_utils::getCurrentObjectPoseFromPredictedPath( + // obstacle.predicted_paths.at(0), obstacle.time_stamp, planner_data.current_time); + // if (current_interpolated_obstacle_pose) { + // return tier4_autoware_utils::calcSignedArcLength( + // planner_data.traj.points, planner_data.current_pose.position, + // current_interpolated_obstacle_pose->position) - offset; + // } + // + // RCLCPP_INFO_EXPRESSION( + // rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), true, + // "Failed to interpolated obstacle pose from predicted path. Use non-interpolated obstacle + // pose."); + // } + + const size_t ego_idx = findExtendedNearestIndex(planner_data.traj, planner_data.current_pose); + return tier4_autoware_utils::calcSignedArcLength( + planner_data.traj.points, ego_idx, obstacle.collision_point) - + offset; +} + +// Note: If stop planning is not required, cruise planning will be done instead. +bool PIDBasedPlanner::isStopRequired(const TargetObstacle & obstacle) +{ + const bool is_cruise_obstacle = isCruiseObstacle(obstacle.classification.label); + const bool is_stop_obstacle = isStopObstacle(obstacle.classification.label); + + if (is_cruise_obstacle) { + return std::abs(obstacle.velocity) < obstacle_velocity_threshold_from_cruise_to_stop_; + } else if (is_stop_obstacle && !is_cruise_obstacle) { + return true; + } + + return false; +} + +Trajectory PIDBasedPlanner::planStop( + const ObstacleCruisePlannerData & planner_data, + const boost::optional & stop_obstacle_info, DebugData & debug_data) +{ + bool will_collide_with_obstacle = false; + + size_t zero_vel_idx = 0; + bool zero_vel_found = false; + if (stop_obstacle_info) { + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), is_showing_debug_info_, + "stop planning"); + + auto local_stop_obstacle_info = stop_obstacle_info.get(); + + // check if the ego will collide with the obstacle with a limit acceleration + const double feasible_dist_to_stop = + calcMinimumDistanceToStop(planner_data.current_vel, longitudinal_info_.min_strong_accel); + if (local_stop_obstacle_info.dist_to_stop < feasible_dist_to_stop) { + will_collide_with_obstacle = true; + local_stop_obstacle_info.dist_to_stop = feasible_dist_to_stop; + } + + // set zero velocity index + const auto opt_zero_vel_idx = doStop( + planner_data, local_stop_obstacle_info, debug_data.obstacles_to_stop, + debug_data.stop_wall_marker); + if (opt_zero_vel_idx) { + zero_vel_idx = opt_zero_vel_idx.get(); + zero_vel_found = true; + } + } + + // generate output trajectory + auto output_traj = planner_data.traj; + if (zero_vel_found) { + // publish stop reason + const auto stop_pose = planner_data.traj.points.at(zero_vel_idx).pose; + const auto stop_reasons_msg = + makeStopReasonArray(planner_data.current_time, stop_pose, stop_obstacle_info); + stop_reasons_pub_->publish(stop_reasons_msg); + + // insert zero_velocity + for (size_t traj_idx = zero_vel_idx; traj_idx < output_traj.points.size(); ++traj_idx) { + output_traj.points.at(traj_idx).longitudinal_velocity_mps = 0.0; + } + } + + // publish stop_speed_exceeded if the ego will collide with the obstacle + const auto stop_speed_exceeded_msg = + createStopSpeedExceededMsg(planner_data.current_time, will_collide_with_obstacle); + stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg); + + return output_traj; +} + +boost::optional PIDBasedPlanner::doStop( + const ObstacleCruisePlannerData & planner_data, const StopObstacleInfo & stop_obstacle_info, + std::vector & debug_obstacles_to_stop, + visualization_msgs::msg::MarkerArray & debug_wall_marker) const +{ + const size_t ego_idx = findExtendedNearestIndex(planner_data.traj, planner_data.current_pose); + + // TODO(murooka) Should I use interpolation? + const auto modified_stop_info = [&]() -> boost::optional> { + const double dist_to_stop = stop_obstacle_info.dist_to_stop; + + const size_t obstacle_zero_vel_idx = + getIndexWithLongitudinalOffset(planner_data.traj.points, dist_to_stop, ego_idx); + + // check if there is already stop line between obstacle and zero_vel_idx + const auto behavior_zero_vel_idx = + tier4_autoware_utils::searchZeroVelocityIndex(planner_data.traj.points); + if (behavior_zero_vel_idx) { + const double zero_vel_diff_length = tier4_autoware_utils::calcSignedArcLength( + planner_data.traj.points, obstacle_zero_vel_idx, behavior_zero_vel_idx.get()); + if ( + 0 < zero_vel_diff_length && + zero_vel_diff_length < longitudinal_info_.safe_distance_margin) { + const double modified_dist_to_stop = + dist_to_stop + longitudinal_info_.safe_distance_margin - min_behavior_stop_margin_; + const size_t modified_obstacle_zero_vel_idx = + getIndexWithLongitudinalOffset(planner_data.traj.points, modified_dist_to_stop, ego_idx); + return std::make_pair(modified_obstacle_zero_vel_idx, modified_dist_to_stop); + } + } + + return std::make_pair(obstacle_zero_vel_idx, dist_to_stop); + }(); + if (!modified_stop_info) { + return {}; + } + const size_t modified_zero_vel_idx = modified_stop_info->first; + const double modified_dist_to_stop = modified_stop_info->second; + + // virtual wall marker for stop + const auto marker_pose = obstacle_cruise_utils::calcForwardPose( + planner_data.traj, ego_idx, modified_dist_to_stop + vehicle_info_.max_longitudinal_offset_m); + if (marker_pose) { + visualization_msgs::msg::MarkerArray wall_msg; + const auto markers = tier4_autoware_utils::createStopVirtualWallMarker( + marker_pose.get(), "obstacle stop", planner_data.current_time, 0); + tier4_autoware_utils::appendMarkerArray(markers, &debug_wall_marker); + } + + debug_obstacles_to_stop.push_back(stop_obstacle_info.obstacle); + return modified_zero_vel_idx; +} + +void PIDBasedPlanner::planCruise( + const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, + const boost::optional & cruise_obstacle_info, DebugData & debug_data) +{ + // do cruise + if (cruise_obstacle_info) { + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), is_showing_debug_info_, + "cruise planning"); + + vel_limit = doCruise( + planner_data, cruise_obstacle_info.get(), debug_data.obstacles_to_cruise, + debug_data.cruise_wall_marker); + + // update debug values + debug_values_.setValues(DebugValues::TYPE::CRUISE_TARGET_VELOCITY, vel_limit->max_velocity); + debug_values_.setValues( + DebugValues::TYPE::CRUISE_TARGET_ACCELERATION, vel_limit->constraints.min_acceleration); + } else { + // reset previous target velocity if adaptive cruise is not enabled + prev_target_vel_ = {}; + } +} + +VelocityLimit PIDBasedPlanner::doCruise( + const ObstacleCruisePlannerData & planner_data, const CruiseObstacleInfo & cruise_obstacle_info, + std::vector & debug_obstacles_to_cruise, + visualization_msgs::msg::MarkerArray & debug_wall_marker) +{ + const double dist_to_cruise = cruise_obstacle_info.dist_to_cruise; + const double normalized_dist_to_cruise = cruise_obstacle_info.normalized_dist_to_cruise; + + const size_t ego_idx = findExtendedNearestIndex(planner_data.traj, planner_data.current_pose); + + // calculate target velocity with acceleration limit by PID controller + const double pid_output_vel = pid_controller_->calc(normalized_dist_to_cruise); + [[maybe_unused]] const double prev_vel = + prev_target_vel_ ? prev_target_vel_.get() : planner_data.current_vel; + + const double additional_vel = [&]() { + if (normalized_dist_to_cruise > 0) { + return pid_output_vel * output_ratio_during_accel_; + } + return pid_output_vel; + }(); + + const double positive_target_vel = + std::max(min_cruise_target_vel_, planner_data.current_vel + additional_vel); + + // calculate target acceleration + const double target_acc = vel_to_acc_weight_ * additional_vel; + const double target_acc_with_acc_limit = + std::clamp(target_acc, min_accel_during_cruise_, longitudinal_info_.max_accel); + + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), is_showing_debug_info_, + "target_velocity %f", positive_target_vel); + + prev_target_vel_ = positive_target_vel; + + // set target longitudinal motion + const auto vel_limit = createVelocityLimitMsg( + planner_data.current_time, positive_target_vel, target_acc_with_acc_limit, + longitudinal_info_.max_jerk, longitudinal_info_.min_jerk); + + // virtual wall marker for cruise + const double dist_to_rss_wall = dist_to_cruise + vehicle_info_.max_longitudinal_offset_m; + const size_t wall_idx = + getIndexWithLongitudinalOffset(planner_data.traj.points, dist_to_rss_wall, ego_idx); + + const auto markers = tier4_autoware_utils::createSlowDownVirtualWallMarker( + planner_data.traj.points.at(wall_idx).pose, "obstacle cruise", planner_data.current_time, 0); + tier4_autoware_utils::appendMarkerArray(markers, &debug_wall_marker); + + debug_obstacles_to_cruise.push_back(cruise_obstacle_info.obstacle); + + return vel_limit; +} + +void PIDBasedPlanner::publishDebugValues(const ObstacleCruisePlannerData & planner_data) const +{ + const auto debug_values_msg = convertDebugValuesToMsg(planner_data.current_time, debug_values_); + debug_values_pub_->publish(debug_values_msg); +} + +void PIDBasedPlanner::updateParam(const std::vector & parameters) +{ + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.min_accel_during_cruise", min_accel_during_cruise_); + + // pid controller + double kp = pid_controller_->getKp(); + double ki = pid_controller_->getKi(); + double kd = pid_controller_->getKd(); + + tier4_autoware_utils::updateParam(parameters, "pid_based_planner.kp", kp); + tier4_autoware_utils::updateParam(parameters, "pid_based_planner.ki", ki); + tier4_autoware_utils::updateParam(parameters, "pid_based_planner.kd", kd); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.output_ratio_during_accel", output_ratio_during_accel_); + + // vel_to_acc_weight + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.vel_to_acc_weight", vel_to_acc_weight_); + + // min_cruise_target_vel + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.min_cruise_target_vel", min_cruise_target_vel_); + + pid_controller_->updateParam(kp, ki, kd); +} diff --git a/planning/obstacle_cruise_planner/src/polygon_utils.cpp b/planning/obstacle_cruise_planner/src/polygon_utils.cpp new file mode 100644 index 0000000000000..444e9007303d3 --- /dev/null +++ b/planning/obstacle_cruise_planner/src/polygon_utils.cpp @@ -0,0 +1,309 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_cruise_planner/polygon_utils.hpp" + +namespace +{ +namespace bg = boost::geometry; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; + +void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) +{ + Point2d point; + point.x() = geom_point.x; + point.y() = geom_point.y; + + bg::append(polygon.outer(), point); +} + +void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point32 & geom_point) +{ + Point2d point; + point.x() = geom_point.x; + point.y() = geom_point.y; + + bg::append(polygon.outer(), point); +} + +void appendPointToPolygon(Polygon2d & polygon, const Point2d point) +{ + bg::append(polygon.outer(), point); +} + +bool isClockWise(const Polygon2d & polygon) +{ + const int n = polygon.outer().size(); + const double x_offset = polygon.outer().at(0).x(); + const double y_offset = polygon.outer().at(0).y(); + double sum = 0.0; + for (std::size_t i = 0; i < polygon.outer().size(); ++i) { + sum += + (polygon.outer().at(i).x() - x_offset) * (polygon.outer().at((i + 1) % n).y() - y_offset) - + (polygon.outer().at(i).y() - y_offset) * (polygon.outer().at((i + 1) % n).x() - x_offset); + } + + return sum < 0.0; +} + +Polygon2d inverseClockWise(const Polygon2d & polygon) +{ + Polygon2d inverted_polygon; + for (int i = polygon.outer().size() - 1; 0 <= i; --i) { + inverted_polygon.outer().push_back(polygon.outer().at(i)); + } + return inverted_polygon; +} + +Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, + const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width) +{ + Polygon2d polygon; + + const double longitudinal_offset = vehicle_info.max_longitudinal_offset_m; + const double width = vehicle_info.vehicle_width_m / 2.0 + expand_width; + const double rear_overhang = vehicle_info.rear_overhang_m; + + { // base step + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(base_step_pose, longitudinal_offset, width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_step_pose, longitudinal_offset, -width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_step_pose, -rear_overhang, -width, 0.0).position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_step_pose, -rear_overhang, width, 0.0).position); + } + + { // next step + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(next_step_pose, longitudinal_offset, width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_step_pose, longitudinal_offset, -width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_step_pose, -rear_overhang, -width, 0.0).position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_step_pose, -rear_overhang, width, 0.0).position); + } + + polygon = isClockWise(polygon) ? polygon : inverseClockWise(polygon); + + Polygon2d hull_polygon; + bg::convex_hull(polygon, hull_polygon); + + return hull_polygon; +} +} // namespace + +namespace polygon_utils +{ +Polygon2d convertObstacleToPolygon( + const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape) +{ + Polygon2d polygon; + + if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + const auto point0 = tier4_autoware_utils::calcOffsetPose( + pose, shape.dimensions.x / 2.0, shape.dimensions.y / 2.0, 0.0) + .position; + const auto point1 = tier4_autoware_utils::calcOffsetPose( + pose, -shape.dimensions.x / 2.0, shape.dimensions.y / 2.0, 0.0) + .position; + const auto point2 = tier4_autoware_utils::calcOffsetPose( + pose, -shape.dimensions.x / 2.0, -shape.dimensions.y / 2.0, 0.0) + .position; + const auto point3 = tier4_autoware_utils::calcOffsetPose( + pose, shape.dimensions.x / 2.0, -shape.dimensions.y / 2.0, 0.0) + .position; + + appendPointToPolygon(polygon, point0); + appendPointToPolygon(polygon, point1); + appendPointToPolygon(polygon, point2); + appendPointToPolygon(polygon, point3); + + // NOTE: push back the first point in order to close polygon + appendPointToPolygon(polygon, polygon.outer().front()); + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + const double radius = shape.dimensions.x / 2.0; + constexpr int circle_discrete_num = 6; + for (int i = 0; i < circle_discrete_num; ++i) { + geometry_msgs::msg::Point point; + point.x = std::cos( + (static_cast(i) / static_cast(circle_discrete_num)) * 2.0 * M_PI + + M_PI / static_cast(circle_discrete_num)) * + radius + + pose.position.x; + point.y = std::sin( + (static_cast(i) / static_cast(circle_discrete_num)) * 2.0 * M_PI + + M_PI / static_cast(circle_discrete_num)) * + radius + + pose.position.y; + appendPointToPolygon(polygon, point); + } + + // NOTE: push back the first point in order to close polygon + appendPointToPolygon(polygon, polygon.outer().front()); + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + for (const auto point : shape.footprint.points) { + appendPointToPolygon(polygon, point); + } + if (polygon.outer().size() > 0) { + // NOTE: push back the first point in order to close polygon + appendPointToPolygon(polygon, polygon.outer().front()); + } + } else { + throw std::logic_error("The shape type is not supported in obstacle_cruise_planner."); + } + + return isClockWise(polygon) ? polygon : inverseClockWise(polygon); +} + +boost::optional getFirstCollisionIndex( + const std::vector & traj_polygons, const Polygon2d & obj_polygon, + std::vector & collision_geom_points) +{ + for (size_t i = 0; i < traj_polygons.size(); ++i) { + std::deque collision_polygons; + boost::geometry::intersection(traj_polygons.at(i), obj_polygon, collision_polygons); + + 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()) { + geometry_msgs::msg::Point collision_geom_point; + collision_geom_point.x = collision_point.x(); + collision_geom_point.y = collision_point.y(); + collision_geom_points.push_back(collision_geom_point); + } + } + } + + if (has_collision) { + return i; + } + } + + return {}; +} + +boost::optional getFirstNonCollisionIndex( + const std::vector & traj_polygons, + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_auto_perception_msgs::msg::Shape & shape, const size_t start_idx) +{ + constexpr double epsilon = 1e-3; + + size_t latest_collision_idx = start_idx; + for (const auto & path_point : predicted_path.path) { + const auto obj_polygon = convertObstacleToPolygon(path_point, shape); + for (size_t i = start_idx; i < traj_polygons.size(); ++i) { + const double dist = bg::distance(traj_polygons.at(i), obj_polygon); + if (dist <= epsilon) { + latest_collision_idx = i; + break; + } + if (i == traj_polygons.size() - 1) { + return latest_collision_idx; + } + } + } + return {}; +} + +bool willCollideWithSurroundObstacle( + const autoware_auto_planning_msgs::msg::Trajectory & traj, + const std::vector & traj_polygons, + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_auto_perception_msgs::msg::Shape & shape, const double max_dist, + const double ego_obstacle_overlap_time_threshold, + const double max_prediction_time_for_collision_check) +{ + constexpr double epsilon = 1e-3; + + bool is_found = false; + size_t start_predicted_path_idx = 0; + for (size_t i = 0; i < predicted_path.path.size(); ++i) { + const auto & path_point = predicted_path.path.at(i); + if ( + max_prediction_time_for_collision_check < + rclcpp::Duration(predicted_path.time_step).seconds() * static_cast(i)) { + return false; + } + + for (size_t j = 0; j < traj.points.size(); ++j) { + const auto & traj_point = traj.points.at(j); + const double approximated_dist = + tier4_autoware_utils::calcDistance2d(path_point.position, traj_point.pose.position); + if (approximated_dist > max_dist) { + continue; + } + + const auto & traj_polygon = traj_polygons.at(j); + const auto obj_polygon = polygon_utils::convertObstacleToPolygon(path_point, shape); + const double dist = bg::distance(traj_polygon, obj_polygon); + + if (dist < epsilon) { + if (!is_found) { + start_predicted_path_idx = i; + is_found = true; + } else { + const double overlap_time = (static_cast(i) - start_predicted_path_idx) * + rclcpp::Duration(predicted_path.time_step).seconds(); + if (ego_obstacle_overlap_time_threshold < overlap_time) { + return true; + } + } + } else { + is_found = false; + } + } + } + + return false; +} +std::vector createOneStepPolygons( + const autoware_auto_planning_msgs::msg::Trajectory & traj, + const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width) +{ + std::vector polygons; + + for (size_t i = 0; i < traj.points.size(); ++i) { + const auto polygon = [&]() { + if (i == 0) { + return createOneStepPolygon( + traj.points.at(i).pose, traj.points.at(i).pose, vehicle_info, expand_width); + } + return createOneStepPolygon( + traj.points.at(i - 1).pose, traj.points.at(i).pose, vehicle_info, expand_width); + }(); + + polygons.push_back(polygon); + } + return polygons; +} +} // namespace polygon_utils diff --git a/planning/obstacle_cruise_planner/src/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp new file mode 100644 index 0000000000000..41be60c4e8726 --- /dev/null +++ b/planning/obstacle_cruise_planner/src/utils.cpp @@ -0,0 +1,111 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_cruise_planner/utils.hpp" + +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +namespace obstacle_cruise_utils +{ +bool isVehicle(const uint8_t label) +{ + return label == ObjectClassification::CAR || label == ObjectClassification::TRUCK || + label == ObjectClassification::BUS || label == ObjectClassification::MOTORCYCLE; +} + +visualization_msgs::msg::Marker getObjectMarker( + const geometry_msgs::msg::Pose & obstacle_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 = tier4_autoware_utils::createDefaultMarker( + "map", current_time, ns, idx, visualization_msgs::msg::Marker::SPHERE, + tier4_autoware_utils::createMarkerScale(2.0, 2.0, 2.0), + tier4_autoware_utils::createMarkerColor(r, g, b, 0.8)); + + marker.lifetime = rclcpp::Duration::from_seconds(0.8); + marker.pose = obstacle_pose; + + return marker; +} + +boost::optional calcForwardPose( + const autoware_auto_planning_msgs::msg::Trajectory & traj, const size_t nearest_idx, + const double target_length) +{ + if (traj.points.empty()) { + return {}; + } + + size_t search_idx = nearest_idx; + double length_to_search_idx = 0.0; + for (; search_idx < traj.points.size(); ++search_idx) { + length_to_search_idx = + tier4_autoware_utils::calcSignedArcLength(traj.points, nearest_idx, search_idx); + if (length_to_search_idx > target_length) { + break; + } else if (search_idx == traj.points.size() - 1) { + return {}; + } + } + + if (search_idx == 0 && !traj.points.empty()) { + return traj.points.at(0).pose; + } + + const auto & pre_pose = traj.points.at(search_idx - 1).pose; + const auto & next_pose = traj.points.at(search_idx).pose; + + geometry_msgs::msg::Pose target_pose; + + // lerp position + const double seg_length = + tier4_autoware_utils::calcDistance2d(pre_pose.position, next_pose.position); + const double lerp_ratio = (length_to_search_idx - target_length) / seg_length; + target_pose.position.x = + pre_pose.position.x + (next_pose.position.x - pre_pose.position.x) * lerp_ratio; + target_pose.position.y = + pre_pose.position.y + (next_pose.position.y - pre_pose.position.y) * lerp_ratio; + target_pose.position.z = + pre_pose.position.z + (next_pose.position.z - pre_pose.position.z) * lerp_ratio; + + // lerp orientation + const double pre_yaw = tf2::getYaw(pre_pose.orientation); + const double next_yaw = tf2::getYaw(next_pose.orientation); + target_pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(pre_yaw + (next_yaw - pre_yaw) * lerp_ratio); + + return target_pose; +} + +boost::optional getCurrentObjectPoseFromPredictedPath( + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time) +{ + for (size_t i = 0; i < predicted_path.path.size(); ++i) { + const auto & obj_p = predicted_path.path.at(i); + + const double object_time = + (obj_base_time + rclcpp::Duration(predicted_path.time_step) * static_cast(i) - + current_time) + .seconds(); + if (object_time >= 0) { + return obj_p; + } + } + + return boost::none; +} +} // namespace obstacle_cruise_utils