test_motion_builder.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 #include <gtest/gtest.h>
3 
4 #include <play_motion_builder_msgs/BuildMotionAction.h>
5 #include <play_motion_builder_msgs/RunMotionAction.h>
6 #include <play_motion_builder_msgs/EditMotion.h>
7 #include <play_motion_builder_msgs/StoreMotion.h>
8 #include <play_motion_builder_msgs/ChangeJoints.h>
9 
10 #include <actionlib/client/simple_action_client.h>
11 #include <sensor_msgs/JointState.h>
12 
13 #include <unordered_map>
14 #include <mutex>
15 #include <fstream>
16 
17 namespace pal
18 {
19 class TestBot
20 {
21 public:
22  TestBot() : as_(1, &queue_)
23  {
24  nh_.setCallbackQueue(&queue_);
25 
26  joints_["arm_left_joint"] = 0.0;
27  joints_["arm_right_joint"] = 0.0;
28  joints_["head_joint"] = 0.0;
29 
30  joint_pub_ = nh_.advertise<sensor_msgs::JointState>("/joint_states", 1);
31  timer_ = nh_.createTimer(ros::Duration(0.05), &TestBot::timerCb, this);
32 
33  as_.start();
34  }
35 
36  void changeJointPos(double left_arm, double right_arm, double head)
37  {
38  std::lock_guard<std::mutex> lock(joint_mutex_);
39 
40  joints_["arm_left_joint"] = left_arm;
41  joints_["arm_right_joint"] = right_arm;
42  joints_["head_joint"] = head;
43  }
44 
45 private:
46  ros::NodeHandle nh_;
47  ros::CallbackQueue queue_;
48  ros::AsyncSpinner as_;
49 
50  ros::Publisher joint_pub_;
51  ros::Timer timer_;
52 
53  std::unordered_map<std::string, double> joints_;
54  std::mutex joint_mutex_;
55 
56  void timerCb(const ros::TimerEvent&)
57  {
58  sensor_msgs::JointState js;
59  js.header.stamp = ros::Time::now();
60  {
61  std::lock_guard<std::mutex> lock(joint_mutex_);
62  for (const auto& joint : joints_)
63  {
64  js.name.push_back(joint.first);
65  js.position.push_back(joint.second);
66  }
67  }
68  joint_pub_.publish(js);
69  }
70 };
71 
72 template <typename InputIterator1, typename InputIterator2>
73 bool range_equal(InputIterator1 first1, InputIterator1 last1, InputIterator2 first2,
74  InputIterator2 last2)
75 {
76  while (first1 != last1 && first2 != last2)
77  {
78  if (*first1 != *first2)
79  return false;
80  ++first1;
81  ++first2;
82  }
83  return (first1 == last1) && (first2 == last2);
84 }
85 
86 bool compare_files(const std::string& filename1, const std::string& filename2)
87 {
88  std::ifstream file1(filename1);
89  std::ifstream file2(filename2);
90 
91  std::istreambuf_iterator<char> begin1(file1);
92  std::istreambuf_iterator<char> begin2(file2);
93 
94  std::istreambuf_iterator<char> end;
95 
96  return range_equal(begin1, end, begin2, end);
97 }
98 
99 // Test not in building mode
100 TEST(MotionBuilderTest, actionNotStartedTest)
101 {
102  // Call edit service
103  play_motion_builder_msgs::EditMotion srv_edit;
104  ASSERT_TRUE(ros::service::waitForService("play_motion_builder_node/edit_motion",
105  ros::Duration(5.0)));
106  ros::service::call<play_motion_builder_msgs::EditMotion>(
107  "play_motion_builder_node/edit_motion", srv_edit);
108  EXPECT_FALSE(srv_edit.response.ok);
109  EXPECT_EQ("No motion being built", srv_edit.response.message);
110 
111  // Call Store service
112  play_motion_builder_msgs::StoreMotion srv_store;
113  ASSERT_TRUE(ros::service::waitForService("play_motion_builder_node/store_motion",
114  ros::Duration(5.0)));
115  ros::service::call<play_motion_builder_msgs::StoreMotion>(
116  "play_motion_builder_node/store_motion", srv_store);
117  EXPECT_FALSE(srv_store.response.ok);
118  EXPECT_EQ("No motion being built, could not store.", srv_store.response.message);
119 
120  // Call change joints service
121  play_motion_builder_msgs::ChangeJoints srv_change_joints;
122  ASSERT_TRUE(ros::service::waitForService("play_motion_builder_node/change_joints",
123  ros::Duration(5.0)));
124  ros::service::call<play_motion_builder_msgs::ChangeJoints>(
125  "play_motion_builder_node/change_joints", srv_change_joints);
126  EXPECT_FALSE(srv_change_joints.response.ok);
127  EXPECT_EQ("No motion being built", srv_change_joints.response.message);
128 
129  // Call play action
130  ros::NodeHandle nh;
131  play_motion_builder_msgs::RunMotionGoal rmg;
132  actionlib::SimpleActionClient<play_motion_builder_msgs::RunMotionAction> client(
133  nh, "play_motion_builder_node/run");
134  client.waitForServer(ros::Duration(5));
135  client.sendGoalAndWait(rmg, ros::Duration(5));
136  // Check state
137  EXPECT_EQ(actionlib::SimpleClientGoalState::ABORTED, client.getState().state_);
138 }
139 
140 TEST(MotionBuilderTest, buildNewMotion)
141 {
142  TestBot tb;
143 
144  ros::NodeHandle nh;
145  actionlib::SimpleActionClient<play_motion_builder_msgs::BuildMotionAction> client(
146  nh, "play_motion_builder_node/build");
147 
148  ASSERT_TRUE(client.waitForServer(ros::Duration(5.0))) << "Server didn't start";
149 
150  // Send an empty goal.motion to start a new one
151  play_motion_builder_msgs::BuildMotionGoal goal;
152  client.sendGoal(goal);
153  ros::spinOnce();
154  ros::Duration(0.1).sleep(); // Wait a bit for the goal to be received
155 
156  // Call edit service in LIST model
157  play_motion_builder_msgs::EditMotion srv_edit;
158  srv_edit.request.action = play_motion_builder_msgs::EditMotion::Request::LIST;
159  ros::service::call<play_motion_builder_msgs::EditMotion>(
160  "play_motion_builder_node/edit_motion", srv_edit);
161  // Check list is empty
162  EXPECT_TRUE(srv_edit.response.ok) << srv_edit.response.message;
163  EXPECT_EQ(0, srv_edit.response.motion.keyframes.size());
164 
165  // Capture position and check added
166  srv_edit.request.action = play_motion_builder_msgs::EditMotion::Request::APPEND;
167  ros::service::call<play_motion_builder_msgs::EditMotion>(
168  "play_motion_builder_node/edit_motion", srv_edit);
169  // Check list has one element
170  EXPECT_TRUE(srv_edit.response.ok) << srv_edit.response.message;
171  EXPECT_EQ(1, srv_edit.response.motion.keyframes.size());
172  // Review element
173  EXPECT_EQ(3, srv_edit.response.motion.joints.size());
174  EXPECT_EQ(3, srv_edit.response.motion.keyframes[0].pose.size());
175  for (size_t i = 0; i < srv_edit.response.motion.joints.size(); ++i)
176  {
177  if (srv_edit.response.motion.joints[i] == "arm_left_joint" ||
178  srv_edit.response.motion.joints[i] == "arm_right_joint" ||
179  srv_edit.response.motion.joints[i] == "head_joint")
180  {
181  EXPECT_NEAR(0.0, srv_edit.response.motion.keyframes[0].pose[i], 0.001);
182  }
183  else
184  {
185  EXPECT_TRUE(false) << "Unknown joint added " << srv_edit.response.motion.joints[i];
186  }
187  }
188  EXPECT_NEAR(0.0, srv_edit.response.motion.keyframes[0].time_from_last, 0.001);
189 
190  // Move robot and capture
191  tb.changeJointPos(0.1, 0.2, 0.3);
192  ros::Duration(0.15).sleep(); // Wait a bit for the change to take effect
193  srv_edit.request.action = play_motion_builder_msgs::EditMotion::Request::APPEND;
194  ros::service::call<play_motion_builder_msgs::EditMotion>(
195  "play_motion_builder_node/edit_motion", srv_edit);
196  // Check list has one element
197  EXPECT_TRUE(srv_edit.response.ok) << srv_edit.response.message;
198  EXPECT_EQ(2, srv_edit.response.motion.keyframes.size());
199  // Review element
200  EXPECT_EQ(3, srv_edit.response.motion.joints.size());
201  EXPECT_EQ(3, srv_edit.response.motion.keyframes[0].pose.size());
202  for (size_t i = 0; i < srv_edit.response.motion.joints.size(); ++i)
203  {
204  if (srv_edit.response.motion.joints[i] == "arm_left_joint")
205  {
206  EXPECT_NEAR(0.1, srv_edit.response.motion.keyframes[1].pose[i], 0.001);
207  }
208  else if (srv_edit.response.motion.joints[i] == "arm_right_joint")
209  {
210  EXPECT_NEAR(0.2, srv_edit.response.motion.keyframes[1].pose[i], 0.001);
211  }
212  else if (srv_edit.response.motion.joints[i] == "head_joint")
213  {
214  EXPECT_NEAR(0.3, srv_edit.response.motion.keyframes[1].pose[i], 0.001);
215  }
216  else
217  {
218  EXPECT_TRUE(false) << "Unknown joint added " << srv_edit.response.motion.joints[i];
219  }
220  }
221  EXPECT_NEAR(5.0, srv_edit.response.motion.keyframes[1].time_from_last, 0.001);
222 
223  // Move robot and edit first
224  tb.changeJointPos(0.4, 0.5, 0.6);
225  ros::Duration(0.15).sleep(); // Wait a bit for the change to take effect
226  srv_edit.request.action = play_motion_builder_msgs::EditMotion::Request::EDIT;
227  srv_edit.request.step_id = 0;
228  ros::service::call<play_motion_builder_msgs::EditMotion>(
229  "play_motion_builder_node/edit_motion", srv_edit);
230  // Check list has one element
231  EXPECT_TRUE(srv_edit.response.ok) << srv_edit.response.message;
232  EXPECT_EQ(2, srv_edit.response.motion.keyframes.size());
233  // Review element
234  EXPECT_EQ(3, srv_edit.response.motion.joints.size());
235  EXPECT_EQ(3, srv_edit.response.motion.keyframes[0].pose.size());
236  for (size_t i = 0; i < srv_edit.response.motion.joints.size(); ++i)
237  {
238  if (srv_edit.response.motion.joints[i] == "arm_left_joint")
239  {
240  EXPECT_NEAR(0.4, srv_edit.response.motion.keyframes[0].pose[i], 0.001);
241  }
242  else if (srv_edit.response.motion.joints[i] == "arm_right_joint")
243  {
244  EXPECT_NEAR(0.5, srv_edit.response.motion.keyframes[0].pose[i], 0.001);
245  }
246  else if (srv_edit.response.motion.joints[i] == "head_joint")
247  {
248  EXPECT_NEAR(0.6, srv_edit.response.motion.keyframes[0].pose[i], 0.001);
249  }
250  else
251  {
252  EXPECT_TRUE(false) << "Unknown joint added " << srv_edit.response.motion.joints[i];
253  }
254  }
255  EXPECT_NEAR(5.0, srv_edit.response.motion.keyframes[1].time_from_last, 0.001);
256 
257  // Copy motion 0 as last motion
258  srv_edit.request.action = play_motion_builder_msgs::EditMotion::Request::COPY_AS_LAST;
259  srv_edit.request.step_id = 0;
260  ros::service::call<play_motion_builder_msgs::EditMotion>(
261  "play_motion_builder_node/edit_motion", srv_edit);
262  // Check list has one element
263  EXPECT_TRUE(srv_edit.response.ok) << srv_edit.response.message;
264  EXPECT_EQ(3, srv_edit.response.motion.keyframes.size());
265  // Review element
266  EXPECT_EQ(3, srv_edit.response.motion.joints.size());
267  EXPECT_EQ(3, srv_edit.response.motion.keyframes[2].pose.size());
268  for (size_t i = 0; i < srv_edit.response.motion.joints.size(); ++i)
269  {
270  if (srv_edit.response.motion.joints[i] == "arm_left_joint")
271  {
272  EXPECT_NEAR(0.4, srv_edit.response.motion.keyframes[2].pose[i], 0.001);
273  }
274  else if (srv_edit.response.motion.joints[i] == "arm_right_joint")
275  {
276  EXPECT_NEAR(0.5, srv_edit.response.motion.keyframes[2].pose[i], 0.001);
277  }
278  else if (srv_edit.response.motion.joints[i] == "head_joint")
279  {
280  EXPECT_NEAR(0.6, srv_edit.response.motion.keyframes[2].pose[i], 0.001);
281  }
282  else
283  {
284  EXPECT_TRUE(false) << "Unknown joint added " << srv_edit.response.motion.joints[i];
285  }
286  }
287  EXPECT_NEAR(5.0, srv_edit.response.motion.keyframes[2].time_from_last, 0.001);
288 
289  // Copy motion 1 as motion 2
290  srv_edit.request.action = play_motion_builder_msgs::EditMotion::Request::COPY_AS_NEXT;
291  srv_edit.request.step_id = 1;
292  ros::service::call<play_motion_builder_msgs::EditMotion>(
293  "play_motion_builder_node/edit_motion", srv_edit);
294  // Check list has one element
295  EXPECT_TRUE(srv_edit.response.ok) << srv_edit.response.message;
296  EXPECT_EQ(4, srv_edit.response.motion.keyframes.size());
297  // Review element
298  EXPECT_EQ(3, srv_edit.response.motion.joints.size());
299  EXPECT_EQ(3, srv_edit.response.motion.keyframes[2].pose.size());
300  for (size_t i = 0; i < srv_edit.response.motion.joints.size(); ++i)
301  {
302  if (srv_edit.response.motion.joints[i] == "arm_left_joint")
303  {
304  EXPECT_NEAR(0.1, srv_edit.response.motion.keyframes[2].pose[i], 0.001);
305  }
306  else if (srv_edit.response.motion.joints[i] == "arm_right_joint")
307  {
308  EXPECT_NEAR(0.2, srv_edit.response.motion.keyframes[2].pose[i], 0.001);
309  }
310  else if (srv_edit.response.motion.joints[i] == "head_joint")
311  {
312  EXPECT_NEAR(0.3, srv_edit.response.motion.keyframes[2].pose[i], 0.001);
313  }
314  else
315  {
316  EXPECT_TRUE(false) << "Unknown joint added " << srv_edit.response.motion.joints[i];
317  }
318  }
319  EXPECT_NEAR(5.0, srv_edit.response.motion.keyframes[2].time_from_last, 0.001);
320 
321  // Remove motion 2
322  srv_edit.request.action = play_motion_builder_msgs::EditMotion::Request::REMOVE;
323  srv_edit.request.step_id = 2;
324  ros::service::call<play_motion_builder_msgs::EditMotion>(
325  "play_motion_builder_node/edit_motion", srv_edit);
326  // Check list has one element
327  EXPECT_TRUE(srv_edit.response.ok) << srv_edit.response.message;
328  EXPECT_EQ(3, srv_edit.response.motion.keyframes.size());
329  // Review element
330  EXPECT_EQ(3, srv_edit.response.motion.joints.size());
331  EXPECT_EQ(3, srv_edit.response.motion.keyframes[2].pose.size());
332  for (size_t i = 0; i < srv_edit.response.motion.joints.size(); ++i)
333  {
334  if (srv_edit.response.motion.joints[i] == "arm_left_joint")
335  {
336  EXPECT_NEAR(0.4, srv_edit.response.motion.keyframes[2].pose[i], 0.001);
337  }
338  else if (srv_edit.response.motion.joints[i] == "arm_right_joint")
339  {
340  EXPECT_NEAR(0.5, srv_edit.response.motion.keyframes[2].pose[i], 0.001);
341  }
342  else if (srv_edit.response.motion.joints[i] == "head_joint")
343  {
344  EXPECT_NEAR(0.6, srv_edit.response.motion.keyframes[2].pose[i], 0.001);
345  }
346  else
347  {
348  EXPECT_TRUE(false) << "Unknown joint added " << srv_edit.response.motion.joints[i];
349  }
350  }
351  EXPECT_NEAR(5.0, srv_edit.response.motion.keyframes[2].time_from_last, 0.001);
352 
353  // Test storing a motion
354  play_motion_builder_msgs::StoreMotion srv_store;
355  srv_store.request.file_path = "/tmp/test_motion_new.yaml";
356  srv_store.request.ros_name = "test_motion_new";
357  srv_store.request.meta.name = "Test Motion New";
358  srv_store.request.meta.usage = "Test";
359  srv_store.request.meta.description = "Motion created to test the system";
360  ros::service::call<play_motion_builder_msgs::StoreMotion>(
361  "play_motion_builder_node/store_motion", srv_store);
362  EXPECT_TRUE(srv_store.response.ok) << srv_store.response.message;
363 
364  // Compare files
365  std::string filepath;
366  nh.getParam("test_file_path_new", filepath);
367  EXPECT_TRUE(compare_files(srv_store.request.file_path, filepath));
368 
369  // Stop motion builder
370  client.cancelGoal();
371  ros::Duration(0.1).sleep(); // Wait a moment
372 
373  // Make sure system is done
374  srv_edit.request.action = play_motion_builder_msgs::EditMotion::Request::LIST;
375  ros::service::call<play_motion_builder_msgs::EditMotion>(
376  "play_motion_builder_node/edit_motion", srv_edit);
377  EXPECT_FALSE(srv_edit.response.ok);
378  EXPECT_EQ("No motion being built", srv_edit.response.message);
379 }
380 
381 TEST(MotionBuilderTest, editExistingMotion)
382 {
383  TestBot tb;
384 
385  ros::NodeHandle nh;
386  actionlib::SimpleActionClient<play_motion_builder_msgs::BuildMotionAction> client(
387  nh, "play_motion_builder_node/build");
388 
389  ASSERT_TRUE(client.waitForServer(ros::Duration(5.0))) << "Server didn't start";
390 
391  // Send an empty goal.motion to start a new one
392  play_motion_builder_msgs::BuildMotionGoal goal;
393  goal.motion = "test_motion_edit";
394  client.sendGoal(goal);
395  ros::spinOnce();
396  ros::Duration(0.1).sleep(); // Wait a bit for the goal to be received
397 
398  // Check that the motion was properly loaded
399  play_motion_builder_msgs::EditMotion srv_edit;
400  srv_edit.request.action = play_motion_builder_msgs::EditMotion::Request::LIST;
401  ros::service::call<play_motion_builder_msgs::EditMotion>(
402  "play_motion_builder_node/edit_motion", srv_edit);
403  // Check motion is properly loaded
404  EXPECT_TRUE(srv_edit.response.ok) << srv_edit.response.message;
405  EXPECT_EQ(3, srv_edit.response.motion.keyframes.size());
406  EXPECT_EQ(2, srv_edit.response.motion.joints.size());
407 
408  size_t head_index, arm_left_index;
409  for (size_t i = 0; i < srv_edit.response.motion.joints.size(); ++i)
410  {
411  if (srv_edit.response.motion.joints[i] == "head_joint")
412  head_index = i;
413  else if (srv_edit.response.motion.joints[i] == "arm_left_joint")
414  arm_left_index = i;
415  }
416  // Check keyframe 1
417  EXPECT_NEAR(0.0, srv_edit.response.motion.keyframes[0].time_from_last, 0.001);
418  EXPECT_NEAR(0.1, srv_edit.response.motion.keyframes[0].pose[head_index], 0.001);
419  EXPECT_NEAR(0.2, srv_edit.response.motion.keyframes[0].pose[arm_left_index], 0.001);
420  // Check keyframe 2
421  EXPECT_NEAR(1.0, srv_edit.response.motion.keyframes[1].time_from_last, 0.001);
422  EXPECT_NEAR(0.11, srv_edit.response.motion.keyframes[1].pose[head_index], 0.001);
423  EXPECT_NEAR(0.21, srv_edit.response.motion.keyframes[1].pose[arm_left_index], 0.001);
424  // Check keyframe 3
425  EXPECT_NEAR(2.0, srv_edit.response.motion.keyframes[2].time_from_last, 0.001);
426  EXPECT_NEAR(0.12, srv_edit.response.motion.keyframes[2].pose[head_index], 0.001);
427  EXPECT_NEAR(0.22, srv_edit.response.motion.keyframes[2].pose[arm_left_index], 0.001);
428 
429  // Add a joint
430  play_motion_builder_msgs::ChangeJoints srv_change;
431  // Try to change joint inside a group (should fail)
432  srv_change.request.joints_to_add.push_back("arm_right_joint");
433  ros::service::call<play_motion_builder_msgs::ChangeJoints>(
434  "play_motion_builder_node/change_joints", srv_change);
435  ASSERT_FALSE(srv_change.response.ok) << srv_change.response.message;
436  EXPECT_EQ(2, srv_change.response.used_joints.size());
437  EXPECT_EQ("arm_left", srv_change.response.current_group);
438 
439  // Change joint group
440  srv_change.request.joints_to_add.clear();
441  srv_change.request.group = "both_arms";
442  ros::service::call<play_motion_builder_msgs::ChangeJoints>(
443  "play_motion_builder_node/change_joints", srv_change);
444  ASSERT_TRUE(srv_change.response.ok) << srv_change.response.message;
445  EXPECT_EQ(3, srv_change.response.used_joints.size());
446  EXPECT_EQ("both_arms", srv_change.response.current_group);
447 
448  // Remove extra joint
449  srv_change.request.joints_to_remove.push_back("head_joint");
450  srv_change.request.group = "";
451  ros::service::call<play_motion_builder_msgs::ChangeJoints>(
452  "play_motion_builder_node/change_joints", srv_change);
453  ASSERT_TRUE(srv_change.response.ok) << srv_change.response.message;
454  EXPECT_EQ(2, srv_change.response.used_joints.size());
455  EXPECT_EQ("both_arms", srv_change.response.current_group);
456 
457  // Change time increment of step 2
458  srv_edit.request.action = play_motion_builder_msgs::EditMotion::Request::EDIT_TIME;
459  srv_edit.request.step_id = 1;
460  srv_edit.request.time = 5.5;
461  ros::service::call<play_motion_builder_msgs::EditMotion>(
462  "play_motion_builder_node/edit_motion", srv_edit);
463  EXPECT_TRUE(srv_edit.response.ok) << srv_edit.response.message;
464  EXPECT_NEAR(5.5, srv_edit.response.motion.keyframes[1].time_from_last, 0.001);
465 
466  // Test storing a motion
467  play_motion_builder_msgs::StoreMotion srv_store;
468  srv_store.request.file_path = "/tmp/test_motion_edit.yaml";
469  srv_store.request.ros_name = "test_motion_edit";
470  ros::service::call<play_motion_builder_msgs::StoreMotion>(
471  "play_motion_builder_node/store_motion", srv_store);
472  EXPECT_TRUE(srv_store.response.ok) << srv_store.response.message;
473  // Check result is fine
474  // Compare files
475  std::string filepath;
476  nh.getParam("test_file_path_edit", filepath);
477  EXPECT_TRUE(compare_files(srv_store.request.file_path, filepath));
478 
479  // Stop motion builder
480  client.cancelGoal();
481  ros::Duration(0.1).sleep(); // Wait a moment
482 
483  // Make sure system is done
484  srv_edit.request.action = play_motion_builder_msgs::EditMotion::Request::LIST;
485  ros::service::call<play_motion_builder_msgs::EditMotion>(
486  "play_motion_builder_node/edit_motion", srv_edit);
487  EXPECT_FALSE(srv_edit.response.ok);
488  EXPECT_EQ("No motion being built", srv_edit.response.message);
489 }
490 
491 } // namespace pal
492 
493 int main(int argc, char** argv)
494 {
495  ros::init(argc, argv, "test_motion_builder");
496  ros::start();
497  testing::InitGoogleTest(&argc, argv);
498  return RUN_ALL_TESTS();
499 }
ros::NodeHandle
ros::start
ROSCPP_DECL void start()
ros::NodeHandle::createTimer
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
pal::TestBot::queue_
ros::CallbackQueue queue_
Definition: test_motion_builder.cpp:47
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
main
int main(int argc, char **argv)
Definition: test_motion_builder.cpp:493
actionlib::SimpleActionClient::waitForServer
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
pal::TestBot::joint_mutex_
std::mutex joint_mutex_
Definition: test_motion_builder.cpp:54
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
pal::TestBot::joints_
std::unordered_map< std::string, double > joints_
Definition: test_motion_builder.cpp:53
actionlib::SimpleActionClient::getState
SimpleClientGoalState getState() const
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
ros::NodeHandle::setCallbackQueue
void setCallbackQueue(CallbackQueueInterface *queue)
actionlib::SimpleActionClient::sendGoalAndWait
SimpleClientGoalState sendGoalAndWait(const Goal &goal, const ros::Duration &execute_timeout=ros::Duration(0, 0), const ros::Duration &preempt_timeout=ros::Duration(0, 0))
pal::TestBot::changeJointPos
void changeJointPos(double left_arm, double right_arm, double head)
Definition: test_motion_builder.cpp:36
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
pal::range_equal
bool range_equal(InputIterator1 first1, InputIterator1 last1, InputIterator2 first2, InputIterator2 last2)
Definition: test_motion_builder.cpp:73
pal::TestBot::nh_
ros::NodeHandle nh_
Definition: test_motion_builder.cpp:46
actionlib::SimpleActionClient::sendGoal
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
pal::TestBot::timerCb
void timerCb(const ros::TimerEvent &)
Definition: test_motion_builder.cpp:56
ros::Time::now
static Time now()
pal::TestBot::as_
ros::AsyncSpinner as_
Definition: test_motion_builder.cpp:48
pal::TEST
TEST(MotionBuilderTest, actionNotStartedTest)
Definition: test_motion_builder.cpp:100
ros::Duration::sleep
bool sleep() const
ros::spinOnce
ROSCPP_DECL void spinOnce()
simple_action_client.h
ros::service::waitForService
ROSCPP_DECL bool waitForService(const std::string &service_name, int32_t timeout)
pal::TestBot::joint_pub_
ros::Publisher joint_pub_
Definition: test_motion_builder.cpp:50

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