forked from anEEStudent/Airsim-with-Kimera-Semantics
-
Notifications
You must be signed in to change notification settings - Fork 0
/
kimera_semantics.launch
136 lines (120 loc) · 7.8 KB
/
kimera_semantics.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
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
<?xml version="1.0" encoding="ISO-8859-15"?>
<launch>
<arg name="voxel_size" default="0.05"/>
<arg name="max_ray_length_m" default="5"/>
<arg name="should_use_sim_time" default="false" />
<param name="use_sim_time" value="$(arg should_use_sim_time)" />
<!-- Change sensor frame to:
- 1. VIO's estimated base_link: `left_cam_base_link`
- 2. Or, if you want to use simulator's ground-truth: `left_cam`
-->
<arg name="sensor_frame" default="left_cam"/>
<!-- If you want to play directly from a rosbag -->
<arg name="play_bag" default="false"/>
<!-- Let's go at twice real-time to show the speed! It can go faster, but then rviz struggles. -->
<arg name="rosbag_rate" default="1.0"/>
<arg name="bag_file" default="/home/teppei/Downloads/airsim_test_default.bag"/>
<!-- If you just want to run 3D reconstruction without semantics, set this flag to false-->
<arg name="metric_semantic_reconstruction" default="true"/>
<arg name="semantic_label_2_color_csv_filepath" default="$(find kimera_semantics_ros)/cfg/blocks.csv"/>
<!-- Input -->
<arg name="semantic_pointcloud" default="/semantic_pointcloud"/>
<arg name="left_cam_info_topic" default="/airsim_node/drone_1/front_left_custom/Scene/camera_info"/>
<arg name="right_cam_info_topic" default="/airsim_node/drone_1/front_right_custom/Scene/camera_info"/>
<arg name="left_cam_topic" default="/airsim_node/drone_1/front_left_custom/Scene"/>
<arg name="right_cam_topic" default="/airsim_node/drone_1/front_right_custom/Scene"/>
<arg name="left_cam_segmentation_topic" default="/airsim_node/drone_1/front_left_custom/Segmentation"/>
<arg name="left_cam_depth_topic" default="/airsim_node/drone_1/front_left_custom/DepthVis"/>
<arg name="use_freespace_pointcloud" default="false" />
<arg name="freespace_pointcloud" default="/dev/null"/>
<!-- Run rosbag if requested with play_bag arg -->
<node name="player" pkg="rosbag" type="play" output="screen"
args="--clock --rate $(arg rosbag_rate) $(arg bag_file)" if="$(arg play_bag)">
<!-- The rosbag we first generated did not follow ROS naming standards for image topics,
so we remap the topics accordingly here.-->
<remap from="/tesse/left_cam" to="$(arg left_cam_topic)"/>
<remap from="/tesse/segmentation" to="$(arg left_cam_segmentation_topic)"/>
<remap from="/tesse/depth" to="$(arg left_cam_depth_topic)"/>
</node>
<!-- Generate input pointcloud with semantic labels for kimera-semantics:
- 1. Using the depth image and registered semantic image (run_stereo_dense=false).
- 2. Using stereo depth reconstruction (run_stereo_dense=true). -->
<arg name="publish_point_clouds" default="true"/>
<arg name="run_stereo_dense" default="false"/>
<group if="$(arg publish_point_clouds)">
<!-- Launch Nodelet manager: used by depth_image_proc and disparity_image_proc -->
<node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"
output="screen"/>
<!-- Run stereo_dense_reconstruction_node (only if we don't use the depth image). -->
<arg name="left_cam_stereo_depth_topic" value="/depth_image"/>
<group if="$(arg run_stereo_dense)">
<arg name="pointcloud" value="/points2"/>
<include file="$(find kimera_semantics_ros)/launch/stereo_depth.launch">
<arg name="left_cam_info_topic" value="$(arg left_cam_info_topic)"/>
<arg name="right_cam_info_topic" value="$(arg right_cam_info_topic)"/>
<arg name="left_cam_topic" value="$(arg left_cam_topic)"/>
<arg name="right_cam_topic" value="$(arg right_cam_topic)"/>
<arg name="pointcloud" value="$(arg pointcloud)"/>
<arg name="disparity_image" value="/disparity"/>
<arg name="depth_image" value="$(arg left_cam_stereo_depth_topic)"/>
<arg name="convert_disparity_img_to_depth_img" value="true"/>
</include>
</group>
<!-- Converts registered depth image and RGB image into an RGB pointcloud.
Using depth and semantic image, it generates semantic pointcloud. -->
<node pkg="nodelet" type="nodelet" name="cloudify"
args="load depth_image_proc/point_cloud_xyzrgb nodelet_manager
-no-bond" output="screen">
<!-- Input -->
<remap from="rgb/camera_info" to="$(arg left_cam_info_topic)"/>
<remap from="rgb/image_rect_color" to="$(arg left_cam_segmentation_topic)" if="$(arg metric_semantic_reconstruction)"/>
<remap from="rgb/image_rect_color" to="$(arg left_cam_topic)" unless="$(arg metric_semantic_reconstruction)"/>
<remap from="depth_registered/image_rect" to="$(arg left_cam_stereo_depth_topic)" if="$(arg run_stereo_dense)"/>
<remap from="depth_registered/image_rect" to="$(arg left_cam_depth_topic)" unless="$(arg run_stereo_dense)"/>
<!-- Output -->
<remap from="depth_registered/points" to="$(arg semantic_pointcloud)"/>
<!-- Params -->
<param name="queue_size" value="20"/>
</node>
</group>
<arg name="pkg_type" default="kimera_semantics_ros" if="$(arg metric_semantic_reconstruction)"/>
<arg name="server_type" default="kimera_semantics_node" if="$(arg metric_semantic_reconstruction)"/>
<arg name="pkg_type" default="voxblox_ros" unless="$(arg metric_semantic_reconstruction)"/>
<arg name="server_type" default="tsdf_server" unless="$(arg metric_semantic_reconstruction)"/>
<node name="kimera_semantics_node" pkg="$(arg pkg_type)" type="$(arg server_type)" output="screen"
args="-alsologtostderr -colorlogtostderr" clear_params="true">
<!-- Input -->
<remap from="pointcloud" to="$(arg semantic_pointcloud)"/>
<!-- Params -->
<param name="tsdf_voxel_size" value="$(arg voxel_size)" />
<param name="tsdf_voxels_per_side" value="32" />
<param name="max_ray_length_m" value="$(arg max_ray_length_m)" />
<param name="min_time_between_msgs_sec" value="0.2" />
<param name="voxel_carving_enabled" value="true" />
<param name="color_mode" value="lambert_color"/>
<param name="use_const_weight" value="false" />
<param name="use_freespace_pointcloud" value="$(arg use_freespace_pointcloud)" />
<remap from="freespace_pointcloud" to="$(arg freespace_pointcloud)"/>
<param name="sensor_frame" value="$(arg sensor_frame)"/>
<param name="use_tf_transforms" value="true" />
<param name="enable_icp" value="false" />
<param name="icp_iterations" value="10" />
<param name="verbose" value="true" />
<!-- Method to update voxels' information: "fast" or "merged" -->
<param name="method" value="fast" />
<!-- "color", "semantic" or "semantic_probability" -->
<param name="semantic_color_mode" value="semantic"/>
<param name="semantic_measurement_probability" value="0.8" />
<!-- The only dynamic label we have right now are humans, with label 20 -->
<rosparam param="dynamic_semantic_labels">[24]</rosparam>
<!-- Is this needed? -->
<param name="slice_level" value="1.0" />
<param name="semantic_label_2_color_csv_filepath"
value="$(arg semantic_label_2_color_csv_filepath)"/>
<param name="publish_pointclouds" value="false"/>
<param name="update_mesh_every_n_sec" value="1.0" />
<param name="mesh_filename" value="$(find kimera_semantics_ros)/mesh_results/$(anon tesse).ply" />
</node>
<node pkg="tf2_ros" type="static_transform_publisher" name="left_cam_broadcaster" args="0.2 0 0 0 0 0 1 base_link_gt left_cam" />
<node pkg="tf2_ros" type="static_transform_publisher" name="right_cam_broadcaster" args="-0.2 0 0 0 0 0 1 base_link_gt right_cam" />
</launch>