diff --git a/include/subscriber_imu.h b/include/subscriber_imu.h index 88c893292b05b8724c126cf765ac936ea23ffd94..4d8409910eb315856fd7422f495ebbdd281b4365 100644 --- a/include/subscriber_imu.h +++ b/include/subscriber_imu.h @@ -36,7 +36,9 @@ namespace wolf class SubscriberImu : public Subscriber { protected: - bool flip_x_axis_, in_degrees_; + bool in_degrees_; + int x_axis_, y_axis_, z_axis_; + int x_axis_sgn_, y_axis_sgn_, z_axis_sgn_; int dim_; std::string cov_source_; bool publish_bias_; diff --git a/src/subscriber_imu.cpp b/src/subscriber_imu.cpp index e72d6775f66b641e1c2d5d06e93efc9ed9cb9a8b..2126350bd46932af13500dc2c848257be5c519fc 100644 --- a/src/subscriber_imu.cpp +++ b/src/subscriber_imu.cpp @@ -19,7 +19,6 @@ SubscriberImu::SubscriberImu(const std::string& _unique_name, } else WOLF_ERROR("the problem's dimmension should be 2 or 3"); - 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"); @@ -31,6 +30,84 @@ SubscriberImu::SubscriberImu(const std::string& _unique_name, { in_degrees_ = false; } + + int imu_x_axis, imu_y_axis, imu_z_axis; + try + { + imu_x_axis = _server.getParam<int>(prefix_ + "/imu_x_axis"); + imu_y_axis = _server.getParam<int>(prefix_ + "/imu_y_axis"); + imu_z_axis = _server.getParam<int>(prefix_ + "/imu_z_axis"); + } + catch(...) + { + imu_x_axis = 1; + imu_y_axis = 2; + imu_z_axis = 3; + } + //TODO: Assert that we have right-handed axes + + if(abs(imu_x_axis) == 1) + { + x_axis_ = 1; + x_axis_sgn_ = (imu_x_axis > 0 ? 1:-1); + if(abs(imu_y_axis) == 2){ + assert(abs(imu_z_axis) == 3); + y_axis_ = 2; + y_axis_sgn_ = (imu_y_axis > 0 ? 1:-1); + z_axis_ = 3; + z_axis_sgn_ = (imu_z_axis > 0 ? 1:-1); + } + else{ + assert(abs(imu_y_axis) == 3); + assert(abs(imu_z_axis) == 2); + z_axis_ = 2; + z_axis_sgn_ = (imu_y_axis > 0 ? 1:-1); + y_axis_ = 3; + y_axis_sgn_ = (imu_z_axis > 0 ? 1:-1); + } + } + else if(abs(imu_x_axis) == 2) + { + y_axis_ = 1; + y_axis_sgn_ = (imu_x_axis > 0 ? 1:-1); + if(abs(imu_y_axis) == 1){ + assert(abs(imu_z_axis) == 3); + x_axis_ = 2; + x_axis_sgn_ = (imu_y_axis > 0 ? 1:-1); + z_axis_ = 3; + z_axis_sgn_ = (imu_z_axis > 0 ? 1:-1); + } + else{ + assert(abs(imu_y_axis) == 3); + assert(abs(imu_z_axis) == 1); + z_axis_ = 2; + z_axis_sgn_ = (imu_y_axis > 0 ? 1:-1); + x_axis_ = 3; + x_axis_sgn_ = (imu_z_axis > 0 ? 1:-1); + } + } + else + { + assert(abs(imu_x_axis) == 3); + z_axis_ = 1; + z_axis_sgn_ = (imu_x_axis > 0 ? 1:-1); + if(abs(imu_y_axis) == 1){ + assert(abs(imu_z_axis) == 2); + x_axis_ = 2; + x_axis_sgn_ = (imu_y_axis > 0 ? 1:-1); + y_axis_ = 3; + y_axis_sgn_ = (imu_z_axis > 0 ? 1:-1); + } + else{ + assert(abs(imu_y_axis) == 2); + assert(abs(imu_z_axis) == 1); + y_axis_ = 2; + y_axis_sgn_ = (imu_y_axis > 0 ? 1:-1); + x_axis_ = 3; + x_axis_sgn_ = (imu_z_axis > 0 ? 1:-1); + } + } + } void SubscriberImu::initialize(ros::NodeHandle& nh, const std::string& topic) @@ -47,29 +124,47 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg) ROS_DEBUG("callback Imu!"); //Fill data Eigen::Vector6d data; - 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::Vector6d data_orig; + data_orig << 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; + //} + data(0) = data_orig(x_axis_-1)*x_axis_sgn_; + data(3) = data_orig(x_axis_+2)*x_axis_sgn_; + data(1) = data_orig(y_axis_-1)*y_axis_sgn_; + data(4) = data_orig(y_axis_+2)*y_axis_sgn_; + data(2) = data_orig(z_axis_-1)*z_axis_sgn_; + data(5) = data_orig(z_axis_+2)*z_axis_sgn_; - if (in_degrees_) - { - data.tail<3>() *= M_PI/180.0; - } + WOLF_INFO("x_axis_ = ", x_axis_, " y_axis_ = ", y_axis_, " z_axis_ = ", z_axis_); + WOLF_INFO("data_orig = ", data_orig.transpose()); + WOLF_INFO("data = ", data.transpose()); + + + if (in_degrees_) + { + data.tail<3>() *= M_PI/180.0; + } //Fill cov Eigen::Matrix6d cov(Eigen::Matrix6d::Zero());