add use hardware time option

orbbec · Feb 2, 2024 · 689a527 · 689a527
1 parent 754e2ea
commit 689a527
Show file tree

Hide file tree

Showing 31 changed files with 38 additions and 6 deletions.
diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h
@@ -448,5 +448,6 @@ class OBCameraNode {
   std::condition_variable colorFrameCV_;
 
   bool ordered_pc_ = false;
+  bool use_hardware_time_ = false;
 };
 }  // namespace orbbec_camera
diff --git a/orbbec_camera/launch/astra.launch.py b/orbbec_camera/launch/astra.launch.py
@@ -61,6 +61,7 @@ def generate_launch_description():
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/astra2.launch.py b/orbbec_camera/launch/astra2.launch.py
@@ -69,6 +69,7 @@ def generate_launch_description():
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/astra_adv.launch.py b/orbbec_camera/launch/astra_adv.launch.py
@@ -61,6 +61,7 @@ def generate_launch_description():
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/astra_embedded_s.launch.py b/orbbec_camera/launch/astra_embedded_s.launch.py
@@ -61,6 +61,7 @@ def generate_launch_description():
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/astra_pro2.launch.py b/orbbec_camera/launch/astra_pro2.launch.py
@@ -61,6 +61,7 @@ def generate_launch_description():
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/astra_stereo_u3.launch.py b/orbbec_camera/launch/astra_stereo_u3.launch.py
@@ -61,6 +61,7 @@ def generate_launch_description():
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/dabai.launch.py b/orbbec_camera/launch/dabai.launch.py
@@ -61,6 +61,7 @@ def generate_launch_description():
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/dabai_d1.launch.py b/orbbec_camera/launch/dabai_d1.launch.py
@@ -49,6 +49,7 @@ def generate_launch_description():
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/dabai_dcl.py b/orbbec_camera/launch/dabai_dcl.py
@@ -82,6 +82,7 @@ def generate_launch_description():
         DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
         DeclareLaunchArgument('enable_frame_sync', default_value='true'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration
@@ -132,4 +133,4 @@ def generate_launch_description():
                 )
             ]
         )
-        return ld
+        return ld
diff --git a/orbbec_camera/launch/dabai_dcw.launch.py b/orbbec_camera/launch/dabai_dcw.launch.py
@@ -61,6 +61,7 @@ def generate_launch_description():
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/dabai_dcw2.launch.py b/orbbec_camera/launch/dabai_dcw2.launch.py
@@ -62,6 +62,7 @@ def generate_launch_description():
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/dabai_dw.launch.py b/orbbec_camera/launch/dabai_dw.launch.py
@@ -49,6 +49,7 @@ def generate_launch_description():
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/dabai_dw2.launch.py b/orbbec_camera/launch/dabai_dw2.launch.py
@@ -49,6 +49,7 @@ def generate_launch_description():
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/dabai_max.launch.py b/orbbec_camera/launch/dabai_max.launch.py
@@ -51,6 +51,7 @@ def generate_launch_description():
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/dabai_max_pro.launch.py b/orbbec_camera/launch/dabai_max_pro.launch.py
@@ -62,7 +62,8 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_soft_filter', default_value='true'),
         DeclareLaunchArgument('enable_soft_filter', default_value='true'),
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
-        DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1')
+        DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/dabai_pro.launch.py b/orbbec_camera/launch/dabai_pro.launch.py
@@ -49,6 +49,7 @@ def generate_launch_description():
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/deeya.launch.py b/orbbec_camera/launch/deeya.launch.py
@@ -60,6 +60,7 @@ def generate_launch_description():
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/femto.launch.py b/orbbec_camera/launch/femto.launch.py
@@ -71,6 +71,7 @@ def generate_launch_description():
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
         DeclareLaunchArgument('enable_frame_sync', default_value='false'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/femto_bolt.launch.py b/orbbec_camera/launch/femto_bolt.launch.py
@@ -76,6 +76,7 @@ def generate_launch_description():
         DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
         DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/femto_mega.launch.py b/orbbec_camera/launch/femto_mega.launch.py
@@ -78,6 +78,7 @@ def generate_launch_description():
         DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
         DeclareLaunchArgument('enable_frame_sync', default_value='true'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/gemini2.launch.py b/orbbec_camera/launch/gemini2.launch.py
@@ -82,6 +82,7 @@ def generate_launch_description():
         DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
         DeclareLaunchArgument('enable_frame_sync', default_value='true'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration
@@ -132,4 +133,4 @@ def generate_launch_description():
                 )
             ]
         )
-        return ld
+        return ld
diff --git a/orbbec_camera/launch/gemini2L.launch.py b/orbbec_camera/launch/gemini2L.launch.py
@@ -82,6 +82,7 @@ def generate_launch_description():
         DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
         DeclareLaunchArgument('enable_frame_sync', default_value='true'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/gemini2VL.launch.py b/orbbec_camera/launch/gemini2VL.launch.py
@@ -75,6 +75,7 @@ def generate_launch_description():
         DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
         DeclareLaunchArgument('enable_frame_sync', default_value='true'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/gemini2XL.launch.py b/orbbec_camera/launch/gemini2XL.launch.py
@@ -91,6 +91,7 @@ def generate_launch_description():
         DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
         DeclareLaunchArgument('enable_frame_sync', default_value='true'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/gemini_e.launch.py b/orbbec_camera/launch/gemini_e.launch.py
@@ -61,6 +61,7 @@ def generate_launch_description():
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/gemini_e_lite.launch.py b/orbbec_camera/launch/gemini_e_lite.launch.py
@@ -50,6 +50,7 @@ def generate_launch_description():
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/gemini_ew.launch.py b/orbbec_camera/launch/gemini_ew.launch.py
@@ -62,6 +62,7 @@ def generate_launch_description():
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/gemini_ew_lite.launch.py b/orbbec_camera/launch/gemini_ew_lite.launch.py
@@ -54,6 +54,8 @@ def generate_launch_description():
         DeclareLaunchArgument('jpeg_decoder', default_value='avdec_mjpeg'),
         DeclareLaunchArgument('video_convert', default_value='videoconvert'),
         DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
+
     ]
 
     # Node configuration

diff --git a/orbbec_camera/launch/gemini_uw.launch.py b/orbbec_camera/launch/gemini_uw.launch.py
@@ -62,7 +62,8 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_soft_filter', default_value='true'),
         DeclareLaunchArgument('enable_soft_filter', default_value='true'),
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
-        DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1')
+        DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
+        DeclareLaunchArgument('use_hardware_time', default_value='false'),
     ]
 
     # Node configuration

diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp
@@ -604,6 +604,7 @@ void OBCameraNode::getParameters() {
   setAndGetNodeParameter<double>(angular_vel_cov_, "angular_vel_cov", 0.02);
   setAndGetNodeParameter<bool>(ordered_pc_, "ordered_pc", false);
   setAndGetNodeParameter<int>(max_save_images_count_, "max_save_images_count", 10);
+  setAndGetNodeParameter<bool>(use_hardware_time_, "use_hardware_time", false);
 }
 
 void OBCameraNode::setupTopics() {
@@ -1130,8 +1131,8 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
   }
   int width = static_cast<int>(video_frame->width());
   int height = static_cast<int>(video_frame->height());
-
-  auto timestamp = frameTimeStampToROSTime(video_frame->systemTimeStamp());
+  auto frame_time_stamp = use_hardware_time_?video_frame->timeStamp():video_frame->systemTimeStamp();
+  auto timestamp = frameTimeStampToROSTime(frame_time_stamp);
   if (!camera_param_) {
     camera_param_ = pipeline_->getCameraParam();
   }