Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008 #include <iostream>
00009 #include <gtest/gtest.h>
00010 #include "robodyn_controllers/JointTrajectoryFollower.h"
00011
00012 class JointTrajectoryFollowerTest : public ::testing::Test
00013 {
00014 protected:
00015 virtual void SetUp()
00016 {
00017 follower = new PreplannedJointTrajectoryFollower();
00018
00019 trajectory = new trajectory_msgs::JointTrajectory();
00020 trajectory->header.frame_id = "test";
00021 ros::Time::init();
00022 trajectory->header.stamp = ros::Time::now();
00023 trajectory->joint_names.push_back("joint0");
00024 trajectory->joint_names.push_back("joint1");
00025 trajectory->joint_names.push_back("joint2");
00026
00027 trajectory_msgs::JointTrajectoryPoint point;
00028 point.positions.push_back(1.0);
00029 point.positions.push_back(2.0);
00030 point.positions.push_back(3.0);
00031 point.velocities.push_back(0.0);
00032 point.velocities.push_back(0.0);
00033 point.velocities.push_back(0.0);
00034 point.accelerations.push_back(0.0);
00035 point.accelerations.push_back(0.0);
00036 point.accelerations.push_back(0.0);
00037 point.time_from_start = ros::Duration(0.0);
00038 trajectory->points.push_back(point);
00039
00040 trajectory_msgs::JointTrajectoryPoint point2;
00041 point2.positions.push_back(3.1);
00042 point2.positions.push_back(1.5);
00043 point2.positions.push_back(2.2);
00044 point2.velocities.push_back(0.0);
00045 point2.velocities.push_back(0.0);
00046 point2.velocities.push_back(0.0);
00047 point2.accelerations.push_back(0.0);
00048 point2.accelerations.push_back(0.0);
00049 point2.accelerations.push_back(0.0);
00050 point2.time_from_start = ros::Duration(0.5);
00051 trajectory->points.push_back(point2);
00052
00053 trajectory_msgs::JointTrajectoryPoint point3;
00054 point3.positions.push_back(4.3);
00055 point3.positions.push_back(2.2);
00056 point3.positions.push_back(5.1);
00057 point3.velocities.push_back(0.0);
00058 point3.velocities.push_back(0.0);
00059 point3.velocities.push_back(0.0);
00060 point3.accelerations.push_back(0.0);
00061 point3.accelerations.push_back(0.0);
00062 point3.accelerations.push_back(0.0);
00063 point3.time_from_start = ros::Duration(1.2);
00064 trajectory->points.push_back(point3);
00065
00066 trajectory_msgs::JointTrajectoryPoint point4;
00067 point4.positions.push_back(6.0);
00068 point4.positions.push_back(3.0);
00069 point4.positions.push_back(1.0);
00070 point4.velocities.push_back(0.0);
00071 point4.velocities.push_back(0.0);
00072 point4.velocities.push_back(0.0);
00073 point4.accelerations.push_back(0.0);
00074 point4.accelerations.push_back(0.0);
00075 point4.accelerations.push_back(0.0);
00076 point4.time_from_start = ros::Duration(2);
00077 trajectory->points.push_back(point4);
00078 }
00079
00080 virtual void TearDown()
00081 {
00082 delete follower;
00083 delete trajectory;
00084 }
00085
00086 PreplannedJointTrajectoryFollower* follower;
00087 trajectory_msgs::JointTrajectory* trajectory;
00088 };
00089
00090 TEST_F(JointTrajectoryFollowerTest, nominalTest)
00091 {
00092 follower->setTrajectory(*trajectory, 1.0);
00093 sensor_msgs::JointState nextPoint;
00094 ros::Duration timeFromStart;
00095
00096 EXPECT_TRUE(follower->isActive());
00097 EXPECT_NO_THROW(follower->getNextPoint(nextPoint, timeFromStart));
00098 EXPECT_EQ(nextPoint.header.frame_id, trajectory->header.frame_id);
00099 EXPECT_EQ(nextPoint.position, trajectory->points[0].positions);
00100 EXPECT_EQ(nextPoint.name, trajectory->joint_names);
00101 EXPECT_EQ(timeFromStart, trajectory->points[0].time_from_start);
00102
00103 EXPECT_TRUE(follower->isActive());
00104 EXPECT_NO_THROW(follower->getNextPoint(nextPoint, timeFromStart));
00105 EXPECT_EQ(nextPoint.header.frame_id, trajectory->header.frame_id);
00106 EXPECT_EQ(nextPoint.position, trajectory->points[1].positions);
00107 EXPECT_EQ(nextPoint.name, trajectory->joint_names);
00108 EXPECT_EQ(timeFromStart, trajectory->points[1].time_from_start);
00109
00110 EXPECT_TRUE(follower->isActive());
00111 EXPECT_NO_THROW(follower->getNextPoint(nextPoint, timeFromStart));
00112 EXPECT_EQ(nextPoint.header.frame_id, trajectory->header.frame_id);
00113 EXPECT_EQ(nextPoint.position, trajectory->points[3].positions);
00114 EXPECT_EQ(nextPoint.name, trajectory->joint_names);
00115 EXPECT_EQ(timeFromStart, trajectory->points[3].time_from_start);
00116
00117 EXPECT_FALSE(follower->isActive());
00118 EXPECT_THROW(follower->getNextPoint(nextPoint, timeFromStart), std::runtime_error);
00119 }
00120
00121 TEST_F(JointTrajectoryFollowerTest, zeroTest)
00122 {
00123 trajectory->points.clear();
00124 trajectory_msgs::JointTrajectoryPoint point;
00125 point.positions.push_back(1.0);
00126 point.positions.push_back(2.0);
00127 point.positions.push_back(3.0);
00128 point.velocities.push_back(0.0);
00129 point.velocities.push_back(0.0);
00130 point.velocities.push_back(0.0);
00131 point.accelerations.push_back(0.0);
00132 point.accelerations.push_back(0.0);
00133 point.accelerations.push_back(0.0);
00134 point.time_from_start = ros::Duration(0.0);
00135 trajectory->points.push_back(point);
00136
00137 trajectory_msgs::JointTrajectoryPoint point2;
00138 point2.positions.push_back(3.1);
00139 point2.positions.push_back(1.5);
00140 point2.positions.push_back(2.2);
00141 point2.velocities.push_back(0.0);
00142 point2.velocities.push_back(0.0);
00143 point2.velocities.push_back(0.0);
00144 point2.accelerations.push_back(0.0);
00145 point2.accelerations.push_back(0.0);
00146 point2.accelerations.push_back(0.0);
00147 point2.time_from_start = ros::Duration(0.0);
00148 trajectory->points.push_back(point2);
00149
00150 trajectory_msgs::JointTrajectoryPoint point3;
00151 point3.positions.push_back(4.3);
00152 point3.positions.push_back(2.2);
00153 point3.positions.push_back(5.1);
00154 point3.velocities.push_back(0.0);
00155 point3.velocities.push_back(0.0);
00156 point3.velocities.push_back(0.0);
00157 point3.accelerations.push_back(0.0);
00158 point3.accelerations.push_back(0.0);
00159 point3.accelerations.push_back(0.0);
00160 point3.time_from_start = ros::Duration(0.0);
00161 trajectory->points.push_back(point3);
00162
00163 trajectory_msgs::JointTrajectoryPoint point4;
00164 point4.positions.push_back(6.0);
00165 point4.positions.push_back(3.0);
00166 point4.positions.push_back(1.0);
00167 point4.velocities.push_back(0.0);
00168 point4.velocities.push_back(0.0);
00169 point4.velocities.push_back(0.0);
00170 point4.accelerations.push_back(0.0);
00171 point4.accelerations.push_back(0.0);
00172 point4.accelerations.push_back(0.0);
00173 point4.time_from_start = ros::Duration(0.0);
00174 trajectory->points.push_back(point4);
00175
00176 follower->setTrajectory(*trajectory, 1);
00177
00178 sensor_msgs::JointState nextPoint;
00179 ros::Duration timeFromStart;
00180
00181 for (unsigned int i = 0; i < trajectory->points.size(); ++i)
00182 {
00183 EXPECT_TRUE(follower->isActive());
00184 EXPECT_NO_THROW(follower->getNextPoint(nextPoint, timeFromStart));
00185 EXPECT_EQ(nextPoint.header.frame_id, trajectory->header.frame_id);
00186 EXPECT_EQ(nextPoint.position, trajectory->points[i].positions);
00187 EXPECT_EQ(nextPoint.name, trajectory->joint_names);
00188 EXPECT_EQ(timeFromStart, trajectory->points[i].time_from_start);
00189 }
00190
00191 EXPECT_FALSE(follower->isActive());
00192 EXPECT_THROW(follower->getNextPoint(nextPoint, timeFromStart), std::runtime_error);
00193
00194 }
00195
00196 TEST_F(JointTrajectoryFollowerTest, resetTest)
00197 {
00198 follower->setTrajectory(*trajectory, 1.0);
00199 sensor_msgs::JointState nextPoint;
00200 ros::Duration timeFromStart;
00201 follower->abort();
00202 EXPECT_FALSE(follower->isActive());
00203 EXPECT_THROW(follower->getNextPoint(nextPoint, timeFromStart), std::runtime_error);
00204
00205 EXPECT_NO_THROW(follower->setTrajectory(*trajectory));
00206 EXPECT_TRUE(follower->isActive());
00207
00208 follower->preempt();
00209 EXPECT_FALSE(follower->isActive());
00210 EXPECT_THROW(follower->getNextPoint(nextPoint, timeFromStart), std::runtime_error);
00211
00212 trajectory_msgs::JointTrajectory tempTraj;
00213 EXPECT_NO_THROW(follower->setTrajectory(tempTraj));
00214 EXPECT_FALSE(follower->isActive());
00215 EXPECT_THROW(follower->getNextPoint(nextPoint, timeFromStart), std::runtime_error);
00216 }
00217
00218 int main(int argc, char** argv)
00219 {
00220 testing::InitGoogleTest(&argc, argv);
00221 return RUN_ALL_TESTS();
00222 }