我在airsim下城市环境中利用雷达传感器使用了ego_planner，但是避障功能似乎没有实现

[Bestboy125]

Ego-planer运行出现：
[rebo replan]: -------------------------------------24846
start: 4.92 19.1 1.39, 0 0 0
goal:7.96 31 1.14, 0.498 1.94 -0.0339
[WARN] [1775116190.694595610]: WARN! terminal point of the current trajectory is in obstacle, skip this planning.
iter=5,time(ms)=0.0228,rebound.
first_optimize_step_success=0
final_plan_success=0
[FSM]: from GEN_NEW_TRAJ to GEN_NEW_TRAJ
[ERROR] [1775116190.704307928]: last_progress_time_ ERROR !!!!!!!!!
[ERROR] [1775116190.704325484]: last_progress_time_ ERROR !!!!!!!!!
[ERROR] [1775116190.704330872]: last_progress_time_ ERROR !!!!!!!!!
[ERROR] [1775116190.704336081]: last_progress_time_ ERROR !!!!!!!!!
[ERROR] [1775116190.704340840]: last_progress_time_ ERROR !!!!!!!!!
我的运行launch文件如下：

<arg name="map_size_x" value="100"/>
<arg name="map_size_y" value="50"/>
<arg name="map_size_z" value="6.0"/>
<arg name="odom_topic" value="/reversed_odom"/>
<!-- <arg name="odom_topic" value="/vins_fusion/imu_propagate"/> -->
<!-- <arg name="odom_topic" value="/airsim_node/drone_1/odom_local_ned"/> -->

<!-- main algorithm params -->
<include file="$(find ego_planner)/launch/advanced_param_exp.xml">
    <arg name="drone_id" value="$(arg drone_id)"/>
    <arg name="map_size_x_" value="$(arg map_size_x)"/>
    <arg name="map_size_y_" value="$(arg map_size_y)"/>
    <arg name="map_size_z_" value="$(arg map_size_z)"/>
    <arg name="odometry_topic" value="$(arg odom_topic)"/>
    <arg name="obj_num_set" value="$(arg obj_num)"/>
    <!-- camera pose: transform of camera frame in the world frame -->
    <!-- depth topic: depth image, 640x480 by default -->
    <!-- don't set cloud_topic if you already set these ones! -->
    <arg name="camera_pose_topic" value="nouse1"/>
    <arg name="depth_topic" value="nouse2"/>
    <!-- <arg name="depth_topic" value="/camera/depth/image_rect_raw"/> -->
    <!-- topic of point cloud measurement, such as from LIDAR  -->
    <!-- don't set camera pose and depth, if you already set this one! -->
    <!-- 使用 reverser/lidar_to_world 变换后的世界系点云，勿直接用 LidarCustom（frame 为 drone_1/LidarCustom） -->
    <arg name="cloud_topic" value="/lidar_world_ned"/>
    <!-- intrinsic params of the depth camera -->
    <!-- Competition camra -->
    <arg name="cx" value="160.0"/>
    <arg name="cy" value="120.0"/>
    <arg name="fx" value="134.2559356689453"/>
    <arg name="fy" value="134.2559356689453"/>
    <!-- D455 -->
    <!-- <arg name="cx" value="318.99688720703125"/>
    <arg name="cy" value="235.61720275878906"/>
    <arg name="fx" value="382.1278076171875"/>
    <arg name="fy" value="382.1278076171875"/> -->

    <!-- maximum velocity and acceleration the drone will reach -->
    <arg name="max_vel" value="2.0" />
    <arg name="max_acc" value="6.0" />
    <!--always set to 1.5 times grater than sensing horizen-->
    <arg name="planning_horizon" value="6" />
    <arg name="use_distinctive_trajs" value="false" />
    <!-- 1: use 2D Nav Goal to select goal  -->
    <!-- 2: use global waypoints below  -->
    <arg name="flight_type" value="1" />
    <!-- global waypoints -->
    <!-- It generates a piecewise min-snap traj passing all waypoints -->
    <arg name="point_num" value="4" />
    <arg name="point0_x" value="15" />
    <arg name="point0_y" value="150" />
    <arg name="point0_z" value="3" />
    <arg name="point1_x" value="15.0" />
    <arg name="point1_y" value="80.0" />
    <arg name="point1_z" value="2.0" />
    <arg name="point2_x" value="15.0" />
    <arg name="point2_y" value="0.0" />
    <arg name="point2_z" value="3.0" />
    <arg name="point3_x" value="-15.0" />
    <arg name="point3_y" value="0.0" />
    <arg name="point3_z" value="4.0" />
    <arg name="point4_x" value="15.0" />
    <arg name="point4_y" value="0.0" />
    <arg name="point4_z" value="5.0" />
