Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #include <gtest/gtest.h>
00038 #include <arm_navigation_msgs/JointTrajectoryWithLimits.h>
00039 #include <spline_smoother/lspb_trajectory.h>
00040
00041
00042 double gen_rand(double min, double max)
00043 {
00044 int rand_num = rand()%100+1;
00045 double result = min + (double)((max-min)*rand_num)/101.0;
00046 return result;
00047 }
00048
00049 TEST(TestLSPBTrajectory, TestLSPBTrajectory)
00050 {
00051 srand(time(NULL));
00052 spline_smoother::LSPBTrajectory traj;
00053
00054
00055 int length = 4;
00056 int joints = 2;
00057
00058 arm_navigation_msgs::JointTrajectoryWithLimits wpt;
00059 wpt.trajectory.points.resize(length);
00060 wpt.trajectory.joint_names.resize(joints);
00061 wpt.trajectory.joint_names[0] = std::string("test0");
00062 wpt.trajectory.joint_names[1] = std::string("test1");
00063
00064 wpt.limits.resize(joints);
00065 wpt.limits[0].max_velocity = 0.5;
00066 wpt.limits[0].has_velocity_limits = 1;
00067
00068 wpt.limits[1].max_velocity = 0.25;
00069 wpt.limits[1].has_velocity_limits = 1;
00070
00071 wpt.limits[0].max_acceleration = 0.25;
00072 wpt.limits[0].has_acceleration_limits = 1;
00073
00074 wpt.limits[1].max_acceleration = 0.5;
00075 wpt.limits[1].has_acceleration_limits = 1;
00076
00077 for (int i=0; i<length; i++)
00078 {
00079 wpt.trajectory.points[i].positions.resize(joints);
00080 wpt.trajectory.points[i].velocities.resize(joints);
00081 wpt.trajectory.points[i].accelerations.resize(joints);
00082 for(int j=0; j<joints; j++)
00083 {
00084 wpt.trajectory.points[i].positions[j] = i+j+gen_rand(0.0,1.0);
00085 wpt.trajectory.points[i].velocities[j] = 0.0;
00086 wpt.trajectory.points[i].accelerations[j] = 0.0;
00087 }
00088 wpt.trajectory.points[i].time_from_start = ros::Duration(i);
00089 }
00090
00091 wpt.trajectory.points[0].positions[0] = 1.693069;
00092 wpt.trajectory.points[0].positions[1] = 2.910891;
00093
00094 wpt.trajectory.points[1].positions[0] = 2.782178;
00095 wpt.trajectory.points[1].positions[1] = 3.594059;
00096
00097 FILE* f = fopen("test_lspb_original.txt","w");
00098 for(int i=0; i<length; i++)
00099 {
00100 fprintf(f,"%f ",wpt.trajectory.points[i].time_from_start.toSec());
00101 for(int j=0; j<joints; j++)
00102 {
00103 fprintf(f,"%f ",wpt.trajectory.points[i].positions[j]);
00104 }
00105 for(int j=0; j<joints; j++)
00106 {
00107 fprintf(f,"%f ",wpt.trajectory.points[i].velocities[j]);
00108 }
00109 fprintf(f,"\n");
00110 }
00111 fclose(f);
00112
00113 spline_smoother::LSPBTrajectoryMsg spline;
00114 bool success = traj.parameterize(wpt.trajectory,wpt.limits,spline);
00115 EXPECT_TRUE(success);
00116
00117
00118 trajectory_msgs::JointTrajectory wpt_out;
00119 int num_seg = spline.segments.size();
00120 std::vector<double> knot_times;
00121 knot_times.resize(num_seg+1);
00122 knot_times[0] = 0.0;
00123 for(int i=1; i < (int) knot_times.size(); i++)
00124 {
00125 knot_times[i] = knot_times[i-1]+spline.segments[i-1].duration.toSec();
00126 }
00127 bool ss = spline_smoother::sampleSplineTrajectory(spline,knot_times,wpt_out);
00128
00129 EXPECT_TRUE(ss);
00130
00131 EXPECT_NEAR(wpt.trajectory.points[0].positions[0],wpt_out.points[0].positions[0],1e-5);
00132 EXPECT_NEAR(wpt.trajectory.points[0].positions[1],wpt_out.points[0].positions[1],1e-5);
00133 EXPECT_NEAR(wpt.trajectory.points[1].positions[0],wpt_out.points[1].positions[0],1e-5);
00134 EXPECT_NEAR(wpt.trajectory.points[1].positions[1],wpt_out.points[1].positions[1],1e-5);
00135
00136
00137 double total_time;
00138 bool st = spline_smoother::getTotalTime(spline,total_time);
00139 EXPECT_TRUE(st);
00140 double dT = 0.01;
00141 int sample_length = (int) (total_time/dT);
00142 std::vector<double> times;
00143 times.resize(sample_length+1);
00144 for (int i=0; i<sample_length; i++)
00145 {
00146 times[i] = dT*i;
00147 }
00148 times[sample_length] = total_time;
00149 bool sw = spline_smoother::write(spline,times,"test_lspb.txt");
00150 EXPECT_TRUE(sw);
00151
00152 }
00153
00154 int main(int argc, char** argv)
00155 {
00156 testing::InitGoogleTest(&argc, argv);
00157 return RUN_ALL_TESTS();
00158 }