xml_calibration_parser.h

Go to the documentation of this file.

#ifndef         XML_CALIBRATION_PARSER_H_
# define        XML_CALIBRATION_PARSER_H_

//xml parser library
#include <tinyxml.h>

//generic C/C++ include
#include <string>
#include <vector>
#include <map>

namespace xml_calibration_parser{

class XmlCalibrationParser
{
 public:
  XmlCalibrationParser(){};
  XmlCalibrationParser(std::string path_to_calibration);
  ~XmlCalibrationParser(){};

  float get_calibration_value(float position, std::string joint_name);

  struct Calibration
  {
    float raw_value;
    float calibrated_value;
  };

  struct JointCalibration
  {
    std::string name;
    std::vector<Calibration> calibrations;
  };


  std::vector<JointCalibration> getJointsCalibrations();

 protected:
  void parse_calibration_file( TiXmlNode* pParent );
  std::vector<Calibration> parse_joint_attributes( TiXmlNode* pParent );

  std::vector<JointCalibration> jointsCalibrations;
  // use a map to easily access the value
  typedef std::map<std::string, std::vector<float> > mapType;
  mapType joints_calibrations_map;

  int build_calibration_table();

  std::vector<float> calibration_to_lookup_table(std::vector<Calibration> calib);

  float compute_lookup_value(int index, std::vector<Calibration> calib);

  float linear_interpolate( float x ,
                            float x0, float y0,
                            float x1, float y1 );

  // consts for the lookup tables
  static const float lookup_precision;
  static const float lookup_offset;

  int round(float number);

  int return_index_from_raw_position(float raw_position);

  static inline float return_raw_position_from_index(int lookup_index)
  {
    return ((float)lookup_index)/lookup_precision;
  };

}; // end class XmlCalibrationParser

} // end namespace
#endif      /* !XML_CALIBRATION_PARSER_H_ */