From ecdc817a114c7fe44334975395f056d327b3d39d Mon Sep 17 00:00:00 2001
From: TinyO <tinysilk@hotmail.com>
Date: Wed, 11 Sep 2019 12:42:27 +0800
Subject: [PATCH] fix(*): S200B support.

---
 .../src/wrapper_nodelet.cc                    | 28 +++++++++++++++----
 1 file changed, 23 insertions(+), 5 deletions(-)

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 29e4283..1650a3b 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
@@ -307,6 +307,21 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
           {Option::GYROSCOPE_RANGE, "standard/gyro_range"}};
     }
 
+    // device options of standard200b
+    if (model_ == Model::STANDARD200B) {
+    option_names_ = {
+        {Option::BRIGHTNESS, "standard200b/brightness"},
+        {Option::EXPOSURE_MODE, "standard200b/exposure_mode"},
+        {Option::MAX_GAIN, "standard200b/max_gain"},
+        {Option::MAX_EXPOSURE_TIME, "standard200b/max_exposure_time"},
+        {Option::DESIRED_BRIGHTNESS, "standard200b/desired_brightness"},
+        {Option::MIN_EXPOSURE_TIME, "standard200b/min_exposure_time"},
+        {Option::ACCELEROMETER_RANGE, "standard200b/accel_range"},
+        {Option::GYROSCOPE_RANGE, "standard200b/gyro_range"},
+        {Option::ACCELEROMETER_LOW_PASS_FILTER, "standard200b/accel_low_filter"},
+        {Option::GYROSCOPE_LOW_PASS_FILTER, "standard200b/gyro_low_filter"}};
+    }
+
     for (auto &&it = option_names_.begin(); it != option_names_.end(); ++it) {
       if (!api_->Supports(it->first))
         continue;
@@ -333,8 +348,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
       NODELET_INFO_STREAM("Advertized on topic " << topic);
     }
 
-    // Only STANDARD2/STANDARD210A need publish mono_topics
-    if (model_ == Model::STANDARD2 || model_ == Model::STANDARD210A) {
+    // Only STANDARD2/STANDARD210A/STANDARD200B need publish mono_topics
+    if (model_ == Model::STANDARD2 ||
+        model_ == Model::STANDARD210A || model_ == Model::STANDARD200B) {
       for (auto &&it = mono_topics.begin(); it != mono_topics.end(); ++it) {
         auto &&topic = mono_topics[it->first];
         if (it->first == Stream::LEFT ||
@@ -349,7 +365,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
 
     int depth_type = 0;
     private_nh_.getParamCached("depth_type", depth_type);
-    if (model_ == Model::STANDARD2 || model_ == Model::STANDARD210A) {
+    if (model_ == Model::STANDARD2 ||
+        model_ == Model::STANDARD210A || model_ == Model::STANDARD200B) {
       camera_encodings_ = {{Stream::LEFT, enc::BGR8},
                           {Stream::RIGHT, enc::BGR8},
                           {Stream::LEFT_RECTIFIED, enc::BGR8},
@@ -1100,7 +1117,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     int request_index = 0;
 
     model_ = api_->GetModel();
-    if (model_ == Model::STANDARD2 || model_ == Model::STANDARD210A) {
+    if (model_ == Model::STANDARD2 ||
+        model_ == Model::STANDARD210A || model_ == Model::STANDARD200B) {
       private_nh_.getParamCached("standard2/request_index", request_index);
       switch (request_index) {
         case 0:
@@ -1127,7 +1145,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
 
     std::int32_t process_mode = 0;
     if (model_ == Model::STANDARD2 ||
-        model_ == Model::STANDARD210A) {
+        model_ == Model::STANDARD210A || model_ == Model::STANDARD200B) {
       private_nh_.getParamCached("standard2/imu_process_mode", process_mode);
       api_->EnableProcessMode(process_mode);
     }