Added files for the calibration demo

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

sensor/extrinsic_calibration_manager/launch/aip_demo/tag_based.launch.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="vehicle_id" default="default"/>
+  <let name="sensor_model" value="aip_demo"/>
+  <arg name="camera_name" default=""/>
+  <arg name="rviz" default="false"/>
+
+  <group>
+    <push-ros-namespace namespace="sensor_kit"/>
+    <include file="$(find-pkg-share extrinsic_calibration_manager)/launch/$(var sensor_model)/tag_based_sensor_kit.launch.xml">
+      <arg name="vehicle_id" value="$(var vehicle_id)"/>
+      <arg name="camera_name" value="$(var camera_name)"/>
+      <arg name="rviz" value="$(var rviz)"/>
+    </include>
+  </group>
+</launch>

sensor/extrinsic_calibration_manager/launch/aip_demo/tag_based_base.launch.xml

@@ -0,0 +1,162 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="vehicle_id" default="default"/>
+  <let name="sensor_model" value="aip_demo"/>
+  <arg name="camera_name" default=""/>
+  <arg name="rviz" default="false"/>
+
+  <!-- we do not use the standard image_raw name to avoid naming conflicts -->
+  <let name="rviz_profile" value="$(find-pkg-share extrinsic_tag_based_base_calibrator)/rviz/demo.rviz"/>
+
+  <!-- Common tag parameters -->
+  <let name="waypoint_tag_size" value="0.6"/>
+  <let name="ground_tag_size" value="0.22"/>
+  <let name="wheel_tag_size" value="0.166"/>
+
+  <!-- calibration api -->
+  <arg name="base_frame" default="base_link"/>
+  <arg name="sensor_kit_frame" default="sensor_kit_base_link"/>
+  <arg name="lidar_base_frame" default="hesai"/>
+  <arg name="lidar_frame" default="hesai"/>
+
+  <!-- top: lidartag detector -->
+  <group>
+    <push-ros-namespace namespace="top"/>
+
+    <include file="$(find-pkg-share lidartag)/launch/lidartag_pandar_40p.launch.xml">
+      <arg name="pointcloud_topic" value="/aw_points"/>
+    </include>
+
+    <node pkg="tier4_tag_utils" exec="lidartag_filter_node" name="lidartag_filter" output="screen">
+      <param name="max_no_observation_time" value="3.0"/>
+      <param name="new_hypothesis_distance" value="1.5"/>
+
+      <param name="new_hypothesis_transl" value="0.1"/>
+      <param name="new_hypothesis_rot" value="15.0"/>
+      <param name="measurement_noise_transl" value="0.05"/>
+      <param name="measurement_noise_rot" value="5.0"/>
+      <param name="process_noise_transl" value="0.01"/>
+      <param name="process_noise_transl_dot" value="0.001"/>
+      <param name="process_noise_rot" value="1.0"/>
+      <param name="process_noise_rot_dot" value="0.1"/>
+    </node>
+  </group>
+
+  <!-- base -> sensor_kit: extrinsic_calibration_client -->
+  <arg name="base_to_sensor_kit_src_yaml" default="$(find-pkg-share individual_params)/config/$(var vehicle_id)/$(var sensor_model)/sensors_calibration.yaml"/>
+  <arg name="base_to_sensor_kit_dst_yaml" default="$(env HOME)/sensors_calibration.yaml"/>
+
+  <node pkg="extrinsic_calibration_client" exec="extrinsic_calibration_client" name="base_to_sensor_kit_extrinsic_calibration_client" output="screen">
+    <remap from="extrinsic_calibration_manager" to="base_to_sensor_kit_extrinsic_calibration_manager"/>
+    <param name="src_path" value="$(var base_to_sensor_kit_src_yaml)"/>
+    <param name="dst_path" value="$(var base_to_sensor_kit_dst_yaml)"/>
+  </node>
+
+  <!-- base -> sensor_kit: extrinsic_calibration_manager -->
+  <node pkg="extrinsic_calibration_manager" exec="extrinsic_calibration_manager" name="base_to_sensor_kit_extrinsic_calibration_manager" output="screen">
+    <remap from="extrinsic_calibration_manager" to="base_to_sensor_kit_extrinsic_calibration_manager"/>
+    <param name="parent_frame" value="$(var base_frame)"/>
+    <param name="child_frames" value="
+    [$(var sensor_kit_frame)]"/>
+  </node>
+
+  <!-- base-lidar calibrator -->
+  <include file="$(find-pkg-share extrinsic_tag_based_base_calibrator)/launch/calibrator.launch.xml">
+    <arg name="base_frame" value="base_link"/>
+
+    <arg name="lidar_sensor_kit_frames" value="[$(var sensor_kit_frame)]"/>
+    <arg name="calibration_lidar_parent_frames" value="[$(var lidar_base_frame)]"/>
+    <arg name="lidar_calibration_service_names" value="['$(var base_frame)/$(var sensor_kit_frame)']"/>
+
+    <arg name="camera_sensor_kit_frames" value="['']"/>
+    <arg name="calibration_camera_parent_frames" value="['']"/>
+    <arg name="camera_calibration_service_names" value="['']"/>
+
+    <arg name="main_calibration_sensor_frame" value="$(var lidar_frame)"/>
+    <arg name="calibration_lidar_frames" value="[$(var lidar_frame)]"/>
+    <arg name="calibration_camera_frames" value="['']"/>
+
+    <!-- Topic configuration -->
+    <arg name="calibration_lidar_detections_topics" value="['/top/lidartag/filtered/detections_array']"/>
+    <arg name="calibration_image_detections_topics" value="['']"/>
+    <arg name="calibration_camera_info_topics" value="['']"/>
+    <arg name="calibration_image_topics" value="['']"/>
+
+    <!-- Tag configuration - 2022 - BS -->
+    <!--arg name="waypoint_tag_ids" value="[0]"/>
+    <arg name="ground_tag_ids" value="[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]"/>
+
+    <arg name="left_wheel_tag_id" value="4"/>
+    <arg name="right_wheel_tag_id" value="3"/>
+
+    <arg name="ground_tag_size" value="0.163"/-->
+
+    <!--Tag configuration - 2023/04/21 - Omiya -->
+    <arg name="waypoint_tag_ids" value="[0, 3, 4, 5]"/>
+    <arg name="waypoint_tag_size" value="$(var waypoint_tag_size)"/>
+
+    <arg name="ground_tag_family" value="tag36h11"/>
+    <arg name="ground_tag_rows" value="1"/>
+    <arg name="ground_tag_cols" value="1"/>
+    <arg name="ground_tag_size" value="$(var ground_tag_size)"/>
+    <arg name="ground_tag_spacing" value="0.2"/>
+    <arg
+      name="ground_tag_ids"
+      value="[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]
+"
+    />
+
+    <arg name="auxiliar_tag_family" value="tag36h11"/>
+    <arg name="auxiliar_tag_rows" value="1"/>
+    <arg name="auxiliar_tag_cols" value="1"/>
+    <arg name="auxiliar_tag_size" value="$(var ground_tag_size)"/>
+    <arg name="auxiliar_tag_spacing" value="0.2"/>
+    <arg
+      name="auxiliar_tag_ids"
+      value="[0, 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]"
+    />
+    <!-- 20230512 - had to take 12 away since it was moved during the experiment -->
+
+    <arg name="wheel_tag_family" value="tag16h5"/>
+    <arg name="wheel_tag_rows" value="2"/>
+    <arg name="wheel_tag_cols" value="2"/>
+    <arg name="wheel_tag_size" value="$(var wheel_tag_size)"/>
+    <arg name="wheel_tag_spacing" value="0.2"/>
+    <arg name="left_wheel_tag_id" value="22"/>
+    <arg name="right_wheel_tag_id" value="26"/>
+
+    <!-- Initial intrinsics calibration -->
+    <arg name="initial_intrinsic_calibration_tag_family" value="tag16h5"/>
+    <arg name="initial_intrinsic_calibration_tag_rows" value="1"/>
+    <arg name="initial_intrinsic_calibration_tag_cols" value="1"/>
+    <arg name="initial_intrinsic_calibration_tag_size" value="0.6"/>
+    <arg name="initial_intrinsic_calibration_tag_spacing" value="0.2"/>
+    <arg name="intrinsic_calibration_tag_ids" value="[0, 1, 2, 3, 4, 5]"/>
+
+    <!-- Optimization configuration -->
+    <arg name="ba_optimize_intrinsics" value="true"/>
+    <arg name="ba_share_intrinsics" value="true"/>
+    <arg name="ba_force_shared_ground_plane" value="true"/>
+
+    <!-- Initial intrinsics calibration -->
+    <arg name="initial_intrinsic_calibration_board_type" value="chessboard"/>
+    <arg name="intrinsic_calibration_tag_ids" value="[0, 4, 5]"/>
+
+    <!-- Optimization configuration -->
+    <arg name="ba_optimize_intrinsics" value="true"/>
+    <arg name="ba_share_intrinsics" value="true"/>
+    <arg name="ba_force_shared_ground_plane" value="true"/>
+  </include>
+
+  <node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="0 0 0 0 0 0 velodyne_top estimated_base_link"/>
+
+  <node pkg="rviz2" exec="rviz2" name="rviz2" output="log" args="-d $(var rviz_profile)" if="$(var rviz)"/>
+
+  <node
+    pkg="tf2_ros"
+    exec="static_transform_publisher"
+    name="lidar_hack_broadcaster"
+    output="screen"
+    args="--x 0.0 --y 0.0 --z 0.0 --qx 0.0 --qy -0.0 --qz 0.0 --qw 1.0 --frame-id hesai --child-frame-id lidar"
+  />
+</launch>

