TestBotaDriverFTOffsets.cpp

Go to the documentation of this file.

#include <gtest/gtest.h>

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

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

  BotaDriverTestFTOffsets() : msgCount_(0)
  {
  }

  ~BotaDriverTestFTOffsets() 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 meanwrenchCallback(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(BotaDriverTestFTOffsets, SetFTOffsets)
{
  SCOPED_TRACE("SetFTOffsets");
  ros::Time time_offset;
  ASSERT_EQ(nh_.getParam("topic_name", topicName_), true);
  sub_ = nh_.subscribe(topicName_, 1000, &BotaDriverTestFTOffsets::meanwrenchCallback, (BotaDriverTestFTOffsets*)this);
  ASSERT_EQ(nh_.getParam("test_duration", testDuration_), true);
  time_offset = ros::Time::now() + ros::Duration(testDuration_);
  while (ros::ok() && ros::Time::now() < time_offset)
  {
    ros::spinOnce();
    if (HasFatalFailure())
    {
      FAIL() << "Fatal errors occurred.\n";
      return;
    }
  }
  EXPECT_GT(msgCount_, 0U);
  meanWrenchOffset_.force.x = fxSamples_.mean();
  meanWrenchOffset_.force.y = fySamples_.mean();
  meanWrenchOffset_.force.z = fzSamples_.mean();
  meanWrenchOffset_.torque.x = txSamples_.mean();
  meanWrenchOffset_.torque.y = tySamples_.mean();
  meanWrenchOffset_.torque.z = tzSamples_.mean();
  ASSERT_NEAR(meanWrenchOffset_.force.x, 10000.0, 1000);
  ASSERT_NEAR(meanWrenchOffset_.force.y, -10000.0, 1000);
  ASSERT_NEAR(meanWrenchOffset_.force.z, 20000.0, 1000);
  ASSERT_NEAR(meanWrenchOffset_.torque.x, -20000.0, 100);
  ASSERT_NEAR(meanWrenchOffset_.torque.y, 30000.0, 100);
  ASSERT_NEAR(meanWrenchOffset_.torque.z, -30000.0, 100);
}

}  // namespace bota_driver_testing