Merge branch 'develop' of http://gitlab.mynt.com/mynteye/mynt-eye-s-sdk into develop

* 'develop' of http://gitlab.mynt.com/mynteye/mynt-eye-s-sdk:
  feat(ros): added multiple device launch for ros
  feat(imu): add is_ets flag for imu
  fix(ros): change default range of accel
  feat(ros): add slam launch
  feat(ros): added imu process mode in mynteye.launch
  fix(ros): modified sstandard --> standard
  chore(rviz): use color points cloud
  feat: add mesh to rviz.

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];

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;

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

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: -1
+# 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

wrappers/ros/src/mynt_eye_ros_wrapper/config/process/process_config.yaml

@@ -0,0 +1,15 @@
+# disparity computing method type
+# sgbm: 0
+# bm: 1
+disparity_computing_method: 1
+
+# standard2/imu_process_mode range: [0,3]
+# 0 - none process, 1 - temperature process, 2 - scale and assembly process, 3 - both
+imu_process_mode: 0
+
+enable_left_rect: false
+enable_right_rect: false
+enable_disparity: false
+enable_disparity_norm: false
+enable_points: false
+enable_depth: false

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

wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch

@@ -2,6 +2,13 @@
 <launch>
   <arg name="mynteye" default="mynteye" />
 
+  <!-- mutiple device -->
+  <arg name="is_multiple" default="false" />
+
+  <!-- serial_number -->
+  <!-- if is_mutiple is true, must set serial_number -->
+  <arg name="serial_number" default="" />
+
   <!-- node params -->
 
   <arg name="left_topic" default="left/image_raw" />
@@ -33,192 +40,7 @@
 
   <arg name="gravity" default="9.8" />
 
-  <arg name="mesh_file" default="S1030-0315.STL" />
+  <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)">
 
@@ -227,6 +49,9 @@
 
       <!-- node params -->
 
+      <param name="is_multiple" value="$(arg is_multiple)" />
+      <param name="serial_number" value="$(arg serial_number)" />
+
       <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)" />
@@ -255,66 +80,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 -->

wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye_multiple.launch

@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+<launch>
+      <include file="$(find mynt_eye_ros_wrapper)/launch/sub/mynteye_1.launch" />
+      <include file="$(find mynt_eye_ros_wrapper)/launch/sub/mynteye_2.launch" />
+      <!-- <include file="$(find mynt_eye_ros_wrapper)/launch/sub/mynteye_3.launch" />
+      <include file="$(find mynt_eye_ros_wrapper)/launch/sub/mynteye_4.launch" /> -->
+</launch>

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>

wrappers/ros/src/mynt_eye_ros_wrapper/launch/sub/mynteye_1.launch

@@ -0,0 +1,377 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="mynteye" default="mynteye_1" />
+
+  <!-- mutiple device -->
+  <arg name="is_multiple" default="true" />
+
+  <!-- serial_number -->
+  <!-- if is_mutiple is true, must set serial_number -->
+  <arg name="serial_number" default="07C42B1B0009071F" />
+
+  <!-- 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" />
+
+  <!-- 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)">
+
+    <!-- mynteye_wrapper_node -->
+    <node name="mynteye_wrapper_node_1" pkg="mynt_eye_ros_wrapper" type="mynteye_wrapper_node" output="screen" respawn="true" respawn_delay="5">
+
+      <!-- node params -->
+
+      <param name="is_multiple" value="$(arg is_multiple)" />
+      <param name="serial_number" value="$(arg serial_number)" />
+
+      <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)" />
+
+      <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="standard/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 -->
+    <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>

wrappers/ros/src/mynt_eye_ros_wrapper/launch/sub/mynteye_2.launch

@@ -0,0 +1,377 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="mynteye" default="mynteye_2" />
+
+  <!-- mutiple device -->
+  <arg name="is_multiple" default="true" />
+
+  <!-- serial_number -->
+  <!-- if is_mutiple is true, must set serial_number -->
+  <arg name="serial_number" default="07C4061B0009071F" />
+
+  <!-- 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" />
+
+  <!-- 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)">
+
+    <!-- mynteye_wrapper_node -->
+    <node name="mynteye_wrapper_node_2" pkg="mynt_eye_ros_wrapper" type="mynteye_wrapper_node" output="screen" respawn="true" respawn_delay="5">
+
+      <!-- node params -->
+
+      <param name="is_multiple" value="$(arg is_multiple)" />
+      <param name="serial_number" value="$(arg serial_number)" />
+
+      <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)" />
+
+      <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="standard/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 -->
+    <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>

wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz

@@ -9,6 +9,8 @@ Panels:
         - /Images1
         - /Images1/Rectified1
         - /Disparity1
+        - /Points1
+        - /Marker1
       Splitter Ratio: 0.5
     Tree Height: 824
   - Class: rviz/Selection
