一、多点导航

1.1 下载wp_map_tools工具

链接：https://github.com/6-robot/wp_map_tools

使用步骤：

1. 安装依赖

   sudo apt update
   cd wp_map_tools/scripts/
   ./install_for_humble.sh
   

2. 编译

   
   colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select wp_map_tools

1.2集成autoware

因为这里没有仿真程序，直接启动会报错。集成到autoware需要知道依赖下面话题自己编写程序

autoware topic
    /planning/mission_planning/goal
        type: geometry_msgs::msg::PoseStamped
        功能：发送目标点消息
    /planning/mission_planning/state
        type: tier4_planning_msgs::msg::RouteState
        功能：返回autoware路由状态，state字段为4代表可路由，为6代表已经到达目标点
    /autoware/engage
        type: autoware_vehicle_msgs::msg::Engage
        功能：启动车辆自动驾驶功能


wp_map_tools topic
    /waterplus/add_waypoint
        type: wp_map_tools::msg::Waypoint
        功能 接受设置的目标航点(支持多个)

实现逻辑为：接收目标航点 -> 存储到目标航点到队列 -> 发送目标航点到autoware -> 接收是否到达目标点 -> 弹出队头航点 -> 取出下一个航点并发布 -> 启动自动驾驶功能 -> 直到队列为空

demo程序

在wp_map_tools工具的src新建一个文件，编写demo，修改package.xmi & CMakeLists.txt文件，重新编译：

#include <rclcpp/rclcpp.hpp>
#include <queue>
#include <mutex>
#include <memory>
#include "wp_map_tools/msg/waypoint.hpp"
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <tier4_planning_msgs/msg/route_state.hpp>
#include <autoware_vehicle_msgs/msg/engage.hpp>

// 线程安全的 Waypoint 队列
class WaypointQueue {
public:
    void push(const wp_map_tools::msg::Waypoint& wp) {
        std::lock_guard<std::mutex> lock(mutex_);
        queue_.push(wp);
    }

    bool pop(wp_map_tools::msg::Waypoint& wp) {
        std::lock_guard<std::mutex> lock(mutex_);
        if (queue_.empty()) {
            return false;
        }
        wp = queue_.front();
        queue_.pop();
        return true;
    }

    // 读取队头元素（不弹出），成功返回 true
    bool peek(wp_map_tools::msg::Waypoint& wp) const {
        std::lock_guard<std::mutex> lock(mutex_);
        if (queue_.empty()) {
            return false;
        }
        wp = queue_.front();  // 只读取，不 pop
        return true;
    }

    size_t size() const {
        std::lock_guard<std::mutex> lock(mutex_);
        return queue_.size();
    }

private:
    mutable std::mutex mutex_;
    std::queue<wp_map_tools::msg::Waypoint> queue_;
};

