pr2_beer.cpp
Go to the documentation of this file.
1 // Copyright 1996-2019 Cyberbotics Ltd.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 /*
16  * Description: controller for the PR2 to grab a can in a fridge and serve it
17  */
18 
19 #include <signal.h>
20 #include <webots_ros/sensor_set.h>
21 #include "ros/ros.h"
22 
23 #include <webots_ros/set_float.h>
24 #include <webots_ros/set_int.h>
25 
26 #include <std_msgs/Float64.h>
27 #include <std_msgs/Float64MultiArray.h>
28 #include <std_msgs/String.h>
29 
30 #define TIME_STEP 32
31 #define ACCELERATION 10
32 #define FWD_SPEED 3.
33 #define ROTATE_SPEED 6
34 
35 static int controllerCount;
36 static std::vector<std::string> controllerList;
37 static std::string controllerName;
38 static double wheelPosition = 0;
39 static double torsoPosition = 0;
40 static double rotationPosition = 0;
41 static double lShoulderPosition = 0;
42 static double rShoulderPosition = 0;
43 static double llFingerPosition = 0;
44 static double rrFingerPosition = 0;
45 static unsigned int lBumper = 0;
46 static unsigned int rBumper = 0;
47 static double imu_rpy[3] = {0, 0, 0};
48 
49 ros::ServiceClient timeStepClient;
50 webots_ros::set_int timeStepSrv;
51 
52 ros::ServiceClient fllWheelPositionClient;
53 ros::ServiceClient flrWheelPositionClient;
54 ros::ServiceClient frlWheelPositionClient;
55 ros::ServiceClient frrWheelPositionClient;
56 ros::ServiceClient bllWheelPositionClient;
57 ros::ServiceClient blrWheelPositionClient;
58 ros::ServiceClient brlWheelPositionClient;
59 ros::ServiceClient brrWheelPositionClient;
60 
61 webots_ros::set_float fllWheelPositionSrv;
62 webots_ros::set_float flrWheelPositionSrv;
63 webots_ros::set_float frlWheelPositionSrv;
64 webots_ros::set_float frrWheelPositionSrv;
65 webots_ros::set_float bllWheelPositionSrv;
66 webots_ros::set_float blrWheelPositionSrv;
67 webots_ros::set_float brlWheelPositionSrv;
68 webots_ros::set_float brrWheelPositionSrv;
69 
70 ros::ServiceClient fllWheelSpeedClient;
71 ros::ServiceClient flrWheelSpeedClient;
72 ros::ServiceClient frlWheelSpeedClient;
73 ros::ServiceClient frrWheelSpeedClient;
74 ros::ServiceClient bllWheelSpeedClient;
75 ros::ServiceClient blrWheelSpeedClient;
76 ros::ServiceClient brlWheelSpeedClient;
77 ros::ServiceClient brrWheelSpeedClient;
78 
79 webots_ros::set_float fllWheelSpeedSrv;
80 webots_ros::set_float flrWheelSpeedSrv;
81 webots_ros::set_float frlWheelSpeedSrv;
82 webots_ros::set_float frrWheelSpeedSrv;
83 webots_ros::set_float bllWheelSpeedSrv;
84 webots_ros::set_float blrWheelSpeedSrv;
85 webots_ros::set_float brlWheelSpeedSrv;
86 webots_ros::set_float brrWheelSpeedSrv;
87 
88 ros::ServiceClient flRotationClient;
89 ros::ServiceClient frRotationClient;
90 ros::ServiceClient blRotationClient;
91 ros::ServiceClient brRotationClient;
92 
93 webots_ros::set_float flRotationSrv;
94 webots_ros::set_float frRotationSrv;
95 webots_ros::set_float blRotationSrv;
96 webots_ros::set_float brRotationSrv;
97 
98 void fllWheelCallback(const std_msgs::Float64::ConstPtr &value) {
99  wheelPosition = value->data;
100 }
101 void torsoCallback(const std_msgs::Float64::ConstPtr &value) {
102  torsoPosition = value->data;
103 }
104 void flRotationCallback(const std_msgs::Float64::ConstPtr &value) {
105  rotationPosition = value->data;
106 }
107 void lShoulderCallback(const std_msgs::Float64::ConstPtr &value) {
108  lShoulderPosition = value->data;
109 }
110 void rShoulderCallback(const std_msgs::Float64::ConstPtr &value) {
111  rShoulderPosition = value->data;
112 }
113 void llFingerCallback(const std_msgs::Float64::ConstPtr &value) {
114  llFingerPosition = value->data;
115 }
116 void rrFingerCallback(const std_msgs::Float64::ConstPtr &value) {
117  rrFingerPosition = value->data;
118 }
119 void lBumperCallback(const std_msgs::Float64::ConstPtr &value) {
120  lBumper = value->data;
121 }
122 void rBumperCallback(const std_msgs::Float64::ConstPtr &value) {
123  rBumper = value->data;
124 }
125 void imuCallback(const std_msgs::Float64MultiArray::ConstPtr &values) {
126  int i = 0;
127  for (std::vector<double>::const_iterator it = values->data.begin(); it != values->data.end(); ++it) {
128  imu_rpy[i] = *it;
129  i++;
130  }
131 }
132 
133 void move(double speed) {
134  fllWheelPositionSrv.request.position = INFINITY;
135  flrWheelPositionSrv.request.position = INFINITY;
136  frlWheelPositionSrv.request.position = INFINITY;
137  frrWheelPositionSrv.request.position = INFINITY;
138  bllWheelPositionSrv.request.position = INFINITY;
139  blrWheelPositionSrv.request.position = INFINITY;
140  brlWheelPositionSrv.request.position = INFINITY;
141  brrWheelPositionSrv.request.position = INFINITY;
142 
143  fllWheelPositionClient.call(fllWheelPositionSrv);
144  flrWheelPositionClient.call(flrWheelPositionSrv);
145  frlWheelPositionClient.call(frlWheelPositionSrv);
146  frrWheelPositionClient.call(frrWheelPositionSrv);
147  bllWheelPositionClient.call(bllWheelPositionSrv);
148  blrWheelPositionClient.call(blrWheelPositionSrv);
149  brlWheelPositionClient.call(brlWheelPositionSrv);
150  brrWheelPositionClient.call(brrWheelPositionSrv);
151 
152  fllWheelSpeedSrv.request.value = speed;
153  flrWheelSpeedSrv.request.value = speed;
154  frlWheelSpeedSrv.request.value = speed;
155  frrWheelSpeedSrv.request.value = speed;
156  bllWheelSpeedSrv.request.value = speed;
157  blrWheelSpeedSrv.request.value = speed;
158  brlWheelSpeedSrv.request.value = speed;
159  brrWheelSpeedSrv.request.value = speed;
160 
161  fllWheelSpeedClient.call(fllWheelSpeedSrv);
162  flrWheelSpeedClient.call(flrWheelSpeedSrv);
163  frlWheelSpeedClient.call(frlWheelSpeedSrv);
164  frrWheelSpeedClient.call(frrWheelSpeedSrv);
165  bllWheelSpeedClient.call(bllWheelSpeedSrv);
166  blrWheelSpeedClient.call(blrWheelSpeedSrv);
167  brlWheelSpeedClient.call(brlWheelSpeedSrv);
168  brrWheelSpeedClient.call(brrWheelSpeedSrv);
169 }
170 
171 void rotate(double speed) {
172  fllWheelPositionSrv.request.position = INFINITY;
173  flrWheelPositionSrv.request.position = INFINITY;
174  frlWheelPositionSrv.request.position = INFINITY;
175  frrWheelPositionSrv.request.position = INFINITY;
176  bllWheelPositionSrv.request.position = INFINITY;
177  blrWheelPositionSrv.request.position = INFINITY;
178  brlWheelPositionSrv.request.position = INFINITY;
179  brrWheelPositionSrv.request.position = INFINITY;
180 
181  fllWheelPositionClient.call(fllWheelPositionSrv);
182  flrWheelPositionClient.call(flrWheelPositionSrv);
183  frlWheelPositionClient.call(frlWheelPositionSrv);
184  frrWheelPositionClient.call(frrWheelPositionSrv);
185  bllWheelPositionClient.call(bllWheelPositionSrv);
186  blrWheelPositionClient.call(blrWheelPositionSrv);
187  brlWheelPositionClient.call(brlWheelPositionSrv);
188  brrWheelPositionClient.call(brrWheelPositionSrv);
189 
190  fllWheelSpeedSrv.request.value = -speed;
191  flrWheelSpeedSrv.request.value = -speed;
192  frlWheelSpeedSrv.request.value = speed;
193  frrWheelSpeedSrv.request.value = speed;
194  bllWheelSpeedSrv.request.value = -speed;
195  blrWheelSpeedSrv.request.value = -speed;
196  brlWheelSpeedSrv.request.value = speed;
197  brrWheelSpeedSrv.request.value = speed;
198 
199  fllWheelSpeedClient.call(fllWheelSpeedSrv);
200  flrWheelSpeedClient.call(flrWheelSpeedSrv);
201  frlWheelSpeedClient.call(frlWheelSpeedSrv);
202  frrWheelSpeedClient.call(frrWheelSpeedSrv);
203  bllWheelSpeedClient.call(bllWheelSpeedSrv);
204  blrWheelSpeedClient.call(blrWheelSpeedSrv);
205  brlWheelSpeedClient.call(brlWheelSpeedSrv);
206  brrWheelSpeedClient.call(brrWheelSpeedSrv);
207 }
208 
209 void rotate_wheels(double angle) {
210  int speed = -ROTATE_SPEED;
211 
212  if (angle > rotationPosition)
213  speed = ROTATE_SPEED;
214 
215  flRotationSrv.request.position = angle;
216  frRotationSrv.request.position = -angle;
217  blRotationSrv.request.position = -angle;
218  brRotationSrv.request.position = angle;
219 
220  flRotationClient.call(flRotationSrv);
221  frRotationClient.call(frRotationSrv);
222  blRotationClient.call(blRotationSrv);
223  brRotationClient.call(brRotationSrv);
224 
225  fllWheelPositionSrv.request.position = INFINITY;
226  flrWheelPositionSrv.request.position = INFINITY;
227  frlWheelPositionSrv.request.position = INFINITY;
228  frrWheelPositionSrv.request.position = INFINITY;
229  bllWheelPositionSrv.request.position = INFINITY;
230  blrWheelPositionSrv.request.position = INFINITY;
231  brlWheelPositionSrv.request.position = INFINITY;
232  brrWheelPositionSrv.request.position = INFINITY;
233 
234  fllWheelPositionClient.call(fllWheelPositionSrv);
235  flrWheelPositionClient.call(flrWheelPositionSrv);
236  frlWheelPositionClient.call(frlWheelPositionSrv);
237  frrWheelPositionClient.call(frrWheelPositionSrv);
238  bllWheelPositionClient.call(bllWheelPositionSrv);
239  blrWheelPositionClient.call(blrWheelPositionSrv);
240  brlWheelPositionClient.call(brlWheelPositionSrv);
241  brrWheelPositionClient.call(brrWheelPositionSrv);
242 
243  fllWheelSpeedSrv.request.value = -speed;
244  flrWheelSpeedSrv.request.value = speed;
245  frlWheelSpeedSrv.request.value = speed;
246  frrWheelSpeedSrv.request.value = -speed;
247  bllWheelSpeedSrv.request.value = speed;
248  blrWheelSpeedSrv.request.value = -speed;
249  brlWheelSpeedSrv.request.value = -speed;
250  brrWheelSpeedSrv.request.value = speed;
251 
252  fllWheelSpeedClient.call(fllWheelSpeedSrv);
253  flrWheelSpeedClient.call(flrWheelSpeedSrv);
254  frlWheelSpeedClient.call(frlWheelSpeedSrv);
255  frrWheelSpeedClient.call(frrWheelSpeedSrv);
256  bllWheelSpeedClient.call(bllWheelSpeedSrv);
257  blrWheelSpeedClient.call(blrWheelSpeedSrv);
258  brlWheelSpeedClient.call(brlWheelSpeedSrv);
259  brrWheelSpeedClient.call(brrWheelSpeedSrv);
260 }
261 
262 // catch names of the controllers availables on ROS network
263 void controllerNameCallback(const std_msgs::String::ConstPtr &name) {
264  controllerCount++;
265  controllerList.push_back(name->data);
266  ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());
267 }
268 
269 void quit(int sig) {
270  timeStepSrv.request.value = 0;
271  timeStepClient.call(timeStepSrv);
272  ROS_INFO("User stopped the 'pr2_beer' node.");
273  ros::shutdown();
274  exit(0);
275 }
276 
277 int main(int argc, char **argv) {
278  // create a node named 'pr2_beer' on ROS network
279  ros::init(argc, argv, "pr2_beer", ros::init_options::AnonymousName);
280  ros::NodeHandle n;
281 
282  signal(SIGINT, quit);
283 
284  // subscribe to the topic model_name to get the list of available controllers
285  ros::Subscriber nameSub = n.subscribe("model_name", 100, controllerNameCallback);
286  while (controllerCount == 0 || controllerCount < nameSub.getNumPublishers()) {
287  // the spinOnce() function is called multiple times to be sure to not skip any controller
288  ros::spinOnce();
289  ros::spinOnce();
290  ros::spinOnce();
291  }
292  ros::spinOnce();
293 
294  // if there is more than one controller available, let the user choose
295  if (controllerCount == 1)
296  controllerName = controllerList[0];
297  else {
298  int wantedController = 0;
299  std::cout << "Choose the # of the controller you want to use:\n";
300  std::cin >> wantedController;
301  if (1 <= wantedController && wantedController <= controllerCount)
302  controllerName = controllerList[wantedController - 1];
303  else {
304  ROS_ERROR("Invalid number for controller choice.");
305  return 1;
306  }
307  }
308  // leave topic once it's not necessary anymore
309  nameSub.shutdown();
310 
311  ros::ServiceClient fllWheelAccelerationClient;
312  ros::ServiceClient flrWheelAccelerationClient;
313  ros::ServiceClient frlWheelAccelerationClient;
314  ros::ServiceClient frrWheelAccelerationClient;
315  ros::ServiceClient bllWheelAccelerationClient;
316  ros::ServiceClient blrWheelAccelerationClient;
317  ros::ServiceClient brlWheelAccelerationClient;
318  ros::ServiceClient brrWheelAccelerationClient;
319 
320  webots_ros::set_float fllWheelAccelerationSrv;
321  webots_ros::set_float flrWheelAccelerationSrv;
322  webots_ros::set_float frlWheelAccelerationSrv;
323  webots_ros::set_float frrWheelAccelerationSrv;
324  webots_ros::set_float bllWheelAccelerationSrv;
325  webots_ros::set_float blrWheelAccelerationSrv;
326  webots_ros::set_float brlWheelAccelerationSrv;
327  webots_ros::set_float brrWheelAccelerationSrv;
328 
329  ros::ServiceClient headTiltClient;
330  webots_ros::set_float headTiltSrv;
331  ros::ServiceClient torsoClient;
332  webots_ros::set_float torsoSrv;
333  ros::ServiceClient lShoulderRollClient;
334  webots_ros::set_float lShoulderRollSrv;
335  ros::ServiceClient lShoulderLiftClient;
336  webots_ros::set_float lShoulderLiftSrv;
337  ros::ServiceClient rShoulderRollClient;
338  webots_ros::set_float rShoulderRollSrv;
339  ros::ServiceClient rShoulderLiftClient;
340  webots_ros::set_float rShoulderLiftSrv;
341  ros::ServiceClient ruArmRollClient;
342  webots_ros::set_float ruArmRollSrv;
343  ros::ServiceClient rElbowLiftClient;
344  webots_ros::set_float rElbowLiftSrv;
345  ros::ServiceClient lElbowLiftClient;
346  webots_ros::set_float lElbowLiftSrv;
347  ros::ServiceClient rWristRollClient;
348  webots_ros::set_float rWristRollSrv;
349  ros::ServiceClient llFingerClient;
350  webots_ros::set_float llFingerSrv;
351  ros::ServiceClient lrFingerClient;
352  webots_ros::set_float lrFingerSrv;
353  ros::ServiceClient lrFingerTipClient;
354  webots_ros::set_float lrFingerTipSrv;
355  ros::ServiceClient llFingerTipClient;
356  webots_ros::set_float llFingerTipSrv;
357  ros::ServiceClient rlFingerClient;
358  webots_ros::set_float rlFingerSrv;
359  ros::ServiceClient rrFingerClient;
360  webots_ros::set_float rrFingerSrv;
361 
362  ros::ServiceClient rlFingerSpeedClient;
363  webots_ros::set_float rlFingerSpeedSrv;
364  ros::ServiceClient rrFingerSpeedClient;
365  webots_ros::set_float rrFingerSpeedSrv;
366  ros::ServiceClient llFingerSpeedClient;
367  webots_ros::set_float llFingerSpeedSrv;
368  ros::ServiceClient lrFingerSpeedClient;
369  webots_ros::set_float lrFingerSpeedSrv;
370  ros::ServiceClient lShoulderRollSpeedClient;
371  webots_ros::set_float lShoulderRollSpeedSrv;
372  ros::ServiceClient rShoulderRollSpeedClient;
373  webots_ros::set_float rShoulderRollSpeedSrv;
374 
375  ros::ServiceClient fllWheelSetClient;
376  ros::ServiceClient flRotationSetClient;
377  ros::ServiceClient torsoSetClient;
378  ros::ServiceClient llFingerSetClient;
379  ros::ServiceClient rrFingerSetClient;
380  ros::ServiceClient lShoulderSetClient;
381  ros::ServiceClient rShoulderSetClient;
382  ros::ServiceClient lBumperSetClient;
383  ros::ServiceClient rBumperSetClient;
384  ros::ServiceClient imuSetClient;
385  ros::ServiceClient cameraSetClient;
386  webots_ros::sensor_set setSensorSrv;
387  setSensorSrv.request.period = TIME_STEP;
388 
389  timeStepClient = n.serviceClient<webots_ros::set_int>(controllerName + "/robot/time_step");
390  timeStepSrv.request.value = TIME_STEP;
391 
392  fllWheelPositionClient = n.serviceClient<webots_ros::set_float>(controllerName + "/fl_caster_l_wheel_joint/set_position");
393  flrWheelPositionClient = n.serviceClient<webots_ros::set_float>(controllerName + "/fl_caster_r_wheel_joint/set_position");
394  frlWheelPositionClient = n.serviceClient<webots_ros::set_float>(controllerName + "/fr_caster_l_wheel_joint/set_position");
395  frrWheelPositionClient = n.serviceClient<webots_ros::set_float>(controllerName + "/fr_caster_r_wheel_joint/set_position");
396  bllWheelPositionClient = n.serviceClient<webots_ros::set_float>(controllerName + "/bl_caster_l_wheel_joint/set_position");
397  blrWheelPositionClient = n.serviceClient<webots_ros::set_float>(controllerName + "/bl_caster_r_wheel_joint/set_position");
398  brlWheelPositionClient = n.serviceClient<webots_ros::set_float>(controllerName + "/br_caster_l_wheel_joint/set_position");
399  brrWheelPositionClient = n.serviceClient<webots_ros::set_float>(controllerName + "/br_caster_r_wheel_joint/set_position");
400 
401  fllWheelSpeedClient = n.serviceClient<webots_ros::set_float>(controllerName + "/fl_caster_l_wheel_joint/set_velocity");
402  flrWheelSpeedClient = n.serviceClient<webots_ros::set_float>(controllerName + "/fl_caster_r_wheel_joint/set_velocity");
403  frlWheelSpeedClient = n.serviceClient<webots_ros::set_float>(controllerName + "/fr_caster_l_wheel_joint/set_velocity");
404  frrWheelSpeedClient = n.serviceClient<webots_ros::set_float>(controllerName + "/fr_caster_r_wheel_joint/set_velocity");
405  bllWheelSpeedClient = n.serviceClient<webots_ros::set_float>(controllerName + "/bl_caster_l_wheel_joint/set_velocity");
406  blrWheelSpeedClient = n.serviceClient<webots_ros::set_float>(controllerName + "/bl_caster_r_wheel_joint/set_velocity");
407  brlWheelSpeedClient = n.serviceClient<webots_ros::set_float>(controllerName + "/br_caster_l_wheel_joint/set_velocity");
408  brrWheelSpeedClient = n.serviceClient<webots_ros::set_float>(controllerName + "/br_caster_r_wheel_joint/set_velocity");
409 
410  fllWheelAccelerationClient =
411  n.serviceClient<webots_ros::set_float>(controllerName + "/fl_caster_l_wheel_joint/set_acceleration");
412  flrWheelAccelerationClient =
413  n.serviceClient<webots_ros::set_float>(controllerName + "/fl_caster_r_wheel_joint/set_acceleration");
414  frlWheelAccelerationClient =
415  n.serviceClient<webots_ros::set_float>(controllerName + "/fr_caster_l_wheel_joint/set_acceleration");
416  frrWheelAccelerationClient =
417  n.serviceClient<webots_ros::set_float>(controllerName + "/fr_caster_r_wheel_joint/set_acceleration");
418  bllWheelAccelerationClient =
419  n.serviceClient<webots_ros::set_float>(controllerName + "/bl_caster_l_wheel_joint/set_acceleration");
420  blrWheelAccelerationClient =
421  n.serviceClient<webots_ros::set_float>(controllerName + "/bl_caster_r_wheel_joint/set_acceleration");
422  brlWheelAccelerationClient =
423  n.serviceClient<webots_ros::set_float>(controllerName + "/br_caster_l_wheel_joint/set_acceleration");
424  brrWheelAccelerationClient =
425  n.serviceClient<webots_ros::set_float>(controllerName + "/br_caster_r_wheel_joint/set_acceleration");
426 
427  flRotationClient = n.serviceClient<webots_ros::set_float>(controllerName + "/fl_caster_rotation_joint/set_position");
428  frRotationClient = n.serviceClient<webots_ros::set_float>(controllerName + "/fr_caster_rotation_joint/set_position");
429  blRotationClient = n.serviceClient<webots_ros::set_float>(controllerName + "/bl_caster_rotation_joint/set_position");
430  brRotationClient = n.serviceClient<webots_ros::set_float>(controllerName + "/br_caster_rotation_joint/set_position");
431 
432  headTiltClient = n.serviceClient<webots_ros::set_float>(controllerName + "/head_tilt_joint/set_position");
433  torsoClient = n.serviceClient<webots_ros::set_float>(controllerName + "/torso_lift_joint/set_position");
434  lShoulderRollClient = n.serviceClient<webots_ros::set_float>(controllerName + "/l_shoulder_pan_joint/set_position");
435  rShoulderRollClient = n.serviceClient<webots_ros::set_float>(controllerName + "/r_shoulder_pan_joint/set_position");
436  lShoulderLiftClient = n.serviceClient<webots_ros::set_float>(controllerName + "/l_shoulder_lift_joint/set_position");
437  rShoulderLiftClient = n.serviceClient<webots_ros::set_float>(controllerName + "/r_shoulder_lift_joint/set_position");
438  ruArmRollClient = n.serviceClient<webots_ros::set_float>(controllerName + "/r_upper_arm_roll_joint/set_position");
439  lElbowLiftClient = n.serviceClient<webots_ros::set_float>(controllerName + "/l_elbow_flex_joint/set_position");
440  rElbowLiftClient = n.serviceClient<webots_ros::set_float>(controllerName + "/r_elbow_flex_joint/set_position");
441  rWristRollClient = n.serviceClient<webots_ros::set_float>(controllerName + "/r_wrist_roll_joint/set_position");
442  llFingerClient = n.serviceClient<webots_ros::set_float>(controllerName + "/l_gripper_l_finger_joint/set_position");
443  lrFingerClient = n.serviceClient<webots_ros::set_float>(controllerName + "/l_gripper_r_finger_joint/set_position");
444  rlFingerClient = n.serviceClient<webots_ros::set_float>(controllerName + "/r_gripper_l_finger_joint/set_position");
445  rrFingerClient = n.serviceClient<webots_ros::set_float>(controllerName + "/r_gripper_r_finger_joint/set_position");
446  llFingerTipClient = n.serviceClient<webots_ros::set_float>(controllerName + "/l_gripper_l_finger_tip_joint/set_position");
447  lrFingerTipClient = n.serviceClient<webots_ros::set_float>(controllerName + "/l_gripper_r_finger_tip_joint/set_position");
448 
449  llFingerSpeedClient = n.serviceClient<webots_ros::set_float>(controllerName + "/l_gripper_l_finger_joint/set_velocity");
450  lrFingerSpeedClient = n.serviceClient<webots_ros::set_float>(controllerName + "/l_gripper_r_finger_joint/set_velocity");
451  rlFingerSpeedClient = n.serviceClient<webots_ros::set_float>(controllerName + "/r_gripper_l_finger_joint/set_velocity");
452  rrFingerSpeedClient = n.serviceClient<webots_ros::set_float>(controllerName + "/r_gripper_r_finger_joint/set_velocity");
453  lShoulderRollSpeedClient = n.serviceClient<webots_ros::set_float>(controllerName + "/l_shoulder_pan_joint/set_velocity");
454  rShoulderRollSpeedClient = n.serviceClient<webots_ros::set_float>(controllerName + "/r_shoulder_pan_joint/set_velocity");
455 
456  fllWheelSetClient =
457  n.serviceClient<webots_ros::sensor_set>(controllerName + "/fl_caster_l_wheel_joint/position_sensor/set_sensor");
458  flRotationSetClient =
459  n.serviceClient<webots_ros::sensor_set>(controllerName + "/fl_caster_rotation_joint/position_sensor/set_sensor");
460  torsoSetClient = n.serviceClient<webots_ros::sensor_set>(controllerName + "/torso_lift_joint/position_sensor/set_sensor");
461  llFingerSetClient =
462  n.serviceClient<webots_ros::sensor_set>(controllerName + "/l_gripper_l_finger_joint/position_sensor/set_sensor");
463  rrFingerSetClient =
464  n.serviceClient<webots_ros::sensor_set>(controllerName + "/r_gripper_r_finger_joint/position_sensor/set_sensor");
465  lShoulderSetClient =
466  n.serviceClient<webots_ros::sensor_set>(controllerName + "/l_shoulder_pan_joint/position_sensor/set_sensor");
467  rShoulderSetClient =
468  n.serviceClient<webots_ros::sensor_set>(controllerName + "/r_shoulder_pan_joint/position_sensor/set_sensor");
469  lBumperSetClient =
470  n.serviceClient<webots_ros::sensor_set>(controllerName + "/r_gripper_l_finger_tip_contact_sensor/set_sensor");
471  rBumperSetClient =
472  n.serviceClient<webots_ros::sensor_set>(controllerName + "/l_gripper_l_finger_tip_contact_sensor/set_sensor");
473  imuSetClient = n.serviceClient<webots_ros::sensor_set>(controllerName + "/imu_sensor/set_sensor");
474  cameraSetClient = n.serviceClient<webots_ros::sensor_set>(controllerName + "/wide_stereo_l_stereo_camera_sensor/set_sensor");
475 
476  fllWheelAccelerationSrv.request.value = ACCELERATION;
477  fllWheelAccelerationClient.call(fllWheelAccelerationSrv);
478  flrWheelAccelerationSrv.request.value = ACCELERATION;
479  flrWheelAccelerationClient.call(flrWheelAccelerationSrv);
480  frlWheelAccelerationSrv.request.value = ACCELERATION;
481  frlWheelAccelerationClient.call(frlWheelAccelerationSrv);
482  frrWheelAccelerationSrv.request.value = ACCELERATION;
483  frrWheelAccelerationClient.call(frrWheelAccelerationSrv);
484  bllWheelAccelerationSrv.request.value = ACCELERATION;
485  bllWheelAccelerationClient.call(bllWheelAccelerationSrv);
486  blrWheelAccelerationSrv.request.value = ACCELERATION;
487  blrWheelAccelerationClient.call(blrWheelAccelerationSrv);
488  brlWheelAccelerationSrv.request.value = ACCELERATION;
489  brlWheelAccelerationClient.call(brlWheelAccelerationSrv);
490  brrWheelAccelerationSrv.request.value = ACCELERATION;
491  brrWheelAccelerationClient.call(brrWheelAccelerationSrv);
492 
493  fllWheelSetClient.call(setSensorSrv);
494  flRotationSetClient.call(setSensorSrv);
495  torsoSetClient.call(setSensorSrv);
496  llFingerSetClient.call(setSensorSrv);
497  rrFingerSetClient.call(setSensorSrv);
498  lShoulderSetClient.call(setSensorSrv);
499  rShoulderSetClient.call(setSensorSrv);
500  lBumperSetClient.call(setSensorSrv);
501  rBumperSetClient.call(setSensorSrv);
502  imuSetClient.call(setSensorSrv);
503  cameraSetClient.call(setSensorSrv);
504 
505  ros::Subscriber fllWheelSubscriber =
506  n.subscribe(controllerName + "/fl_caster_l_wheel_joint/position_sensor/32", 1, fllWheelCallback);
507  ros::Subscriber flRotationSubscriber =
508  n.subscribe(controllerName + "/fl_caster_rotation_joint/position_sensor/32", 1, flRotationCallback);
509  ros::Subscriber torsoSubscriber;
510  ros::Subscriber lBumperSubscriber;
511  ros::Subscriber rBumperSubscriber;
512  ros::Subscriber rrFingerSubscriber;
513  ros::Subscriber lShoulderSubscriber;
514  ros::Subscriber llFingerSubscriber;
515  ros::Subscriber rShoulderSubscriber;
516  ros::Subscriber imuSubscriber;
517 
518  int i = 0;
519  int step = 0;
520  int sequence = 0;
521  double finger_pos = 0;
522 
523  // idle state
524  lShoulderLiftSrv.request.value = 1.35;
525  lShoulderLiftClient.call(lShoulderLiftSrv);
526  rShoulderLiftSrv.request.value = 1.35;
527  rShoulderLiftClient.call(rShoulderLiftSrv);
528  lElbowLiftSrv.request.value = -2.2;
529  lElbowLiftClient.call(lElbowLiftSrv);
530  rElbowLiftSrv.request.value = -2.2;
531  rElbowLiftClient.call(rElbowLiftSrv);
532  llFingerSrv.request.value = 0.4;
533  llFingerClient.call(llFingerSrv);
534  lrFingerSrv.request.value = 0.4;
535  lrFingerClient.call(lrFingerSrv);
536  rlFingerSrv.request.value = 0.05;
537  rlFingerClient.call(rlFingerSrv);
538  rrFingerSrv.request.value = 0.2;
539  rrFingerClient.call(rrFingerSrv);
540  torsoSrv.request.value = 0.33;
541  torsoClient.call(torsoSrv);
542 
543  while (ros::ok()) {
544  step++;
545  if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success)
546  ROS_ERROR("Failed to call service time_step for next step.");
547 
548  ros::spinOnce();
549  ros::spinOnce();
550 
551  // turn wheels to be able to rotate on itself
552  if (rotationPosition > -0.78 && sequence < 2) {
553  if (sequence == 0) {
554  rotate_wheels(-0.785);
555  imuSubscriber = n.subscribe(controllerName + "/imu_sensor/32", 1, imuCallback);
556  }
557  sequence = 1;
558  }
559  // turn 180° on itself to face fridge
560  else if (imu_rpy[2] > -2.75 && sequence < 3) {
561  if (sequence == 1) {
562  ROS_INFO("PR2 rotates to face the fridge.");
563  rotate(-ROTATE_SPEED);
564  }
565  sequence = 2;
566  }
567  // turn wheels to be able to go forward
568  else if (rotationPosition < 0 && sequence < 4) {
569  if (sequence == 2) {
570  rotate_wheels(0);
571  imuSubscriber.shutdown();
572  }
573  sequence = 3;
574  }
575  // move in front of the fridge
576  else if (wheelPosition < 45 && sequence < 5) {
577  if (sequence == 3) {
578  ROS_INFO("PR2 moves to the fridge.");
579  flRotationSubscriber.shutdown();
580  torsoSubscriber = n.subscribe(controllerName + "/torso_lift_joint/position_sensor/32", 1, torsoCallback);
581  move(FWD_SPEED);
582  }
583  sequence = 4;
584  }
585  // move right hand in front of the door's handle
586  else if (torsoPosition < 0.325 && sequence < 6) {
587  if (sequence == 4) {
588  ROS_INFO("PR2 places its right hand in front of the door's handle.");
589  move(0);
590  rShoulderRollSrv.request.value = 0.41;
591  rShoulderRollClient.call(rShoulderRollSrv);
592  rShoulderRollSpeedSrv.request.value = 0.21;
593  rShoulderRollSpeedClient.call(rShoulderRollSpeedSrv);
594  rShoulderLiftSrv.request.value = -0.3;
595  rShoulderLiftClient.call(rShoulderLiftSrv);
596  ruArmRollSrv.request.value = -3.14;
597  ruArmRollClient.call(ruArmRollSrv);
598  rElbowLiftSrv.request.value = -0.3;
599  rElbowLiftClient.call(rElbowLiftSrv);
600  rWristRollSrv.request.value = 1.57;
601  rWristRollClient.call(rWristRollSrv);
602  }
603  sequence = 5;
604  }
605  // move the hand on the handle
606  else if (wheelPosition < 46.5 && sequence < 7) {
607  if (sequence == 5) {
608  ROS_INFO("PR2 grabs the handle.");
609  torsoSubscriber.shutdown();
610  rBumperSubscriber =
611  n.subscribe(controllerName + "/r_gripper_l_finger_tip_contact_sensor/32_bumper", 1, rBumperCallback);
612  move(FWD_SPEED / 3);
613  }
614  sequence = 6;
615  }
616  // grab the handle of the door
617  else if (!rBumper && sequence < 8) {
618  if (sequence == 6) {
619  move(0);
620  rlFingerSrv.request.value = 0;
621  rlFingerClient.call(rlFingerSrv);
622  rrFingerSrv.request.value = 0;
623  rrFingerClient.call(rrFingerSrv);
624  }
625  sequence = 7;
626  }
627  // open the door
628  else if (wheelPosition > 39 && sequence < 9) {
629  if (sequence == 7) {
630  ROS_INFO("PR2 opens the door.");
631  rBumperSubscriber.shutdown();
632  rrFingerSubscriber = n.subscribe(controllerName + "/r_gripper_r_finger_joint/position_sensor/32", 1, rrFingerCallback);
633  rlFingerSpeedSrv.request.value = 0;
634  rlFingerSpeedClient.call(rlFingerSpeedSrv);
635  rrFingerSpeedSrv.request.value = 0;
636  rrFingerSpeedClient.call(rrFingerSpeedSrv);
637  rShoulderRollSrv.request.value = -0.25;
638  rShoulderRollClient.call(rShoulderRollSrv);
639  move(-FWD_SPEED / 2);
640  }
641  sequence = 8;
642  }
643  // release the door's handle
644  else if (rrFingerPosition < 0.39 && sequence < 10) {
645  if (sequence == 8) {
646  move(0);
647  rlFingerSrv.request.value = 0.4;
648  rlFingerClient.call(rlFingerSrv);
649  rrFingerSrv.request.value = 0.4;
650  rrFingerClient.call(rrFingerSrv);
651  rlFingerSpeedSrv.request.value = 0.2;
652  rlFingerSpeedClient.call(rlFingerSpeedSrv);
653  rrFingerSpeedSrv.request.value = 0.2;
654  rrFingerSpeedClient.call(rrFingerSpeedSrv);
655  rShoulderRollSrv.request.value = -0.5;
656  rShoulderRollClient.call(rShoulderRollSrv);
657  rShoulderRollSpeedSrv.request.value = 1;
658  rShoulderRollSpeedClient.call(rShoulderRollSpeedSrv);
659  }
660  sequence = 9;
661  }
662  // pull back right arm, extend left one and move a little forward
663  else if (wheelPosition < 42 && sequence < 11) {
664  if (sequence == 9) {
665  rrFingerSubscriber.shutdown();
666  lShoulderSubscriber = n.subscribe(controllerName + "/l_shoulder_pan_joint/position_sensor/32", 1, lShoulderCallback);
667  lShoulderLiftSrv.request.value = 0;
668  lShoulderLiftClient.call(lShoulderLiftSrv);
669  rShoulderLiftSrv.request.value = 1.2;
670  rShoulderLiftClient.call(rShoulderLiftSrv);
671  ruArmRollSrv.request.value = 0;
672  ruArmRollClient.call(ruArmRollSrv);
673  lElbowLiftSrv.request.value = 0;
674  lElbowLiftClient.call(lElbowLiftSrv);
675  move(FWD_SPEED / 3);
676  }
677  sequence = 10;
678  }
679  // push the door to stay wide open
680  else if (lShoulderPosition > -0.55 && sequence < 12) {
681  if (sequence == 10) {
682  move(0);
683  rElbowLiftSrv.request.value = -2.3;
684  rElbowLiftClient.call(rElbowLiftSrv);
685  lShoulderRollSrv.request.value = -0.57;
686  lShoulderRollClient.call(lShoulderRollSrv);
687  lShoulderRollSpeedSrv.request.value = 0.6;
688  lShoulderRollSpeedClient.call(lShoulderRollSpeedSrv);
689  }
690  sequence = 11;
691  } else if (step < 1260 && sequence < 13) {
692  if (sequence == 11) {
693  headTiltSrv.request.value = 0.4;
694  headTiltClient.call(headTiltSrv);
695  }
696  sequence = 12;
697  }
698  // move forward next to the fridge
699  else if (wheelPosition < 48 && sequence < 14) {
700  if (sequence == 12) {
701  ROS_INFO("PR2 moves closer to the fridge.");
702  move(FWD_SPEED / 2);
703  rShoulderRollSrv.request.value = -0.1;
704  rShoulderRollClient.call(rShoulderRollSrv);
705  lShoulderLiftSrv.request.value = 0.65;
706  lShoulderLiftClient.call(lShoulderLiftSrv);
707  lElbowLiftSrv.request.value = -0.65;
708  lElbowLiftClient.call(lElbowLiftSrv);
709  lShoulderRollSrv.request.value = -0.48;
710  lShoulderRollClient.call(lShoulderRollSrv);
711  lShoulderRollSpeedSrv.request.value = 0.01;
712  lShoulderRollSpeedClient.call(lShoulderRollSpeedSrv);
713  }
714  sequence = 13;
715  }
716  // place the left hand in front of the can
717  else if (lShoulderPosition < -0.236 && sequence < 15) {
718  if (sequence == 13) {
719  ROS_INFO("PR2 moves its left-hand around the can.");
720  move(0);
721  lShoulderRollSrv.request.value = -0.235;
722  lShoulderRollClient.call(lShoulderRollSrv);
723  lShoulderRollSpeedSrv.request.value = 1;
724  lShoulderRollSpeedClient.call(lShoulderRollSpeedSrv);
725  }
726  sequence = 14;
727  }
728  // place left hand around the can
729  else if (wheelPosition < 50.5 && sequence < 16) {
730  if (sequence == 14) {
731  lShoulderSubscriber.shutdown();
732  llFingerSubscriber = n.subscribe(controllerName + "/l_gripper_l_finger_joint/position_sensor/32", 1, llFingerCallback);
733  lBumperSubscriber =
734  n.subscribe(controllerName + "/l_gripper_l_finger_tip_contact_sensor/32_bumper", 1, lBumperCallback);
735  move(FWD_SPEED / 3);
736  }
737  sequence = 15;
738  }
739  // grab the can
740  else if (llFingerPosition > (finger_pos - 0.04) && sequence < 17) {
741  if (sequence == 15) {
742  ROS_INFO("PR2 grabs the can.");
743  move(0);
744  llFingerSrv.request.value = 0;
745  llFingerClient.call(llFingerSrv);
746  lrFingerSrv.request.value = 0;
747  lrFingerClient.call(lrFingerSrv);
748  llFingerSpeedSrv.request.value = 0.015;
749  llFingerSpeedClient.call(llFingerSpeedSrv);
750  lrFingerSpeedSrv.request.value = 0.015;
751  lrFingerSpeedClient.call(lrFingerSpeedSrv);
752  llFingerTipSrv.request.value = 0.4;
753  llFingerTipClient.call(llFingerTipSrv);
754  lrFingerTipSrv.request.value = 0.4;
755  lrFingerTipClient.call(lrFingerTipSrv);
756  }
757  if (i == 1) {
758  finger_pos = llFingerPosition;
759  i++;
760  }
761  if (lBumper)
762  i++;
763  sequence = 16;
764  }
765  // move backward out of the fridge
766  else if (wheelPosition > 40 && sequence < 18) {
767  if (sequence == 16) {
768  llFingerSubscriber.shutdown();
769  lBumperSubscriber.shutdown();
770  rShoulderSubscriber = n.subscribe(controllerName + "/r_shoulder_pan_joint/position_sensor/32", 1, rShoulderCallback);
771  move(-FWD_SPEED);
772  llFingerSpeedSrv.request.value = 0;
773  llFingerSpeedClient.call(llFingerSpeedSrv);
774  lrFingerSpeedSrv.request.value = 0;
775  lrFingerSpeedClient.call(lrFingerSpeedSrv);
776  }
777  sequence = 17;
778  }
779  // stop and open right shoulder
780  else if (rShoulderPosition > -0.59 && sequence < 19) {
781  if (sequence == 17) {
782  move(0);
783  rShoulderRollSrv.request.value = -0.6;
784  rShoulderRollClient.call(rShoulderRollSrv);
785  }
786  sequence = 18;
787  }
788  // extend right arm along the door
789  else if (wheelPosition < 40.55 && sequence < 20) {
790  if (sequence == 18) {
791  move(FWD_SPEED / 2);
792  rShoulderLiftSrv.request.value = 0;
793  rShoulderLiftClient.call(rShoulderLiftSrv);
794  rElbowLiftSrv.request.value = 0;
795  rElbowLiftClient.call(rElbowLiftSrv);
796  }
797  sequence = 19;
798  }
799  // close the door
800  else if (rShoulderPosition < 0.5 && sequence < 21) {
801  if (sequence == 19) {
802  ROS_INFO("PR2 closes the door.");
803  move(0);
804  rShoulderRollSrv.request.value = 0.5;
805  rShoulderRollClient.call(rShoulderRollSrv);
806  rShoulderRollSpeedSrv.request.value = 0.18;
807  rShoulderRollSpeedClient.call(rShoulderRollSpeedSrv);
808  lShoulderRollSrv.request.value = 0;
809  lShoulderRollClient.call(lShoulderRollSrv);
810  lShoulderLiftSrv.request.value = 1;
811  lShoulderLiftClient.call(lShoulderLiftSrv);
812  lElbowLiftSrv.request.value = -1;
813  lElbowLiftClient.call(lElbowLiftSrv);
814  flRotationSetClient.call(setSensorSrv);
815  flRotationSubscriber =
816  n.subscribe(controllerName + "/fl_caster_rotation_joint/position_sensor/32", 1, flRotationCallback);
817  }
818  sequence = 20;
819  } else if (wheelPosition < 46 && sequence < 22) {
820  if (sequence == 20) {
821  rShoulderRollSrv.request.value = 0.2;
822  rShoulderRollClient.call(rShoulderRollSrv);
823  move(FWD_SPEED / 4);
824  }
825  sequence = 21;
826  }
827  // rotate wheels to be able to turn on itself
828  else if (rotationPosition > -0.78 && sequence < 23) {
829  if (sequence == 21) {
830  rotate_wheels(-0.785);
831  imuSetClient.call(setSensorSrv);
832  imuSubscriber = n.subscribe(controllerName + "/imu_sensor/32", 1, imuCallback);
833  rShoulderSubscriber.shutdown();
834  }
835  sequence = 22;
836  }
837  // rotate 180° on itself
838  else if (imu_rpy[2] < -0.35 && sequence < 24) {
839  if (sequence == 22) {
840  ROS_INFO("PR2 rotates back to go to the lounge.");
841  rotate(ROTATE_SPEED);
842  rShoulderRollSrv.request.value = 0;
843  rShoulderRollClient.call(rShoulderRollSrv);
844  rShoulderLiftSrv.request.value = 1.35;
845  rShoulderLiftClient.call(rShoulderLiftSrv);
846  rElbowLiftSrv.request.value = -2.2;
847  rElbowLiftClient.call(rElbowLiftSrv);
848  rWristRollSrv.request.value = 0;
849  rWristRollClient.call(rWristRollSrv);
850  }
851  sequence = 23;
852  }
853  // rotate wheels to be able to move forward
854  else if (rotationPosition < 0 && sequence < 25) {
855  if (sequence == 23) {
856  rotate_wheels(0);
857  imuSubscriber.shutdown();
858  }
859  sequence = 24;
860  }
861  // go back to the table
862  else if (wheelPosition < 80 && sequence < 26) {
863  if (sequence == 24) {
864  move(FWD_SPEED);
865  }
866  sequence = 25;
867  }
868  // rotate wheels to be able to turn on itself
869  else if (rotationPosition > -0.78 && sequence < 27) {
870  if (sequence == 25) {
871  rotate_wheels(-0.785);
872  imuSetClient.call(setSensorSrv);
873  imuSubscriber = n.subscribe(controllerName + "/imu_sensor/32", 1, imuCallback);
874  }
875  sequence = 26;
876  }
877  // rotate 90° on itself to face table
878  else if (imu_rpy[2] > -1.15 && sequence < 28) {
879  if (sequence == 26) {
880  ROS_INFO("PR2 faces the table.");
881  torsoSetClient.call(setSensorSrv);
882  torsoSubscriber = n.subscribe(controllerName + "/torso_lift_joint/position_sensor/32", 1, torsoCallback);
883  rotate(-ROTATE_SPEED);
884  }
885  sequence = 27;
886  }
887  // lower the can to lay on the table
888  else if (torsoPosition > 0.17 && sequence < 29) {
889  if (sequence == 27) {
890  ROS_INFO("PR2 slowly lay the can on the table.");
891  move(0);
892  torsoSrv.request.value = 0.165;
893  torsoClient.call(torsoSrv);
894  imuSubscriber.shutdown();
895  llFingerSetClient.call(setSensorSrv);
896  llFingerSubscriber = n.subscribe(controllerName + "/l_gripper_l_finger_joint/position_sensor/32", 1, llFingerCallback);
897  }
898  sequence = 28;
899  }
900  // release the can
901  else if (llFingerPosition < 0.39 && sequence < 30) {
902  if (sequence == 28) {
903  ROS_INFO("PR2 release the can and go back to stand-by position.");
904  llFingerSrv.request.value = 0.4;
905  llFingerClient.call(llFingerSrv);
906  lrFingerSrv.request.value = 0.4;
907  lrFingerClient.call(lrFingerSrv);
908  llFingerTipSrv.request.value = 0;
909  llFingerTipClient.call(llFingerTipSrv);
910  lrFingerTipSrv.request.value = 0;
911  lrFingerTipClient.call(lrFingerTipSrv);
912  llFingerSpeedSrv.request.value = 0.1;
913  llFingerSpeedClient.call(llFingerSpeedSrv);
914  lrFingerSpeedSrv.request.value = 0.1;
915  lrFingerSpeedClient.call(lrFingerSpeedSrv);
916  }
917  sequence = 29;
918  }
919  // bring back arm to initial position
920  else if (sequence == 29) {
921  lElbowLiftSrv.request.value = -2.2;
922  lElbowLiftClient.call(lElbowLiftSrv);
923  lShoulderLiftSrv.request.value = 1.35;
924  lShoulderLiftClient.call(lShoulderLiftSrv);
925  break;
926  }
927  ros::spinOnce();
928  }
929  ros::spinOnce();
930  setSensorSrv.request.period = 0;
931  fllWheelSubscriber.shutdown();
932  flRotationSubscriber.shutdown();
933  torsoSubscriber.shutdown();
934 
935  timeStepSrv.request.value = 0;
936  timeStepClient.call(timeStepSrv);
937  ros::shutdown();
938  return (0);
939 }
ros::NodeHandle::serviceClient
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
rrFingerCallback
void rrFingerCallback(const std_msgs::Float64::ConstPtr &value)
Definition: pr2_beer.cpp:116
frrWheelPositionSrv
webots_ros::set_float frrWheelPositionSrv
Definition: pr2_beer.cpp:64
main
int main(int argc, char **argv)
Definition: pr2_beer.cpp:277
ros::NodeHandle
controllerList
static std::vector< std::string > controllerList
Definition: pr2_beer.cpp:36
lBumper
static unsigned int lBumper
Definition: pr2_beer.cpp:45
controllerName
static std::string controllerName
Definition: pr2_beer.cpp:37
fllWheelPositionClient
ros::ServiceClient fllWheelPositionClient
Definition: pr2_beer.cpp:52
blRotationSrv
webots_ros::set_float blRotationSrv
Definition: pr2_beer.cpp:95
bllWheelSpeedClient
ros::ServiceClient bllWheelSpeedClient
Definition: pr2_beer.cpp:74
fllWheelSpeedSrv
webots_ros::set_float fllWheelSpeedSrv
Definition: pr2_beer.cpp:79
brRotationClient
ros::ServiceClient brRotationClient
Definition: pr2_beer.cpp:91
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
brlWheelPositionSrv
webots_ros::set_float brlWheelPositionSrv
Definition: pr2_beer.cpp:67
ros::init_options::AnonymousName
AnonymousName
rotate_wheels
void rotate_wheels(double angle)
Definition: pr2_beer.cpp:209
blrWheelPositionClient
ros::ServiceClient blrWheelPositionClient
Definition: pr2_beer.cpp:57
flRotationSrv
webots_ros::set_float flRotationSrv
Definition: pr2_beer.cpp:93
fllWheelPositionSrv
webots_ros::set_float fllWheelPositionSrv
Definition: pr2_beer.cpp:61
brrWheelPositionClient
ros::ServiceClient brrWheelPositionClient
Definition: pr2_beer.cpp:59
imuCallback
void imuCallback(const std_msgs::Float64MultiArray::ConstPtr &values)
Definition: pr2_beer.cpp:125
quit
void quit(int sig)
Definition: pr2_beer.cpp:269
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
frrWheelSpeedClient
ros::ServiceClient frrWheelSpeedClient
Definition: pr2_beer.cpp:73
ros::ServiceClient::call
bool call(MReq &req, MRes &res)
controllerCount
static int controllerCount
Definition: pr2_beer.cpp:35
llFingerCallback
void llFingerCallback(const std_msgs::Float64::ConstPtr &value)
Definition: pr2_beer.cpp:113
bllWheelPositionClient
ros::ServiceClient bllWheelPositionClient
Definition: pr2_beer.cpp:56
fllWheelCallback
void fllWheelCallback(const std_msgs::Float64::ConstPtr &value)
Definition: pr2_beer.cpp:98
brrWheelSpeedSrv
webots_ros::set_float brrWheelSpeedSrv
Definition: pr2_beer.cpp:86
timeStepClient
ros::ServiceClient timeStepClient
Definition: pr2_beer.cpp:49
frlWheelPositionClient
ros::ServiceClient frlWheelPositionClient
Definition: pr2_beer.cpp:54
bllWheelPositionSrv
webots_ros::set_float bllWheelPositionSrv
Definition: pr2_beer.cpp:65
rShoulderPosition
static double rShoulderPosition
Definition: pr2_beer.cpp:42
blrWheelSpeedClient
ros::ServiceClient blrWheelSpeedClient
Definition: pr2_beer.cpp:75
llFingerPosition
static double llFingerPosition
Definition: pr2_beer.cpp:43
rBumperCallback
void rBumperCallback(const std_msgs::Float64::ConstPtr &value)
Definition: pr2_beer.cpp:122
frRotationSrv
webots_ros::set_float frRotationSrv
Definition: pr2_beer.cpp:94
brlWheelSpeedClient
ros::ServiceClient brlWheelSpeedClient
Definition: pr2_beer.cpp:76
timeStepSrv
webots_ros::set_int timeStepSrv
Definition: pr2_beer.cpp:50
flRotationCallback
void flRotationCallback(const std_msgs::Float64::ConstPtr &value)
Definition: pr2_beer.cpp:104
n
ros::NodeHandle * n
Definition: pioneer3at.cpp:40
frlWheelSpeedSrv
webots_ros::set_float frlWheelSpeedSrv
Definition: pr2_beer.cpp:81
ACCELERATION
#define ACCELERATION
Definition: pr2_beer.cpp:31
brrWheelPositionSrv
webots_ros::set_float brrWheelPositionSrv
Definition: pr2_beer.cpp:68
flrWheelSpeedSrv
webots_ros::set_float flrWheelSpeedSrv
Definition: pr2_beer.cpp:80
wheelPosition
static double wheelPosition
Definition: pr2_beer.cpp:38
ROS_INFO
#define ROS_INFO(...)
frRotationClient
ros::ServiceClient frRotationClient
Definition: pr2_beer.cpp:89
torsoCallback
void torsoCallback(const std_msgs::Float64::ConstPtr &value)
Definition: pr2_beer.cpp:101
ros::Subscriber::getNumPublishers
uint32_t getNumPublishers() const
ros::ok
ROSCPP_DECL bool ok()
TIME_STEP
#define TIME_STEP
Definition: pr2_beer.cpp:30
lBumperCallback
void lBumperCallback(const std_msgs::Float64::ConstPtr &value)
Definition: pr2_beer.cpp:119
bllWheelSpeedSrv
webots_ros::set_float bllWheelSpeedSrv
Definition: pr2_beer.cpp:83
brlWheelPositionClient
ros::ServiceClient brlWheelPositionClient
Definition: pr2_beer.cpp:58
controllerNameCallback
void controllerNameCallback(const std_msgs::String::ConstPtr &name)
Definition: pr2_beer.cpp:263
brRotationSrv
webots_ros::set_float brRotationSrv
Definition: pr2_beer.cpp:96
ROTATE_SPEED
#define ROTATE_SPEED
Definition: pr2_beer.cpp:33
fllWheelSpeedClient
ros::ServiceClient fllWheelSpeedClient
Definition: pr2_beer.cpp:70
step
unsigned int step
move
void move(double speed)
Definition: pr2_beer.cpp:133
rrFingerPosition
static double rrFingerPosition
Definition: pr2_beer.cpp:44
rBumper
static unsigned int rBumper
Definition: pr2_beer.cpp:46
blRotationClient
ros::ServiceClient blRotationClient
Definition: pr2_beer.cpp:90
brrWheelSpeedClient
ros::ServiceClient brrWheelSpeedClient
Definition: pr2_beer.cpp:77
lShoulderCallback
void lShoulderCallback(const std_msgs::Float64::ConstPtr &value)
Definition: pr2_beer.cpp:107
torsoPosition
static double torsoPosition
Definition: pr2_beer.cpp:39
blrWheelPositionSrv
webots_ros::set_float blrWheelPositionSrv
Definition: pr2_beer.cpp:66
rShoulderCallback
void rShoulderCallback(const std_msgs::Float64::ConstPtr &value)
Definition: pr2_beer.cpp:110
ros::shutdown
ROSCPP_DECL void shutdown()
brlWheelSpeedSrv
webots_ros::set_float brlWheelSpeedSrv
Definition: pr2_beer.cpp:85
lShoulderPosition
static double lShoulderPosition
Definition: pr2_beer.cpp:41
flRotationClient
ros::ServiceClient flRotationClient
Definition: pr2_beer.cpp:88
imu_rpy
static double imu_rpy[3]
Definition: pr2_beer.cpp:47
frlWheelPositionSrv
webots_ros::set_float frlWheelPositionSrv
Definition: pr2_beer.cpp:63
frrWheelPositionClient
ros::ServiceClient frrWheelPositionClient
Definition: pr2_beer.cpp:55
frlWheelSpeedClient
ros::ServiceClient frlWheelSpeedClient
Definition: pr2_beer.cpp:72
rotationPosition
static double rotationPosition
Definition: pr2_beer.cpp:40
frrWheelSpeedSrv
webots_ros::set_float frrWheelSpeedSrv
Definition: pr2_beer.cpp:82
ros::spinOnce
ROSCPP_DECL void spinOnce()
flrWheelPositionSrv
webots_ros::set_float flrWheelPositionSrv
Definition: pr2_beer.cpp:62
ROS_ERROR
#define ROS_ERROR(...)
rotate
void rotate(double speed)
Definition: pr2_beer.cpp:171
flrWheelSpeedClient
ros::ServiceClient flrWheelSpeedClient
Definition: pr2_beer.cpp:71
FWD_SPEED
#define FWD_SPEED
Definition: pr2_beer.cpp:32
flrWheelPositionClient
ros::ServiceClient flrWheelPositionClient
Definition: pr2_beer.cpp:53
blrWheelSpeedSrv
webots_ros::set_float blrWheelSpeedSrv
Definition: pr2_beer.cpp:84

webots_ros
Author(s):
autogenerated on Mon Jul 8 2019 03:19:27