sensor/extrinsic_calibration_manager/launch/aip_demo/tag_based_sensor_kit.launch.xml

@@ -0,0 +1,97 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="vehicle_id" default="default"/>
+  <let name="sensor_model" value="aip_demo"/>
+  <let name="parent_frame" value="sensor_kit_base_link"/>
+
+  <arg name="camera_name"/>
+  <arg name="rviz"/>
+
+  <!-- we do not use the standard image_raw name to avoid naming conflicts -->
+  <let name="image_decompressed_topic" value="/image_raw/decompressed"/>
+  <let name="image_compressed_topic" value="/image_raw/compressed"/>
+
+  <let name="camera_info_topic" value="/camera_info"/>
+  <let name="pointcloud_topic" value="/aw_points"/>
+
+  <let name="calibrate_sensor" value="false"/>
+  <let name="calibrate_sensor" value="true" if="$(eval &quot;'$(var camera_name)' == 'camera0' &quot;)"/>
+
+  <let name="camera_frame" value=""/>
+  <let name="camera_frame" value="camera0/camera_link" if="$(eval &quot;'$(var camera_name)' == 'camera0' &quot;)"/>
+
+  <let name="rviz_profile" value=""/>
+  <let name="rviz_profile" value="$(find-pkg-share extrinsic_tag_based_calibrator)/rviz/tag_calib_camera0_pandar_40p_demo.rviz" if="$(eval &quot;'$(var camera_name)' == 'camera0' &quot;)"/>
+
+  <let name="lidar_model" value="pandar_40p"/>
+
+  <!-- extrinsic_calibration_client -->
+  <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"/>
+
+  <node pkg="extrinsic_calibration_client" exec="extrinsic_calibration_client" name="extrinsic_calibration_client" output="screen" if="$(var calibrate_sensor)">
+    <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" if="$(var calibrate_sensor)">
+    <param name="parent_frame" value="$(var parent_frame)"/>
+    <param name="child_frames" value="
+    [$(var camera_frame)]"/>
+  </node>
+
+  <!-- image decompressor -->
+  <node pkg="image_transport_decompressor" exec="image_transport_decompressor_node" name="decompressor" output="screen">
+    <remap from="decompressor/input/compressed_image" to="$(var image_compressed_topic)"/>
+    <remap from="decompressor/output/raw_image" to="$(var image_decompressed_topic)"/>
+  </node>
+
+  <!-- tag based calibrator -->
+  <include file="$(find-pkg-share extrinsic_tag_based_calibrator)/launch/calibrator.launch.xml" if="$(var calibrate_sensor)">
+    <arg name="ns" value="$(var parent_frame)/$(var camera_name)/camera_link"/>
+    <arg name="parent_frame" value="$(var parent_frame)"/>
+    <arg name="child_frame" value="$(var camera_frame)"/>
+    <arg name="image_topic" value="$(var image_decompressed_topic)"/>
+    <arg name="camera_info_topic" value="$(var camera_info_topic)"/>
+    <arg name="pointcloud_topic" value="$(var pointcloud_topic)"/>
+    <arg name="pointcloud_topic_ex" value="$(var camera_info_topic)"/>
+    <arg name="lidar_model" value="$(var lidar_model)"/>
+  </include>
+
+  <!-- interactive calibrator -->
+  <group if="$(var calibrate_sensor)">
+    <push-ros-namespace namespace="$(var parent_frame)/$(var camera_frame)"/>
+
+    <node pkg="extrinsic_interactive_calibrator" exec="interactive_calibrator" name="interactive_calibrator" output="screen">
+      <remap from="pointcloud" to="$(var pointcloud_topic)"/>
+      <remap from="image" to="$(var image_compressed_topic)"/>
+      <remap from="camera_info" to="$(var camera_info_topic)"/>
+      <remap from="calibration_points_input" to="calibration_points"/>
+
+      <param name="camera_parent_frame" value="$(var parent_frame)"/>
+      <param name="camera_frame" value="$(var camera_frame)"/>
+      <param name="use_calibration_api" value="false"/>
+      <param name="can_publish_tf" value="false"/>
+    </node>
+
+    <include file="$(find-pkg-share intrinsic_camera_calibration)/launch/optimizer.launch.xml"/>
+    <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(var rviz_profile)" if="$(var rviz)"/>
+  </group>
+
+  <node
+    pkg="tf2_ros"
+    exec="static_transform_publisher"
+    name="camera_optical_link_broacaster"
+    output="screen"
+    args="--x 0.0 --y 0.0 --z 0.0 --qx 0.5 --qy -0.5 --qz 0.5 --qw -0.5 --frame-id camera0/camera_link --child-frame-id camera0"
+  />
+
+  <node
+    pkg="tf2_ros"
+    exec="static_transform_publisher"
+    name="lidar_hack_broadcaster"
+    output="screen"
+    args="--x 0.0 --y 0.0 --z 0.0 --qx 0.0 --qy -0.0 --qz 0.0 --qw 1.0 --frame-id hesai --child-frame-id lidar"
+  />
+</launch>