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/linear_trajectory.h>
00040
00041 using namespace spline_smoother;
00042
00043 TEST(TestLinearTrajectory, TestLinearTrajectory)
00044 {
00045 spline_smoother::LinearTrajectory traj;
00046
00047
00048 int length = 4;
00049 int joints = 2;
00050
00051 arm_navigation_msgs::JointTrajectoryWithLimits wpt;
00052 wpt.trajectory.points.resize(length);
00053 wpt.trajectory.joint_names.resize(joints);
00054 wpt.trajectory.joint_names[0] = std::string("test0");
00055 wpt.trajectory.joint_names[1] = std::string("test1");
00056
00057 wpt.limits.resize(joints);
00058 wpt.limits[0].max_velocity = 0.5;
00059 wpt.limits[0].has_velocity_limits = 1;
00060
00061 wpt.limits[1].max_velocity = 0.25;
00062 wpt.limits[1].has_velocity_limits = 1;
00063
00064 for (int i=0; i<length; i++)
00065 {
00066 wpt.trajectory.points[i].positions.resize(joints);
00067 wpt.trajectory.points[i].velocities.resize(joints);
00068 wpt.trajectory.points[i].accelerations.resize(joints);
00069 for(int j=0; j<joints; j++)
00070 {
00071 wpt.trajectory.points[i].positions[j] = i+j;
00072 wpt.trajectory.points[i].velocities[j] = 0.0;
00073 wpt.trajectory.points[i].accelerations[j] = 0.0;
00074 }
00075 wpt.trajectory.points[i].time_from_start = ros::Duration(0.0);
00076 }
00077
00078 spline_smoother::SplineTrajectory spline;
00079 bool success = traj.parameterize(wpt.trajectory,wpt.limits,spline);
00080
00081 double total_time;
00082 bool ss = spline_smoother::getTotalTime(spline,total_time);
00083 EXPECT_TRUE(ss);
00084
00085 double dT = 0.01;
00086 int sample_length = (int) (total_time/dT);
00087
00088 std::vector<double> times;
00089 times.resize(sample_length);
00090 for (int i=0; i<sample_length; i++)
00091 {
00092 times[i] = dT*i;
00093 }
00094
00095
00096 EXPECT_TRUE(success);
00097
00098 trajectory_msgs::JointTrajectory wpt_out;
00099 std::vector<double> times_out;
00100 times_out.resize(length);
00101 for (int i=0; i<length; i++)
00102 {
00103 times_out[i] = i;
00104 }
00105 spline_smoother::sampleSplineTrajectory(spline,times_out,wpt_out);
00106
00107 EXPECT_NEAR(wpt_out.points[0].positions[0], 0.0, 1e-8);
00108 EXPECT_NEAR(wpt_out.points[1].positions[0], 0.25, 1e-8);
00109 EXPECT_NEAR(wpt_out.points[2].positions[0], 0.5, 1e-8);
00110 EXPECT_NEAR(wpt_out.points[3].positions[0], 0.75, 1e-8);
00111
00112 }
00113
00114 int main(int argc, char** argv)
00115 {
00116 testing::InitGoogleTest(&argc, argv);
00117 return RUN_ALL_TESTS();
00118 }