test_calibration.cpp
Go to the documentation of this file.
00001 
00028 #include "sr_utilities/calibration.hpp"
00029 #include <gtest/gtest.h>
00030 #include <ros/ros.h>
00031 
00032 class CalibrationTest
00033 {
00034 public:
00035   CalibrationTest()
00036   {}
00037 
00038   ~CalibrationTest()
00039   {}
00040 };
00041 
00043 // TWO POINTS TABLES //
00045 
00055 TEST(Calibration2Points, before)
00056 {
00057   std::vector<joint_calibration::Point> cal_table;
00058 
00059   joint_calibration::Point point;
00060   point.raw_value = 1.0;
00061   point.calibrated_value = 3.0;
00062   cal_table.push_back(point);
00063   point.raw_value = 2.0;
00064   point.calibrated_value = 5.0;
00065   cal_table.push_back(point);
00066 
00067   shadow_robot::JointCalibration* cal = new shadow_robot::JointCalibration(cal_table);
00068 
00069   EXPECT_EQ(cal->compute(0.0), 1.0);
00070 
00071   delete cal;
00072 }
00073 
00083 TEST(Calibration2Points, in)
00084 {
00085   std::vector<joint_calibration::Point> cal_table;
00086 
00087   joint_calibration::Point point;
00088   point.raw_value = 1.0;
00089   point.calibrated_value = 3.0;
00090   cal_table.push_back(point);
00091   point.raw_value = 2.0;
00092   point.calibrated_value = 5.0;
00093   cal_table.push_back(point);
00094 
00095   shadow_robot::JointCalibration* cal = new shadow_robot::JointCalibration(cal_table);
00096 
00097   EXPECT_EQ(cal->compute(1.5), 4.0);
00098   delete cal;
00099 }
00100 
00110 TEST(Calibration2Points, after)
00111 {
00112   std::vector<joint_calibration::Point> cal_table;
00113 
00114   joint_calibration::Point point;
00115   point.raw_value = 1.0;
00116   point.calibrated_value = 3.0;
00117   cal_table.push_back(point);
00118   point.raw_value = 2.0;
00119   point.calibrated_value = 5.0;
00120   cal_table.push_back(point);
00121 
00122   shadow_robot::JointCalibration* cal = new shadow_robot::JointCalibration(cal_table);
00123 
00124   EXPECT_EQ(cal->compute(3.0), 7.0);
00125   delete cal;
00126 }
00127 
00137 TEST(Calibration2Points, on)
00138 {
00139   std::vector<joint_calibration::Point> cal_table;
00140 
00141   joint_calibration::Point point;
00142   point.raw_value = 1.0;
00143   point.calibrated_value = 3.0;
00144   cal_table.push_back(point);
00145   point.raw_value = 2.0;
00146   point.calibrated_value = 5.0;
00147   cal_table.push_back(point);
00148 
00149   shadow_robot::JointCalibration* cal = new shadow_robot::JointCalibration(cal_table);
00150 
00151   EXPECT_EQ(cal->compute(2.0), 5.0);
00152   EXPECT_EQ(cal->compute(1.0), 3.0);
00153   delete cal;
00154 }
00155 
00156 
00157 
00159 // THREE POINTS TABLES //
00161 
00172 TEST(Calibration3Points, before)
00173 {
00174   std::vector<joint_calibration::Point> cal_table;
00175 
00176   joint_calibration::Point point;
00177   point.raw_value = 1.0;
00178   point.calibrated_value = 3.0;
00179   cal_table.push_back(point);
00180   point.raw_value = 2.0;
00181   point.calibrated_value = 5.0;
00182   cal_table.push_back(point);
00183   point.raw_value = 3.0;
00184   point.calibrated_value = 4.0;
00185   cal_table.push_back(point);
00186 
00187   shadow_robot::JointCalibration* cal = new shadow_robot::JointCalibration(cal_table);
00188 
00189   EXPECT_EQ(cal->compute(0.0), 1.0);
00190   delete cal;
00191 }
00192 
00203 TEST(Calibration3Points, first_interval)
00204 {
00205   std::vector<joint_calibration::Point> cal_table;
00206 
00207   joint_calibration::Point point;
00208   point.raw_value = 1.0;
00209   point.calibrated_value = 3.0;
00210   cal_table.push_back(point);
00211   point.raw_value = 2.0;
00212   point.calibrated_value = 5.0;
00213   cal_table.push_back(point);
00214   point.raw_value = 3.0;
00215   point.calibrated_value = 4.0;
00216   cal_table.push_back(point);
00217 
00218   shadow_robot::JointCalibration* cal = new shadow_robot::JointCalibration(cal_table);
00219 
00220   EXPECT_EQ(cal->compute(1.5), 4.0);
00221   delete cal;
00222 }
00223 
00234 TEST(Calibration3Points, second_interval)
00235 {
00236   std::vector<joint_calibration::Point> cal_table;
00237 
00238   joint_calibration::Point point;
00239   point.raw_value = 1.0;
00240   point.calibrated_value = 3.0;
00241   cal_table.push_back(point);
00242   point.raw_value = 2.0;
00243   point.calibrated_value = 5.0;
00244   cal_table.push_back(point);
00245   point.raw_value = 3.0;
00246   point.calibrated_value = 4.0;
00247   cal_table.push_back(point);
00248 
00249   shadow_robot::JointCalibration* cal = new shadow_robot::JointCalibration(cal_table);
00250 
00251   EXPECT_EQ(cal->compute(2.5), 4.5);
00252   delete cal;
00253 }
00254 
00265 TEST(Calibration3Points, after)
00266 {
00267   std::vector<joint_calibration::Point> cal_table;
00268 
00269   joint_calibration::Point point;
00270   point.raw_value = 1.0;
00271   point.calibrated_value = 3.0;
00272   cal_table.push_back(point);
00273   point.raw_value = 2.0;
00274   point.calibrated_value = 5.0;
00275   cal_table.push_back(point);
00276   point.raw_value = 3.0;
00277   point.calibrated_value = 4.0;
00278   cal_table.push_back(point);
00279 
00280   shadow_robot::JointCalibration* cal = new shadow_robot::JointCalibration(cal_table);
00281 
00282   EXPECT_EQ(cal->compute(4.0), 3.0);
00283   delete cal;
00284 }
00285 
00296 TEST(Calibration3Points, on)
00297 {
00298   std::vector<joint_calibration::Point> cal_table;
00299 
00300   joint_calibration::Point point;
00301   point.raw_value = 1.0;
00302   point.calibrated_value = 3.0;
00303   cal_table.push_back(point);
00304   point.raw_value = 2.0;
00305   point.calibrated_value = 5.0;
00306   cal_table.push_back(point);
00307   point.raw_value = 3.0;
00308   point.calibrated_value = 4.0;
00309   cal_table.push_back(point);
00310 
00311   shadow_robot::JointCalibration* cal = new shadow_robot::JointCalibration(cal_table);
00312 
00313   EXPECT_EQ(cal->compute(1.0), 3.0);
00314   EXPECT_EQ(cal->compute(2.0), 5.0);
00315   EXPECT_EQ(cal->compute(3.0), 4.0);
00316   delete cal;
00317 }
00318 
00319 
00342 TEST(Calibration5Points, mixed_and_big_test)
00343 {
00344   std::vector<joint_calibration::Point> cal_table;
00345 
00346   joint_calibration::Point point;
00347 
00348   point.raw_value = 5.0;
00349   point.calibrated_value = 6.0;
00350   cal_table.push_back(point);
00351 
00352   point.raw_value = 2.0;
00353   point.calibrated_value = 0.0;
00354   cal_table.push_back(point);
00355 
00356   point.raw_value = 1.0;
00357   point.calibrated_value = -2.0;
00358   cal_table.push_back(point);
00359 
00360   point.raw_value = 10.0;
00361   point.calibrated_value = 0.0;
00362   cal_table.push_back(point);
00363 
00364   point.raw_value = 3.0;
00365   point.calibrated_value = -4.0;
00366   cal_table.push_back(point);
00367 
00368   shadow_robot::JointCalibration* cal = new shadow_robot::JointCalibration(cal_table);
00369 
00370   EXPECT_EQ(cal->compute(0.0), -4.0);
00371   EXPECT_EQ(cal->compute(1.0), -2.0);
00372   EXPECT_EQ(cal->compute(1.5), -1.0);
00373   EXPECT_EQ(cal->compute(2.0),  0.0);
00374   EXPECT_EQ(cal->compute(2.5), -2.0);
00375   EXPECT_EQ(cal->compute(3.0), -4.0);
00376   EXPECT_EQ(cal->compute(4.0),  1.0);
00377   EXPECT_EQ(cal->compute(5.0),  6.0);
00378   EXPECT_EQ(cal->compute(7.5), 3.0);
00379   EXPECT_EQ(cal->compute(10.), 0.0);
00380   EXPECT_EQ(cal->compute(15.), -6.0);
00381   delete cal;
00382 }
00383 
00384 
00385 
00387 //     MAIN       //
00389 
00390 int main(int argc, char **argv)
00391 {
00392   ros::init(argc, argv, "calibration_test");
00393 
00394   testing::InitGoogleTest(&argc, argv);
00395   return RUN_ALL_TESTS();
00396 }
00397 /* For the emacs weenies in the crowd.
00398    Local Variables:
00399    c-basic-offset: 2
00400    End:
00401 */
00402 


sr_utilities
Author(s): Ugo Cupcic
autogenerated on Mon Oct 6 2014 07:42:15