TestBotaDriverSignalQuality.cpp

Go to the documentation of this file.

#include <gtest/gtest.h>
#include <geometry_msgs/WrenchStamped.h>
#include <geometry_msgs/Wrench.h>
#include <Eigen/Dense>
#include <ros/ros.h>

#define ASSERT_DOUBLE_NOT_NEAR(val1, val2, abs_error)                                                                  \
  ASSERT_PRED_FORMAT3(!::testing::internal::DoubleNearPredFormat, val1, val2, abs_error)

using namespace Eigen;

namespace bota_driver_testing
{
class BotaDriverSignalQuality : public ::testing::Test
{
protected:
  ros::Subscriber sub_;
  ros::NodeHandle nh_{ "~" };
  std::uint32_t msgCount_;
  std::uint32_t maxCount_;
  std::string topicName_;
  VectorXd fxSamples_;
  VectorXd fySamples_;
  VectorXd fzSamples_;
  VectorXd txSamples_;
  VectorXd tySamples_;
  VectorXd tzSamples_;
  float testDuration_;

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

  ~BotaDriverSignalQuality() 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 wrenchCallback(const geometry_msgs::WrenchStamped::ConstPtr& msg)
  {
    std::uint32_t lpos = msgCount_;
    msgCount_++;
    fxSamples_.conservativeResize(msgCount_);
    fySamples_.conservativeResize(msgCount_);
    fzSamples_.conservativeResize(msgCount_);
    txSamples_.conservativeResize(msgCount_);
    tySamples_.conservativeResize(msgCount_);
    tzSamples_.conservativeResize(msgCount_);

    fxSamples_(lpos) = msg->wrench.force.x;
    fySamples_(lpos) = msg->wrench.force.y;
    fzSamples_(lpos) = msg->wrench.force.z;
    txSamples_(lpos) = msg->wrench.torque.x;
    tySamples_(lpos) = msg->wrench.torque.y;
    tzSamples_(lpos) = msg->wrench.torque.z;
  }
};

TEST_F(BotaDriverSignalQuality, SignalQuality)
{
  SCOPED_TRACE("SignalQualityFT");
  ros::Time time_offset;
  ASSERT_EQ(nh_.getParam("topic_name", topicName_), true);
  sub_ = nh_.subscribe(topicName_, 1, &BotaDriverSignalQuality::wrenchCallback, (BotaDriverSignalQuality*)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);
  fxSamples_ = fxSamples_.array() - fxSamples_.mean();
  double sigma_fx = std::sqrt(fxSamples_.squaredNorm() / msgCount_);
  fySamples_ = fySamples_.array() - fySamples_.mean();
  double sigma_fy = std::sqrt(fySamples_.squaredNorm() / msgCount_);
  fzSamples_ = fzSamples_.array() - fzSamples_.mean();
  double sigma_fz = std::sqrt(fzSamples_.squaredNorm() / msgCount_);
  txSamples_ = txSamples_.array() - txSamples_.mean();
  double sigma_tx = std::sqrt(txSamples_.squaredNorm() / msgCount_);
  tySamples_ = tySamples_.array() - tySamples_.mean();
  double sigma_ty = std::sqrt(tySamples_.squaredNorm() / msgCount_);
  tzSamples_ = tzSamples_.array() - tzSamples_.mean();
  double sigma_tz = std::sqrt(tzSamples_.squaredNorm() / msgCount_);

  double max_sigma_fxy, max_sigma_fz, max_sigma_txy, max_sigma_tz;
  ASSERT_EQ(nh_.getParam("max_sigma_fxy", max_sigma_fxy), true);
  ASSERT_EQ(nh_.getParam("max_sigma_fz", max_sigma_fz), true);
  ASSERT_EQ(nh_.getParam("max_sigma_txy", max_sigma_txy), true);
  ASSERT_EQ(nh_.getParam("max_sigma_tz", max_sigma_tz), true);
  // Compute standard deviation of the signal.
  ASSERT_GT(sigma_fx, 1e-7);
  ASSERT_GT(sigma_fy, 1e-7);
  ASSERT_GT(sigma_fz, 1e-7);
  ASSERT_GT(sigma_tx, 1e-7);
  ASSERT_GT(sigma_ty, 1e-7);
  ASSERT_GT(sigma_tz, 1e-7);
  ASSERT_LT(sigma_fx, max_sigma_fxy);
  ASSERT_LT(sigma_fy, max_sigma_fxy);
  ASSERT_LT(sigma_fz, max_sigma_fz);
  ASSERT_LT(sigma_tx, max_sigma_txy);
  ASSERT_LT(sigma_ty, max_sigma_txy);
  ASSERT_LT(sigma_tz, max_sigma_tz);
}

}  // namespace bota_driver_testing