From b1f89ffbc25e9b7c3d0df7f298ebdfaf48a0ff5e Mon Sep 17 00:00:00 2001
From: John Zhao <johnzhao@slightech.com>
Date: Sat, 14 Apr 2018 16:47:05 +0800
Subject: [PATCH] Add gravity node param

---
 .../launch/mynteye.launch                     |  4 ++++
 .../src/wrapper_nodelet.cc                    | 19 +++++++++++++------
 2 files changed, 17 insertions(+), 6 deletions(-)

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 6eeef94..0250024 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
@@ -13,6 +13,8 @@
   <arg name="right_frame_id" default="$(arg mynteye)_right_frame" />
   <arg name="imu_frame_id" default="$(arg mynteye)_imu_frame" />
 
+  <arg name="gravity" default="9.8" />
+
   <!-- device options, -1 will not set the value -->
 
   <!-- gain range: [0,48]-->
@@ -76,6 +78,8 @@
       <param name="right_frame_id" value="$(arg right_frame_id)" />
       <param name="imu_frame_id" value="$(arg imu_frame_id)" />
 
+      <param name="gravity" value="$(arg gravity)" />
+
       <!-- device options -->
 
       <param name="gain" value="$(arg 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 2076bee..bd7df79 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
@@ -9,6 +9,8 @@
 
 #include <glog/logging.h>
 
+#define _USE_MATH_DEFINES
+#include <cmath>
 #include <map>
 #include <string>
 
@@ -71,6 +73,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     private_nh_.getParam("right_frame_id", right_frame_id_);
     private_nh_.getParam("imu_frame_id", imu_frame_id_);
 
+    gravity_ = 9.8;
+    private_nh_.getParam("gravity", gravity_);
+
     // device options
 
     std::map<std::string, Option> option_map = {
@@ -246,9 +251,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     msg.header.frame_id = imu_frame_id_;
 
     // acceleration should be in m/s^2 (not in g's)
-    msg.linear_acceleration.x = data.imu->accel[0] * 9.8;
-    msg.linear_acceleration.y = data.imu->accel[1] * 9.8;
-    msg.linear_acceleration.z = data.imu->accel[2] * 9.8;
+    msg.linear_acceleration.x = data.imu->accel[0] * gravity_;
+    msg.linear_acceleration.y = data.imu->accel[1] * gravity_;
+    msg.linear_acceleration.z = data.imu->accel[2] * gravity_;
 
     msg.linear_acceleration_covariance[0] = 0;
     msg.linear_acceleration_covariance[1] = 0;
@@ -263,9 +268,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     msg.linear_acceleration_covariance[8] = 0;
 
     // velocity should be in rad/sec
-    msg.angular_velocity.x = data.imu->gyro[0] / 57.2956;
-    msg.angular_velocity.y = data.imu->gyro[1] / 57.2956;
-    msg.angular_velocity.z = data.imu->gyro[2] / 57.2956;
+    msg.angular_velocity.x = data.imu->gyro[0] * M_PI / 180;
+    msg.angular_velocity.y = data.imu->gyro[1] * M_PI / 180;
+    msg.angular_velocity.z = data.imu->gyro[2] * M_PI / 180;
 
     msg.angular_velocity_covariance[0] = 0;
     msg.angular_velocity_covariance[1] = 0;
@@ -338,6 +343,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
   std::string right_frame_id_;
   std::string imu_frame_id_;
 
+  double gravity_;
+
   // device
 
   std::shared_ptr<Device> device_;