test_motion_model.cpp
Go to the documentation of this file.
1 #include <play_motion_builder/motion_model.h>
2 #include <ros/ros.h>
3 #include <gtest/gtest.h>
4 #include <fstream>
5 
6 namespace pal
7 {
8 TEST(MotionBuilderModelTest, jointLimitTest)
9 {
10  JointModel jm; // Empty constructor defined
11  JointModel jm2(-1.0, 1.0); // Constructor with values
12 
13  double valid_pos = 0.0;
14  double too_low_pos = -2.0;
15  double too_big_pos = 2.0;
16 
17  EXPECT_TRUE(jm2.inLimits(valid_pos));
18  EXPECT_FALSE(jm2.inLimits(too_low_pos));
19  EXPECT_FALSE(jm2.inLimits(too_big_pos));
20 }
21 
22 TEST(MotionBuilderModelTest, keyframeTest)
23 {
24  // Empty keyframe
25  KeyFrame k(Motion::DEFAULT_TIME);
26 
27  k.addPosition("Test1", 1.0);
28 
29  EXPECT_EQ(Motion::DEFAULT_TIME, k.getTime());
30  EXPECT_EQ(1.0, k.getJointPosition("Test1"));
31  EXPECT_TRUE(std::isnan(k.getJointPosition("Test2")));
32 
33  // copy constructor
34  KeyFrame k2(k);
35 
36  EXPECT_EQ(k.getTime(), k2.getTime());
37  EXPECT_EQ(1.0, k2.getJointPosition("Test1"));
38  EXPECT_TRUE(std::isnan(k2.getJointPosition("Test2")));
39 
40  k2.addPosition("Test2", 1.0);
41  std::map<std::string, bool> used;
42  used["Test1"] = false;
43  used["Test2"] = true;
44  k2.cleanUnused(used);
45 
46  EXPECT_TRUE(std::isnan(k2.getJointPosition("Test1")));
47  EXPECT_EQ(1.0, k2.getJointPosition("Test2"));
48 
49  k2.addPosition("Test1", 1.0);
50 
51  std::vector<std::string> joint_list;
52  joint_list.push_back("Test1");
53 
54  // Test print point element
55  YAML::Emitter em;
56  em << k2.print(0.0, 1.0, joint_list); // Basic copy;
57  EXPECT_EQ("time_from_start: 5\npositions: [1]", std::string(em.c_str()));
58 
59  YAML::Emitter em2;
60  em2 << k2.print(1.0, 1.0, joint_list); // No downshift, basetime 1
61  EXPECT_EQ("time_from_start: 6\npositions: [1]", std::string(em2.c_str()));
62 
63  YAML::Emitter em3;
64  em3 << k2.print(0.0, 2.0, joint_list); // Downshift 2, basetime 0
65  EXPECT_EQ("time_from_start: 10\npositions: [1]", std::string(em3.c_str()));
66 
67  YAML::Emitter em4;
68  em4 << k2.print(1.0, 2.0, joint_list); // Downshift 2, basetime 1
69  EXPECT_EQ("time_from_start: 11\npositions: [1]", std::string(em4.c_str()));
70 
71  // Test print motion to frame
72  YAML::Emitter em5;
73  em5 << k2.print(joint_list);
74  EXPECT_EQ("joints: [Test1]\npoints:\n - time_from_start: 0\n "
75  "positions: [1]",
76  std::string(em5.c_str()));
77 }
78 
79 TEST(MotionBuilderModelTest, motionTest)
80 {
81  Motion m("", "", {});
82 
83  // Test loading joints
84  EXPECT_FALSE(m.jointsLoaded());
85  for (int i = 0; i < 3; ++i)
86  {
87  JointModel jm(0.0, 3.14159226);
88  m.addJointModel("Test_joint_" + std::to_string(i + 1), jm);
89  }
90  EXPECT_FALSE(m.jointsLoaded());
91  // Test loading groups
92  for (int i = 0; i < 3; ++i)
93  {
94  m.addJointToGroup("TestGroup", "Test_joint_" + std::to_string(i + 1));
95  }
96  EXPECT_TRUE(m.jointsLoaded());
97  m.setCurrentGroup("TestGroup");
98  EXPECT_EQ(3, m.getJoints().size());
99 
100  // Test loading frames
101  sensor_msgs::JointState js1;
102  js1.name.push_back("Test_joint_1");
103  js1.position.push_back(1.1);
104  js1.name.push_back("Test_joint_2");
105  js1.position.push_back(1.2);
106  js1.name.push_back("Test_joint_3");
107  js1.position.push_back(1.3);
108  js1.name.push_back("Test_joint_4");
109  js1.position.push_back(1.4);
110  sensor_msgs::JointStateConstPtr js1c(new sensor_msgs::JointState(js1));
111  m.addKeyFrame(js1c); // Add with default time
112  EXPECT_EQ(0.0, m.getKeyFrame(0).getTime()) << "Should be 0 as it's first "
113  "frame";
114  EXPECT_EQ(1.1, m.getKeyFrame(0).getJointPosition("Test_joint_1"));
115  EXPECT_EQ(1.2, m.getKeyFrame(0).getJointPosition("Test_joint_2"));
116  EXPECT_EQ(1.3, m.getKeyFrame(0).getJointPosition("Test_joint_3"));
117  EXPECT_TRUE(std::isnan(m.getKeyFrame(0).getJointPosition("Test_joint_4")))
118  << "Should be NaN as it is not on used list";
119 
120  m.addKeyFrame(js1c); // Add with default time
121  EXPECT_EQ(Motion::DEFAULT_TIME, m.getKeyFrame(1).getTime());
122  EXPECT_EQ(1.1, m.getKeyFrame(1).getJointPosition("Test_joint_1"));
123  EXPECT_EQ(1.2, m.getKeyFrame(1).getJointPosition("Test_joint_2"));
124  EXPECT_EQ(1.3, m.getKeyFrame(1).getJointPosition("Test_joint_3"));
125  EXPECT_TRUE(std::isnan(m.getKeyFrame(1).getJointPosition("Test_joint_4")))
126  << "Should be NaN as it is not on used list";
127 
128  // Test updating a frame
129  sensor_msgs::JointState js2;
130  js2.name.push_back("Test_joint_1");
131  js2.position.push_back(2.1);
132  js2.name.push_back("Test_joint_2");
133  js2.position.push_back(2.2);
134  js2.name.push_back("Test_joint_3");
135  js2.position.push_back(2.3);
136  js2.name.push_back("Test_joint_4");
137  js2.position.push_back(2.4);
138  sensor_msgs::JointStateConstPtr js2c(new sensor_msgs::JointState(js2));
139 
140  m.updateKeyFrame(js2c, 1);
141  EXPECT_EQ(0.0, m.getKeyFrame(0).getTime()) << "Should be 0 as it's first "
142  "frame";
143  EXPECT_EQ(1.1, m.getKeyFrame(0).getJointPosition("Test_joint_1"));
144  EXPECT_EQ(1.2, m.getKeyFrame(0).getJointPosition("Test_joint_2"));
145  EXPECT_EQ(1.3, m.getKeyFrame(0).getJointPosition("Test_joint_3"));
146  EXPECT_TRUE(std::isnan(m.getKeyFrame(0).getJointPosition("Test_joint_4")))
147  << "Should be NaN as it is not used";
148  EXPECT_EQ(Motion::DEFAULT_TIME, m.getKeyFrame(1).getTime());
149  EXPECT_EQ(2.1, m.getKeyFrame(1).getJointPosition("Test_joint_1"));
150  EXPECT_EQ(2.2, m.getKeyFrame(1).getJointPosition("Test_joint_2"));
151  EXPECT_EQ(2.3, m.getKeyFrame(1).getJointPosition("Test_joint_3"));
152  EXPECT_TRUE(std::isnan(m.getKeyFrame(1).getJointPosition("Test_joint_4")))
153  << "Should be NaN as it is not used";
154 
155  // Test changing the time
156  EXPECT_THROW(m.changeTime(5, 1.0), ros::Exception) << "Trying to modify a non-existing "
157  "frame should result in an Exception";
158  m.changeTime(1, 2.0);
159  EXPECT_EQ(2.0, m.getKeyFrame(1).getTime());
160 
161  // Test changing joint positions
162  EXPECT_THROW(m.changeJoint(5, "", 1.0), ros::Exception)
163  << "Trying to modify a non-existing frame should result in an Exception";
164  EXPECT_THROW(m.changeJoint(1, "Non-existing", 1.0), ros::Exception)
165  << "Trying to modify a non-existing joint should result in an Exception";
166  double res = m.changeJoint(1, "Test_joint_1", 5000.0);
167  EXPECT_EQ(2.1, res);
168  EXPECT_EQ(2.1, m.getKeyFrame(1).getJointPosition("Test_joint_1"))
169  << "If value not in joint limits no change";
170  res = m.changeJoint(1, "Test_joint_1", 3.1);
171  EXPECT_EQ(3.1, res);
172  EXPECT_EQ(3.1, m.getKeyFrame(1).getJointPosition("Test_joint_1"));
173 
174  // Test copying frames
175  m.copyFrame(0); // Copy to last
176  EXPECT_EQ(0.0, m.getKeyFrame(0).getTime()) << "Should be 0 as it's first frame";
177  EXPECT_EQ(1.1, m.getKeyFrame(0).getJointPosition("Test_joint_1"));
178  EXPECT_EQ(1.2, m.getKeyFrame(0).getJointPosition("Test_joint_2"));
179  EXPECT_EQ(1.3, m.getKeyFrame(0).getJointPosition("Test_joint_3"));
180  EXPECT_TRUE(std::isnan(m.getKeyFrame(0).getJointPosition("Test_joint_4")))
181  << "Should be NaN as it is not used";
182  EXPECT_EQ(2.0, m.getKeyFrame(1).getTime());
183  EXPECT_EQ(3.1, m.getKeyFrame(1).getJointPosition("Test_joint_1"));
184  EXPECT_EQ(2.2, m.getKeyFrame(1).getJointPosition("Test_joint_2"));
185  EXPECT_EQ(2.3, m.getKeyFrame(1).getJointPosition("Test_joint_3"));
186  EXPECT_TRUE(std::isnan(m.getKeyFrame(1).getJointPosition("Test_joint_4")))
187  << "Should be NaN as it is not used";
188  EXPECT_EQ(Motion::DEFAULT_TIME, m.getKeyFrame(2).getTime());
189  EXPECT_EQ(1.1, m.getKeyFrame(2).getJointPosition("Test_joint_1"));
190  EXPECT_EQ(1.2, m.getKeyFrame(2).getJointPosition("Test_joint_2"));
191  EXPECT_EQ(1.3, m.getKeyFrame(2).getJointPosition("Test_joint_3"));
192  EXPECT_TRUE(std::isnan(m.getKeyFrame(2).getJointPosition("Test_joint_4")))
193  << "Should be NaN as it is not used";
194 
195  m.copyFrame(1, 2); // Copy frame 1 as frame 2
196  EXPECT_EQ(0.0, m.getKeyFrame(0).getTime()) << "Should be 0 as it's first frame";
197  EXPECT_EQ(1.1, m.getKeyFrame(0).getJointPosition("Test_joint_1"));
198  EXPECT_EQ(1.2, m.getKeyFrame(0).getJointPosition("Test_joint_2"));
199  EXPECT_EQ(1.3, m.getKeyFrame(0).getJointPosition("Test_joint_3"));
200  EXPECT_TRUE(std::isnan(m.getKeyFrame(0).getJointPosition("Test_joint_4")))
201  << "Should be NaN as it is not used";
202  EXPECT_EQ(2.0, m.getKeyFrame(1).getTime());
203  EXPECT_EQ(3.1, m.getKeyFrame(1).getJointPosition("Test_joint_1"));
204  EXPECT_EQ(2.2, m.getKeyFrame(1).getJointPosition("Test_joint_2"));
205  EXPECT_EQ(2.3, m.getKeyFrame(1).getJointPosition("Test_joint_3"));
206  EXPECT_TRUE(std::isnan(m.getKeyFrame(1).getJointPosition("Test_joint_4")))
207  << "Should be NaN as it is not used";
208  EXPECT_EQ(2.0, m.getKeyFrame(2).getTime());
209  EXPECT_EQ(3.1, m.getKeyFrame(2).getJointPosition("Test_joint_1"));
210  EXPECT_EQ(2.2, m.getKeyFrame(2).getJointPosition("Test_joint_2"));
211  EXPECT_EQ(2.3, m.getKeyFrame(2).getJointPosition("Test_joint_3"));
212  EXPECT_TRUE(std::isnan(m.getKeyFrame(2).getJointPosition("Test_joint_4")))
213  << "Should be NaN as it is not used";
214  EXPECT_EQ(Motion::DEFAULT_TIME, m.getKeyFrame(3).getTime());
215  EXPECT_EQ(1.1, m.getKeyFrame(3).getJointPosition("Test_joint_1"));
216  EXPECT_EQ(1.2, m.getKeyFrame(3).getJointPosition("Test_joint_2"));
217  EXPECT_EQ(1.3, m.getKeyFrame(3).getJointPosition("Test_joint_3"));
218  EXPECT_TRUE(std::isnan(m.getKeyFrame(3).getJointPosition("Test_joint_4")))
219  << "Should be NaN as it is not used";
220 
221  // Test removing frames
222  EXPECT_EQ(4, m.size());
223  m.removeKeyFrame(1);
224  m.removeKeyFrame(1); // Remove 2 keyframes
225  EXPECT_EQ(0.0, m.getKeyFrame(0).getTime()) << "Should be 0 as it's first frame";
226  EXPECT_EQ(1.1, m.getKeyFrame(0).getJointPosition("Test_joint_1"));
227  EXPECT_EQ(1.2, m.getKeyFrame(0).getJointPosition("Test_joint_2"));
228  EXPECT_EQ(1.3, m.getKeyFrame(0).getJointPosition("Test_joint_3"));
229  EXPECT_TRUE(std::isnan(m.getKeyFrame(0).getJointPosition("Test_joint_4")))
230  << "Should be NaN as it is not used";
231  EXPECT_EQ(Motion::DEFAULT_TIME, m.getKeyFrame(1).getTime());
232  EXPECT_EQ(1.1, m.getKeyFrame(1).getJointPosition("Test_joint_1"));
233  EXPECT_EQ(1.2, m.getKeyFrame(1).getJointPosition("Test_joint_2"));
234  EXPECT_EQ(1.3, m.getKeyFrame(1).getJointPosition("Test_joint_3"));
235  EXPECT_TRUE(std::isnan(m.getKeyFrame(1).getJointPosition("Test_joint_4")))
236  << "Should be NaN as it is not used";
237  EXPECT_EQ(2, m.size());
238 
239  // Test transformation to yaml
240  m.changeTime(1, 2.0);
241  m.addKeyFrame(js1c);
242 
243  YAML::Emitter em;
244  em << m.print(); // Basic output
245  EXPECT_EQ("joints: [Test_joint_1, Test_joint_2, Test_joint_3]\n"
246  "points:\n "
247  "- time_from_start: 0\n "
248  "positions: [1.1, 1.2, 1.3]\n "
249  "- time_from_start: 2\n "
250  "positions: [1.1, 1.2, 1.3]\n "
251  "- time_from_start: 7\n "
252  "positions: [1.1, 1.2, 1.3]",
253  std::string(em.c_str()));
254 
255  YAML::Emitter em2;
256  em2 << m.print(2.0); // Downshift 2
257  EXPECT_EQ("joints: [Test_joint_1, Test_joint_2, Test_joint_3]\n"
258  "points:\n "
259  "- time_from_start: 0\n "
260  "positions: [1.1, 1.2, 1.3]\n "
261  "- time_from_start: 4\n "
262  "positions: [1.1, 1.2, 1.3]\n "
263  "- time_from_start: 14\n "
264  "positions: [1.1, 1.2, 1.3]",
265  std::string(em2.c_str()));
266 }
267 
268 template <typename InputIterator1, typename InputIterator2>
269 bool range_equal(InputIterator1 first1, InputIterator1 last1, InputIterator2 first2,
270  InputIterator2 last2)
271 {
272  while (first1 != last1 && first2 != last2)
273  {
274  if (*first1 != *first2)
275  return false;
276  ++first1;
277  ++first2;
278  }
279  return (first1 == last1) && (first2 == last2);
280 }
281 
282 bool compare_files(const std::string& filename1, const std::string& filename2)
283 {
284  std::ifstream file1(filename1);
285  std::ifstream file2(filename2);
286 
287  std::istreambuf_iterator<char> begin1(file1);
288  std::istreambuf_iterator<char> begin2(file2);
289 
290  std::istreambuf_iterator<char> end;
291 
292  return range_equal(begin1, end, begin2, end);
293 }
294 
295 TEST(MotionBuilderModelTest, motionFromParamTest)
296 {
297  XmlRpc::XmlRpcValue param_to_load;
298  ros::NodeHandle nh;
299  nh.getParam("/play_motion/motions/test_motion", param_to_load);
300 
301  ASSERT_EQ(XmlRpc::XmlRpcValue::TypeStruct, param_to_load.getType());
302  ASSERT_EQ(XmlRpc::XmlRpcValue::TypeArray, param_to_load["joints"].getType());
303  ASSERT_EQ(XmlRpc::XmlRpcValue::TypeArray, param_to_load["points"].getType());
304 
305  Motion m(param_to_load,
306  "<robot name=\"testbot\"><joint name=\"arm_left_1_joint\" type=\"revolute\"/>"
307  "<joint name=\"arm_left_2_joint\" type=\"revolute\"/>"
308  "<joint name=\"arm_left_3_joint\" type=\"revolute\"/>"
309  "<joint name=\"arm_left_4_joint\" type=\"revolute\"/>"
310  "<joint name=\"arm_left_5_joint\" type=\"revolute\"/>"
311  "<joint name=\"arm_left_6_joint\" type=\"revolute\"/>"
312  "<joint name=\"arm_left_7_joint\" type=\"revolute\"/>"
313  "<joint name=\"arm_right_1_joint\" type=\"revolute\"/>"
314  "<joint name=\"arm_right_2_joint\" type=\"revolute\"/>"
315  "<joint name=\"arm_right_3_joint\" type=\"revolute\"/>"
316  "<joint name=\"arm_right_4_joint\" type=\"revolute\"/>"
317  "<joint name=\"arm_right_5_joint\" type=\"revolute\"/>"
318  "<joint name=\"arm_right_6_joint\" type=\"revolute\"/>"
319  "<joint name=\"arm_right_7_joint\" type=\"revolute\"/>"
320  "<joint name=\"torso_1_joint\" type=\"revolute\"/>"
321  "<joint name=\"torso_2_joint\" type=\"revolute\"/></robot>",
322  "<?xml version=\"1.0\" ?><robot name=\"testbot\"><group name=\"arm_left\">"
323  "<joint name=\"arm_left_1_joint\" /><joint name=\"arm_left_2_joint\" />"
324  "<joint name=\"arm_left_3_joint\"/><joint name=\"arm_left_4_joint\" />"
325  "<joint name=\"arm_left_5_joint\"/><joint name=\"arm_left_6_joint\"/>"
326  "<joint name=\"arm_left_7_joint\"/></group><group name=\"arm_right\">"
327  "<joint name=\"arm_right_1_joint\" /><joint name=\"arm_right_2_joint\" />"
328  "<joint name=\"arm_right_3_joint\" /><joint name=\"arm_right_4_joint\" />"
329  "<joint name=\"arm_right_5_joint\"/><joint name=\"arm_right_6_joint\"/>"
330  "<joint name=\"arm_right_7_joint\"/></group><group name=\"torso\">"
331  "<joint name=\"torso_1_joint\"/><joint name=\"torso_2_joint\"/></group>"
332  "<group name=\"both_arms\"><group name=\"arm_left\" /><group name=\"arm_right\" />"
333  "</group><group name=\"both_arms_torso\"><group name=\"arm_left\" />"
334  "<group name=\"arm_right\" /><group name=\"torso\" /></group></robot>",
335  { "head_1_joint", "head_2_joint" });
336 
337  // Check correct group selected
338  EXPECT_EQ("both_arms_torso", m.getCurrentGroup());
339 
340  // Validate groups
341  m.setCurrentGroup("arm_left");
342  EXPECT_EQ(7, m.getJoints().size());
343  EXPECT_EQ("arm_left_1_joint", m.getJoints()[0]);
344  EXPECT_EQ("arm_left_2_joint", m.getJoints()[1]);
345  EXPECT_EQ("arm_left_3_joint", m.getJoints()[2]);
346  EXPECT_EQ("arm_left_4_joint", m.getJoints()[3]);
347  EXPECT_EQ("arm_left_5_joint", m.getJoints()[4]);
348  EXPECT_EQ("arm_left_6_joint", m.getJoints()[5]);
349  EXPECT_EQ("arm_left_7_joint", m.getJoints()[6]);
350 
351  m.setCurrentGroup("arm_right");
352  EXPECT_EQ(7, m.getJoints().size());
353  EXPECT_EQ("arm_right_1_joint", m.getJoints()[0]);
354  EXPECT_EQ("arm_right_2_joint", m.getJoints()[1]);
355  EXPECT_EQ("arm_right_3_joint", m.getJoints()[2]);
356  EXPECT_EQ("arm_right_4_joint", m.getJoints()[3]);
357  EXPECT_EQ("arm_right_5_joint", m.getJoints()[4]);
358  EXPECT_EQ("arm_right_6_joint", m.getJoints()[5]);
359  EXPECT_EQ("arm_right_7_joint", m.getJoints()[6]);
360 
361  m.setCurrentGroup("torso");
362  EXPECT_EQ(2, m.getJoints().size());
363  EXPECT_EQ("torso_1_joint", m.getJoints()[0]);
364  EXPECT_EQ("torso_2_joint", m.getJoints()[1]);
365 
366  m.setCurrentGroup("both_arms");
367  EXPECT_EQ(14, m.getJoints().size());
368  EXPECT_EQ("arm_left_1_joint", m.getJoints()[0]);
369  EXPECT_EQ("arm_left_2_joint", m.getJoints()[1]);
370  EXPECT_EQ("arm_left_3_joint", m.getJoints()[2]);
371  EXPECT_EQ("arm_left_4_joint", m.getJoints()[3]);
372  EXPECT_EQ("arm_left_5_joint", m.getJoints()[4]);
373  EXPECT_EQ("arm_left_6_joint", m.getJoints()[5]);
374  EXPECT_EQ("arm_left_7_joint", m.getJoints()[6]);
375  EXPECT_EQ("arm_right_1_joint", m.getJoints()[7]);
376  EXPECT_EQ("arm_right_2_joint", m.getJoints()[8]);
377  EXPECT_EQ("arm_right_3_joint", m.getJoints()[9]);
378  EXPECT_EQ("arm_right_4_joint", m.getJoints()[10]);
379  EXPECT_EQ("arm_right_5_joint", m.getJoints()[11]);
380  EXPECT_EQ("arm_right_6_joint", m.getJoints()[12]);
381  EXPECT_EQ("arm_right_7_joint", m.getJoints()[13]);
382 
383  m.setCurrentGroup("both_arms_torso");
384  EXPECT_EQ(16, m.getJoints().size());
385  EXPECT_EQ("arm_left_1_joint", m.getJoints()[0]);
386  EXPECT_EQ("arm_left_2_joint", m.getJoints()[1]);
387  EXPECT_EQ("arm_left_3_joint", m.getJoints()[2]);
388  EXPECT_EQ("arm_left_4_joint", m.getJoints()[3]);
389  EXPECT_EQ("arm_left_5_joint", m.getJoints()[4]);
390  EXPECT_EQ("arm_left_6_joint", m.getJoints()[5]);
391  EXPECT_EQ("arm_left_7_joint", m.getJoints()[6]);
392  EXPECT_EQ("arm_right_1_joint", m.getJoints()[7]);
393  EXPECT_EQ("arm_right_2_joint", m.getJoints()[8]);
394  EXPECT_EQ("arm_right_3_joint", m.getJoints()[9]);
395  EXPECT_EQ("arm_right_4_joint", m.getJoints()[10]);
396  EXPECT_EQ("arm_right_5_joint", m.getJoints()[11]);
397  EXPECT_EQ("arm_right_6_joint", m.getJoints()[12]);
398  EXPECT_EQ("arm_right_7_joint", m.getJoints()[13]);
399  EXPECT_EQ("torso_1_joint", m.getJoints()[14]);
400  EXPECT_EQ("torso_2_joint", m.getJoints()[15]);
401 
402  EXPECT_EQ(4, m.size()) << "There should be 4 keyframes";
403 
404  EXPECT_EQ(0.0, m.getKeyFrame(0).getTime());
405  EXPECT_EQ(3.0, m.getKeyFrame(1).getTime());
406  EXPECT_EQ(3.0, m.getKeyFrame(2).getTime());
407  EXPECT_EQ(3.0, m.getKeyFrame(3).getTime());
408 
409  // Check extends works properly
410  sensor_msgs::JointState js;
411  js.name.push_back("head_1_joint");
412  js.position.push_back(0.0);
413  js.name.push_back("head_2_joint");
414  js.position.push_back(0.0);
415  js.name.push_back("");
416  sensor_msgs::JointStateConstPtr jsc(new sensor_msgs::JointState(js));
417 
418  JointModel jm(0.0, 3.14159226);
419  m.addJointModel("head_1_joint", jm); // Not used
420  m.addJointModel("head_2_joint", jm); // Not used
421  EXPECT_FALSE(m.isJointUsed("head_1_joint"));
422  EXPECT_FALSE(m.isJointUsed("head_2_joint"));
423 
424  m.extendFrames(jsc);
425  EXPECT_FALSE(std::isnan(m.getKeyFrame(0).getJointPosition("head_1_joint")));
426  EXPECT_FALSE(std::isnan(m.getKeyFrame(0).getJointPosition("head_2_joint")));
427  EXPECT_FALSE(std::isnan(m.getKeyFrame(3).getJointPosition("head_1_joint")));
428  EXPECT_FALSE(std::isnan(m.getKeyFrame(3).getJointPosition("head_2_joint")));
429 
430  // Output to tmp file to check
431  YAML::Emitter em;
432  em << YAML::BeginMap << YAML::Key << "play_motion" << YAML::Value << YAML::BeginMap
433  << YAML::Key << "motions" << YAML::Value << YAML::BeginMap << YAML::Key
434  << "test_motion" << YAML::Value
435  << m.print(param_to_load["meta"]["name"], param_to_load["meta"]["usage"],
436  param_to_load["meta"]["description"])
437  << YAML::EndMap << YAML::EndMap << YAML::EndMap;
438 
439  std::ofstream ofile("/tmp/tm.yaml");
440  ofile << em.c_str();
441  ofile.close();
442 
443  // Compare files
444  std::string filepath;
445  nh.getParam("test_file_path", filepath);
446  EXPECT_TRUE(compare_files("/tmp/tm.yaml", filepath));
447 }
448 } // namespace pal
449 
450 int main(int argc, char** argv)
451 {
452  ros::init(argc, argv, "test_motion_model");
453  ros::start();
454  testing::InitGoogleTest(&argc, argv);
455  return RUN_ALL_TESTS();
456 }
ros::NodeHandle
ros::start
ROSCPP_DECL void start()
pal::KeyFrame::addPosition
void addPosition(const std::string &name, double position)
Definition: motion_model.cpp:49
motion_model.h
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
pal::compare_files
bool compare_files(const std::string &filename1, const std::string &filename2)
Definition: test_motion_builder.cpp:86
pal::Motion::setCurrentGroup
bool setCurrentGroup(const std::string &group)
Definition: motion_model.h:218
XmlRpc::XmlRpcValue::getType
Type const & getType() const
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
pal::KeyFrame::print
PrintPoint print(double basetime, double downshift, const std::vector< std::string > &names) const
Definition: motion_model.cpp:86
pal::range_equal
bool range_equal(InputIterator1 first1, InputIterator1 last1, InputIterator2 first2, InputIterator2 last2)
Definition: test_motion_builder.cpp:73
pal::KeyFrame::cleanUnused
void cleanUnused(const std::map< std::string, bool > &used_joints)
Definition: motion_model.cpp:67
pal::KeyFrame::getJointPosition
double getJointPosition(const std::string &joint) const
Definition: motion_model.cpp:54
pal::Motion::DEFAULT_TIME
static const float DEFAULT_TIME
Definition: motion_model.h:141
pal::KeyFrame::getTime
float getTime() const
Definition: motion_model.h:113
pal::JointModel::inLimits
bool inLimits(double position)
Definition: motion_model.h:80
pal::TEST
TEST(MotionBuilderTest, actionNotStartedTest)
Definition: test_motion_builder.cpp:100
main
int main(int argc, char **argv)
Definition: test_motion_model.cpp:450

play_motion_builder
Author(s):
autogenerated on Mon Feb 28 2022 23:13:39