TestBotaDriverSignalQualityImu.cpp

Go to the documentation of this file.

#include <gtest/gtest.h>
#include <sensor_msgs/Imu.h>
#include <Eigen/Dense>
#include <ros/ros.h>

#define G_TO_METERS_PER_SECOND_SQUARED 9.80665
using namespace Eigen;
namespace bota_driver_testing
{
class BotaDriverSignalQualityImu : public ::testing::Test
{
protected:
  ros::Subscriber sub_;
  ros::NodeHandle nh_{ "~" };
  std::uint32_t msgCount_;
  std::uint32_t maxCount_;
  std::string topicName_;
  VectorXd linAccXSamples_;
  VectorXd linAccYSamples_;
  VectorXd linAccZSamples_;
  VectorXd angVelXSamples_;
  VectorXd angVelYSamples_;
  VectorXd angVelZSamples_;
  float testDuration_;

  BotaDriverSignalQualityImu() : msgCount_(0), maxCount_(1000)
  {
  }

  ~BotaDriverSignalQualityImu() override
  {
    // You can do clean-up work that doesn't throw exceptions here.
  }

  // If the constructor and destructor are not enough for setting up
  // and cleaning up each test, you can define the following methods:

  void SetUp() override
  {
    // Code here will be called immediately after the constructor (right
    // before each test).
  }

  void TearDown() override
  {
    // Code here will be called immediately after each test (right
    // before the destructor).
  }

public:
  void imuCallback(const sensor_msgs::Imu::ConstPtr& msg)
  {
    std::uint32_t lpos = msgCount_;
    msgCount_++;
    linAccXSamples_.conservativeResize(msgCount_);
    linAccYSamples_.conservativeResize(msgCount_);
    linAccZSamples_.conservativeResize(msgCount_);
    angVelXSamples_.conservativeResize(msgCount_);
    angVelYSamples_.conservativeResize(msgCount_);
    angVelZSamples_.conservativeResize(msgCount_);

    linAccXSamples_(lpos) = msg->linear_acceleration.x;
    linAccYSamples_(lpos) = msg->linear_acceleration.y;
    linAccZSamples_(lpos) = msg->linear_acceleration.z;
    angVelXSamples_(lpos) = msg->angular_velocity.x;
    angVelYSamples_(lpos) = msg->angular_velocity.y;
    angVelZSamples_(lpos) = msg->angular_velocity.z;
  }
};

TEST_F(BotaDriverSignalQualityImu, SignalQuality)
{
  SCOPED_TRACE("SignalQualityImu");
  ros::Time time_offset;
  ASSERT_EQ(nh_.getParam("topic_name", topicName_), true);
  sub_ = nh_.subscribe(topicName_, 1, &BotaDriverSignalQualityImu::imuCallback, (BotaDriverSignalQualityImu*)this,
                       ros::TransportHints().tcpNoDelay());
  ASSERT_EQ(nh_.getParam("test_duration", testDuration_), true);
  time_offset = ros::Time::now() + ros::Duration(testDuration_);
  while (ros::ok() && ros::Time::now() < time_offset && msgCount_ < maxCount_)
  {
    ros::spinOnce();
    if (HasFatalFailure())
    {
      FAIL() << "Fatal errors occurred.\n";
      return;
    }
  }

  EXPECT_GT(msgCount_, 0U);
  double linacc_xsamples_mean, linacc_ysamples_mean, linacc_zsamples_mean, angvel_xsamples_mean, angvel_ysamples_mean,
      angvel_zsamples_mean, linacc_mean, angvel_mean;
  linacc_xsamples_mean = linAccXSamples_.mean();
  linacc_ysamples_mean = linAccYSamples_.mean();
  linacc_zsamples_mean = linAccZSamples_.mean();
  // compute the mean of linear acceleration.
  linacc_mean = std::sqrt(std::pow(linacc_xsamples_mean, 2) + std::pow(linacc_ysamples_mean, 2) +
                          std::pow(linacc_zsamples_mean, 2));
  angvel_xsamples_mean = angVelXSamples_.mean();
  angvel_ysamples_mean = angVelYSamples_.mean();
  angvel_zsamples_mean = angVelZSamples_.mean();
  // compute the mean of the angular velocities.
  angvel_mean = std::sqrt(std::pow(angvel_xsamples_mean, 2) + std::pow(angvel_ysamples_mean, 2) +
                          std::pow(angvel_zsamples_mean, 2));
  linAccXSamples_ = linAccXSamples_.array() - linAccXSamples_.mean();
  double sigma_ax = std::sqrt(linAccXSamples_.squaredNorm() / msgCount_);
  linAccYSamples_ = linAccYSamples_.array() - linAccYSamples_.mean();
  double sigma_ay = std::sqrt(linAccYSamples_.squaredNorm() / msgCount_);
  linAccZSamples_ = linAccZSamples_.array() - linAccZSamples_.mean();
  double sigma_az = std::sqrt(linAccZSamples_.squaredNorm() / msgCount_);
  angVelXSamples_ = angVelXSamples_.array() - angVelXSamples_.mean();
  double sigma_vx = std::sqrt(angVelXSamples_.squaredNorm() / msgCount_);
  angVelYSamples_ = angVelYSamples_.array() - angVelYSamples_.mean();
  double sigma_vy = std::sqrt(angVelYSamples_.squaredNorm() / msgCount_);
  angVelZSamples_ = angVelZSamples_.array() - angVelZSamples_.mean();
  double sigma_vz = std::sqrt(angVelZSamples_.squaredNorm() / msgCount_);

  double max_sigma_axy, max_sigma_az, max_sigma_vxy, max_sigma_vz, mean_dev_lin_acc, mean_dev_ang_vel;
  ASSERT_EQ(nh_.getParam("max_sigma_axy", max_sigma_axy), true);
  ASSERT_EQ(nh_.getParam("max_sigma_az", max_sigma_az), true);
  ASSERT_EQ(nh_.getParam("max_sigma_vxy", max_sigma_vxy), true);
  ASSERT_EQ(nh_.getParam("max_sigma_vz", max_sigma_vz), true);
  ASSERT_EQ(nh_.getParam("mean_dev_lin_acc", mean_dev_lin_acc), true);
  ASSERT_EQ(nh_.getParam("mean_dev_ang_vel", mean_dev_ang_vel), true);

  // Assert mean linear acceleration is close to 1g and mean angular velocity close to 0.
  ASSERT_NEAR(linacc_mean, 1.0 * G_TO_METERS_PER_SECOND_SQUARED, mean_dev_lin_acc);
  ASSERT_NEAR(angvel_mean, 0.0, mean_dev_ang_vel);

  // Compute standard deviation of the signal.
  ASSERT_GT(sigma_ax, 1e-7);
  ASSERT_GT(sigma_ay, 1e-7);
  ASSERT_GT(sigma_az, 1e-7);
  ASSERT_GT(sigma_vx, 1e-7);
  ASSERT_GT(sigma_vy, 1e-7);
  ASSERT_GT(sigma_vz, 1e-7);
  ASSERT_LT(sigma_ax, max_sigma_axy);
  ASSERT_LT(sigma_ay, max_sigma_axy);
  ASSERT_LT(sigma_az, max_sigma_az);
  ASSERT_LT(sigma_vx, max_sigma_vxy);
  ASSERT_LT(sigma_vy, max_sigma_vxy);
  ASSERT_LT(sigma_vz, max_sigma_vz);
}

}  // namespace bota_driver_testing