diff --git a/include/subscriber_imu.h b/include/subscriber_imu.h index c6205cda79e9dad43a277cabe189c752a69ff45c..7168a12f8e7a20c09c65ac7699125b06e1f2d4d9 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 4d6e9f6e3c7ad0cb757151c1f27518f010830e53..e18a24f43fc00e1333a1f293402b7a88f7ed79dc 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());