From bed7e888d0641633b3b583842da7dd1b4e3ffd89 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Fri, 12 Jun 2020 14:38:56 +0200
Subject: [PATCH] flip x axis

---
 include/subscriber_imu.h |  3 +++
 src/subscriber_imu.cpp   | 27 +++++++++++++++++++++------
 2 files changed, 24 insertions(+), 6 deletions(-)

diff --git a/include/subscriber_imu.h b/include/subscriber_imu.h
index c6205cd..7168a12 100644
--- a/include/subscriber_imu.h
+++ b/include/subscriber_imu.h
@@ -34,6 +34,9 @@ namespace wolf
 
 class SubscriberImu : public Subscriber
 {
+    protected:
+        bool flip_x_axis_;
+
     public:
         // Constructor
         SubscriberImu(const std::string& _unique_name,
diff --git a/src/subscriber_imu.cpp b/src/subscriber_imu.cpp
index 4d6e9f6..e18a24f 100644
--- a/src/subscriber_imu.cpp
+++ b/src/subscriber_imu.cpp
@@ -9,6 +9,8 @@ SubscriberImu::SubscriberImu(const std::string& _unique_name,
         Subscriber(_unique_name, _server, _sensor_ptr)
 {
     assert(std::dynamic_pointer_cast<SensorImu>(_sensor_ptr) != nullptr);
+
+    flip_x_axis_ = _server.getParam<bool>(prefix_ + "/flip_x_axis");
 }
 
 void SubscriberImu::initSubscriber(ros::NodeHandle& nh, const std::string& topic)
@@ -20,12 +22,25 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
 {
     ROS_DEBUG("callback Imu!");
     Eigen::Vector6d data;
-    data << msg->linear_acceleration.x,
-            msg->linear_acceleration.y,
-            msg->linear_acceleration.z,
-            msg->angular_velocity.x,
-            msg->angular_velocity.y,
-            msg->angular_velocity.z;
+    if (flip_x_axis_)
+    {
+        data << msg->linear_acceleration.x,
+               -msg->linear_acceleration.y,
+               -msg->linear_acceleration.z,
+                msg->angular_velocity.x,
+               -msg->angular_velocity.y,
+               -msg->angular_velocity.z;
+    }
+    else
+    {
+        data << msg->linear_acceleration.x,
+                msg->linear_acceleration.y,
+                msg->linear_acceleration.z,
+                msg->angular_velocity.x,
+                msg->angular_velocity.y,
+                msg->angular_velocity.z;
+    }
+
     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