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