// ROS 2 节点示例
class WaypointProcessorNode : public rclcpp::Node {
public:
    WaypointProcessorNode() : Node("waypoint_processor_node") {
        subscription_ = this->create_subscription<wp_map_tools::msg::Waypoint>(
            "/waterplus/add_waypoint", 10,
            std::bind(&WaypointProcessorNode::waypointCallback, this, std::placeholders::_1));
        RouteState_sub_ = this->create_subscription<tier4_planning_msgs::msg::RouteState>(
            "/planning/mission_planning/state", 10,
            std::bind(&WaypointProcessorNode::routeStateCallback, this, std::placeholders::_1));
        pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseStamped>(
            "/planning/mission_planning/goal", 10);  // 发布目标点
        auto_pub_ = this->create_publisher<autoware_vehicle_msgs::msg::Engage>(
            "/autoware/engage", 10);  // 发布目标点
        
        is_first_received_waypoint_ = true;
    }

private:
    void routeStateCallback(const tier4_planning_msgs::msg::RouteState::SharedPtr msg) {
        wp_map_tools::msg::Waypoint head_wp;
        switch(msg->state)
        {
            case 2:
                if (waypoint_queue_.peek(head_wp)) {
                    RCLCPP_WARN(this->get_logger(), "Received waypoint is inaccessible: %s", head_wp.name.c_str());
                } else {
                    RCLCPP_WARN(this->get_logger(), "Queue is empty!");
                }
                break;
            case 4:
                if (waypoint_queue_.peek(head_wp)) {
                    RCLCPP_WARN(this->get_logger(), "Received waypoint is accessible: %s", head_wp.name.c_str());
                } else {
                    RCLCPP_WARN(this->get_logger(), "Queue is empty!");
                }
                break;
            case 6:
                if (waypoint_queue_.peek(head_wp)) {
                    RCLCPP_INFO(this->get_logger(), "Received waypoint is arrived : %s", head_wp.name.c_str());
                    this->processQueue();
                    rclcpp::sleep_for(std::chrono::seconds(1));
                    if (waypoint_queue_.peek(head_wp)){  // 没有目标点就不启动
                        autoware_vehicle_msgs::msg::Engage auto_cmd;
                        auto_cmd.stamp = this->now();
                        auto_cmd.engage = true;
                        auto_pub_->publish(auto_cmd);
                    }
                    else{  // 达到最后一个点的时候，清空状态标志，可以继续设置下一个目标点
                        is_first_received_waypoint_ = true;
                    }

                }
                break;
            default:
                break;
        }

    }
    void waypointCallback(const wp_map_tools::msg::Waypoint::SharedPtr msg) {
        if (is_first_received_waypoint_)
        {
            auto pose_msg = geometry_msgs::msg::PoseStamped();
            pose_msg.header.stamp = this->now();
            pose_msg.header.frame_id = "map";

            // 假设 Waypoint 包含 x, y, z
            pose_msg.pose.position.x = msg->pose.position.x;
            pose_msg.pose.position.y = msg->pose.position.y;
            pose_msg.pose.position.z = msg->pose.position.z;

            // 单位四元数
            pose_msg.pose.orientation.x = msg->pose.orientation.x;
            pose_msg.pose.orientation.y = msg->pose.orientation.y;
            pose_msg.pose.orientation.z = msg->pose.orientation.z;
            pose_msg.pose.orientation.w = msg->pose.orientation.w;

            // 发布 PoseStamped
            pose_pub_->publish(pose_msg);
            RCLCPP_INFO(this->get_logger(), "Received waypoint name: %s", msg->name.c_str());
            is_first_received_waypoint_ = false;
        }

        waypoint_queue_.push(*msg);
    }

    void processQueue() {
        wp_map_tools::msg::Waypoint wp;
        if (waypoint_queue_.pop(wp)) {  // 仅尝试弹出一个元素
            RCLCPP_INFO(this->get_logger(), "Pop waypoint name: %s", wp.name.c_str());
            
            wp_map_tools::msg::Waypoint head_wp;
            if (waypoint_queue_.peek(head_wp)) {
                RCLCPP_WARN(this->get_logger(), "Received waypoint is inaccessible: %s", head_wp.name.c_str());

                auto pose_msg = geometry_msgs::msg::PoseStamped();
                pose_msg.header.stamp = this->now();
                pose_msg.header.frame_id = "map";

                // 假设 Waypoint 包含 x, y, z
                pose_msg.pose.position.x = head_wp.pose.position.x;
                pose_msg.pose.position.y = head_wp.pose.position.y;
                pose_msg.pose.position.z = head_wp.pose.position.z;

                // 单位四元数
                pose_msg.pose.orientation.x = head_wp.pose.orientation.x;
                pose_msg.pose.orientation.y = head_wp.pose.orientation.y;
                pose_msg.pose.orientation.z = head_wp.pose.orientation.z;
                pose_msg.pose.orientation.w = head_wp.pose.orientation.w;

                // 发布 PoseStamped
                pose_pub_->publish(pose_msg);

            } else {
                RCLCPP_WARN(this->get_logger(), "Queue is empty!");
            }
        }
        // 如果队列为空，什么也不做
    }

