feature: edge auto (#190)

* chore: adapted to the new way of using lidartag Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: rename decompressed_topic name to avoid name conflict on live Signed-off-by: Manato HIRABAYASHI <manato.hirabayashi@tier4.jp> * feat: introduce `choice` for bool options Signed-off-by: Manato HIRABAYASHI <manato.hirabayashi@tier4.jp> * fix: init tf tree visualization (#188) * fix: init tf tree visualization Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: fix error Signed-off-by: vividf <yihsiang.fang@tier4.jp> --------- Signed-off-by: vividf <yihsiang.fang@tier4.jp> * fix(intrinsic_camera_calibrator): fixed dot board detector (#189) fix: forgot to add a fix to the dot board detector Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> --------- Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> Signed-off-by: Manato HIRABAYASHI <manato.hirabayashi@tier4.jp> Signed-off-by: vividf <yihsiang.fang@tier4.jp> Co-authored-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> Co-authored-by: Yi-Hsiang Fang (Vivid) <146902905+vividf@users.noreply.github.com>
tier4 · Sep 10, 2024 · a71b965 · a71b965
1 parent 3e56069
commit a71b965
Show file tree

Hide file tree

Showing 9 changed files with 77 additions and 11 deletions.
diff --git a/...rinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/dotboard_detector.py b/...rinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/dotboard_detector.py
@@ -120,6 +120,10 @@ def detect(detection_image, detector):
         if not resized_detection or max(h, w) <= resized_max_resolution:
             (ok, corners) = detect(grayscale, full_res_detector)
 
+            if not ok:
+                self.detection_results_signal.emit(img, None, stamp)
+                return
+
         else:
             # Resize
             resized = cv2.resize(img, (resized_w, resized_h), interpolation=cv2.INTER_NEAREST)

diff --git a/calibrators/tag_based_pnp_calibrator/launch/calibrator.launch.xml b/calibrators/tag_based_pnp_calibrator/launch/calibrator.launch.xml
@@ -3,16 +3,34 @@
   <arg name="camera_info_topic"/>
   <arg name="pointcloud_topic"/>
   <arg name="base_frame" default="base_link"/>
-  <arg name="lidar_model" description="For supported lidar models check the lidartag repository"/>
+
   <arg name="use_receive_time" default="false" description="If sensor synchronization does not work, we can force the use of system time"/>
   <arg name="use_rectified_image" default="true" description="We need to know if the image has been rectified so we can determine the use of K or P"/>
   <arg name="calibration_pairs" default="9" description="Number of lidar-image pairs for the calibration to converge"/>
   <arg name="calibration_pairs_min_distance" default="1.5" description="Minimum allowed between a new detection and current pairs"/>
 
+  <arg name="lidar_model">
+    <choice value="aeva_aeries2"/>
+    <choice value="at128"/>
+    <choice value="innoviz2"/>
+    <choice value="ouster_os1_128"/>
+    <choice value="ouster_os2"/>
+    <choice value="pandar_40p"/>
+    <choice value="pandar_64"/>
+    <choice value="pandar_qt"/>
+    <choice value="pandar_xt32"/>
+    <choice value="robosense_m1"/>
+    <choice value="seyond_falcon"/>
+    <choice value="seyond_robin_w"/>
+    <choice value="velodyne_vlp16"/>
+    <choice value="velodyne_vls128"/>
+  </arg>
+
   <arg name="calibration_service_name" default="extrinsic_calibration"/>
 
   <!-- lidartag detector -->
-  <include file="$(find-pkg-share lidartag)/launch/lidartag_$(var lidar_model).launch.xml">
+  <include file="$(find-pkg-share lidartag)/launch/lidartag.launch.xml">
+    <arg name="lidar_model" value="$(var lidar_model)"/>
     <arg name="pointcloud_topic" value="$(var pointcloud_topic)"/>
   </include>
 

diff --git a/calibrators/tag_based_sfm_calibrator/launch/lidartag_detector.launch.xml b/calibrators/tag_based_sfm_calibrator/launch/lidartag_detector.launch.xml
@@ -3,12 +3,29 @@
   <arg name="ns" default=""/>
   <arg name="pointcloud_topic"/>
   <arg name="filtered_detections_topic" default="lidartag/filtered/detections_array"/>
-  <arg name="lidar_model"/>
+
+  <arg name="lidar_model">
+    <choice value="aeva_aeries2"/>
+    <choice value="at128"/>
+    <choice value="innoviz2"/>
+    <choice value="ouster_os1_128"/>
+    <choice value="ouster_os2"/>
+    <choice value="pandar_40p"/>
+    <choice value="pandar_64"/>
+    <choice value="pandar_qt"/>
+    <choice value="pandar_xt32"/>
+    <choice value="robosense_m1"/>
+    <choice value="seyond_falcon"/>
+    <choice value="seyond_robin_w"/>
+    <choice value="velodyne_vlp16"/>
+    <choice value="velodyne_vls128"/>
+  </arg>
 
   <group>
     <push-ros-namespace namespace="$(var ns)"/>
 
-    <include file="$(find-pkg-share lidartag)/launch/lidartag_$(var lidar_model).launch.xml">
+    <include file="$(find-pkg-share lidartag)/launch/lidartag.launch.xml">
+      <arg name="lidar_model" value="$(var lidar_model)"/>
       <arg name="pointcloud_topic" value="$(var pointcloud_topic)"/>
     </include>
 

diff --git a/sensor_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml b/sensor_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml
@@ -5,10 +5,26 @@
   <arg name="image_compressed_topic" default="/sensing/camera/camera0/image_rect_color/compressed" description="Input compressed image"/>
   <arg name="camera_info_topic" default="/sensing/camera/camera0/camera_info"/>
   <arg name="pointcloud_topic" default="/sensing/lidar/top/pointcloud_raw"/>
-  <arg name="lidar_model" default="velodyne_vls128" description="this lidar must be supported by the lidartag package"/>
   <arg name="use_rectified_image" default="true" description="image_rect vs. raw"/>
   <arg name="view_only_ui" default="true" description="By default we use a minimal UI"/>
 
+  <arg name="lidar_model">
+    <choice value="aeva_aeries2"/>
+    <choice value="at128"/>
+    <choice value="innoviz2"/>
+    <choice value="ouster_os1_128"/>
+    <choice value="ouster_os2"/>
+    <choice value="pandar_40p"/>
+    <choice value="pandar_64"/>
+    <choice value="pandar_qt"/>
+    <choice value="pandar_xt32"/>
+    <choice value="robosense_m1"/>
+    <choice value="seyond_falcon"/>
+    <choice value="seyond_robin_w"/>
+    <choice value="velodyne_vlp16"/>
+    <choice value="velodyne_vls128"/>
+  </arg>
+
   <let name="rviz_profile" value=""/>
   <let name="rviz_profile" value="$(find-pkg-share tag_based_pnp_calibrator)/rviz/default_profile.rviz"/>
 

diff --git a/...calibration_manager/launch/default_project/tag_based_sfm_base_lidar_calibrator.launch.xml b/...calibration_manager/launch/default_project/tag_based_sfm_base_lidar_calibrator.launch.xml
@@ -12,7 +12,7 @@
   <let name="main_calibration_sensor_frame" value="pandar_top"/>
 
   <!-- Lidar configuration -->
-  <let name="calibration_lidar_model" value="pandar64"/>
+  <let name="calibration_lidar_model" value="pandar_64"/>
 
   <!-- Topic configuration -->
   <let name="calibration_lidar_topic" value="/sensing/lidar/top/pointcloud_raw"/>

diff --git a/sensor_calibration_manager/launch/edge_auto/tag_based_pnp_calibrator.launch.xml b/sensor_calibration_manager/launch/edge_auto/tag_based_pnp_calibrator.launch.xml
@@ -9,12 +9,19 @@
     <choice value="pandar_xt32"/>
   </arg>
 
-  <arg name="view_only_ui" default="true" description="By default we use a minimal UI"/>
+  <arg name="view_only_ui" default="true" description="By default we use a minimal UI">
+    <choice value="true"/>
+    <choice value="false"/>
+  </arg>
+  <arg name="use_receive_time" default="false" description="Use receive time instead of topic timestamps to make corresponding pairs">
+    <choice value="false"/>
+    <choice value="true"/>
+  </arg>
   <arg name="calibration_pairs" default="9" description="Number of lidar-image pairs for the calibration to converge"/>
   <arg name="calibration_pairs_min_distance" default="1.5" description="Minimum allowed between a new detection and current pairs"/>
 
   <!-- we do not use the standard image_raw name to avoid naming conflicts -->
-  <let name="image_decompressed_topic" value="/sensing/camera/$(var camera_name)/image_raw"/>
+  <let name="image_decompressed_topic" value="/sensing/camera/$(var camera_name)/image_raw/decompressed"/>
   <let name="image_compressed_topic" value="/sensing/camera/$(var camera_name)/image_raw/compressed"/>
 
   <let name="camera_info_topic" value="/sensing/camera/$(var camera_name)/camera_info"/>
@@ -50,6 +57,7 @@
       <arg name="use_rectified_image" value="$(var use_rectified_image)"/>
       <arg name="calibration_pairs" value="$(var calibration_pairs)"/>
       <arg name="calibration_pairs_min_distance" value="$(var calibration_pairs_min_distance)"/>
+      <arg name="use_receive_time" value="$(var use_receive_time)"/>
     </include>
 
     <!-- interactive calibrator -->

diff --git a/sensor_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml b/sensor_calibration_manager/launch/rdv/tag_based_pnp_calibrator.launch.xml
@@ -23,7 +23,7 @@
 
   <let name="camera_frame" value="$(var camera_name)/camera_link"/>
   <let name="rviz_profile" value="$(find-pkg-share tag_based_pnp_calibrator)/rviz/default_profile.rviz"/>
-  <let name="lidar_model" value="pandar64"/>
+  <let name="lidar_model" value="pandar_64"/>
   <let name="lidar_frame" value="pandar_top"/>
   <let name="use_rectified_image" value="false"/>
 

diff --git a/sensor_calibration_manager/launch/rdv/tag_based_sfm_base_lidar_calibrator.launch.xml b/sensor_calibration_manager/launch/rdv/tag_based_sfm_base_lidar_calibrator.launch.xml
@@ -12,7 +12,7 @@
   <arg name="main_calibration_sensor_frame" default="pandar_top"/>
 
   <!-- Lidar configuration -->
-  <arg name="calibration_lidar_model" default="pandar64"/>
+  <arg name="calibration_lidar_model" default="pandar_64"/>
 
   <!-- Topic configuration -->
   <arg name="calibration_lidar_topic" default="/sensing/lidar/top/pointcloud_raw"/>

diff --git a/sensor_calibration_manager/sensor_calibration_manager/sensor_calibration_manager.py b/sensor_calibration_manager/sensor_calibration_manager/sensor_calibration_manager.py
@@ -286,7 +286,10 @@ def tfs_graph_callback(self, tfs_dict):
         self.tfs_graph_signal.emit(copy.deepcopy(tfs_dict))
 
     def tf_graph_callback2(self, tfs_dict):
-        if self.calibrator.state != CalibratorState.WAITING_TFS:
+        if (
+            self.calibrator.state != CalibratorState.WAITING_SERVICES
+            and self.calibrator.state != CalibratorState.WAITING_TFS
+        ):
             return
 
         for parent, children_and_tf_dict in tfs_dict.items():