@@ -29,7 +31,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: Depth
+    SyncSource: Points
 Visualization Manager:
   Class: ""
   Displays:
@@ -66,7 +68,7 @@ Visualization Manager:
           Unreliable: false
           Value: true
         - Class: rviz/Image
-          Enabled: true
+          Enabled: false
           Image Topic: /mynteye/right/image_raw
           Max Value: 1
           Median window: 5
@@ -76,7 +78,7 @@ Visualization Manager:
           Queue Size: 1
           Transport Hint: raw
           Unreliable: false
-          Value: true
+          Value: false
         - Class: rviz/Group
           Displays:
             - Class: rviz/Image
@@ -133,10 +135,10 @@ Visualization Manager:
           Transport Hint: raw
           Unreliable: false
           Value: true
-      Enabled: false
+      Enabled: true
       Name: Disparity
     - Class: rviz/Image
-      Enabled: true
+      Enabled: false
       Image Topic: /mynteye/depth/image_raw
       Max Value: 1
       Median window: 5
@@ -146,18 +148,18 @@ Visualization Manager:
       Queue Size: 1
       Transport Hint: raw
       Unreliable: false
-      Value: true
+      Value: false
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 10
+        Max Value: 6.22900009
-        Min Value: -10
+        Min Value: 0
         Value: true
       Axis: Z
       Channel Name: intensity
       Class: rviz/PointCloud2
       Color: 255; 255; 255
-      Color Transformer: RGB8
+      Color Transformer: AxisColor
       Decay Time: 0
       Enabled: true
       Invert Rainbow: false
@@ -186,6 +188,14 @@ Visualization Manager:
       Topic: /mynteye/imu/data_raw
       Unreliable: false
       Value: false
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /mynteye/camera_mesh
+      Name: Marker
+      Namespaces:
+        "": true
+      Queue Size: 100
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -226,10 +236,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: 0.785398006
+      Pitch: -1.46479619
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.785398006
+      Yaw: 1.54539752
     Saved: ~
 Window Geometry:
   Depth:
@@ -247,7 +257,7 @@ Window Geometry:
     collapsed: false
   LeftRect:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000026500000378fc020000000ffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000378000000d600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000008004c00650066007403000001bd000000860000024e00000190fb0000000a00520069006700680074030000038d000001dc0000025000000190fb00000010004c006500660074005200650063007402000001bd000002170000024e00000190fb00000012005200690067006800740052006500630074020000040c000002170000024f00000190fb0000000a0044006500700074006803000002160000024c0000024f00000190fb0000001200440069007300700061007200690074007902000001bd000002170000024e0000018ffb0000001a004400690073007000610072006900740079004e006f0072006d020000040c000002170000025100000190000000010000010f00000378fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000378000000ac00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003bf0000037800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 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
   Right:
     collapsed: false
   RightRect:

wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc

@@ -348,15 +348,15 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
                         sensor_msgs::Temperature>(temperature_topic, 100);
     NODELET_INFO_STREAM("Advertized on topic " << temperature_topic);
 
-    // pub_mesh_ = nh_.advertise<visualization_msgs::Marker>("camera_mesh", 0 );
+    pub_mesh_ = nh_.advertise<visualization_msgs::Marker>("camera_mesh", 0 );
-    // // where to get the mesh from
+    // where to get the mesh from
-    // std::string mesh_file;
+    std::string mesh_file;
-    // if (private_nh_.getParam("mesh_file", mesh_file)) {
+    if (private_nh_.getParam("mesh_file", mesh_file)) {
-    //   mesh_msg_.mesh_resource = "package://mynt_eye_ros_wrapper/mesh/"+mesh_file;
+      mesh_msg_.mesh_resource = "package://mynt_eye_ros_wrapper/mesh/"+mesh_file;
-    // } else {
+    } else {
-    //   LOG(INFO) << "no mesh found for visualisation, set ros param mesh_file, if desired";
+      LOG(INFO) << "no mesh found for visualisation, set ros param mesh_file, if desired";
-    //   mesh_msg_.mesh_resource = "";
+      mesh_msg_.mesh_resource = "";
-    // }
+    }
 
     // stream toggles
 
