From 50dc95aa71c33cc5e04aae1281bfd624b4e6e587 Mon Sep 17 00:00:00 2001
From: kalman <kalmanxiong@slightech.com>
Date: Fri, 28 Dec 2018 19:16:02 +0800
Subject: [PATCH] fix(ros): fix loop_rate bug

---
 .../src/wrapper_nodelet.cc                    | 26 +++++++++++++++++--
 1 file changed, 24 insertions(+), 2 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 e74f322..d3ec807 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
@@ -343,7 +343,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     NODELET_INFO_STREAM("Advertized service " << DEVICE_INFO_SERVICE);
 
     publishStaticTransforms();
-    ros::Rate loop_rate(60);
+    ros::Rate loop_rate(frame_rate_);
     while (private_nh_.ok()) {
       publishTopics();
       loop_rate.sleep();
@@ -867,8 +867,30 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     int request_index = 0;
 
     model_ = api_->GetModel();
-    if (model_ == Model::STANDARD2)
+    if (model_ == Model::STANDARD2) {
       private_nh_.getParam("request_index", request_index);
+      switch (request_index) {
+        case 0:
+        case 4:
+          frame_rate_ = 10;
+          break;
+        case 1:
+        case 5:
+          frame_rate_ = 20;
+          break;
+        case 2:
+        case 6:
+          frame_rate_ = 30;
+          break;
+        case 3:
+          frame_rate_ = 60;
+          break;
+      }
+    }
+    if (model_ == Model::STANDARD) {
+      request_index = 0;
+      frame_rate_ = api_->GetOptionValue(Option::FRAME_RATE);
+    }
 
     NODELET_FATAL_COND(m <= 0, "No MYNT EYE devices :(");
     if (m <= 1) {