From d33ae90bf328b15674aa8dbb1f763cfcdae5e809 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Mon, 14 Dec 2020 16:39:48 +0100
Subject: [PATCH] added optional parameter to specify input in deg/s

---
 include/subscriber_imu.h |  2 +-
 src/subscriber_imu.cpp   | 14 ++++++++++++++
 2 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/include/subscriber_imu.h b/include/subscriber_imu.h
index 57435ce..e60f4ca 100644
--- a/include/subscriber_imu.h
+++ b/include/subscriber_imu.h
@@ -35,7 +35,7 @@ namespace wolf
 class SubscriberImu : public Subscriber
 {
     protected:
-        bool flip_x_axis_;
+        bool flip_x_axis_, in_degrees_;
         std::string cov_source_;
         bool publish_bias_;
         ros::Publisher pub_bias_accel_, pub_bias_gyro_, pub_imu_corrected_;
diff --git a/src/subscriber_imu.cpp b/src/subscriber_imu.cpp
index a563af9..1cc22de 100644
--- a/src/subscriber_imu.cpp
+++ b/src/subscriber_imu.cpp
@@ -14,6 +14,15 @@ SubscriberImu::SubscriberImu(const std::string& _unique_name,
     flip_x_axis_ = _server.getParam<bool>(prefix_ + "/flip_x_axis");
     cov_source_  = _server.getParam<std::string>(prefix_ + "/cov_source");
     assert(cov_source_ == "msg" or cov_source_=="sensor");
+
+    try
+    {
+        in_degrees_  = _server.getParam<bool>(prefix_ + "/in_degrees");
+    }
+    catch(...)
+    {
+        in_degrees_ = false;
+    }
 }
 
 void SubscriberImu::initialize(ros::NodeHandle& nh, const std::string& topic)
@@ -48,6 +57,11 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
                 msg->angular_velocity.z;
     }
 
+    if (in_degrees_)
+    {
+        data.tail<3>() *= M_PI/180.0;
+    }
+
     Eigen::Matrix6d cov(Eigen::Matrix6d::Zero());
     cov.topLeftCorner<3,3>()     = Eigen::Map<const Eigen::Matrix3d>(msg->linear_acceleration_covariance.data());
     cov.bottomRightCorner<3,3>() = Eigen::Map<const Eigen::Matrix3d>(msg->angular_velocity_covariance.data());
-- 
GitLab