From fee2e140c2c466653201570d3be303fe2eb51154 Mon Sep 17 00:00:00 2001
From: Osenberg <junyangqu@slightech.com>
Date: Thu, 21 Mar 2019 19:43:38 +0800
Subject: [PATCH 1/4] feat(ros): added imu process mode in mynteye.launch

---
 .../ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch     | 7 +++++++
 .../ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc    | 7 +++++++
 2 files changed, 14 insertions(+)

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 b82dc5d..0ef6e42 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
@@ -58,6 +58,10 @@
 
   <arg name="standard2/request_index" default="$(arg index_s2_2)" />
 
+  <!-- standard2/imu_process_mode range: [0,3] -->
+  <!-- 0 - none process, 1 - temperature process, 2 - scale and assembly process, 3 - both -->
+  <arg name="standard2/imu_process_mode" default="0" />
+
   <!-- MYNTEYE-S1030, Reslution: 752x480, Format: YUYV -->
   <arg name="index_s_0"   default="0" />
   <!-- MYNTEYE-S1030, Reslution: 376x240, Format: YUYV -->
@@ -272,6 +276,9 @@
       <param name="standard/request_index" value="$(arg standard/request_index)" />
       <param name="standard2/request_index" value="$(arg standard2/request_index)" />
 
+      <!-- imu data process -->
+      <param name="standard2/imu_process_mode" value="$(arg standard2/imu_process_mode)" />
+
       <!-- device options of standard-->
 
       <param name="standard/gain" value="$(arg standard/gain)" />
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 0ff0e2d..c6d96ce 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
@@ -1099,6 +1099,13 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
       frame_rate_ = api_->GetOptionValue(Option::FRAME_RATE);
     }
 
