forked from tier4/CalibrationTools
/
mapping_based_sensor_kit.launch.xml
87 lines (69 loc) · 4.29 KB
/
mapping_based_sensor_kit.launch.xml
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
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="vehicle_id" default="tutorial_vehicle"/>
<let name="sensor_model" value="tutorial_vehicle_sensor_kit"/>
<arg name="rviz"/>
<let name="rviz_profile" value="$(find-pkg-share extrinsic_mapping_based_calibrator)/rviz/x1.rviz"/>
<arg name="src_yaml" default="$(find-pkg-share individual_params)/config/$(var vehicle_id)/$(var sensor_model)/sensor_kit_calibration.yaml"/>
<arg name="dst_yaml" default="$(env HOME)/sensor_kit_calibration.yaml"/>
<let name="camera_calibration_service_names" value="['']"/>
<let name="camera_calibration_sensor_kit_frames" value="['']"/>
<let name="calibration_camera_frames" value="['']"/>
<let name="calibration_camera_optical_link_frames" value="['']"/>
<let name="calibration_camera_info_topics" value="['']"/>
<let name="calibration_image_topics" value="['']"/>
<let name="lidar_calibration_sensor_kit_frames" value="[sensor_kit_base_link]"/>
<let
name="lidar_calibration_service_names"
value="[/sensor_kit/sensor_kit_base_link/rs_bpearl_front_base_link]"
/>
<let name="calibration_lidar_base_frames" value="[rs_bpearl_front_base_link]"/>
<let name="calibration_lidar_frames" value="[rs_bpearl_front]"/>
<let name="mapping_lidar_frame" value="rs_helios_top"/>
<let name="mapping_pointcloud" value="/sensing/lidar/top/outlier_filtered/pointcloud"/>
<let name="detected_objects" value="/perception/object_recognition/detection/objects"/>
<let name="calibration_pointcloud_topics" value="[
/sensing/lidar/front/pointcloud_raw_ex]"/>
<group>
<!-- extrinsic_calibration_client -->
<node pkg="extrinsic_calibration_client" exec="extrinsic_calibration_client" name="extrinsic_calibration_client" output="screen">
<param name="src_path" value="$(var src_yaml)"/>
<param name="dst_path" value="$(var dst_yaml)"/>
</node>
<!-- extrinsic_calibration_manager -->
<node pkg="extrinsic_calibration_manager" exec="extrinsic_calibration_manager" name="extrinsic_calibration_manager" output="screen">
<param name="parent_frame" value="sensor_kit_base_link"/>
<param name="child_frames" value="[rs_bpearl_front_base_link]"/>
</node>
</group>
<!-- mapping based calibrator -->
<include file="$(find-pkg-share extrinsic_mapping_based_calibrator)/launch/calibrator.launch.xml">
<arg name="ns" value=""/>
<arg name="camera_calibration_service_names" value="$(var camera_calibration_service_names)"/>
<arg name="lidar_calibration_service_names" value="$(var lidar_calibration_service_names)"/>
<arg name="camera_calibration_sensor_kit_frames" value="$(var camera_calibration_sensor_kit_frames)"/>
<arg name="lidar_calibration_sensor_kit_frames" value="$(var lidar_calibration_sensor_kit_frames)"/>
<arg name="calibration_camera_frames" value="$(var calibration_camera_frames)"/>
<arg name="calibration_camera_optical_link_frames" value="$(var calibration_camera_optical_link_frames)"/>
<arg name="calibration_lidar_base_frames" value="$(var calibration_lidar_base_frames)"/>
<arg name="calibration_lidar_frames" value="$(var calibration_lidar_frames)"/>
<arg name="mapping_lidar_frame" value="$(var mapping_lidar_frame)"/>
<arg name="mapping_pointcloud" value="$(var mapping_pointcloud)"/>
<arg name="detected_objects" value="$(var detected_objects)"/>
<arg name="calibration_camera_info_topics" value="$(var calibration_camera_info_topics)"/>
<arg name="calibration_image_topics" value="$(var calibration_image_topics)"/>
<arg name="calibration_pointcloud_topics" value="$(var calibration_pointcloud_topics)"/>
<arg name="mapping_max_range" value="150.0"/>
<arg name="local_map_num_keyframes" value="30"/>
<arg name="dense_pointcloud_num_keyframes" value="20"/>
<arg name="ndt_resolution" value="0.5"/>
<arg name="ndt_max_iterations" value="100"/>
<arg name="ndt_epsilon" value="0.005"/>
<arg name="lost_frame_max_acceleration" value="15.0"/>
<arg name="lidar_calibration_max_frames" value="10"/>
<arg name="calibration_eval_max_corr_distance" value="0.2"/>
<arg name="solver_iterations" value="100"/>
<arg name="calibration_skip_keyframes" value="15"/>
</include>
<node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(var rviz_profile)" if="$(var rviz)"/>
</launch>