    rclcpp::Subscription<wp_map_tools::msg::Waypoint>::SharedPtr subscription_;
    rclcpp::Subscription<tier4_planning_msgs::msg::RouteState>::SharedPtr RouteState_sub_;
    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub_;
    rclcpp::Publisher<autoware_vehicle_msgs::msg::Engage>::SharedPtr auto_pub_;
    WaypointQueue waypoint_queue_;
    bool is_first_received_waypoint_;
};

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<WaypointProcessorNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

新添加一个launch文件：

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    launch_file_dir = os.path.join(get_package_share_directory('wp_map_tools'), 'launch')

    wp_edit_cmd = Node(
            package='wp_map_tools',
            executable='wp_edit_node',
            name='wp_edit_node',
            output='screen'
        )

    map_tool = Node(
            package='wp_map_tools',
            executable='demo_map_tools',
            name='demo_map_tools',
            output='screen'
        )

    ld = LaunchDescription()
    ld.add_action(wp_edit_cmd)
    ld.add_action(map_tool)
    return ld

一个简单的demo程序就这样完成了，启动autowate的时候，启动这个launch文件就可以是实现多点导航。

具体操作

添加工具

添加航点话题

设置多航点，点击Auto，看车辆是否依次按照目标点行驶

存在问题

这里只是一个简单的demo程序，程序无法处理下面情况：

1.无法处理无法达到的目标点。可以在case 2里面进一步处理。

2.无法同步RVIZ删除的点。可以依赖/waypoint_markers/update这个topic去处理。

二、避障绕障

正常情况下，按照官方的配置一步步操作，车辆可以跟踪路径，但是不能不能避障（有障碍物的时候在RVIZ中没有obstacle_stop），说明感知模块出了问题。

可以尝试修改autoware_launch/launch/components/tier4_perception_component.launch.xml文件中的lidar_detection_model为clustering，或者替换为其他的方法尝试一下。

2.1调试过程

避障和绕障都在planning，不过首先确定感知模块能感知障碍物。

1.如果想绕障碍：default_preset.yaml中需要打开launch_dynamic_obstacle_avoidance

2.在静态配置文件和动态配置文件，同时打开unknown的识别，因为实际使用中大部分识别结果都是unknown

3.静态避障中hysteresis_factor_expand_rate参数似乎对能否成功绕开障碍物很重要。

下面给出几个配置文件的具体配置：

autoware_launch/config/planning/preset/default_preset.yaml

