The optimized trajectories will collide with obstacles about ego-planner-v2 HOT 9 CLOSED

<arg name="drone_id" value="0"/>
<arg name="planning_horizon" value="7.5" />
<arg name="use_multitopology_trajs" value="false" />

<arg name="max_vel" value="1" />
<arg name="max_acc" value="0.5" />
<arg name="max_jer" value="10.0" />

<arg name="odom_topic" value="/m100/ground_truth/odometry"/>
<arg name="cloud_topic" value="/velodyne_points"/>

<arg name="camera_pose_topic" value="pcl_render_node/camera_pose"/>
<arg name="depth_topic" value="pcl_render_node/depth"/>
<arg name="cx" value="321.04638671875"/>
<arg name="cy" value="243.44969177246094"/>
<arg name="fx" value="387.229248046875"/>
<arg name="fy" value="387.229248046875"/>

<node pkg="ego_planner" name="drone_$(arg drone_id)_ego_planner_node" type="ego_planner_node" output="screen">
    <remap from="~odom_world" to="$(arg odom_topic)"/>

    <remap from="~planning/trajectory" to = "/drone_$(arg drone_id)_planning/trajectory"/>
    <remap from="~planning/heartbeat" to = "/drone_$(arg drone_id)_traj_server/heartbeat"/>

    <remap from="/goal" to = "/goal_with_id"/>
    <remap from="~grid_map/odom" to="$(arg odom_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/thresh_replan_time" value="1.0" type="double"/>
    <param name="fsm/planning_horizon" value="$(arg planning_horizon)" type="double"/>
    <param name="fsm/emergency_time" value="1.0" type="double"/>
    <param name="fsm/fail_safe" value="true"/>

    <param name="grid_map/resolution" value="0.1" />
    <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="10" />
    <param name="grid_map/local_update_range_y" value="10" />
    <param name="grid_map/local_update_range_z" value="10" />

    <param name="grid_map/obstacles_inflation" value="0.1" />
    <param name="grid_map/local_map_margin" value="10"/>
    <param name="grid_map/enable_virtual_wall" value="false"/>
    <param name="grid_map/virtual_ceil" value="3.0"/>
    <param name="grid_map/virtual_ground" value="-0.1"/>
    <param name="grid_map/ground_height" value="-0.01"/>
    <!-- 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/fading_time" value="1000.0"/>
    <param name="grid_map/min_ray_length" value="0.1"/>
    <param name="grid_map/max_ray_length" value="4.5"/>
    <param name="grid_map/visualization_truncate_height" value="20"/>
    <param name="grid_map/show_occ_time" value="false"/>
    <param name="grid_map/pose_type" value="1"/>
    <param name="grid_map/frame_id" value="world"/>
    <!-- planner manager -->
    <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/polyTraj_piece_length" value="1.5" 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_multitopology_trajs" value="$(arg use_multitopology_trajs)" type="bool"/>
    <param name="manager/drone_id" value="$(arg drone_id)"/>
    <!-- trajectory optimization -->
    <param name="optimization/constraint_points_perPiece" value="5" type="int"/>
    <param name="optimization/weight_obstacle" value="10000.0" type="double"/>
    <param name="optimization/weight_obstacle_soft" value="5000.0" type="double"/>
    <param name="optimization/weight_swarm" value="10000.0" type="double"/>
    <param name="optimization/weight_feasibility" value="10000.0" type="double"/>
    <param name="optimization/weight_sqrvariance" value="10000.0" type="double"/>
    <param name="optimization/weight_time" value="10.0" type="double"/>
    <param name="optimization/obstacle_clearance" value="1.5" type="double"/>
    <param name="optimization/obstacle_clearance_soft" value="0.4" type="double"/>
    <param name="optimization/swarm_clearance" value="0.15000000000000002" 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="optimization/max_jer" value="$(arg max_jer)" type="double"/>
</node>

<!-- trajectory server -->
<node pkg="ego_planner" name="drone_$(arg drone_id)_traj_server" type="traj_server" output="screen">
    <remap from="position_cmd" to="drone_$(arg drone_id)_planning/pos_cmd"/>
    <remap from="~planning/trajectory" to="drone_$(arg drone_id)_planning/trajectory"/>
    <param name="traj_server/time_forward" value="1.0" type="double"/>
</node>

<!-- rotors仿真器 -->
<include file="$(find mbplanner)/launch/mbplanner_m100_sim_cave2.launch">
</include>

<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ego_planner)/launch/mbp.rviz" required="true">
</node>