robodyn_controllers: JointTrajectoryFollower

Go to the documentation of this file.
00001 /*
00002  * JointTrajectoryFollower_Test.cpp
00003  *
00004  *  Created on: Sept 05, 2012
00005  *      Author: Ross Taylor
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; // pointer for easy destruction between tests
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 }