Skip to content

Commit

Permalink
update ego-planner for 2dlidar
Browse files Browse the repository at this point in the history
  • Loading branch information
@ZL199809
ZL199809 committed Dec 4, 2023
1 parent 99d5d1a commit aa5b5b2
Show file tree
Hide file tree
Showing 8 changed files with 267 additions and 21 deletions.
2 changes: 1 addition & 1 deletion Modules/ego_planner_swarm/plan_manage/launch_for_prometheus/advanced_param.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@
<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.3" />
<param name="grid_map/obstacles_inflation" value="0.1" />
<param name="grid_map/local_map_margin" value="10"/>
<!-- ground_height -->
<param name="grid_map/ground_height" value="$(arg ground_height)"/>
Expand Down
8 changes: 4 additions & 4 deletions ...ego_planner_swarm/plan_manage/launch_for_prometheus/sitl_ego_planner_basic_octomap.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
<arg name="map_origin_x_" value="-25.0"/>
<arg name="map_origin_y_" value="-25.0"/>
<!-- 可以通过限制ground_height和map_size_z来使得飞机在特定高度飞行 -->
<arg name="ground_height" value="1.4"/>
<arg name="ground_height" value="0.5"/>
<!-- 虚拟天花板高度要小于等于ground_height+z_size，否则重置该高度 -->
<!-- 里程计话题 -->
<arg name="odometry_topic" value="prometheus/odom"/>
Expand All @@ -28,10 +28,10 @@
<!-- scan -->
<arg name="scan_topic" value="no_scan"/>
<!-- 最大速度及加速度 -->
<arg name="max_vel" value="0.5" />
<arg name="max_acc" value="0.5" />
<arg name="max_vel" value="1" />
<arg name="max_acc" value="1" />
<!-- 规划的范围，一般设置为感知范围的1.5倍 -->
<arg name="planning_horizon" value="37.5" />
<arg name="planning_horizon" value="15" />
<!-- ？ -->
<arg name="use_distinctive_trajs" value="true" />
<!-- 1: use 2D Nav Goal to select goal -->
Expand Down
1 change: 1 addition & 0 deletions Scripts/simulation/ego_planner/ego_planner_p450_2dlidar.sh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
gnome-terminal --window -e 'bash -c "roscore; exec bash"' \
--tab -e 'bash -c "sleep 3; roslaunch prometheus_gazebo sitl_p450_2dlidar.launch uav1_init_x:=-0.0 uav1_init_y:=-0.0; exec bash"' \
--tab -e 'bash -c "sleep 4; roslaunch prometheus_uav_control uav_control_main_outdoor.launch; exec bash"' \
--tab -e 'bash -c "sleep 4; roslaunch prometheus_gazebo filter_lidar.launch; exec bash"' \
--tab -e 'bash -c "sleep 5; roslaunch prometheus_gazebo scan_to_octomap.launch; exec bash"' \
--tab -e 'bash -c "sleep 6; roslaunch ego_planner sitl_ego_planner_basic_octomap.launch; exec bash"' \

2 changes: 1 addition & 1 deletion Simulator/gazebo_simulator/gazebo_models/sensor_models/hokuyo_ust_20lx/hokuyo_ust_20lx.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@
<always_on>1</always_on>
<!-- 更新频率 10HZ -->
<update_rate>10</update_rate>
<visualize>true</visualize>
<visualize>false</visualize>
</sensor>
</link>
</model>
Expand Down
255 changes: 250 additions & 5 deletions Simulator/gazebo_simulator/gazebo_worlds/planning_worlds/planning_easy.world
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,40 +84,285 @@
</include>
</model>

<model name='cylinder7'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>7.0 3.0 1.5 0 0 0</pose>
</include>
</model>

<model name='cylinder8'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>9.0 3.0 2 0 0 0</pose>
</include>
</model>

<model name='cylinder9'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>11.0 3.0 2 0 0 0</pose>
</include>
</model>

<model name='cylinder2'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>6.0 3.0 1.5 0 0 0</pose>
<pose>5.0 2.0 2 0 0 0</pose>
</include>
</model>

<model name='cylinder3'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>6.0 -3.0 1.5 0 0 0</pose>
<pose>5.0 -2.0 1.5 0 0 0</pose>
</include>
</model>

<model name='cylinder4'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>-3.0 0.0 1.5 0 0 0</pose>
<pose>-2.0 0.0 1.5 0 0 0</pose>
</include>
</model>

