quaternion_to_rpy.h

Go to the documentation of this file.

#ifndef QUATERNION_TO_RPY_H
#define QUATERNION_TO_RPY_H
 
#include "PlotJuggler/transform_function.h"
 
class QuaternionToRollPitchYaw : public PJ::TransformFunction
{
public:
  QuaternionToRollPitchYaw();
 
  const char* name() const override
  {
    return "quaternion_to_RPY";
  }
 
  void reset() override;
 
  int numInputs() const override
  {
    return 4;
  }
 
  int numOutputs() const override
  {
    return 3;
  }
 
  void setScale(double scale)
  {
    _scale = scale;
  }
 
  void setWarp(bool wrap)
  {
    _wrap = wrap;
  }
 
  void calculate() override;
 
  void calculateNextPoint(size_t index, const std::array<double, 4>& quat,
                          std::array<double, 3>& rpy);
 
private:
  double _prev_roll = 0;
  double _prev_yaw = 0;
  double _prev_pitch = 0;
  double _roll_offset = 0;
  double _pitch_offset = 0;
  double _yaw_offset = 0;
  double _scale = 1.0;
  bool _wrap = true;
  double _last_timestamp = std::numeric_limits<double>::lowest();
};
 
#endif  // QUATERNION_TO_RPY_H