@@ -623,8 +623,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
   }
 
   void publishTopics() {
-    // publishMesh();
+    publishMesh();
-
     if ((camera_publishers_[Stream::LEFT].getNumSubscribers() > 0 ||
         mono_publishers_[Stream::LEFT].getNumSubscribers() > 0) &&
         !is_published_[Stream::LEFT]) {
@@ -1035,39 +1034,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
 
  private:
   void initDevice() {
-    NODELET_INFO_STREAM("Detecting MYNT EYE devices");
-
-    Context context;
-    auto &&devices = context.devices();
-
-    size_t n = devices.size();
-    NODELET_FATAL_COND(n <= 0, "No MYNT EYE devices :(");
-
-    NODELET_INFO_STREAM("MYNT EYE devices:");
-    for (size_t i = 0; i < n; i++) {
-      auto &&device = devices[i];
-      auto &&name = device->GetInfo(Info::DEVICE_NAME);
-      NODELET_INFO_STREAM("  index: " << i << ", name: " << name);
-    }
-
     std::shared_ptr<Device> device = nullptr;
-    if (n <= 1) {
+
-      device = devices[0];
+    device = selectDevice();
-      NODELET_INFO_STREAM("Only one MYNT EYE device, select index: 0");
-    } else {
-      while (true) {
-        size_t i;
-        NODELET_INFO_STREAM(
-            "There are " << n << " MYNT EYE devices, select index: ");
-        std::cin >> i;
-        if (i >= n) {
-          NODELET_WARN_STREAM("Index out of range :(");
-          continue;
-        }
-        device = devices[i];
-        break;
-      }
-    }
 
     api_ = API::Create(device);
     auto &&requests = device->GetStreamRequests();
@@ -1100,6 +1069,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");
@@ -1116,6 +1092,82 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     computeRectTransforms();
   }
 
+  std::shared_ptr<Device> selectDevice() {
+    NODELET_INFO_STREAM("Detecting MYNT EYE devices");
+
+    Context context;
+    auto &&devices = context.devices();
+
+    bool is_multiple = false;
+    private_nh_.getParam("is_multiple", is_multiple);
+    if (is_multiple) {
+      std::string sn;
+      private_nh_.getParam("serial_number", sn);
+      NODELET_FATAL_COND(sn.empty(), "Must set serial_number "
+          "in mynteye_1.launch and mynteye_2.launch.");
+
+      size_t n = devices.size();
+      NODELET_FATAL_COND(n <= 0, "No MYNT EYE devices :(");
+
+      NODELET_INFO_STREAM("MYNT EYE devices:");
+      for (size_t i = 0; i < n; i++) {
+        auto &&device = devices[i];
+        auto &&name = device->GetInfo(Info::DEVICE_NAME);
+        auto &&serial_number = device->GetInfo(Info::SERIAL_NUMBER);
+        NODELET_INFO_STREAM("  index: " << i << ", name: " <<
+            name << ", serial number: " << serial_number);
+      }
+      for (size_t i = 0; i < n; i++) {
+        auto &&device = devices[i];
+        auto &&name = device->GetInfo(Info::DEVICE_NAME);
+        auto &&serial_number = device->GetInfo(Info::SERIAL_NUMBER);
+        NODELET_INFO_STREAM("  index: " << i << ", name: " <<
+            name << ", serial number: " << serial_number);
+        if (sn == serial_number)
+          return device;
+#if 0
+        if (i == (n - 1)) {
+          /*
+          NODELET_FATAL_COND(i == (n - 1), "No corresponding device was found,"
+              " check the serial_number configuration. ");
+              */
+          NODELET_FATAL_COND(i == (n - 1), "No corresponding device was found,"
+              " check the serial_number configuration. ");
+          return nullptr;
+        }
+#endif
+      }
+    }
+    size_t n = devices.size();
+    NODELET_FATAL_COND(n <= 0, "No MYNT EYE devices :(");
+
+    NODELET_INFO_STREAM("MYNT EYE devices:");
+    for (size_t i = 0; i < n; i++) {
+      auto &&device = devices[i];
+      auto &&name = device->GetInfo(Info::DEVICE_NAME);
+      NODELET_INFO_STREAM("  index: " << i << ", name: " << name);
+    }
+
+    if (n <= 1) {
+      NODELET_INFO_STREAM("Only one MYNT EYE device, select index: 0");
+      return devices[0];
+    } else {
+      while (true) {
+        size_t i;
+        NODELET_INFO_STREAM(
+            "There are " << n << " MYNT EYE devices, select index: ");
+        std::cin >> i;
+        if (i >= n) {
+          NODELET_WARN_STREAM("Index out of range :(");
+          continue;
+        }
+        return devices[i];
+      }
+    }
+
+    return nullptr;
+  }
+
   std::shared_ptr<IntrinsicsBase> getDefaultIntrinsics() {
     auto res = std::make_shared<IntrinsicsPinhole>();
     res->width = 640;
@@ -1169,9 +1221,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     mesh_msg_.pose.orientation.w = 0;
 
     // fill position
-    mesh_msg_.pose.position.x = -0.25;
+    mesh_msg_.pose.position.x = 0;
-    mesh_msg_.pose.position.y = -0.05;
+    mesh_msg_.pose.position.y = 0;
-    mesh_msg_.pose.position.z = -0.1;
+    mesh_msg_.pose.position.z = 0;
 
     // scale -- needed
     mesh_msg_.scale.x = 0.003;