← Back to branch summary

~shadowrobot/sr-ros-interface-ethercat/electric

« back to all changes in this revision

Viewing changes to sr_remappers/src/calibration_parser.cpp

  • Committer: Ugo Cupcic
  • Date: 2012-08-08 10:17:21 UTC
  • mfrom: (1.1.552 shadow_robot_ethercat)
  • Revision ID: ugo@shadowrobot.com-20120808101721-lutmwmwmt06srqya
1.0.0 stable release for the etherCAT hardware

Show diffs side-by-side

added added

removed removed

Lines of Context:
sr_remappers/src/calibration_parser.cpp
1
 
/**
2
 
* @file   calibration_parser.cpp
3
 
* @author Ugo Cupcic <ugo@shadowrobot.com>, Contact <contact@shadowrobot.com>
4
 
* @date   Thu May 13 11:41:56 2010
5
 
*
6
 
* @brief This is where the calibration matrix is read from a file, stored and where the actual mapping take place.
7
 
*
8
 
*
9
 
*/
10
 
 
11
 
#include <ros/ros.h>
12
 
 
13
 
#include <fstream>
14
 
#include <boost/algorithm/string.hpp>
15
 
#include <boost/algorithm/string/find_iterator.hpp>
16
 
#include "sr_remappers/calibration_parser.h"
17
 
 
18
 
using namespace std;
19
 
 
20
 
const std::string CalibrationParser::default_path = "/etc/robot/mappings/default_mapping";
21
 
 
22
 
CalibrationParser::CalibrationParser()
23
 
{
24
 
  ROS_WARN("No calibration path was specified, using default path");
25
 
  init(default_path);
26
 
}
27
 
 
28
 
CalibrationParser::CalibrationParser(std::string path)
29
 
{
30
 
  init(path);
31
 
}
32
 
 
33
 
int CalibrationParser::init(std::string path)
34
 
{
35
 
  //reserve enough lines
36
 
  calibration_matrix.reserve(25);
37
 
 
38
 
  ifstream calibration_file;
39
 
  calibration_file.open(path.c_str());
40
 
 
41
 
  //can't find the file
42
 
  if(!calibration_file.is_open())
43
 
  {
44
 
    ROS_ERROR("Couldn't open the file %s", path.c_str());
45
 
    return -1;
46
 
  }
47
 
 
48
 
  //we read the file and put all the data in this matrix
49
 
  std::vector< std::vector<double> > tmp_matrix;
50
 
 
51
 
  string line;
52
 
  while( !calibration_file.eof() )
53
 
  {
54
 
    getline( calibration_file, line );
55
 
 
56
 
    //remove leading and trailing whitespaces
57
 
    line = boost::algorithm::trim_copy(line);
58
 
 
59
 
    //ignore empty line
60
 
    if( line.size() == 0 )
61
 
      continue;
62
 
 
63
 
    //ignore comments
64
 
    if( line[0] == '#' )
65
 
      continue;
66
 
 
67
 
    std::vector<std::string> splitted_string;
68
 
    boost::split(splitted_string, line, boost::is_any_of("\t "));
69
 
 
70
 
    std::vector<double> double_line(splitted_string.size());
71
 
    for(unsigned int index_col=0; index_col< splitted_string.size(); ++index_col)
72
 
      double_line[index_col] = convertToDouble(splitted_string[index_col]);
73
 
 
74
 
    calibration_matrix.push_back(double_line);
75
 
  }
76
 
  calibration_file.close();
77
 
 
78
 
  return 0;
79
 
}
80
 
 
81
 
 
82
 
std::vector<double> CalibrationParser::get_remapped_vector(std::vector<double> input_vector)
83
 
{
84
 
  //check the size of the matrix
85
 
  if(input_vector.size() != calibration_matrix.size())
86
 
  {
87
 
    ROS_ERROR("The size of the given vector doesn't correspond to the mapping: received %d, wanted %d", input_vector.size(), calibration_matrix.size());
88
 
    return std::vector<double>(calibration_matrix[0].size());
89
 
  }
90
 
 
91
 
  std::vector<double> result(calibration_matrix[0].size());
92
 
  double tmp_value;
93
 
 
94
 
  for(unsigned int col = 0; col < calibration_matrix[0].size(); ++col)
95
 
  {
96
 
    tmp_value = 0.0;
97
 
    for(unsigned int index_vec = 0; index_vec < calibration_matrix.size(); ++index_vec)
98
 
    {
99
 
      tmp_value += (input_vector[index_vec]*calibration_matrix[index_vec][col]);
100
 
    }
101
 
    result[col] = tmp_value;
102
 
  }
103
 
 
104
 
  return result;
105
 
}