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