feat(obstacle_cruise_planner): add new package

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

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,25 +173,95 @@ 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="",
         package="rclcpp_components",
         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")

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}
+)