+    std::int32_t process_mode = 0;
+    if (model_ == Model::STANDARD2 ||
+        model_ == Model::STANDARD210A) {
+      private_nh_.getParam("standard2/imu_process_mode", process_mode);
+      api_->EnableProcessMode(process_mode);
+    }
+
     NODELET_FATAL_COND(m <= 0, "No MYNT EYE devices :(");
     if (m <= 1) {
       NODELET_INFO_STREAM("Only one stream request, select index: 0");

From 588781028f840360f4770cce1f416a74c3402ce0 Mon Sep 17 00:00:00 2001
From: TinyOh <tinyoh@slightech.com>
Date: Fri, 22 Mar 2019 18:09:02 +0800
Subject: [PATCH 2/4] feat(ros): add slam launch

---
 .../config/device/standard.yaml               |  58 ++++
 .../config/device/standard2.yaml              | 108 ++++++++
 .../config/process/process_config.yaml        |  11 +
 .../config/slam/vins_fusion.yaml              |  19 ++
 .../launch/mynteye.launch                     | 247 +-----------------
 .../launch/slam/vins_fusion.launch            | 129 +++++++++
 .../src/wrapper_nodelet.cc                    |   1 +
 7 files changed, 330 insertions(+), 243 deletions(-)
 create mode 100644 wrappers/ros/src/mynt_eye_ros_wrapper/config/device/standard.yaml
 create mode 100644 wrappers/ros/src/mynt_eye_ros_wrapper/config/device/standard2.yaml
 create mode 100644 wrappers/ros/src/mynt_eye_ros_wrapper/config/process/process_config.yaml
 create mode 100644 wrappers/ros/src/mynt_eye_ros_wrapper/config/slam/vins_fusion.yaml
 create mode 100644 wrappers/ros/src/mynt_eye_ros_wrapper/launch/slam/vins_fusion.launch

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
new file mode 100644
index 0000000..d02323b
--- /dev/null
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/config/device/standard.yaml
@@ -0,0 +1,58 @@
+# standard/gain range: [0,48]
+sstandard/gain: -1
+# standard/gain: 24
+
+# standard/brightness range: [0,240]
+standard/brightness: -1
+# standard/brightness: 120
+
+# contrast range: [0,255]
+standard/contrast: -1
+# standard/contrast: 127
+
+# standard/frame_rate range: {10,15,20,25,30,35,40,45,50,55,60}
+standard/frame_rate: -1
+# standard/frame_rate: 25
+
+# standard/imu_frequency range: {100,200,250,333,500}
+standard/imu_frequency: -1
+# standard/imu_frequency: 200
+
+# standard/exposure_mode, 0: auto-exposure, 1: manual-exposure
+standard/exposure_mode: -1
+# standard/exposure_mode: 0
+
+# standard/max_gain range: [0,48]
+standard/max_gain: -1
+# standard/max_gain: 48
+
+# standard/max_exposure_time range: [0,240]
+standard/max_exposure_time: -1
+# standard/max_exposure_time: 240
+
+# standard/desired_brightness range: [0,255]
+standard/desired_brightness: -1
+# standard/desired_brightness: 192
+
+# standard/ir_control range: [0,160]
+standard/ir_control: 80
+# standard/ir_control: 0
+
+# standard/hdr_mode, 0: 10-bit, 1: 12-bit
+standard/hdr_mode: -1
+# standard/hdr_mode: 0
+
+# standard/accel_range range: {4,8,16,32}
+standard/accel_range: -1
+# standard/accel_range: 8
+
+# standard/gyro_range range: {500,1000,2000,4000}
+standard/gyro_range: -1
+# standard/gyro_range: 1000
+
+# MYNTEYE-S1030, Reslution: 752x480, Format: YUYV
+# index_s_0: 0
+# MYNTEYE-S1030, Reslution: 376x240, Format: YUYV
+# index_s_1: 1
+
+standard/request_index: 0
\ No newline at end of file
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/config/device/standard2.yaml b/wrappers/ros/src/mynt_eye_ros_wrapper/config/device/standard2.yaml
new file mode 100644
index 0000000..d4eaafb
--- /dev/null
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/config/device/standard2.yaml
@@ -0,0 +1,108 @@
+
+
+# Attention:The format of MYNTEYE-S2100/S2000 is YUYV.S210A's format is BGR888
+
+# MYNTEYE-S2100/S2000/S210A, Reslution: 1280x400, Format: YUYV/BGR888, Fps: 10
+# index_s2_0: 0
+# MYNTEYE-S2100/S2000/S210A, Reslution: 1280x400, Format: YUYV/BGR888, Fps: 20
+# index_s2_1: 1
+# MYNTEYE-S2100/S2000/S210A, Reslution: 1280x400, Format: YUYV/BGR888, Fps: 30
+# index_s2_2: 2
+# MYNTEYE-S2100/S2000/S210A, Reslution: 1280x400, Format: YUYV/BGR888, Fps: 60
+# index_s2_3: 3
+# MYNTEYE-S2100/S2000/S210A, Reslution: 2560x800, Format: YUYV/BGR888, Fps: 10
+# index_s2_4: 4
+# MYNTEYE-S2100/S2000/S210A, Reslution: 2560x800, Format: YUYV/BGR888, Fps: 20
+# index_s2_5: 5
+# MYNTEYE-S2100/S2000/S210A, Reslution: 2560x800, Format: YUYV/BGR888, Fps: 30
+# index_s2_6: 6
+
+standard2/request_index: 2
+
+# device options of standard2, -1 will not set the value
+
+# standard2/brightness range: [0,240]
+standard2/brightness: -1
+# standard2/brightness: 120
+
+# standard2/exposure_mode, 0: auto-exposure, 1: manual-exposure
+standard2/exposure_mode: -1
+# standard2/exposure_mode: 0
+
+# standard2/max_gain range: [0,255]
+standard2/max_gain: -1
+# standard2/max_gain: 8
+
+# standard2/max_exposure_time range: [0,1000]
+standard2/max_exposure_time: -1
+# standard2/max_exposure_time: 333
+
+# standard2/desired_brightness range: [1,255]
+standard2/desired_brightness: -1
+# standard2/desired_brightness: 122
+
+# standard2/min_exposure_time range: [0,1000]
+standard2/min_exposure_time: -1
+# standard2/min_exposure_time: 0
+
+# standard2/ir_control range: [0,160]
+standard2/ir_control: 80
+# standard2/ir_control: 0
+
+# standard2/accel_range range: [6,48]
+standard2/accel_range: 24
+# standard2/accel_range: 6
+
+# standard2/gyro_range range: [250,4000]
+standard2/gyro_range: -1
+# standard2/gyro_range: 1000
+
+# standard2/accel_low_filter range: [0,2]
+standard2/accel_low_filter: -1
+# standard2/accel_low_filter: 2
+
+# standard2/gyro_low_filter range: [23,64]
+standard2/gyro_low_filter: -1
+# standard2/gyro_low_filter: 64
+
+# device options of standard210a, -1 will not set the value
+
+# standard210a/brightness range: [0,240]
+standard210a/brightness: -1
+# standard210a/brightness: 120
+
+# standard210a/exposure_mode, 0: auto-exposure, 1: manual-exposure
+standard210a/exposure_mode: -1
+# standard210a/exposure_mode: 0
+
+# standard210a/max_gain range: [0,255]
+standard210a/max_gain: -1
+# standard210a/max_gain: 8
+
+# standard210a/max_exposure_time range: [0,1000]
+standard210a/max_exposure_time: -1
+# standard210a/max_exposure_time: 333
+
+# standard210a/desired_brightness range: [1,255]
+standard210a/desired_brightness: -1
+# standard210a/desired_brightness: 122
+
+# standard210a/min_exposure_time range: [0,1000]
+standard210a/min_exposure_time: -1
+# standard210a/min_exposure_time: 0
+
+# standard210a/accel_range range: [6,48]
+standard210a/accel_range: -1
+# standard210a/accel_range: 6
+
+# standard210a/gyro_range range: [250,4000]
+standard210a/gyro_range: -1
+# standard210a/gyro_range: 1000
+
+# standard210a/accel_low_filter range: [0,2]
+standard210a/accel_low_filter: -1
+# standard210a/accel_low_filter: 2
+
+# standard210a/gyro_low_filter range: [23,64]
+standard210a/gyro_low_filter: -1
+# standard210a/gyro_low_filter: 64
\ No newline at end of file
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/config/process/process_config.yaml b/wrappers/ros/src/mynt_eye_ros_wrapper/config/process/process_config.yaml
new file mode 100644
index 0000000..4a2514f
--- /dev/null
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/config/process/process_config.yaml
@@ -0,0 +1,11 @@
+# disparity computing method type
+# sgbm: 0
+# bm: 1
+disparity_computing_method: 1
+
+enable_left_rect: false
+enable_right_rect: false
+enable_disparity: false
+enable_disparity_norm: false
+enable_points: false
+enable_depth: false
\ No newline at end of file
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/config/slam/vins_fusion.yaml b/wrappers/ros/src/mynt_eye_ros_wrapper/config/slam/vins_fusion.yaml
new file mode 100644
index 0000000..26129ae
--- /dev/null
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/config/slam/vins_fusion.yaml
@@ -0,0 +1,19 @@
+
+# Attention:The format of MYNTEYE-S2100/S2000 is YUYV.S210A's format is BGR888
+
+# MYNTEYE-S2100/S2000/S210A, Reslution: 1280x400, Format: YUYV/BGR888, Fps: 10
+# index_s2_0: "0"
+# MYNTEYE-S2100/S2000/S210A, Reslution: 1280x400, Format: YUYV/BGR888, Fps: 20
+# index_s2_1: "1"
+# MYNTEYE-S2100/S2000/S210A, Reslution: 1280x400, Format: YUYV/BGR888, Fps: 30
+# index_s2_2: "2"
+# MYNTEYE-S2100/S2000/S210A, Reslution: 1280x400, Format: YUYV/BGR888, Fps: 60
+# index_s2_3: "3"
+# MYNTEYE-S2100/S2000/S210A, Reslution: 2560x800, Format: YUYV/BGR888, Fps: 10
+# index_s2_4: "4"
+# MYNTEYE-S2100/S2000/S210A, Reslution: 2560x800, Format: YUYV/BGR888, Fps: 20
+# index_s2_5: "5"
+# MYNTEYE-S2100/S2000/S210A, Reslution: 2560x800, Format: YUYV/BGR888, Fps: 30
+# index_s2_6: "6"
+
+standard2/request_index: 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 aed751b..c348f3c 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
@@ -35,190 +35,6 @@
 
   <arg name="mesh_file" default="S1030-0315.obj" />
 
-  <!-- stream toggles -->
-
-  <!-- Request index -->
-
-  <!-- Attention:The format of MYNTEYE-S2100/S2000 is YUYV.S210A's format is BGR888 -->
-
-  <!-- MYNTEYE-S2100/S2000/S210A, Reslution: 1280x400, Format: YUYV/BGR888, Fps: 10 -->
-  <arg name="index_s2_0"   default="0" />
-  <!-- MYNTEYE-S2100/S2000/S210A, Reslution: 1280x400, Format: YUYV/BGR888, Fps: 20 -->
-  <arg name="index_s2_1"   default="1" />
-  <!-- MYNTEYE-S2100/S2000/S210A, Reslution: 1280x400, Format: YUYV/BGR888, Fps: 30 -->
-  <arg name="index_s2_2"   default="2" />
-  <!-- MYNTEYE-S2100/S2000/S210A, Reslution: 1280x400, Format: YUYV/BGR888, Fps: 60 -->
-  <arg name="index_s2_3"   default="3" />
-  <!-- MYNTEYE-S2100/S2000/S210A, Reslution: 2560x800, Format: YUYV/BGR888, Fps: 10 -->
-  <arg name="index_s2_4"   default="4" />
-  <!-- MYNTEYE-S2100/S2000/S210A, Reslution: 2560x800, Format: YUYV/BGR888, Fps: 20 -->
-  <arg name="index_s2_5"   default="5" />
-  <!-- MYNTEYE-S2100/S2000/S210A, Reslution: 2560x800, Format: YUYV/BGR888, Fps: 30 -->
-  <arg name="index_s2_6"   default="6" />
-
-  <arg name="standard2/request_index" default="$(arg index_s2_2)" />
-
-  <!-- MYNTEYE-S1030, Reslution: 752x480, Format: YUYV -->
-  <arg name="index_s_0"   default="0" />
-  <!-- MYNTEYE-S1030, Reslution: 376x240, Format: YUYV -->
-  <arg name="index_s_1"   default="1" />
-
-  <arg name="standard/request_index" default="$(arg index_s_0)" />
-
-  <!-- disparity computing method type -->
-  <arg name="sgbm"   default="0" />
-  <arg name="bm"   default="1" />
-  <arg name="disparity_computing_method"   default="$(arg bm)" />
-
-  <arg name="enable_left_rect" default="false" />
-  <arg name="enable_right_rect" default="false" />
-  <arg name="enable_disparity" default="false" />
-  <arg name="enable_disparity_norm" default="false" />
-  <arg name="enable_points" default="false" />
-  <arg name="enable_depth" default="false" />
-
-  <!-- device options of standard, -1 will not set the value -->
-
-  <!-- standard/gain range: [0,48] -->
-  <arg name="standard/gain" default="-1" />
-  <!-- <arg name="standard/gain" default="24" /> -->
-
-  <!-- standard/brightness range: [0,240] -->
-  <arg name="standard/brightness" default="-1" />
-  <!-- <arg name="standard/brightness" default="120" /> -->
-
-  <!-- contrast range: [0,255] -->
-  <arg name="standard/contrast" default="-1" />
-  <!-- <arg name="standard/contrast" default="127" /> -->
-
-  <!-- standard/frame_rate range: {10,15,20,25,30,35,40,45,50,55,60} -->
-  <arg name="standard/frame_rate" default="-1" />
-  <!-- <arg name="standard/frame_rate" default="25" /> -->
-
-  <!-- standard/imu_frequency range: {100,200,250,333,500} -->
-  <arg name="standard/imu_frequency" default="-1" />
-  <!-- <arg name="standard/imu_frequency" default="200" /> -->
-
-  <!-- standard/exposure_mode, 0: auto-exposure, 1: manual-exposure -->
-  <arg name="standard/exposure_mode" default="-1" />
-  <!-- <arg name="standard/exposure_mode" default="0" /> -->
-
-  <!-- standard/max_gain range: [0,48] -->
-  <arg name="standard/max_gain" default="-1" />
-  <!-- <arg name="standard/max_gain" default="48" /> -->
-
-  <!-- standard/max_exposure_time range: [0,240] -->
-  <arg name="standard/max_exposure_time" default="-1" />
-  <!-- <arg name="standard/max_exposure_time" default="240" /> -->
-
-  <!-- standard/desired_brightness range: [0,255] -->
-  <arg name="standard/desired_brightness" default="-1" />
-  <!-- <arg name="standard/desired_brightness" default="192" /> -->
-
-  <!-- standard/ir_control range: [0,160] -->
-  <arg name="standard/ir_control" default="80" />
-  <!-- <arg name="standard/ir_control" default="0" /> -->
-
-  <!-- standard/hdr_mode, 0: 10-bit, 1: 12-bit -->
-  <arg name="standard/hdr_mode" default="-1" />
-  <!-- <arg name="standard/hdr_mode" default="0" /> -->
-
-  <!-- standard/accel_range range: {4,8,16,32} -->
-  <arg name="standard/accel_range" default="-1" />
-  <!-- <arg name="standard/accel_range" default="8" /> -->
-
-  <!-- standard/gyro_range range: {500,1000,2000,4000} -->
-  <arg name="standard/gyro_range" default="-1" />
-  <!-- <arg name="standard/gyro_range" default="1000" /> -->
-
-  <!-- device options of standard2, -1 will not set the value -->
-
-  <!-- standard2/brightness range: [0,240] -->
-  <arg name="standard2/brightness" default="-1" />
-  <!-- <arg name="standard2/brightness" default="120" /> -->
-
-  <!-- standard2/exposure_mode, 0: auto-exposure, 1: manual-exposure -->
-  <arg name="standard2/exposure_mode" default="-1" />
-  <!-- <arg name="standard2/exposure_mode" default="0" /> -->
-
-  <!-- standard2/max_gain range: [0,255] -->
-  <arg name="standard2/max_gain" default="-1" />
-  <!-- <arg name="standard2/max_gain" default="8" /> -->
-
-  <!-- standard2/max_exposure_time range: [0,1000] -->
-  <arg name="standard2/max_exposure_time" default="-1" />
-  <!-- <arg name="standard2/max_exposure_time" default="333" /> -->
-
-  <!-- standard2/desired_brightness range: [1,255] -->
-  <arg name="standard2/desired_brightness" default="-1" />
-  <!-- <arg name="standard2/desired_brightness" default="122" /> -->
-
-  <!-- standard2/min_exposure_time range: [0,1000] -->
-  <arg name="standard2/min_exposure_time" default="-1" />
-  <!-- <arg name="standard2/min_exposure_time" default="0" /> -->
-
-  <!-- standard2/ir_control range: [0,160] -->
-  <arg name="standard2/ir_control" default="80" />
-  <!-- <arg name="standard2/ir_control" default="0" /> -->
-
-  <!-- standard2/accel_range range: [6,48] -->
-  <arg name="standard2/accel_range" default="24" />
-  <!-- <arg name="standard2/accel_range" default="6" /> -->
-
-  <!-- standard2/gyro_range range: [250,4000] -->
-  <arg name="standard2/gyro_range" default="-1" />
-  <!-- <arg name="standard2/gyro_range" default="1000" /> -->
-
-  <!-- standard2/accel_low_filter range: [0,2] -->
-  <arg name="standard2/accel_low_filter" default="-1" />
-  <!-- <arg name="standard2/accel_low_filter" default="2" /> -->
-
-  <!-- standard2/gyro_low_filter range: [23,64] -->
-  <arg name="standard2/gyro_low_filter" default="-1" />
-  <!-- <arg name="standard2/gyro_low_filter" default="64" /> -->
-
-  <!-- device options of standard210a, -1 will not set the value -->
-
-  <!-- standard210a/brightness range: [0,240] -->
-  <arg name="standard210a/brightness" default="-1" />
-  <!-- <arg name="standard210a/brightness" default="120" /> -->
-
-  <!-- standard210a/exposure_mode, 0: auto-exposure, 1: manual-exposure -->
-  <arg name="standard210a/exposure_mode" default="-1" />
-  <!-- <arg name="standard210a/exposure_mode" default="0" /> -->
-
-  <!-- standard210a/max_gain range: [0,255] -->
-  <arg name="standard210a/max_gain" default="-1" />
-  <!-- <arg name="standard210a/max_gain" default="8" /> -->
-
-  <!-- standard210a/max_exposure_time range: [0,1000] -->
-  <arg name="standard210a/max_exposure_time" default="-1" />
-  <!-- <arg name="standard210a/max_exposure_time" default="333" /> -->
-
-  <!-- standard210a/desired_brightness range: [1,255] -->
-  <arg name="standard210a/desired_brightness" default="-1" />
-  <!-- <arg name="standard210a/desired_brightness" default="122" /> -->
-
-  <!-- standard210a/min_exposure_time range: [0,1000] -->
-  <arg name="standard210a/min_exposure_time" default="-1" />
-  <!-- <arg name="standard210a/min_exposure_time" default="0" /> -->
-
-  <!-- standard210a/accel_range range: [6,48] -->
-  <arg name="standard210a/accel_range" default="-1" />
-  <!-- <arg name="standard210a/accel_range" default="6" /> -->
-
-  <!-- standard210a/gyro_range range: [250,4000] -->
-  <arg name="standard210a/gyro_range" default="-1" />
-  <!-- <arg name="standard210a/gyro_range" default="1000" /> -->
-
-  <!-- standard210a/accel_low_filter range: [0,2] -->
-  <arg name="standard210a/accel_low_filter" default="-1" />
-  <!-- <arg name="standard210a/accel_low_filter" default="2" /> -->
-
-  <!-- standard210a/gyro_low_filter range: [23,64] -->
-  <arg name="standard210a/gyro_low_filter" default="-1" />
-  <!-- <arg name="standard210a/gyro_low_filter" default="64" /> -->
-
   <!-- Push down all topics/nodelets into "mynteye" namespace -->
   <group ns="$(arg mynteye)">
 
@@ -255,66 +71,11 @@
 
       <param name="temperature_frame_id" value="$(arg temperature_frame_id)" />
 
+      <rosparam file="$(find mynt_eye_ros_wrapper)/config/device/standard.yaml" />
+      <rosparam file="$(find mynt_eye_ros_wrapper)/config/device/standard2.yaml" />
+      <rosparam file="$(find mynt_eye_ros_wrapper)/config/process/process_config.yaml" />
+
       <param name="gravity" value="$(arg gravity)" />
-
-      <param name="disparity_computing_method" value="$(arg disparity_computing_method)" />
-
-      <!-- stream toggles -->
-
-      <param name="enable_left_rect" value="$(arg enable_left_rect)" />
-      <param name="enable_right_rect" value="$(arg enable_right_rect)" />
-      <param name="enable_disparity" value="$(arg enable_disparity)" />
-      <param name="enable_disparity_norm" value="$(arg enable_disparity_norm)" />
-      <param name="enable_points" value="$(arg enable_points)" />
-      <param name="enable_depth" value="$(arg enable_depth)" />
-
-      <!-- stream request index -->
-      <param name="standard/request_index" value="$(arg standard/request_index)" />
-      <param name="standard2/request_index" value="$(arg standard2/request_index)" />
-
-      <!-- device options of standard-->
-
-      <param name="sstandard/gain" value="$(arg standard/gain)" />
-      <param name="standard/brightness" value="$(arg standard/brightness)" />
-      <param name="standard/contrast" value="$(arg standard/contrast)" />
-      <param name="standard/frame_rate" value="$(arg standard/frame_rate)" />
-      <param name="standard/imu_frequency" value="$(arg standard/imu_frequency)" />
-      <param name="standard/exposure_mode" value="$(arg standard/exposure_mode)" />
-      <param name="standard/max_gain" value="$(arg standard/max_gain)" />
-      <param name="standard/max_exposure_time" value="$(arg standard/max_exposure_time)" />
-      <param name="standard/desired_brightness" value="$(arg standard/desired_brightness)" />
-      <param name="standard/ir_control" value="$(arg standard/ir_control)" />
-      <param name="standard/hdr_mode" value="$(arg standard/hdr_mode)" />
-      <param name="standard/accel_range" value="$(arg standard/accel_range)" />
-      <param name="standard/gyro_range" value="$(arg standard/gyro_range)" />
-
-      <!-- device options of standard2-->
-
-      <param name="standard2/brightness" value="$(arg standard2/brightness)" />
-      <param name="standard2/exposure_mode" value="$(arg standard2/exposure_mode)" />
-      <param name="standard2/max_gain" value="$(arg standard2/max_gain)" />
-      <param name="standard2/max_exposure_time" value="$(arg standard2/max_exposure_time)" />
-      <param name="standard2/desired_brightness" value="$(arg standard2/desired_brightness)" />
-      <param name="standard2/min_exposure_time" value="$(arg standard2/min_exposure_time)" />
-      <param name="standard2/ir_control" value="$(arg standard2/ir_control)" />
-      <param name="standard2/accel_range" value="$(arg standard2/accel_range)" />
-      <param name="standard2/gyro_range" value="$(arg standard2/gyro_range)" />
-      <param name="standard2/accel_low_filter" value="$(arg standard2/accel_low_filter)" />
-      <param name="standard2/gyro_low_filter" value="$(arg standard2/gyro_low_filter)" />
-
-      <!-- device options of standard210a-->
-
-      <param name="standard210a/brightness" value="$(arg standard210a/brightness)" />
-      <param name="standard210a/exposure_mode" value="$(arg standard210a/exposure_mode)" />
-      <param name="standard210a/max_gain" value="$(arg standard210a/max_gain)" />
-      <param name="standard210a/max_exposure_time" value="$(arg standard210a/max_exposure_time)" />
-      <param name="standard210a/desired_brightness" value="$(arg standard210a/desired_brightness)" />
-      <param name="standard210a/min_exposure_time" value="$(arg standard210a/min_exposure_time)" />
-      <param name="standard210a/accel_range" value="$(arg standard210a/accel_range)" />
-      <param name="standard210a/gyro_range" value="$(arg standard210a/gyro_range)" />
-      <param name="standard210a/accel_low_filter" value="$(arg standard210a/accel_low_filter)" />
-      <param name="standard210a/gyro_low_filter" value="$(arg standard210a/gyro_low_filter)" />
-
     </node>
 
     <!-- disable compressed depth plugin for image topics -->
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/slam/vins_fusion.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/slam/vins_fusion.launch
new file mode 100644
index 0000000..6a8d81f
--- /dev/null
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/slam/vins_fusion.launch
@@ -0,0 +1,129 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="mynteye" default="mynteye" />
+
+  <!-- node params -->
+
+  <arg name="left_topic" default="left/image_raw" />
+  <arg name="right_topic" default="right/image_raw" />
+  <arg name="left_rect_topic" default="left_rect/image_rect" />
+  <arg name="right_rect_topic" default="right_rect/image_rect" />
+  <arg name="disparity_topic" default="disparity/image_raw" />
+  <arg name="disparity_norm_topic" default="disparity/image_norm" />
+  <arg name="depth_topic" default="depth/image_raw" />
+  <arg name="points_topic" default="points/data_raw" />
+
+  <arg name="left_mono_topic" default="left/image_mono" />
+  <arg name="right_mono_topic" default="right/image_mono" />
+
+  <arg name="imu_topic" default="imu/data_raw" />
+  <arg name="temperature_topic" default="temperature/data_raw" />
+
+  <arg name="base_frame_id" default="$(arg mynteye)_link" />
+  <arg name="left_frame_id" default="$(arg mynteye)_left_frame" />
+  <arg name="right_frame_id" default="$(arg mynteye)_right_frame" />
+  <arg name="left_rect_frame_id" default="$(arg mynteye)_left_rect_frame" />
+  <arg name="right_rect_frame_id" default="$(arg mynteye)_right_rect_frame" />
+  <arg name="disparity_frame_id" default="$(arg mynteye)_disparity_frame" />
+  <arg name="disparity_norm_frame_id" default="$(arg mynteye)_disparity_norm_frame" />
+  <arg name="points_frame_id" default="$(arg mynteye)_points_frame" />
+  <arg name="depth_frame_id" default="$(arg mynteye)_depth_frame" />
+
+  <arg name="temperature_frame_id" default="$(arg mynteye)_temperature_frame" />
+
+  <arg name="gravity" default="9.8" />
+
+  <arg name="mesh_file" default="S1030-0315.obj" />
+
+  <!-- Push down all topics/nodelets into "mynteye" namespace -->
+  <group ns="$(arg mynteye)">
+
+    <!-- mynteye_wrapper_node -->
+    <node name="mynteye_wrapper_node" pkg="mynt_eye_ros_wrapper" type="mynteye_wrapper_node" output="screen" respawn="true" respawn_delay="5">
+
+      <!-- node params -->
+
+      <param name="left_topic" value="$(arg left_topic)" />
+      <param name="right_topic" value="$(arg right_topic)" />
+      <param name="left_rect_topic" value="$(arg left_rect_topic)" />
+      <param name="right_rect_topic" value="$(arg right_rect_topic)" />
+      <param name="disparity_topic" value="$(arg disparity_topic)" />
+      <param name="disparity_norm_topic" value="$(arg disparity_norm_topic)" />
+      <param name="points_topic" value="$(arg points_topic)" />
+      <param name="depth_topic" value="$(arg depth_topic)" />
+      <param name="mesh_file" value="$(arg mesh_file)" />
+
+      <param name="left_mono_topic" value="$(arg left_mono_topic)" />
+      <param name="right_mono_topic" value="$(arg right_mono_topic)" />
+
+      <param name="imu_topic" value="$(arg imu_topic)" />
+      <param name="temperature_topic" value="$(arg temperature_topic)" />
+
+      <param name="base_frame_id" value="$(arg base_frame_id)" />
+      <param name="left_frame_id" value="$(arg left_frame_id)" />
+      <param name="right_frame_id" value="$(arg right_frame_id)" />
+      <param name="left_rect_frame_id" value="$(arg left_rect_frame_id)" />
+      <param name="right_rect_frame_id" value="$(arg right_rect_frame_id)" />
+      <param name="disparity_frame_id" value="$(arg disparity_frame_id)" />
+      <param name="disparity_norm_frame_id" value="$(arg disparity_norm_frame_id)" />
+      <param name="points_frame_id" value="$(arg points_frame_id)" />
+      <param name="depth_frame_id" value="$(arg depth_frame_id)" />
+
+      <param name="temperature_frame_id" value="$(arg temperature_frame_id)" />
+
+      <rosparam file="$(find mynt_eye_ros_wrapper)/config/device/standard.yaml" />
+      <rosparam file="$(find mynt_eye_ros_wrapper)/config/device/standard2.yaml" />
+      <rosparam file="$(find mynt_eye_ros_wrapper)/config/process/process_config.yaml" />
+      <rosparam file="$(find mynt_eye_ros_wrapper)/config/slam/vins_fusion.yaml" />
+
+      <param name="gravity" value="$(arg gravity)" />
+    </node>
+
+    <!-- disable compressed depth plugin for image topics -->
+    <group ns="$(arg left_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg left_rect_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg left_mono_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg right_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg right_mono_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg right_rect_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg disparity_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg disparity_norm_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg depth_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+  </group> <!-- mynteye -->
+</launch>
\ No newline at end of file
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 0ff0e2d..94ab231 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
@@ -1076,6 +1076,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     model_ = api_->GetModel();
     if (model_ == Model::STANDARD2 || model_ == Model::STANDARD210A) {
       private_nh_.getParam("standard2/request_index", request_index);
+      std::cout << request_index << std::endl;
       switch (request_index) {
         case 0:
         case 4:

From 06ff521d43b44ecda27d663c384563a503c0c71c Mon Sep 17 00:00:00 2001
From: kalman <kalmanxiong@slightech.com>
Date: Fri, 22 Mar 2019 19:13:12 +0800
Subject: [PATCH 3/4] fix(ros): change default range of accel

---
 .../ros/src/mynt_eye_ros_wrapper/config/device/standard2.yaml   | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/config/device/standard2.yaml b/wrappers/ros/src/mynt_eye_ros_wrapper/config/device/standard2.yaml
index d4eaafb..c633900 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/config/device/standard2.yaml
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/config/device/standard2.yaml
@@ -50,7 +50,7 @@ standard2/ir_control: 80
 # standard2/ir_control: 0
 
 # standard2/accel_range range: [6,48]
-standard2/accel_range: 24
+standard2/accel_range: -1
 # standard2/accel_range: 6
 
 # standard2/gyro_range range: [250,4000]

From 239c855c187b6376d44f2f5074c26c8cc4b71dc9 Mon Sep 17 00:00:00 2001
From: kalman <kalmanxiong@slightech.com>
Date: Fri, 22 Mar 2019 19:15:09 +0800
Subject: [PATCH 4/4] feat(imu): add is_ets flag for imu

---
 src/mynteye/device/channel/def.h                    | 2 ++
 src/mynteye/device/standard2/channels_adapter_s2.cc | 3 ++-
 2 files changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/mynteye/device/channel/def.h b/src/mynteye/device/channel/def.h
index a903854..ec8b2b9 100644
--- a/src/mynteye/device/channel/def.h
+++ b/src/mynteye/device/channel/def.h
@@ -72,6 +72,8 @@ struct ImuSegment {
   std::uint32_t frame_id;
   std::uint64_t timestamp;
   std::uint8_t flag;
+  // Is external time source
+  bool is_ets;
   std::int16_t temperature;
   std::int16_t accel[3];
   std::int16_t gyro[3];
diff --git a/src/mynteye/device/standard2/channels_adapter_s2.cc b/src/mynteye/device/standard2/channels_adapter_s2.cc
index 13efb89..0c0157b 100644
--- a/src/mynteye/device/standard2/channels_adapter_s2.cc
+++ b/src/mynteye/device/standard2/channels_adapter_s2.cc
@@ -55,7 +55,8 @@ struct ImuData {
 void unpack_imu_segment(const ImuData &imu, ImuSegment *seg) {
   seg->frame_id = imu.frame_id;
   seg->timestamp = imu.timestamp;
-  seg->flag = imu.flag;
+  seg->flag = imu.flag & 0b0011;
+  seg->is_ets = (imu.flag & 0b0100 == 0b0100);
   seg->temperature = imu.temperature;
   seg->accel[0] = (seg->flag == 1) ? imu.accel_or_gyro[0] : 0;
   seg->accel[1] = (seg->flag == 1) ? imu.accel_or_gyro[1] : 0;