added optional parameter to specify input in deg/s

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

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());