From bc53e7354952ffe45e5f7be47aa5649bebe84ac5 Mon Sep 17 00:00:00 2001
From: "Jacob.lsx" <jacob.lsx@outlook.com>
Date: Mon, 31 May 2021 14:10:01 +0800
Subject: [PATCH] [Update]: imu assembly

---
 samples/camera_with_junior_device_api.cc                    | 2 ++
 samples/get_depth_with_region.cc                            | 1 +
 samples/get_imu.cc                                          | 1 +
 .../src/mynt_eye_ros_wrapper/config/device/standard.yaml    | 4 ++++
 wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch | 2 +-
 .../ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc     | 6 ++++--
 6 files changed, 13 insertions(+), 3 deletions(-)

diff --git a/samples/camera_with_junior_device_api.cc b/samples/camera_with_junior_device_api.cc
index c7c87bb..d62e621 100644
--- a/samples/camera_with_junior_device_api.cc
+++ b/samples/camera_with_junior_device_api.cc
@@ -31,6 +31,8 @@ int main(int argc, char *argv[]) {
   auto &&request = device::select_request(device, &ok);
   if (!ok) return 1;
   device->ConfigStreamRequest(request);
+  device->EnableImuCorrespondence(true);
+  device->EnableProcessMode(ProcessMode::PROC_IMU_ASSEMBLY);
 
   std::size_t left_count = 0;
   device->SetStreamCallback(
diff --git a/samples/get_depth_with_region.cc b/samples/get_depth_with_region.cc
index ebd56ba..6337ad4 100644
--- a/samples/get_depth_with_region.cc
+++ b/samples/get_depth_with_region.cc
@@ -170,6 +170,7 @@ int main(int argc, char *argv[]) {
 
   api->SetOptionValue(Option::IR_CONTROL, 80);
 
+  api->SetDisparityComputingMethodType(DisparityComputingMethod::SGBM);
   api->EnableStreamData(Stream::DISPARITY_NORMALIZED);
   api->EnableStreamData(Stream::DEPTH);
 
diff --git a/samples/get_imu.cc b/samples/get_imu.cc
index 8f6be00..e444407 100644
--- a/samples/get_imu.cc
+++ b/samples/get_imu.cc
@@ -33,6 +33,7 @@ int main(int argc, char *argv[]) {
   api->EnableMotionDatas();
   // Enable imu timestamp correspondence int device;
   api->EnableImuTimestampCorrespondence(true);
+  api->EnableProcessMode(ProcessMode::PROC_IMU_ASSEMBLY);
 
   api->Start(Source::ALL);
 
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/config/device/standard.yaml b/wrappers/ros/src/mynt_eye_ros_wrapper/config/device/standard.yaml
index 2d6b511..c48b0cb 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/config/device/standard.yaml
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/config/device/standard.yaml
@@ -56,3 +56,7 @@ standard/gyro_range: -1
 # index_s_1: 1
 
 standard/request_index: 0
+
+
+# 0 - none process, 1 - scale and assembly process, 2 - temperature process, 3 - both
+standard/imu_process_mode: 1
\ No newline at end of file
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
index c770900..2bff6a9 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
@@ -89,7 +89,7 @@
       <rosparam file="$(find mynt_eye_ros_wrapper)/config/mesh/mesh.yaml" command="load" />
 
       <param name="gravity" value="$(arg gravity)" />
-      <!-- <param name="ros_output_framerate_cut"     value="2" /> -->
+<!--      <param name="ros_output_framerate_cut" value="4" />-->
     </node>
 
     <!-- disable compressed depth plugin for image topics -->
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
index d6a5714..0513c3c 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
@@ -1177,12 +1177,14 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
           break;
       }
     }
+
+    std::int32_t process_mode = 0;
     if (model_ == Model::STANDARD) {
       private_nh_.getParamCached("standard/request_index", request_index);
       frame_rate_ = api_->GetOptionValue(Option::FRAME_RATE);
+      private_nh_.getParamCached("standard/imu_process_mode", process_mode);
+      api_->EnableProcessMode(process_mode);
     }
-
-    std::int32_t process_mode = 0;
     if (model_ == Model::STANDARD2 ||
         model_ == Model::STANDARD210A || model_ == Model::STANDARD200B) {
       private_nh_.getParamCached("standard2/imu_process_mode", process_mode);