<model name='cylinder5'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>-6.0 3.0 1.5 0 0 0</pose>
<pose>-5.0 2.0 1.5 0 0 0</pose>
</include>
</model>

<model name='cylinder6'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>-6.0 -3.0 1.5 0 0 0</pose>
<pose>-5.0 -2.0 1.5 0 0 0</pose>
</include>
</model>

<model name='cylinder4_clone_0'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>-1.97992 4.00703 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder4_clone_1'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>-1.9564 -2.0159 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder4_clone_10'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>7.0065 -4.01751 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder4_clone_11'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>9.0654 0.083267 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder4_clone_12'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>9.0896 -2.91154 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder4_clone_13'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>3.01135 -2.03851 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder4_clone_14'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>2.02671 5.11168 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder4_clone_15'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>2.02671 5.11168 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder4_clone_16'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>1.547 -0.460893 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder4_clone_17'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>-1.00318 -3.92453 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder4_clone_18'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>-3.97554 -3.89536 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder4_clone_19'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>-4.01198 -0.900726 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder4_clone_20'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>-3.99798 4.28559 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder4_clone_3'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>-0.02147 -2 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder4_clone_4'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>1.91528 1.99383 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder4_clone_5'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>3.98158 3.97316 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder3_clone'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>6.07287 0.13588 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder3_clone_0'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>4.15022 -4.06495 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder3_clone_1'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>8.06445 -0.95966 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder3_clone_10'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>3.03343 -6.9094 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder3_clone_11'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>-0.97445 -5.80554 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder3_clone_12'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>-3.04238 -6.01581 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder3_clone_13'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>-4.97933 -5.0259 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder3_clone_14'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>5.96757 5.00455 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder3_clone_2'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>7.01908 -1.94151 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder3_clone_3'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>2.02255 -4.00149 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder3_clone_4'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>0.17873 2.05724 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder3_clone_5'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>-1.85729 2.09778 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder3_clone_6'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>-3.98585 1.05791 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder3_clone_7'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>6.05648 -5.87728 1.5 0 -0 0</pose>
</include>
</model>


<model name='cylinder3_clone_8'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>4.12375 -5.90525 1.5 0 -0 0</pose>
</include>
</model>

<model name='cylinder3_clone_9'>
<include>
<uri>model://obs_cylinder_small</uri>
<pose>1.03519 -5.79704 1.5 0 -0 0</pose>
</include>
</model>
</world>
</sdf>
16 changes: 8 additions & 8 deletions Simulator/gazebo_simulator/launch_uav_with_sensor/2dlidar_mapping.rviz
View file
Original file line number Diff line number Diff line change
Expand Up @@ -272,7 +272,7 @@ Visualization Manager:
Marker Topic: /uav1_ego_planner_node/goal_point
Name: goal_point_uav1
Namespaces:
{}
"": true
Queue Size: 100
Value: true
- Class: rviz/Marker
Expand Down Expand Up @@ -304,7 +304,7 @@ Visualization Manager:
Marker Topic: /uav1_ego_planner_node/optimal_list
Name: optimal_traj_uav1
Namespaces:
{}
"": true
Queue Size: 100
Value: true
- Class: rviz/Marker
Expand Down Expand Up @@ -527,25 +527,25 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
Distance: 12.875259399414062
Distance: 28.1563720703125
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 1.151947259902954
Y: -1.9054487943649292
Z: 2.0008840560913086
X: 0.38798582553863525
Y: -1.8761192560195923
Z: -0.3502359390258789
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.6902021765708923
Pitch: 1.5147963762283325
Target Frame: <Fixed Frame>
Yaw: 3.899448871612549
Yaw: 3.633082151412964
Saved: ~
Window Geometry:
Displays:
Expand Down
2 changes: 1 addition & 1 deletion Simulator/gazebo_simulator/launch_uav_with_sensor/scan_to_octomap.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
<!-- 发布地图的坐标系 -->
<param name="frame_id" type="string" value="world" />
<!-- 传感器最大感知范围 (speedup!) -->
<param name="sensor_model/max_range" value="10.0" />
<param name="sensor_model/max_range" value="10" />
<!-- 局部点云话题输入 -->
<remap from="cloud_in" to="/uav1/prometheus/scan_point_cloud" />
<!-- 直通滤波的 Z 轴范围，保留 [-1.0, 10.0] 范围内的点 -->
Expand Down
Loading

0 comments on commit aa5b5b2

Please sign in to comment.