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 <filters/filter_chain.h>
00039 #include <arm_navigation_msgs/FilterJointTrajectory.h>
00040
00041 using namespace filters;
00042
00043 TEST(TestLinearSplineVelocityScaler, TestLinearSplineVelocityScaler1)
00044 {
00045
00046 FilterChain<arm_navigation_msgs::FilterJointTrajectory> chain("arm_navigation_msgs::FilterJointTrajectory");
00047 ASSERT_TRUE(chain.configure("TestLinearSplineVelocityScaler"));
00048
00049
00050 int length = 4;
00051 int joints = 2;
00052
00053 arm_navigation_msgs::FilterJointTrajectory wpt;
00054 arm_navigation_msgs::FilterJointTrajectory wpt_out;
00055 wpt.request.trajectory.points.resize(length);
00056 wpt.request.trajectory.joint_names.resize(joints);
00057 wpt.request.trajectory.joint_names[0] = std::string("test0");
00058 wpt.request.trajectory.joint_names[1] = std::string("test1");
00059
00060 wpt.request.limits.resize(joints);
00061 wpt.request.limits[0].max_velocity = 0.5;
00062 wpt.request.limits[0].has_velocity_limits = 1;
00063
00064 wpt.request.limits[1].max_velocity = 0.25;
00065 wpt.request.limits[1].has_velocity_limits = 1;
00066
00067 for (int i=0; i<length; i++)
00068 {
00069 wpt.request.trajectory.points[i].positions.resize(joints);
00070 wpt.request.trajectory.points[i].velocities.resize(joints);
00071 wpt.request.trajectory.points[i].accelerations.resize(joints);
00072 for(int j=0; j<joints; j++)
00073 {
00074 wpt.request.trajectory.points[i].positions[j] = i+j;
00075 wpt.request.trajectory.points[i].velocities[j] = 0.0;
00076 wpt.request.trajectory.points[i].accelerations[j] = 0.0;
00077 }
00078 wpt.request.trajectory.points[i].time_from_start = ros::Duration(0.0);
00079 }
00080 chain.update(wpt, wpt_out);
00081
00082
00083 EXPECT_NEAR(wpt_out.request.trajectory.points[0].time_from_start.toSec(), 0.0, 1e-8);
00084 EXPECT_NEAR(wpt_out.request.trajectory.points[1].time_from_start.toSec(), 4.0, 1e-8);
00085 EXPECT_NEAR(wpt_out.request.trajectory.points[2].time_from_start.toSec(), 8.0, 1e-8);
00086 EXPECT_NEAR(wpt_out.request.trajectory.points[3].time_from_start.toSec(), 12.0, 1e-8);
00087
00088 }
00089
00090 int main(int argc, char** argv)
00091 {
00092 testing::InitGoogleTest(&argc, argv);
00093 ros::init(argc, argv, "test_linear_spline_velocity_scaler");
00094 return RUN_ALL_TESTS();
00095 }