Go to the documentation of this file.00001
00002
00003
00004
00005
00006 #include <iostream>
00007 #include <gtest/gtest.h>
00008 #include <pr2_base_trajectory_action/pr2_base_trajectory_action_controller.h>
00009
00010 using namespace std;
00011 using namespace pr2_base_trajectory_action;
00012
00013 TEST(TestSuite, testSpline)
00014 {
00015 Motion mx0(0.0, 0.23599999999283272);
00016 Motion mx1(1.0, 0.0);
00017 Motion my0(0.0, 0.0);
00018 Motion my1(0.0, 0.0);
00019 Motion myaw0(0.0, 0.0);
00020 Motion myaw1(0.0, 0.0);
00021 double duration = 4.237288128;
00022 CubicSpline spx(mx0, mx1, duration);
00023 CubicSpline spy(my0, my1, duration);
00024 CubicSpline spyaw(myaw0, myaw1, duration);
00025
00026 Trajectory::Segment s;
00027 s.start_time = 0.0;
00028 s.duration = duration;
00029 s.splines[0] = spx;
00030 s.splines[1] = spy;
00031 s.splines[2] = spyaw;
00032
00033 cout << "x: " << endl;
00034 for (int i = 0; i < 6; ++i)
00035 cout << " " << i << ": " << spx.coefficients[i] << endl;
00036 std::vector<double> spx_true(6, 0.0);
00037 spx_true[1] = 0.236;
00038 spx_true[2] = 0.055696;
00039 spx_true[3] = -0.0131443;
00040 for(int i = 0; i < 6; ++i)
00041 ASSERT_NEAR(spx.coefficients[i], spx_true[i], 0.001);
00042
00043 cout << "y: " << endl;
00044 for (int i = 0; i < 6; ++i)
00045 cout << " " << i << ": " << spy.coefficients[i] << endl;
00046 for(int i = 0; i < 6; ++i)
00047 ASSERT_NEAR(spy.coefficients[i], 0.0, 0.001);
00048
00049 cout << "yaw: " << endl;
00050 for (int i = 0; i < 6; ++i)
00051 cout << " " << i << ": " << spyaw.coefficients[i] << endl;
00052 for(int i = 0; i < 6; ++i)
00053 ASSERT_NEAR(spyaw.coefficients[i], 0.0, 0.001);
00054
00055 std::vector<double> sample_true;
00056 sample_true.push_back(0.0);
00057 sample_true.push_back(0.0754577);
00058 sample_true.push_back(0.158811);
00059 sample_true.push_back(0.247932);
00060 sample_true.push_back(0.340689);
00061 sample_true.push_back(0.434954);
00062 sample_true.push_back(0.528598);
00063 sample_true.push_back(0.61949);
00064 sample_true.push_back(0.705503);
00065 sample_true.push_back(0.784505);
00066 sample_true.push_back(0.854369);
00067 sample_true.push_back(0.912964);
00068 sample_true.push_back(0.958162);
00069 sample_true.push_back(0.987832);
00070 sample_true.push_back(0.999846);
00071
00072 int i = 0;
00073 for (double t = 0.0; t < duration; t += 0.3) {
00074 Motion x,y,yaw;
00075 spx.sampleWithTimeBounds(duration, t, x);
00076 spy.sampleWithTimeBounds(duration, t, y);
00077 spyaw.sampleWithTimeBounds(duration, t, yaw);
00078 ASSERT_NEAR(x.position, sample_true[i], 0.001);
00079 cout << "t: " << t << ", x: " << x.position << " y: " << y.position << " yaw: " << yaw.position << endl;
00080 ++i;
00081 }
00082
00083 i = 0;
00084 for (double t = 0.0; t < duration; t += 0.3) {
00085 BaseMotion b;
00086 s.sample(t, b);
00087 cout << "t: " << t << ", " << b << endl;
00088 ASSERT_NEAR(b.x.position, sample_true[i], 0.001);
00089 ++i;
00090 }
00091 }
00092
00093
00094 int main(int argc, char** argv) {
00095 testing::InitGoogleTest(&argc, argv);
00096 return RUN_ALL_TESTS();
00097 }