launch:
  - arg:
      name: behavior_path_planner_type
      default: behavior_path_planner
    # option: behavior_path_planner
    #         path_generator

  # behavior path modules
  - arg:
      name: launch_static_obstacle_avoidance
      default: "true"
  - arg:
      name: launch_avoidance_by_lane_change_module
      default: "true"
  - arg:
      name: launch_dynamic_obstacle_avoidance
      default: "true"
  - arg:
      name: launch_sampling_planner_module
      default: "false" # Warning, experimental module, use only in simulations
  - arg:
      name: launch_lane_change_right_module
      default: "true"
  - arg:
      name: launch_lane_change_left_module
      default: "true"
  - arg:
      name: launch_external_request_lane_change_right_module
      default: "false"
  - arg:
      name: launch_external_request_lane_change_left_module
      default: "false"
  - arg:
      name: launch_goal_planner_module
      default: "true"
  - arg:
      name: launch_start_planner_module
      default: "true"
  - arg:
      name: launch_side_shift_module
      default: "true"

  # behavior velocity modules
  - arg:
      name: launch_crosswalk_module
      default: "true"
  - arg:
      name: launch_walkway_module
      default: "true"
  - arg:
      name: launch_traffic_light_module
      default: "true"
  - arg:
      name: launch_intersection_module
      default: "true"
  - arg:
      name: launch_merge_from_private_module
      default: "false"
  - arg:
      name: launch_blind_spot_module
      default: "true"
  - arg:
      name: launch_detection_area_module
      default: "true"
  - arg:
      name: launch_virtual_traffic_light_module
      default: "true"
  - arg:
      name: launch_no_stopping_area_module
      default: "true"
  - arg:
      name: launch_stop_line_module
      default: "true"
  - arg:
      name: launch_occlusion_spot_module
      default: "false"
  - arg:
      name: launch_run_out_module
      default: "true"
  - arg:
      name: launch_speed_bump_module
      default: "false"
  - arg:
      name: launch_no_drivable_lane_module
      default: "false"

  # motion planning modules
  - arg:
      name: motion_path_smoother_type
      default: elastic_band
      # option: elastic_band
      #         none

  - arg:
      name: motion_path_planner_type
      default: path_optimizer
      # option: path_optimizer
      #         path_sampler
      #         none

  # motion velocity planner modules
  - arg:
      name: launch_obstacle_stop_module
      default: "false"  # 第一处修改
  - arg:
      name: launch_obstacle_slow_down_module
      default: "true"
  - arg:
      name: launch_obstacle_cruise_module
      default: "true"
  - arg:
      name: launch_dynamic_obstacle_stop_module
      default: "false"  # 第二处修改
  - arg:
      name: launch_out_of_lane_module
      default: "true"
  - arg:
      name: launch_obstacle_velocity_limiter_module
      default: "true"
  - arg:
      name: launch_mvp_run_out_module
      default: "false"

  - arg:
      name: motion_stop_planner_type
      default: obstacle_cruise_planner
      # option: obstacle_stop_planner
      #         obstacle_cruise_planner
      #         none

  - arg:
      name: velocity_smoother_type
      default: JerkFiltered
      # option: JerkFiltered
      #         L2
      #         Linf(Unstable)
      #         Analytical

  - arg:
      name: launch_surround_obstacle_checker
      default: "false"

  # parking modules
  - arg:
      name: launch_parking_module
      default: "true"

autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/dynamic_obstacle_avoidance.param.yaml

