joint_states_settler_unittest.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 #include <gtest/gtest.h>
36 
37 #include <joint_states_settler/ConfigGoal.h>
39 
40 using namespace std;
41 using namespace joint_states_settler;
42 
43 // Number of samples
44 static const unsigned int N = 9;
45 
46 // Number of channels/joints
47 static const unsigned int C = 3;
48 
49 // List of joint names
50 static const char* names[C] = { "A", "B", "C" };
51 
52 // List of joint positions
53 static const double data[N][C] = { { 0, 0, 10},
54  { 1, 1, 15},
55  { 2, 2, 20},
56  { 3, 3, 50},
57  { 4, 4, 30},
58  { 3, 5, 35},
59  { 2, 6, 25},
60  { 1, 7, 20},
61  { 0, 8, 25 } };
62 
63 // 2 possible sets of timestamps for the data
64 static const unsigned int times[N][2] = { { 0, 0 },
65  { 1, 1 },
66  { 2, 2 },
67  { 3, 3 },
68  { 4, 4 },
69  { 5, 15 },
70  { 6, 16 },
71  { 7, 17 },
72  { 8, 18 } };
73 
74 // add data to the settler, and see what intervals come out
75 vector<calibration_msgs::Interval> addToSettler(JointStatesSettler& settler, unsigned int time_channel)
76 {
77  vector<calibration_msgs::Interval> intervals;
78 
79  for (unsigned int i=0; i<N; i++)
80  {
81  sensor_msgs::JointStatePtr msg(new sensor_msgs::JointState);
82  msg->header.stamp = ros::Time(times[i][time_channel], 0);
83 
84  msg->name.resize(C);
85  msg->position.resize(C);
86  msg->velocity.resize(C);
87  msg->effort.resize(C);
88 
89  for (unsigned int j=0; j<C; j++)
90  {
91  msg->name[j] = names[j];
92  msg->position[j] = data[i][j];
93  }
94 
95  intervals.push_back(settler.add(msg));
96  }
97 
98  return intervals;
99 }
100 
101 // Tight tol on A, and loose on C
102 ConfigGoal config1()
103 {
104  ConfigGoal config;
105  config.joint_names.clear();
106  config.joint_names.push_back("A");
107  config.joint_names.push_back("C");
108 
109  config.tolerances.clear();
110  config.tolerances.push_back(2.5);
111  config.tolerances.push_back(100);
112 
113  config.max_step = ros::Duration(2,0);
114 
115  config.cache_size = 100;
116 
117  return config;
118 }
119 
120 // All loose tolerances
121 ConfigGoal config2()
122 {
123  ConfigGoal config;
124  config.joint_names.clear();
125  config.joint_names.push_back("A");
126  config.joint_names.push_back("B");
127  config.joint_names.push_back("C");
128 
129  config.tolerances.clear();
130  config.tolerances.push_back(100);
131  config.tolerances.push_back(100);
132  config.tolerances.push_back(100);
133 
134  config.max_step = ros::Duration(2,0);
135 
136  config.cache_size = 100;
137 
138  return config;
139 }
140 
141 void doEasyCheck(const vector<calibration_msgs::Interval>& intervals)
142 {
143  ASSERT_EQ(intervals.size(), N);
144  EXPECT_EQ(intervals[0].start.sec, (unsigned int) 0);
145  EXPECT_EQ(intervals[0].end.sec, (unsigned int) 0);
146  EXPECT_EQ(intervals[1].start.sec, (unsigned int) 0);
147  EXPECT_EQ(intervals[1].end.sec, (unsigned int) 1);
148  EXPECT_EQ(intervals[2].start.sec, (unsigned int) 0);
149  EXPECT_EQ(intervals[2].end.sec, (unsigned int) 2);
150  EXPECT_EQ(intervals[3].start.sec, (unsigned int) 1);
151  EXPECT_EQ(intervals[3].end.sec, (unsigned int) 3);
152  EXPECT_EQ(intervals[4].start.sec, (unsigned int) 2);
153  EXPECT_EQ(intervals[4].end.sec, (unsigned int) 4);
154  EXPECT_EQ(intervals[5].start.sec, (unsigned int) 2);
155  EXPECT_EQ(intervals[5].end.sec, (unsigned int) 5);
156  EXPECT_EQ(intervals[6].start.sec, (unsigned int) 2);
157  EXPECT_EQ(intervals[6].end.sec, (unsigned int) 6);
158  EXPECT_EQ(intervals[7].start.sec, (unsigned int) 5);
159  EXPECT_EQ(intervals[7].end.sec, (unsigned int) 7);
160  EXPECT_EQ(intervals[8].start.sec, (unsigned int) 6);
161  EXPECT_EQ(intervals[8].end.sec, (unsigned int) 8);
162 }
163 
164 void doMaxStepCheck(const vector<calibration_msgs::Interval>& intervals)
165 {
166  ASSERT_EQ(intervals.size(), N);
167  EXPECT_EQ(intervals[0].start.sec, (unsigned int) 0);
168  EXPECT_EQ(intervals[0].end.sec, (unsigned int) 0);
169  EXPECT_EQ(intervals[1].start.sec, (unsigned int) 0);
170  EXPECT_EQ(intervals[1].end.sec, (unsigned int) 1);
171  EXPECT_EQ(intervals[2].start.sec, (unsigned int) 0);
172  EXPECT_EQ(intervals[2].end.sec, (unsigned int) 2);
173  EXPECT_EQ(intervals[3].start.sec, (unsigned int) 0);
174  EXPECT_EQ(intervals[3].end.sec, (unsigned int) 3);
175  EXPECT_EQ(intervals[4].start.sec, (unsigned int) 0);
176  EXPECT_EQ(intervals[4].end.sec, (unsigned int) 4);
177  EXPECT_EQ(intervals[5].start.sec, (unsigned int) 15);
178  EXPECT_EQ(intervals[5].end.sec, (unsigned int) 15);
179  EXPECT_EQ(intervals[6].start.sec, (unsigned int) 15);
180  EXPECT_EQ(intervals[6].end.sec, (unsigned int) 16);
181  EXPECT_EQ(intervals[7].start.sec, (unsigned int) 15);
182  EXPECT_EQ(intervals[7].end.sec, (unsigned int) 17);
183  EXPECT_EQ(intervals[8].start.sec, (unsigned int) 15);
184  EXPECT_EQ(intervals[8].end.sec, (unsigned int) 18);
185 }
186 
187 
189 {
190  JointStatesSettler settler;
191 
192  bool config_result = settler.configure(config1());
193  ASSERT_TRUE(config_result);
194 
195  vector<calibration_msgs::Interval> intervals = addToSettler(settler, 0);
196 
197  doEasyCheck(intervals);
198 }
199 
200 TEST(JointStatesSettler, maxStepCheck)
201 {
202  JointStatesSettler settler;
203 
204  bool config_result = settler.configure(config2());
205  ASSERT_TRUE(config_result);
206 
207  vector<calibration_msgs::Interval> intervals = addToSettler(settler, 1);
208 
209  doMaxStepCheck(intervals);
210 }
211 
212 TEST(JointStatesSettler, reconfigureCheck)
213 {
214  JointStatesSettler settler;
215 
216  bool config_result = settler.configure(config1());
217  ASSERT_TRUE(config_result);
218 
219  vector<calibration_msgs::Interval> intervals = addToSettler(settler, 0);
220 
221  doEasyCheck(intervals);
222 
223  config_result = settler.configure(config2());
224  ASSERT_TRUE(config_result);
225 
226  intervals = addToSettler(settler, 1);
227 
228  doMaxStepCheck(intervals);
229 }
230 
231 
232 int main(int argc, char **argv)
233 {
234  testing::InitGoogleTest(&argc, argv);
235  return RUN_ALL_TESTS();
236 }
ROSCPP_DECL void start()
void doMaxStepCheck(const vector< calibration_msgs::Interval > &intervals)
TEST(JointStatesSettler, easyCheck)
void doEasyCheck(const vector< calibration_msgs::Interval > &intervals)
bool configure(const joint_states_settler::ConfigGoal &goal)
calibration_msgs::Interval add(const sensor_msgs::JointStateConstPtr msg)
int main(int argc, char **argv)
static const unsigned int C
ConfigGoal config2()
static const unsigned int times[N][2]
vector< calibration_msgs::Interval > addToSettler(JointStatesSettler &settler, unsigned int time_channel)
ConfigGoal config1()
static const double data[N][C]
static const unsigned int N
static const char * names[C]


joint_states_settler
Author(s): Vijay Pradeep, Eitan Marder-Eppstein
autogenerated on Thu Jun 6 2019 19:17:25