quat_rpy_test.cpp

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/Vector3.h>
#include <au_core/math_util.h>
#include <tf2/transform_datatypes.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <mutex>

const double EPSILON = 0.01;

std::mutex eulerMutex;
geometry_msgs::Vector3 eulerMsg;

bool areSame(double a, double b)
{
  return fabs(a - b) < EPSILON;
}

void compareRpy(const sensor_msgs::Imu::ConstPtr& imu, geometry_msgs::Vector3& euler)
{
  tf2::Quaternion rot(imu->orientation.x, imu->orientation.y, imu->orientation.z, imu->orientation.w);
  double r, p, y;
  tf2::Matrix3x3(rot).getRPY(r, p, y);
  r = au_core::radToDeg(r);
  p = au_core::radToDeg(p);
  y = au_core::radToDeg(y);

  std::lock_guard<std::mutex> lock(eulerMutex);
  ROS_INFO_COND(!areSame(r, eulerMsg.x), "%s: %.4f != %.4f", "roll", r, eulerMsg.x);
  ROS_INFO_COND(!areSame(p, eulerMsg.y), "%s: %.4f != %.4f", "pitch", p, eulerMsg.y);
  ROS_INFO_COND(!areSame(y, eulerMsg.z), "%s: %.4f != %.4f", "yaw", y, eulerMsg.z);
}

void compareQuat(const sensor_msgs::Imu::ConstPtr& imu, geometry_msgs::Vector3& euler)
{
  std::lock_guard<std::mutex> lock(eulerMutex);

  tf2::Quaternion q;
  q.setRPY(au_core::degToRad(euler.x), au_core::degToRad(euler.y), au_core::degToRad(euler.z));

  ROS_INFO_COND(!areSame(q.w(), imu->orientation.w), "%s: %.4f != %.4f", "w", q.w(), imu->orientation.w);
  ROS_INFO_COND(!areSame(q.x(), imu->orientation.x), "%s: %.4f != %.4f", "x", q.x(), imu->orientation.x);
  ROS_INFO_COND(!areSame(q.y(), imu->orientation.y), "%s: %.4f != %.4f", "y", q.y(), imu->orientation.y);
  ROS_INFO_COND(!areSame(q.z(), imu->orientation.z), "%s: %.4f != %.4f", "z", q.z(), imu->orientation.z);
}

void imuCb(const sensor_msgs::Imu::ConstPtr& msg)
{
//  compareRpy(msg, eulerMsg);
  compareQuat(msg, eulerMsg);
}

void eulerCb(const geometry_msgs::Vector3::ConstPtr& msg)
{
  std::lock_guard<std::mutex> lock(eulerMutex);
  eulerMsg.x = (msg->x);
  eulerMsg.y = (msg->y);
  eulerMsg.z = (msg->z);
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "quat_test");
  ros::NodeHandle nh;

  ros::Subscriber imu_sub = nh.subscribe<sensor_msgs::Imu>("/gx5_25/imu", 1, imuCb);
  ros::Subscriber euler_sub = nh.subscribe<geometry_msgs::Vector3>("/gx5_25/euler", 1, eulerCb);

  ros::spin();
}