joint_states_settler: joint_states_settler: joint_states_settler

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 #include <gtest/gtest.h>
00036 
00037 #include <joint_states_settler/ConfigGoal.h>
00038 #include <joint_states_settler/joint_states_settler.h>
00039 
00040 using namespace std;
00041 using namespace joint_states_settler;
00042 
00043 // Number of samples
00044 static const unsigned int N = 9;
00045 
00046 // Number of channels/joints
00047 static const unsigned int C = 3;
00048 
00049 // List of joint names
00050 static const char* names[C] = { "A", "B", "C" };
00051 
00052 // List of joint positions
00053 static const double data[N][C] = { { 0,  0, 10},
00054                                    { 1,  1, 15},
00055                                    { 2,  2, 20},
00056                                    { 3,  3, 50},
00057                                    { 4,  4, 30},
00058                                    { 3,  5, 35},
00059                                    { 2,  6, 25},
00060                                    { 1,  7, 20},
00061                                    { 0,  8, 25 } };
00062 
00063 // 2 possible sets of timestamps for the data
00064 static const unsigned int times[N][2] = { { 0,  0 },
00065                                           { 1,  1 },
00066                                           { 2,  2 },
00067                                           { 3,  3 },
00068                                           { 4,  4 },
00069                                           { 5, 15 },
00070                                           { 6, 16 },
00071                                           { 7, 17 },
00072                                           { 8, 18 } };
00073 
00074 // add data to the settler, and see what intervals come out
00075 vector<calibration_msgs::Interval> addToSettler(JointStatesSettler& settler, unsigned int time_channel)
00076 {
00077   vector<calibration_msgs::Interval> intervals;
00078 
00079   for (unsigned int i=0; i<N; i++)
00080   {
00081     sensor_msgs::JointStatePtr msg(new sensor_msgs::JointState);
00082     msg->header.stamp = ros::Time(times[i][time_channel], 0);
00083 
00084     msg->name.resize(C);
00085     msg->position.resize(C);
00086     msg->velocity.resize(C);
00087     msg->effort.resize(C);
00088 
00089     for (unsigned int j=0; j<C; j++)
00090     {
00091       msg->name[j] = names[j];
00092       msg->position[j] = data[i][j];
00093     }
00094 
00095     intervals.push_back(settler.add(msg));
00096   }
00097 
00098   return intervals;
00099 }
00100 
00101 // Tight tol on A, and loose on C
00102 ConfigGoal config1()
00103 {
00104   ConfigGoal config;
00105   config.joint_names.clear();
00106   config.joint_names.push_back("A");
00107   config.joint_names.push_back("C");
00108 
00109   config.tolerances.clear();
00110   config.tolerances.push_back(2.5);
00111   config.tolerances.push_back(100);
00112 
00113   config.max_step = ros::Duration(2,0);
00114 
00115   config.cache_size = 100;
00116 
00117   return config;
00118 }
00119 
00120 // All loose tolerances
00121 ConfigGoal config2()
00122 {
00123   ConfigGoal config;
00124   config.joint_names.clear();
00125   config.joint_names.push_back("A");
00126   config.joint_names.push_back("B");
00127   config.joint_names.push_back("C");
00128 
00129   config.tolerances.clear();
00130   config.tolerances.push_back(100);
00131   config.tolerances.push_back(100);
00132   config.tolerances.push_back(100);
00133 
00134   config.max_step = ros::Duration(2,0);
00135 
00136   config.cache_size = 100;
00137 
00138   return config;
00139 }
00140 
00141 void doEasyCheck(const vector<calibration_msgs::Interval>& intervals)
00142 {
00143   ASSERT_EQ(intervals.size(), N);
00144   EXPECT_EQ(intervals[0].start.sec, (unsigned int) 0);
00145   EXPECT_EQ(intervals[0].end.sec,   (unsigned int) 0);
00146   EXPECT_EQ(intervals[1].start.sec, (unsigned int) 0);
00147   EXPECT_EQ(intervals[1].end.sec,   (unsigned int) 1);
00148   EXPECT_EQ(intervals[2].start.sec, (unsigned int) 0);
00149   EXPECT_EQ(intervals[2].end.sec,   (unsigned int) 2);
00150   EXPECT_EQ(intervals[3].start.sec, (unsigned int) 1);
00151   EXPECT_EQ(intervals[3].end.sec,   (unsigned int) 3);
00152   EXPECT_EQ(intervals[4].start.sec, (unsigned int) 2);
00153   EXPECT_EQ(intervals[4].end.sec,   (unsigned int) 4);
00154   EXPECT_EQ(intervals[5].start.sec, (unsigned int) 2);
00155   EXPECT_EQ(intervals[5].end.sec,   (unsigned int) 5);
00156   EXPECT_EQ(intervals[6].start.sec, (unsigned int) 2);
00157   EXPECT_EQ(intervals[6].end.sec,   (unsigned int) 6);
00158   EXPECT_EQ(intervals[7].start.sec, (unsigned int) 5);
00159   EXPECT_EQ(intervals[7].end.sec,   (unsigned int) 7);
00160   EXPECT_EQ(intervals[8].start.sec, (unsigned int) 6);
00161   EXPECT_EQ(intervals[8].end.sec,   (unsigned int) 8);
00162 }
00163 
00164 void doMaxStepCheck(const vector<calibration_msgs::Interval>& intervals)
00165 {
00166   ASSERT_EQ(intervals.size(), N);
00167   EXPECT_EQ(intervals[0].start.sec, (unsigned int) 0);
00168   EXPECT_EQ(intervals[0].end.sec,   (unsigned int) 0);
00169   EXPECT_EQ(intervals[1].start.sec, (unsigned int) 0);
00170   EXPECT_EQ(intervals[1].end.sec,   (unsigned int) 1);
00171   EXPECT_EQ(intervals[2].start.sec, (unsigned int) 0);
00172   EXPECT_EQ(intervals[2].end.sec,   (unsigned int) 2);
00173   EXPECT_EQ(intervals[3].start.sec, (unsigned int) 0);
00174   EXPECT_EQ(intervals[3].end.sec,   (unsigned int) 3);
00175   EXPECT_EQ(intervals[4].start.sec, (unsigned int) 0);
00176   EXPECT_EQ(intervals[4].end.sec,   (unsigned int) 4);
00177   EXPECT_EQ(intervals[5].start.sec, (unsigned int) 15);
00178   EXPECT_EQ(intervals[5].end.sec,   (unsigned int) 15);
00179   EXPECT_EQ(intervals[6].start.sec, (unsigned int) 15);
00180   EXPECT_EQ(intervals[6].end.sec,   (unsigned int) 16);
00181   EXPECT_EQ(intervals[7].start.sec, (unsigned int) 15);
00182   EXPECT_EQ(intervals[7].end.sec,   (unsigned int) 17);
00183   EXPECT_EQ(intervals[8].start.sec, (unsigned int) 15);
00184   EXPECT_EQ(intervals[8].end.sec,   (unsigned int) 18);
00185 }
00186 
00187 
00188 TEST(JointStatesSettler, easyCheck)
00189 {
00190   JointStatesSettler settler;
00191 
00192   bool config_result = settler.configure(config1());
00193   ASSERT_TRUE(config_result);
00194 
00195   vector<calibration_msgs::Interval> intervals = addToSettler(settler, 0);
00196 
00197   doEasyCheck(intervals);
00198 }
00199 
00200 TEST(JointStatesSettler, maxStepCheck)
00201 {
00202   JointStatesSettler settler;
00203 
00204   bool config_result = settler.configure(config2());
00205   ASSERT_TRUE(config_result);
00206 
00207   vector<calibration_msgs::Interval> intervals = addToSettler(settler, 1);
00208 
00209   doMaxStepCheck(intervals);
00210 }
00211 
00212 TEST(JointStatesSettler, reconfigureCheck)
00213 {
00214   JointStatesSettler settler;
00215 
00216   bool config_result = settler.configure(config1());
00217   ASSERT_TRUE(config_result);
00218 
00219   vector<calibration_msgs::Interval> intervals = addToSettler(settler, 0);
00220 
00221   doEasyCheck(intervals);
00222 
00223   config_result = settler.configure(config2());
00224   ASSERT_TRUE(config_result);
00225 
00226   intervals = addToSettler(settler, 1);
00227 
00228   doMaxStepCheck(intervals);
00229 }
00230 
00231 
00232 int main(int argc, char **argv)
00233 {
00234   testing::InitGoogleTest(&argc, argv);
00235   return RUN_ALL_TESTS();
00236 }
joint_states_settler_unittest.cpp