</include>
<!-- trajectory server -->
<node pkg="ego_planner" name="drone_$(arg drone_id)_traj_server" type="traj_server" output="screen">
    <!-- <remap from="position_cmd" to="/setpoints_cmd"/> -->
    <!-- 与 advanced_param_exp 中 /planning/bspline 重映射一致 -->
    <remap from="/planning/bspline" to="/drone_$(arg drone_id)_planning/bspline"/>
    <param name="traj_server/time_forward" value="1.0" type="double"/>
</node>

我的参数文件如下：

<!-- 与源码中绝对话题名一致：ego_replan_fsm 订阅 /odom_world，grid_map 使用 /grid_map/* -->
<remap from="/odom_world" to="$(arg odometry_topic)"/>
<remap from="/planning/bspline" to="/drone_$(arg drone_id)_planning/bspline"/>
<remap from="/planning/data_display" to="/drone_$(arg drone_id)_planning/data_display"/>
<remap from="/planning/broadcast_bspline_from_planner" to="/broadcast_bspline"/>
<remap from="/planning/broadcast_bspline_to_planner" to="/broadcast_bspline"/>

<remap from="/grid_map/odom" to="$(arg odometry_topic)"/>
<remap from="/grid_map/cloud" to="$(arg cloud_topic)"/>
<remap from="/grid_map/pose" to="$(arg camera_pose_topic)"/>
<remap from="/grid_map/depth" to="$(arg depth_topic)"/>


<!-- planning fsm -->
<param name="fsm/flight_type" value="$(arg flight_type)" type="int"/>
<param name="fsm/thresh_replan_time" value="1.0" type="double"/>
<param name="fsm/thresh_no_replan_meter" value="1.0" type="double"/>
<param name="fsm/planning_horizon" value="$(arg planning_horizon)" type="double"/> <!--always set to 1.5 times grater than sensing horizen-->
<param name="fsm/planning_horizen_time" value="3" type="double"/>
<param name="fsm/emergency_time" value="1.0" type="double"/>
<param name="fsm/realworld_experiment" value="true"/>
<param name="fsm/fail_safe" value="true"/>

<param name="fsm/waypoint_num" value="$(arg point_num)" type="int"/>
<param name="fsm/waypoint0_x" value="$(arg point0_x)" type="double"/>
<param name="fsm/waypoint0_y" value="$(arg point0_y)" type="double"/>
<param name="fsm/waypoint0_z" value="$(arg point0_z)" type="double"/>
<param name="fsm/waypoint1_x" value="$(arg point1_x)" type="double"/>
<param name="fsm/waypoint1_y" value="$(arg point1_y)" type="double"/>
<param name="fsm/waypoint1_z" value="$(arg point1_z)" type="double"/>
<param name="fsm/waypoint2_x" value="$(arg point2_x)" type="double"/>
<param name="fsm/waypoint2_y" value="$(arg point2_y)" type="double"/>
<param name="fsm/waypoint2_z" value="$(arg point2_z)" type="double"/>
<param name="fsm/waypoint3_x" value="$(arg point3_x)" type="double"/>
<param name="fsm/waypoint3_y" value="$(arg point3_y)" type="double"/>
<param name="fsm/waypoint3_z" value="$(arg point3_z)" type="double"/>
<param name="fsm/waypoint4_x" value="$(arg point4_x)" type="double"/>
<param name="fsm/waypoint4_y" value="$(arg point4_y)" type="double"/>
<param name="fsm/waypoint4_z" value="$(arg point4_z)" type="double"/>

<param name="grid_map/resolution"      value="0.15" /> 
<param name="grid_map/map_size_x"   value="$(arg map_size_x_)" /> 
<param name="grid_map/map_size_y"   value="$(arg map_size_y_)" /> 
<param name="grid_map/map_size_z"   value="$(arg map_size_z_)" /> 
<param name="grid_map/local_update_range_x"  value="5.5" /> 
<param name="grid_map/local_update_range_y"  value="5.5" /> 
<param name="grid_map/local_update_range_z"  value="4.5" /> 
<param name="grid_map/obstacles_inflation"     value="0.299" /> 
<param name="grid_map/local_map_margin" value="10"/>
<param name="grid_map/ground_height"        value="-0.5"/>
<!-- camera parameter -->
<param name="grid_map/cx" value="$(arg cx)"/>
<param name="grid_map/cy" value="$(arg cy)"/>
<param name="grid_map/fx" value="$(arg fx)"/>
<param name="grid_map/fy" value="$(arg fy)"/>
<!-- depth filter -->
<param name="grid_map/use_depth_filter" value="true"/>
<param name="grid_map/depth_filter_tolerance" value="0.15"/>
<param name="grid_map/depth_filter_maxdist"   value="5.0"/>
<param name="grid_map/depth_filter_mindist"   value="0.2"/>
<param name="grid_map/depth_filter_margin"    value="2"/>
<param name="grid_map/k_depth_scaling_factor" value="1000.0"/>
<param name="grid_map/skip_pixel" value="2"/>
<!-- local fusion -->
<param name="grid_map/p_hit"  value="0.65"/>
<param name="grid_map/p_miss" value="0.35"/>
<param name="grid_map/p_min"  value="0.12"/>
<param name="grid_map/p_max"  value="0.90"/>
<param name="grid_map/p_occ"  value="0.80"/>
<param name="grid_map/min_ray_length" value="0.3"/>
<param name="grid_map/max_ray_length" value="5.0"/>

<!-- self pointcloud filtering (lidar on body, avoid self-occupation) -->
<param name="grid_map/use_self_cloud_filter" value="false"/>
<!-- body center to lidar ~0.5m, add margin for self returns + inflation -->
<param name="grid_map/self_cloud_filter_radius" value="1.2"/>

<param name="grid_map/visualization_truncate_height"   value="1.8"/>
<param name="grid_map/show_occ_time"  value="false"/>
<param name="grid_map/pose_type"     value="2"/>  
<param name="grid_map/frame_id"      value="world_ned"/>

<param name="manager/max_vel" value="$(arg max_vel)" type="double"/>
<param name="manager/max_acc" value="$(arg max_acc)" type="double"/>
<param name="manager/max_jerk" value="4" type="double"/>
<param name="manager/control_points_distance" value="0.4" type="double"/>
<param name="manager/feasibility_tolerance" value="0.05" type="double"/>
<param name="manager/planning_horizon" value="$(arg planning_horizon)" type="double"/>
<param name="manager/use_distinctive_trajs" value="$(arg use_distinctive_trajs)" type="bool"/>
<param name="manager/drone_id" value="$(arg drone_id)"/>

<param name="optimization/lambda_smooth" value="1.0" type="double"/>
<param name="optimization/lambda_collision" value="0.5" type="double"/>
<param name="optimization/lambda_feasibility" value="0.1" type="double"/>
<param name="optimization/lambda_fitness" value="1.0" type="double"/>
<param name="optimization/dist0" value="0.5" type="double"/>
<param name="optimization/swarm_clearance" value="0.5" type="double"/>
<param name="optimization/max_vel" value="$(arg max_vel)" type="double"/>
<param name="optimization/max_acc" value="$(arg max_acc)" type="double"/>

<param name="bspline/limit_vel" value="$(arg max_vel)" type="double"/>
<param name="bspline/limit_acc" value="$(arg max_acc)" type="double"/>
<param name="bspline/limit_ratio" value="1.1" type="double"/>

<param name="prediction/obj_num" value="$(arg obj_num_set)" type="int"/>
<param name="prediction/lambda" value="1.0" type="double"/>
<param name="prediction/predict_rate" value="1.0" type="double"/>

实验截图：