-
Notifications
You must be signed in to change notification settings - Fork 202
/
exploration.launch
90 lines (72 loc) · 3.62 KB
/
exploration.launch
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
<launch>
<!-- size of map, change the size in x, y, z according to your application -->
<arg name="map_size_x" value="50.0"/>
<arg name="map_size_y" value="50.0"/>
<arg name="map_size_z" value=" 10.0"/>
<arg name="init_x" value="0"/>
<arg name="init_y" value="0"/>
<arg name="init_z" value="1.0"/>
<!-- topic of your odometry such as VIO or LIO -->
<arg name="odom_topic" value="/state_ukf/odom" />
<!-- main algorithm params -->
<include file="$(find exploration_manager)/launch/algorithm.xml">
<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="box_min_x" value="-10.0"/>
<arg name="box_min_y" value="-15.0"/>
<arg name="box_min_z" value=" 0.0"/>
<arg name="box_max_x" value="10.0"/>
<arg name="box_max_y" value="15.0"/>
<arg name="box_max_z" value=" 2.0"/>
<arg name="odometry_topic" value="$(arg odom_topic)"/>
<!-- sensor pose: transform of camera frame in the world frame -->
<arg name="sensor_pose_topic" value="/pcl_render_node/sensor_pose"/>
<!-- depth topic: depth image, 640x480 by default -->
<!-- cloud topic: point cloud measurement -->
<!-- subscribe ONLY TO ONE of the two topics -->
<arg name="depth_topic" value="/pcl_render_node/depth"/>
<arg name="cloud_topic" value="/pcl_render_node/cloud"/>
<!-- intrinsic params of the depth camera -->
<arg name="cx" value="321.04638671875"/>
<arg name="cy" value="243.44969177246094"/>
<arg name="fx" value="387.229248046875"/>
<arg name="fy" value="387.229248046875"/>
<!-- maximum velocity and acceleration the drone will reach -->
<arg name="max_vel" value="2.0" />
<arg name="max_acc" value="2.0" />
</include>
<!-- trajectory server -->
<node pkg="plan_manage" name="traj_server" type="traj_server" output="screen">
<remap from="/position_cmd" to="planning/pos_cmd"/>
<remap from="/odom_world" to="$(arg odom_topic)"/>
<param name="traj_server/time_forward" value="1.5" type="double"/>
<param name="traj_server/pub_traj_id" value="4" type="int"/>
<param name="traj_server/init_x" value="$(arg init_x)" type="double"/>
<param name="traj_server/init_y" value="$(arg init_y)" type="double"/>
<param name="traj_server/init_z" value="$(arg init_z)" type="double"/>
<param name="perception_utils/top_angle" value="0.56125" type="double"/>
<param name="perception_utils/left_angle" value="0.69222" type="double"/>
<param name="perception_utils/right_angle" value="0.68901" type="double"/>
<param name="perception_utils/max_dist" value="4.5" type="double"/>
<param name="perception_utils/vis_dist" value="1.0" type="double"/>
</node>
<node pkg="waypoint_generator" name="waypoint_generator" type="waypoint_generator" output="screen">
<remap from="~odom" to="$(arg odom_topic)"/>
<remap from="~goal" to="/move_base_simple/goal"/>
<remap from="~traj_start_trigger" to="/traj_start_trigger" />
<param name="waypoint_type" value="point"/>
</node>
<!-- use simulator -->
<include file="$(find exploration_manager)/launch/simulator.xml">
<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="init_x" value="$(arg init_x)"/>
<arg name="init_y" value="$(arg init_y)"/>
<arg name="init_z" value="$(arg init_z)"/>
<arg name="odometry_topic" value="$(arg odom_topic)" />
<arg name="c_num" value="0"/>
<arg name="p_num" value="130"/>
</include>
</launch>