/**:
  ros__parameters:
    dynamic_avoidance:
      common:
        enable_debug_info: false
        use_hatched_road_markings: true

      # avoidance is performed for the object type with true
      target_object:
        car: true
        truck: true
        bus: true
        trailer: true
        unknown: true  # false -> true
        bicycle: true
        motorcycle: true
        pedestrian: true

        max_obstacle_vel: 100.0 # [m/s]
        min_obstacle_vel: 0.0 # [m/s]

        successive_num_to_entry_dynamic_avoidance_condition: 1  # 5 -> 1
        successive_num_to_exit_dynamic_avoidance_condition: 1

        min_obj_lat_offset_to_ego_path: 0.0 # [m]
        max_obj_lat_offset_to_ego_path: 1.0 # [m]

        cut_in_object:
          min_time_to_start_cut_in: 1.0 # [s]
          min_lon_offset_ego_to_object: 0.0 # [m]
          min_object_vel: 0.5 # [m/s]

        cut_out_object:
          max_time_from_outside_ego_path: 2.0 # [s]
          min_object_lat_vel: 0.3 # [m/s]
          min_object_vel: 0.5 # [m/s]

        crossing_object:
          min_overtaking_object_vel: 1.0
          max_overtaking_object_angle: 1.05
          min_oncoming_object_vel: 1.0
          max_oncoming_object_angle: 0.523
          max_pedestrian_crossing_vel: 0.8

        front_object:
          max_object_angle: 0.785
          min_object_vel: -0.5               # [m/s] The value is negative considering the noisy velocity of the stopped vehicle.
          max_ego_path_lat_cover_ratio: 0.3  # [-] The object will be ignored if the ratio of the object covering the ego's path is higher than this value.

        stopped_object:
          max_object_vel: 0.5 # [m/s] The object will be determined as stopped if the velocity is smaller than this value.

      drivable_area_generation:
        expand_drivable_area: true
        polygon_generation_method: "ego_path_base" # choose "ego_path_base" and "object_path_base"
        object_path_base:
          min_longitudinal_polygon_margin: 3.0 # [m]

        lat_offset_from_obstacle: 0.3 # [m]
        margin_distance_around_pedestrian: 0.65 # [m]
        predicted_path:
          end_time_to_consider: 1.0 # [s]
          threshold_confidence: 0.0 # [] not probability
        max_lat_offset_to_avoid: 0.5 # [m]
        max_time_for_object_lat_shift: 0.0 # [s]
        lpf_gain_for_lat_avoid_to_offset: 0.9 # [-]

        max_ego_lat_acc: 0.3        # [m/ss]
        max_ego_lat_jerk: 0.3       # [m/sss]
        delay_time_ego_shift: 1.0   # [s]

        # for same directional object
        overtaking_object:
          max_time_to_collision: 40.0 # [s]
          start_duration_to_avoid: 1.0  # [s]
          end_duration_to_avoid: 1.0  # [s]
          duration_to_hold_avoidance: 3.0 # [s]

        # for opposite directional object
        oncoming_object:
          max_time_to_collision: 40.0 # [s] This value should be the same as overtaking_object's one to suppress chattering of this value for parked vehicles
          start_duration_to_avoid: 1.0  # [s]
          end_duration_to_avoid: 0.0  # [s]

autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/static_obstacle_avoidance.param.yaml

# see AvoidanceParameters description in avoidance_module_data.hpp for description.
/**:
  ros__parameters:
    avoidance:
      resample_interval_for_planning: 0.5               # [m] FOR DEVELOPER
      resample_interval_for_output: 6.0                 # [m] FOR DEVELOPER

      # drivable lane setting. this module is able to use not only current lane but also right/left lane
      # if the current lane(=lanelet::Lanelet) and the right/left lane share the boundary(=lanelet::Linestring) in HDMap.
      # "current_lane"           : use only current lane. this module doesn't use adjacent lane to avoid object.
      # "same_direction_lane"    : this module uses same direction lane to avoid object if need.
      # "opposite_direction_lane": this module uses both same direction and opposite direction lane.
      use_lane_type: "opposite_direction_lane"
      # drivable area setting
      use_intersection_areas: true
      use_hatched_road_markings: true
      use_freespace_areas: true

      # avoidance is performed for the object type with true
      target_object:
        car:
          th_moving_speed: 1.0                          # [m/s]
          th_moving_time: 2.0                           # [s]
          longitudinal_margin: 0.0                      # [m]
          lateral_margin:
            soft_margin: 0.5                            # [m]
            hard_margin: 0.2                            # [m]
            hard_margin_for_parked_vehicle: 0.2         # [m]
          max_expand_ratio: 0.0                         # [-] FOR DEVELOPER
          envelope_buffer_margin: 0.1                   # [m] FOR DEVELOPER
          th_error_eclipse_long_radius : 0.6            # [m]
        truck:
          th_moving_speed: 1.0
          th_moving_time: 2.0
          longitudinal_margin: 0.0
          lateral_margin:
            soft_margin: 0.5
            hard_margin: 0.2
            hard_margin_for_parked_vehicle: 0.2
          max_expand_ratio: 0.0
          envelope_buffer_margin: 0.1
          th_error_eclipse_long_radius : 0.6
        bus:
          th_moving_speed: 1.0
          th_moving_time: 2.0
          longitudinal_margin: 0.0
          lateral_margin:
            soft_margin: 0.5
            hard_margin: 0.2
            hard_margin_for_parked_vehicle: 0.2
          max_expand_ratio: 0.0
          envelope_buffer_margin: 0.1
          th_error_eclipse_long_radius : 0.6
        trailer:
          th_moving_speed: 1.0
          th_moving_time: 2.0
          longitudinal_margin: 0.0
          lateral_margin:
            soft_margin: 0.5
            hard_margin: 0.2
            hard_margin_for_parked_vehicle: 0.2
          max_expand_ratio: 0.0
          envelope_buffer_margin: 0.1
          th_error_eclipse_long_radius : 0.6
        unknown:
          th_moving_speed: 0.50  # 0.28 -> 0.50
          th_moving_time: 1.0
          longitudinal_margin: 0.0
          lateral_margin:
            soft_margin: 0.5
            hard_margin: 0.4  # -0.2 -> 0.4
            hard_margin_for_parked_vehicle: 0.2  # -0.2 -> 0.2
          max_expand_ratio: 0.0
          envelope_buffer_margin: 0.5  # 0.1 -> 0.5
          th_error_eclipse_long_radius : 0.6
        bicycle:
          th_moving_speed: 0.28
          th_moving_time: 1.0
          longitudinal_margin: 0.6
          lateral_margin:
            soft_margin: 0.7
            hard_margin: 0.5
            hard_margin_for_parked_vehicle: 0.2
          max_expand_ratio: 0.0
          envelope_buffer_margin: 0.5
          th_error_eclipse_long_radius : 0.6
        motorcycle:
          th_moving_speed: 1.0
          th_moving_time: 1.0
          longitudinal_margin: 0.6
          lateral_margin:
            soft_margin: 0.5
            hard_margin: 0.3
            hard_margin_for_parked_vehicle: 0.2
          max_expand_ratio: 0.0
          envelope_buffer_margin: 0.5
          th_error_eclipse_long_radius : 0.6
        pedestrian:
          th_moving_speed: 0.28
          th_moving_time: 1.0
          longitudinal_margin: 0.6
          lateral_margin:
            soft_margin: 0.7
            hard_margin: 0.5
            hard_margin_for_parked_vehicle: 0.2
          max_expand_ratio: 0.5  # 0.0  -> 0.5
          envelope_buffer_margin: 0.5
          th_error_eclipse_long_radius : 0.6
        lower_distance_for_polygon_expansion: 30.0      # [m] FOR DEVELOPER
        upper_distance_for_polygon_expansion: 100.0     # [m] FOR DEVELOPER

      # For target object filtering
      target_filtering:
        # avoidance target type
        target_type:
          car: true                                     # [-]
          truck: true                                   # [-]
          bus: true                                     # [-]
          trailer: true                                 # [-]
          unknown: true                                 # [-]
          bicycle: true                                 # [-]
          motorcycle: true                              # [-]
          pedestrian: true                              # [-]
        # detection range
        object_check_goal_distance: 20.0                # [m]
        object_check_return_pose_distance: 20.0         # [m]
        # lost object compensation
        max_compensation_time: 2.0

        # detection area generation parameters
        detection_area:
          static: false                                 # [-]
          min_forward_distance: 50.0                    # [m]
          max_forward_distance: 150.0                   # [m]
          backward_distance: 10.0                       # [m]

        # filtering parking objects
        parked_vehicle:
          th_offset_from_centerline: 1.0                # [m]
          th_shiftable_ratio: 0.8                       # [-]
          min_road_shoulder_width: 0.5                  # [m] FOR DEVELOPER

        # for merging/deviating vehicle
        merging_vehicle:
          th_overhang_distance: 0.0                     # [m]

        # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked.
        avoidance_for_ambiguous_vehicle:
          # policy for ego behavior for ambiguous vehicle.
          # "auto"   : generate candidate path. if RTC is running as AUTO mode, the ego avoids it automatically.
          # "manual" : generate candidate path and wait operator approval even if RTC is running as AUTO mode.
          # "ignore" : never avoid it.
          policy: "manual"                              # [-]
          condition:
            th_stopped_time: 3.0                        # [s]
            th_moving_distance: 1.0                     # [m]
          ignore_area:
            traffic_light:
              front_distance: 20.0                      # [m]
            crosswalk:
              front_distance: 20.0                      # [m]
              behind_distance: 0.0                      # [m]
          wait_and_see:
            target_behaviors: ["MERGING", "DEVIATING"]  # [-]
            th_closest_distance: 10.0                   # [m]

        # params for filtering objects that are in intersection
        intersection:
          yaw_deviation: 0.349                          # [rad] (default 20.0deg)

        freespace:
          condition:
            th_stopped_time: 5.0                        # [-]

      # For safety check
      safety_check:
        # safety check target type
        target_type:
          car: true                                     # [-]
          truck: true                                   # [-]
          bus: true                                     # [-]
          trailer: true                                 # [-]
          unknown: true                                # [-]  # false -> true
          bicycle: true                                 # [-]
          motorcycle: true                              # [-]
          pedestrian: true                              # [-]
        # safety check configuration
        enable: true                                    # [-]
        check_current_lane: true                       # [-]  # false -> true
        check_shift_side_lane: true                     # [-]
        check_other_side_lane: false                    # [-]
        check_unavoidable_object: true                 # [-]  # false -> true
        check_other_object: true                        # [-]
        # collision check parameters
        check_all_predicted_path: true                 # [-]  # false -> true
        safety_check_backward_distance: 100.0           # [m]
        hysteresis_factor_expand_rate: 1.675              # [-]
        hysteresis_factor_safe_count: 3                 # [-]
        collision_check_yaw_diff_threshold: 3.1416      # [rad]
        # predicted path parameters
        min_velocity: 1.38                              # [m/s]
        max_velocity: 50.0                              # [m/s]
        time_resolution: 0.5                            # [s]
        time_horizon_for_front_object: 3.0              # [s]
        time_horizon_for_rear_object: 10.0              # [s]
        delay_until_departure: 0.0                      # [s]
        # rss parameters
        extended_polygon_policy: "along_path"           # [-] select "rectangle" or "along_path"
        expected_front_deceleration: -1.0               # [m/ss]
        expected_rear_deceleration: -1.0                # [m/ss]
        rear_vehicle_reaction_time: 2.0                 # [s]
        rear_vehicle_safety_time_margin: 1.0            # [s]
        lateral_distance_max_threshold: 2.0             # [m]
        longitudinal_distance_min_threshold: 3.0        # [m]
        longitudinal_velocity_delta_time: 0.0           # [s]

      # For avoidance maneuver
      avoidance:
        # avoidance lateral parameters
        lateral:
          th_avoid_execution: 0.09                      # [m] FOR DEVELOPER
          th_small_shift_length: 0.101                  # [m] FOR DEVELOPER
          soft_drivable_bound_margin: 0.3               # [m]
          hard_drivable_bound_margin: 0.3               # [m]
          max_right_shift_length: 5.0                   # [m] FOR DEVELOPER
          max_left_shift_length: 5.0                    # [m] FOR DEVELOPER
          max_deviation_from_lane: 0.2                  # [m]
          # approve the next shift line after reaching this percentage of the current shift line length.
          # this parameter is added to allow waiting for the return of shift approval until the occlusion behind the avoid target is clear.
          # this feature can be disabled by setting this parameter to 0.0.
          ratio_for_return_shift_approval: 0.5          # [-]
        # avoidance distance parameters
        longitudinal:
          min_prepare_time: 1.0                         # [s]
          max_prepare_time: 2.0                         # [s]
          min_prepare_distance: 1.0                     # [m]
          min_slow_down_speed: 1.38                     # [m/s]
          buf_slow_down_speed: 0.56                     # [m/s] FOR DEVELOPER
          nominal_avoidance_speed: 8.33                 # [m/s] FOR DEVELOPER
          consider_front_overhang: true                 # [-]
          consider_rear_overhang: true                  # [-]
        # return dead line
        return_dead_line:
          goal:
            enable: true                                # [-]
            buffer: 3.0                                 # [m]
          traffic_light:
            enable: true                                # [-]
            buffer: 3.0                                 # [m]

      # For cancel maneuver
      cancel:
        enable: true                                    # [-]
        force:
          duration_time: 2.0                            # [s]

      # For yield maneuver
      yield:
        enable: true                                    # [-]
        enable_during_shifting: false                   # [-]

      # For stop maneuver
      stop:
        max_distance: 20.0                              # [m]
        stop_buffer: 1.0                                # [m] FOR DEVELOPER

      policy:
        # policy for module behavior. select "reliable" or "not_enough".
        # "reliable": this module fully trusts the perception results and makes all decisions automatically.
        # "not_enough": because the perception results are not fully reliable, the system waits for the operator's judgment in situations where long-range perception is required.
        detection_reliability: "reliable"
        # policy for rtc request. select "per_shift_line" or "per_avoidance_maneuver".
        # "per_shift_line": request approval for each shift line.
        # "per_avoidance_maneuver": request approval for avoidance maneuver (avoid + return).
        make_approval_request: "per_shift_line"
        # policy for vehicle slow down behavior. select "best_effort" or "reliable".
        # "best_effort": slow down deceleration & jerk are limited by constraints.
        #                but there is a possibility that the vehicle can't stop in front of the vehicle.
        # "reliable": insert stop or slow down point with ignoring decel/jerk constraints.
        #             make it possible to increase chance to avoid but uncomfortable deceleration maybe happen.
        deceleration: "best_effort"                     # [-]
        # policy for avoidance lateral margin. select "best_effort" or "reliable".
        # "best_effort": output avoidance path with shorten lateral margin when there is no enough longitudinal
        #                margin to avoid.
        # "reliable": module output avoidance path with safe (rtc cooperate) state only when the vehicle can avoid
        #             with expected lateral margin.
        lateral_margin: "best_effort"                   # [-]
        # if true, module doesn't wait deceleration and outputs avoidance path by best effort margin.
        use_shorten_margin_immediately: true            # [-]

      # --------------------------------------
      # FOLLOWING PARAMETERS ARE FOR DEVELOPER
      # --------------------------------------

      constraints:
        # lateral constraints
        lateral:
          velocity: [1.39, 4.17, 11.1]                  # [m/s]
          max_accel_values: [0.5, 0.5, 0.5]             # [m/ss]
          min_jerk_values: [0.2, 0.2, 0.2]              # [m/sss]
          max_jerk_values: [3.0, 3.0, 3.0]              # [m/sss]

        # longitudinal constraints
        longitudinal:
          nominal_deceleration: -1.0                    # [m/ss]
          nominal_jerk: 0.5                             # [m/sss]
          max_deceleration: -1.5                        # [m/ss]
          max_jerk: 1.0                                 # [m/sss]
          max_acceleration: 0.5                         # [m/ss]
          min_velocity_to_limit_max_acceleration: 2.78  # [m/ss]

      # path generation method. select "shift_line_base" or "optimization_base" or "both".
      # "shift_line_base"  : Create avoidance path based on shift line.
      #                      User can control avoidance maneuver execution via RTC.
      #                      However, this method doesn't support complex avoidance scenario (e.g. S-shape maneuver).
      # "optimization_base": This module selects avoidance target object
      #                      and bpp module clips drivable area based on avoidance target object polygon shape.
      #                      But this module doesn't modify the path shape.
      #                      On the other hand, autoware_path_optimizer module optimizes path shape instead of this module
      #                      so that the path can be within drivable area. This method is able to deal with complex avoidance scenario.
      #                      However, user can't control avoidance manuever execution.
      # "both"             : Use both method.
      path_generation_method: "shift_line_base"

      shift_line_pipeline:
        trim:
          quantize_size: 0.1
          th_similar_grad_1: 0.1
          th_similar_grad_2: 0.2
          th_similar_grad_3: 0.5

      # for debug
      debug:
        enable_other_objects_marker: true
        enable_other_objects_info: true
        enable_detection_area_marker: false
        enable_drivable_bound_marker: false
        enable_safety_check_marker: false
        enable_shift_line_marker: false
        enable_lane_marker: false
        enable_misc_marker: false