locked_robot_state_test.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Willow Garage, Inc.
5  * Copyright (c) 2014, SRI International
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Author: Acorn Pooley, Ioan Sucan */
37 
38 #include <gtest/gtest.h>
42 #include <urdf_parser/urdf_parser.h>
43 
44 static const char* URDF_STR = "<?xml version=\"1.0\" ?>"
45  "<robot name=\"one_robot\">"
46  "<link name=\"base_link\">"
47  " <inertial>"
48  " <mass value=\"2.81\"/>"
49  " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
50  " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
51  " </inertial>"
52  " <collision name=\"my_collision\">"
53  " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
54  " <geometry>"
55  " <box size=\"1 2 1\" />"
56  " </geometry>"
57  " </collision>"
58  " <visual>"
59  " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
60  " <geometry>"
61  " <box size=\"1 2 1\" />"
62  " </geometry>"
63  " </visual>"
64  "</link>"
65  "<joint name=\"joint_a\" type=\"continuous\">"
66  " <axis xyz=\"0 0 1\"/>"
67  " <parent link=\"base_link\"/>"
68  " <child link=\"link_a\"/>"
69  " <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>"
70  "</joint>"
71  "<link name=\"link_a\">"
72  " <inertial>"
73  " <mass value=\"1.0\"/>"
74  " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
75  " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
76  " </inertial>"
77  " <collision>"
78  " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
79  " <geometry>"
80  " <box size=\"1 2 1\" />"
81  " </geometry>"
82  " </collision>"
83  " <visual>"
84  " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
85  " <geometry>"
86  " <box size=\"1 2 1\" />"
87  " </geometry>"
88  " </visual>"
89  "</link>"
90  "<joint name=\"joint_b\" type=\"fixed\">"
91  " <parent link=\"link_a\"/>"
92  " <child link=\"link_b\"/>"
93  " <origin rpy=\" 0.0 -0.42 0 \" xyz=\"0.0 0.5 0 \"/>"
94  "</joint>"
95  "<link name=\"link_b\">"
96  " <inertial>"
97  " <mass value=\"1.0\"/>"
98  " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
99  " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
100  " </inertial>"
101  " <collision>"
102  " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
103  " <geometry>"
104  " <box size=\"1 2 1\" />"
105  " </geometry>"
106  " </collision>"
107  " <visual>"
108  " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
109  " <geometry>"
110  " <box size=\"1 2 1\" />"
111  " </geometry>"
112  " </visual>"
113  "</link>"
114  " <joint name=\"joint_c\" type=\"prismatic\">"
115  " <axis xyz=\"1 0 0\"/>"
116  " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.09\" velocity=\"0.2\"/>"
117  " <safety_controller k_position=\"20.0\" k_velocity=\"500.0\" "
118  "soft_lower_limit=\"0.0\" soft_upper_limit=\"0.089\"/>"
119  " <parent link=\"link_b\"/>"
120  " <child link=\"link_c\"/>"
121  " <origin rpy=\" 0.0 0.42 0.0 \" xyz=\"0.0 -0.1 0 \"/>"
122  " </joint>"
123  "<link name=\"link_c\">"
124  " <inertial>"
125  " <mass value=\"1.0\"/>"
126  " <origin rpy=\"0 0 0\" xyz=\"0.0 0 .0\"/>"
127  " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
128  " </inertial>"
129  " <collision>"
130  " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
131  " <geometry>"
132  " <box size=\"1 2 1\" />"
133  " </geometry>"
134  " </collision>"
135  " <visual>"
136  " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
137  " <geometry>"
138  " <box size=\"1 2 1\" />"
139  " </geometry>"
140  " </visual>"
141  "</link>"
142  " <joint name=\"mim_f\" type=\"prismatic\">"
143  " <axis xyz=\"1 0 0\"/>"
144  " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>"
145  " <parent link=\"link_c\"/>"
146  " <child link=\"link_d\"/>"
147  " <origin rpy=\" 0.0 0.1 0.0 \" xyz=\"0.1 0.1 0 \"/>"
148  " <mimic joint=\"joint_f\" multiplier=\"1.5\" offset=\"0.1\"/>"
149  " </joint>"
150  " <joint name=\"joint_f\" type=\"prismatic\">"
151  " <axis xyz=\"1 0 0\"/>"
152  " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>"
153  " <parent link=\"link_d\"/>"
154  " <child link=\"link_e\"/>"
155  " <origin rpy=\" 0.0 0.1 0.0 \" xyz=\"0.1 0.1 0 \"/>"
156  " </joint>"
157  "<link name=\"link_d\">"
158  " <collision>"
159  " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
160  " <geometry>"
161  " <box size=\"1 2 1\" />"
162  " </geometry>"
163  " </collision>"
164  " <visual>"
165  " <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
166  " <geometry>"
167  " <box size=\"1 2 1\" />"
168  " </geometry>"
169  " </visual>"
170  "</link>"
171  "<link name=\"link_e\">"
172  " <collision>"
173  " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
174  " <geometry>"
175  " <box size=\"1 2 1\" />"
176  " </geometry>"
177  " </collision>"
178  " <visual>"
179  " <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
180  " <geometry>"
181  " <box size=\"1 2 1\" />"
182  " </geometry>"
183  " </visual>"
184  "</link>"
185  "</robot>";
186 
187 static const char* SRDF_STR =
188  "<?xml version=\"1.0\" ?>"
189  "<robot name=\"one_robot\">"
190  "<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"planar\"/>"
191  "<group name=\"base_from_joints\">"
192  "<joint name=\"base_joint\"/>"
193  "<joint name=\"joint_a\"/>"
194  "<joint name=\"joint_c\"/>"
195  "</group>"
196  "<group name=\"mim_joints\">"
197  "<joint name=\"joint_f\"/>"
198  "<joint name=\"mim_f\"/>"
199  "</group>"
200  "<group name=\"base_with_subgroups\">"
201  "<group name=\"base_from_base_to_tip\"/>"
202  "<joint name=\"joint_c\"/>"
203  "</group>"
204  "<group name=\"base_from_base_to_tip\">"
205  "<chain base_link=\"base_link\" tip_link=\"link_b\"/>"
206  "<joint name=\"base_joint\"/>"
207  "</group>"
208  "</robot>";
209 
210 // index of joints from URDF
211 enum
212 {
213  JOINT_A = 3,
214  JOINT_C = 4,
215  MIM_F = 5,
217 };
218 
219 static moveit::core::RobotModelPtr getModel()
220 {
221  static moveit::core::RobotModelPtr model;
222  if (!model)
223  {
224  urdf::ModelInterfaceSharedPtr urdf(urdf::parseURDF(URDF_STR));
226  srdf->initString(*urdf, SRDF_STR);
227  model.reset(new moveit::core::RobotModel(urdf, srdf));
228  }
229  return model;
230 }
231 
232 // Test constructors and robot model loading
233 TEST(LockedRobotState, load)
234 {
235  moveit::core::RobotModelPtr model = getModel();
236 
238 
239  moveit::core::RobotState state2(model);
240  state2.setToDefaultValues();
242 
243  robot_interaction::LockedRobotStatePtr ls4(new robot_interaction::LockedRobotState(model));
244 }
245 
246 // sanity test the URDF and enum
247 TEST(LockedRobotState, URDF_sanity)
248 {
249  moveit::core::RobotModelPtr model = getModel();
251 
252  EXPECT_EQ(ls1.getState()->getVariableNames()[JOINT_A], "joint_a");
253 }
254 
255 // A superclass to test the robotStateChanged() virtual method
257 {
258 public:
259  Super1(const robot_model::RobotModelPtr& model) : LockedRobotState(model), cnt_(0)
260  {
261  }
262 
263  virtual void robotStateChanged()
264  {
265  cnt_++;
266  }
267 
268  int cnt_;
269 };
270 
271 static void modify1(robot_state::RobotState* state)
272 {
273  state->setVariablePosition(JOINT_A, 0.00006);
274 }
275 
277 {
278  moveit::core::RobotModelPtr model = getModel();
279 
280  Super1 ls1(model);
281 
282  EXPECT_EQ(ls1.cnt_, 0);
283 
284  robot_state::RobotState cp1(*ls1.getState());
285  cp1.setVariablePosition(JOINT_A, 0.00001);
286  cp1.setVariablePosition(JOINT_C, 0.00002);
287  cp1.setVariablePosition(JOINT_F, 0.00003);
288  ls1.setState(cp1);
289 
290  EXPECT_EQ(ls1.cnt_, 1);
291 
292  ls1.modifyState(modify1);
293  EXPECT_EQ(ls1.cnt_, 2);
294 
295  ls1.modifyState(modify1);
296  EXPECT_EQ(ls1.cnt_, 3);
297 }
298 
299 // Class for testing LockedRobotState in multithreaded environment.
300 // Contains thread functions for modifying/checking a LockedRobotState.
301 class MyInfo
302 {
303 public:
304  MyInfo() : quit_(false)
305  {
306  }
307 
308  // Thread that repeatedly sets state to different values
309  void setThreadFunc(robot_interaction::LockedRobotState* locked_state, int* counter, double offset);
310 
311  // Thread that repeatedly modifies state with different values
312  void modifyThreadFunc(robot_interaction::LockedRobotState* locked_state, int* counter, double offset);
313 
314  // Thread that repeatedly checks that state is valid (not half-updated)
315  void checkThreadFunc(robot_interaction::LockedRobotState* locked_state, int* counter);
316 
317  // Thread that waits for other threads to complete
318  void waitThreadFunc(robot_interaction::LockedRobotState* locked_state, int** counters, int max);
319 
320 private:
321  // helper function for modifyThreadFunc
322  void modifyFunc(robot_state::RobotState* state, double val);
323 
324  // Checks state for validity and self-consistancy.
325  void checkState(robot_interaction::LockedRobotState& locked_state);
326 
327  // mutex protects quit_ and counter variables
328  boost::mutex cnt_lock_;
329 
330  // set to true by waitThreadFunc() when all counters have reached at least
331  // max.
332  bool quit_;
333 };
334 
335 // Check the state. It should always be valid.
337 {
338  robot_state::RobotStateConstPtr s = locked_state.getState();
339 
340  robot_state::RobotState cp1(*s);
341 
342  // take some time
343  cnt_lock_.lock();
344  cnt_lock_.unlock();
345  cnt_lock_.lock();
346  cnt_lock_.unlock();
347  cnt_lock_.lock();
348  cnt_lock_.unlock();
349 
350  // check mim_f == joint_f
351  EXPECT_EQ(s->getVariablePositions()[MIM_F], s->getVariablePositions()[JOINT_F] * 1.5 + 0.1);
352 
353  robot_state::RobotState cp2(*s);
354 
355  EXPECT_NE(cp1.getVariablePositions(), cp2.getVariablePositions());
356  EXPECT_NE(cp1.getVariablePositions(), s->getVariablePositions());
357 
358  int cnt = cp1.getVariableCount();
359  for (int i = 0; i < cnt; ++i)
360  {
361  EXPECT_EQ(cp1.getVariablePositions()[i], cp2.getVariablePositions()[i]);
362  EXPECT_EQ(cp1.getVariablePositions()[i], s->getVariablePositions()[i]);
363  }
364 
365  // check mim_f == joint_f
366  EXPECT_EQ(s->getVariablePositions()[MIM_F], s->getVariablePositions()[JOINT_F] * 1.5 + 0.1);
367 }
368 
369 // spin, checking the state
371 {
372  bool go = true;
373  while (go)
374  {
375  for (int loops = 0; loops < 100; ++loops)
376  {
377  checkState(*locked_state);
378  }
379 
380  cnt_lock_.lock();
381  go = !quit_;
382  ++*counter;
383  cnt_lock_.unlock();
384  }
385 }
386 
387 // spin, setting the state to different values
388 void MyInfo::setThreadFunc(robot_interaction::LockedRobotState* locked_state, int* counter, double offset)
389 {
390  bool go = true;
391  while (go)
392  {
393  double val = offset;
394  for (int loops = 0; loops < 100; ++loops)
395  {
396  val += 0.0001;
397  robot_state::RobotState cp1(*locked_state->getState());
398 
399  cp1.setVariablePosition(JOINT_A, val + 0.00001);
400  cp1.setVariablePosition(JOINT_C, val + 0.00002);
401  cp1.setVariablePosition(JOINT_F, val + 0.00003);
402 
403  locked_state->setState(cp1);
404  }
405 
406  cnt_lock_.lock();
407  go = !quit_;
408  ++*counter;
409  cnt_lock_.unlock();
410 
411  checkState(*locked_state);
412 
413  val += 0.000001;
414  }
415 }
416 
417 // modify the state in place. Used by MyInfo::modifyThreadFunc()
418 void MyInfo::modifyFunc(robot_state::RobotState* state, double val)
419 {
420  state->setVariablePosition(JOINT_A, val + 0.00001);
421  state->setVariablePosition(JOINT_C, val + 0.00002);
422  state->setVariablePosition(JOINT_F, val + 0.00003);
423 }
424 
425 // spin, modifying the state to different values
426 void MyInfo::modifyThreadFunc(robot_interaction::LockedRobotState* locked_state, int* counter, double offset)
427 {
428  bool go = true;
429  while (go)
430  {
431  double val = offset;
432  for (int loops = 0; loops < 100; ++loops)
433  {
434  val += 0.0001;
435 
436  locked_state->modifyState(boost::bind(&MyInfo::modifyFunc, this, _1, val));
437  }
438 
439  cnt_lock_.lock();
440  go = !quit_;
441  ++*counter;
442  cnt_lock_.unlock();
443 
444  checkState(*locked_state);
445 
446  val += 0.000001;
447  }
448 }
449 
450 // spin until all counters reach at least max
451 void MyInfo::waitThreadFunc(robot_interaction::LockedRobotState* locked_state, int** counters, int max)
452 {
453  bool go = true;
454  while (go)
455  {
456  go = false;
457  cnt_lock_.lock();
458  for (int i = 0; counters[i]; ++i)
459  {
460  if (counters[i][0] < max)
461  go = true;
462  }
463  cnt_lock_.unlock();
464 
465  checkState(*locked_state);
466  checkState(*locked_state);
467  }
468  cnt_lock_.lock();
469  quit_ = true;
470  cnt_lock_.unlock();
471 }
472 
473 // Run several threads and ensure they modify the state consistantly
474 // ncheck - # of checkThreadFunc threads to run
475 // nset - # of setThreadFunc threads to run
476 // nmod - # of modifyThreadFunc threads to run
477 static void runThreads(int ncheck, int nset, int nmod)
478 {
479  MyInfo info;
480 
481  moveit::core::RobotModelPtr model = getModel();
483 
484  int num = ncheck + nset + nmod;
485 
486  typedef int* int_ptr;
487  typedef boost::thread* thread_ptr;
488  int* cnt = new int[num];
489  int_ptr* counters = new int_ptr[num + 1];
490  thread_ptr* threads = new thread_ptr[num];
491 
492  int p = 0;
493  double val = 0.1;
494 
495  // These threads check the validity of the RobotState
496  for (int i = 0; i < ncheck; ++i)
497  {
498  cnt[p] = 0;
499  counters[p] = &cnt[p];
500  threads[p] = new boost::thread(&MyInfo::checkThreadFunc, &info, &ls1, &cnt[p]);
501  val += 0.1;
502  p++;
503  }
504 
505  // These threads set the RobotState to new values
506  for (int i = 0; i < nset; ++i)
507  {
508  cnt[p] = 0;
509  counters[p] = &cnt[p];
510  threads[p] = new boost::thread(&MyInfo::setThreadFunc, &info, &ls1, &cnt[p], val);
511  val += 0.1;
512  p++;
513  }
514 
515  // These threads modify the RobotState in place
516  for (int i = 0; i < nmod; ++i)
517  {
518  cnt[p] = 0;
519  counters[p] = &cnt[p];
520  threads[p] = new boost::thread(&MyInfo::modifyThreadFunc, &info, &ls1, &cnt[p], val);
521  val += 0.1;
522  p++;
523  }
524 
525  ASSERT_EQ(p, num);
526  counters[p] = NULL;
527 
528  // this thread waits for all the other threads to make progress, then stops
529  // everything.
530  boost::thread wthread(&MyInfo::waitThreadFunc, &info, &ls1, counters, 1000);
531 
532  // wait for all threads to finish
533  for (int i = 0; i < p; ++i)
534  {
535  threads[i]->join();
536  wthread.join();
537  }
538 
539  // clean up
540  for (int i = 0; i < p; ++i)
541  {
542  delete threads[i];
543  }
544  delete[] cnt;
545  delete[] counters;
546  delete[] threads;
547 }
548 
550 {
551  runThreads(1, 1, 0);
552 }
553 
555 {
556  runThreads(1, 2, 0);
557 }
558 
560 {
561  runThreads(1, 3, 0);
562 }
563 
565 {
566  runThreads(1, 0, 1);
567 }
568 
570 {
571  runThreads(1, 0, 1);
572 }
573 
575 {
576  runThreads(1, 0, 1);
577 }
578 
580 {
581  runThreads(1, 1, 1);
582 }
583 
585 {
586  runThreads(1, 2, 1);
587 }
588 
590 {
591  runThreads(1, 1, 2);
592 }
593 
595 {
596  runThreads(1, 3, 1);
597 }
598 
600 {
601  runThreads(1, 1, 3);
602 }
603 
605 {
606  runThreads(1, 3, 3);
607 }
608 
609 TEST(LockedRobotState, set3mod3c3)
610 {
611  runThreads(3, 3, 3);
612 }
613 
614 int main(int argc, char** argv)
615 {
616  testing::InitGoogleTest(&argc, argv);
617  int arg;
618 
619  return RUN_ALL_TESTS();
620 }
robot_state::RobotStateConstPtr getState() const
get read-only access to the state.
void waitThreadFunc(robot_interaction::LockedRobotState *locked_state, int **counters, int max)
void modifyThreadFunc(robot_interaction::LockedRobotState *locked_state, int *counter, double offset)
XmlRpcServer s
boost::mutex cnt_lock_
void modifyFunc(robot_state::RobotState *state, double val)
Super1(const robot_model::RobotModelPtr &model)
virtual void robotStateChanged()
void setState(const robot_state::RobotState &state)
Set the state to the new value.
TEST(LockedRobotState, load)
static void runThreads(int ncheck, int nset, int nmod)
def go()
LockedRobotState(const robot_state::RobotState &state)
#define EXPECT_EQ(a, b)
void checkState(robot_interaction::LockedRobotState &locked_state)
void setThreadFunc(robot_interaction::LockedRobotState *locked_state, int *counter, double offset)
static const char * URDF_STR
int main(int argc, char **argv)
void modifyState(const ModifyStateFunction &modify)
static moveit::core::RobotModelPtr getModel()
void checkThreadFunc(robot_interaction::LockedRobotState *locked_state, int *counter)
Maintain a RobotState in a multithreaded environment.
static const char * SRDF_STR
double max(double a, double b)
static void modify1(robot_state::RobotState *state)


robot_interaction
Author(s): Ioan Sucan
autogenerated on Wed Jul 10 2019 04:04:01