e_puck_line.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 // line_following example
16 // this example reproduce the e-puck_line_demo example
17 // but uses the ROS controller on the e-puck instead.
18 // The node connect to an e-puck and then uses values from its sensors
19 // to follow and line and get around obstacles.
20 // the duration of the example is given as argument to the node.
21 
22 #include "ros/ros.h"
23 
24 #include <webots_ros/set_int.h>
25 
26 #include <webots_ros/set_float.h>
27 
28 #include <sensor_msgs/Range.h>
29 
30 #include <signal.h>
31 #include <std_msgs/Float64MultiArray.h>
32 #include <std_msgs/String.h>
33 #include <std_msgs/UInt16.h>
34 #include <std_msgs/UInt8MultiArray.h>
35 #include <cstdlib>
36 
37 // Global defines
38 #define TRUE 1
39 #define FALSE 0
40 #define NO_SIDE -1
41 #define LEFT 0
42 #define RIGHT 1
43 #define WHITE 0
44 #define BLACK 1
45 #define SIMULATION 0 // for wb_robot_get_mode() function
46 #define REALITY 2 // for wb_robot_get_mode() function
47 #define TIME_STEP 32 // [ms]
48 
49 // 8 IR proximity sensors
50 #define NB_DIST_SENS 8
51 #define PS_RIGHT_00 0
52 #define PS_RIGHT_45 1
53 #define PS_RIGHT_90 2
54 #define PS_RIGHT_REAR 3
55 #define PS_LEFT_REAR 4
56 #define PS_LEFT_90 5
57 #define PS_LEFT_45 6
58 #define PS_LEFT_00 7
59 
60 int psValue[NB_DIST_SENS] = {0, 0, 0, 0, 0, 0, 0, 0};
61 const int PS_OFFSET_SIMULATION[NB_DIST_SENS] = {300, 300, 300, 300, 300, 300, 300, 300};
62 const int PS_OFFSET_REALITY[NB_DIST_SENS] = {480, 170, 320, 500, 600, 680, 210, 640};
63 
64 // 3 IR ground color sensors
65 #define NB_GROUND_SENS 3
66 #define GS_WHITE 900
67 #define GS_LEFT 0
68 #define GS_CENTER 1
69 #define GS_RIGHT 2
70 unsigned short gsValue[NB_GROUND_SENS] = {0, 0, 0};
71 
72 // LEDs
73 #define NB_LEDS 10
74 
75 //------------------------------------------------------------------------------
76 //
77 // ROS CALLBACKS
78 //
79 //------------------------------------------------------------------------------
80 
81 static char modelList[10][100];
82 static int count = 0;
83 static int countDist = NB_DIST_SENS;
84 static int countGnd = NB_GROUND_SENS;
85 static int step = TIME_STEP;
86 double accelerometerValues[3] = {0, 0, 0};
87 std::vector<unsigned char> image;
88 
90 webots_ros::set_int setTimeStepSrv;
91 
92 void psCallback(const sensor_msgs::Range::ConstPtr &value) {
93  if (countDist < NB_DIST_SENS)
95  ((value->range - PS_OFFSET_SIMULATION[countDist]) < 0) ? 0 : (value->range - PS_OFFSET_SIMULATION[countDist]);
96  countDist++;
97 }
98 
99 void gsCallback(const sensor_msgs::Range::ConstPtr &value) {
100  if (countGnd < NB_GROUND_SENS)
101  gsValue[countGnd] = value->range;
102  countGnd++;
103 }
104 
105 void modelNameCallback(const std_msgs::String::ConstPtr &name) {
106  count++;
107  strcpy(modelList[count], name->data.c_str());
108  ROS_INFO("Model #%d: %s.", count, name->data.c_str());
109 }
110 
111 void quit(int sig) {
112  setTimeStepSrv.request.value = 0;
113  setTimeStepClient.call(setTimeStepSrv);
114  ROS_INFO("User stopped the 'e_puck_line' node.");
115  ros::shutdown();
116  exit(0);
117 }
118 
119 //------------------------------------------------------------------------------
120 //
121 // BEHAVIORAL MODULES
122 //
123 //------------------------------------------------------------------------------
124 
126 // LFM - Line Following Module
127 //
128 // This module implements a very simple, Braitenberg-like behavior in order
129 // to follow a black line on the ground. Output speeds are stored in
130 // lfm_speed[LEFT] and lfm_speed[RIGHT].
131 
132 int lfm_speed[2];
133 
134 #define LFM_FORWARD_SPEED 200
135 #define LFM_K_GS_SPEED 0.4
136 
138  int DeltaS = 0;
139 
140  DeltaS = gsValue[GS_RIGHT] - gsValue[GS_LEFT];
141 
144 }
145 
147 // OAM - Obstacle Avoidance Module
148 //
149 // The OAM routine first detects obstacles in front of the robot, then records
150 // their side in "oam_side" and avoid the detected obstacle by
151 // turning away according to very simple weighted connections between
152 // proximity sensors and motors. "oam_active" becomes active when as soon as
153 // an object is detected and "oam_reset" inactivates the module and set
154 // "oam_side" to NO_SIDE. Output speeds are in oam_speed[LEFT] and oam_speed[RIGHT].
155 
157 int oam_speed[2];
159 
160 #define OAM_OBST_THRESHOLD 100
161 #define OAM_FORWARD_SPEED 150
162 #define OAM_K_PS_90 0.2
163 #define OAM_K_PS_45 0.9
164 #define OAM_K_PS_00 1.2
165 #define OAM_K_MAX_DELTAS 600
166 
168  int max_ds_value, i;
169  int Activation[] = {0, 0};
170 
171  // Module RESET
172  if (oam_reset) {
173  oam_active = FALSE;
174  oam_side = NO_SIDE;
175  }
176  oam_reset = 0;
177 
178  // Determine the presence and the side of an obstacle
179  max_ds_value = 0;
180  for (i = PS_RIGHT_00; i <= PS_RIGHT_45; i++) {
181  if (max_ds_value < psValue[i])
182  max_ds_value = psValue[i];
183  Activation[RIGHT] += psValue[i];
184  }
185  for (i = PS_LEFT_45; i <= PS_LEFT_00; i++) {
186  if (max_ds_value < psValue[i])
187  max_ds_value = psValue[i];
188  Activation[LEFT] += psValue[i];
189  }
190  if (max_ds_value > OAM_OBST_THRESHOLD)
191  oam_active = TRUE;
192 
193  if (oam_active && oam_side == NO_SIDE) { // check for side of obstacle only when not already detected
194  if (Activation[RIGHT] > Activation[LEFT])
195  oam_side = RIGHT;
196  else
197  oam_side = LEFT;
198  }
199 
200  // Forward speed
203 
204  // Go away from obstacle
205  if (oam_active) {
206  int DeltaS = 0;
207  // The rotation of the robot is determined by the location and the side of the obstacle
208  if (oam_side == LEFT) {
209  //(((psValue[PS_LEFT_90]-PS_OFFSET)<0)?0:(psValue[PS_LEFT_90]-PS_OFFSET)));
210  DeltaS -= (int)(OAM_K_PS_90 * psValue[PS_LEFT_90]);
211  //(((psValue[PS_LEFT_45]-PS_OFFSET)<0)?0:(psValue[PS_LEFT_45]-PS_OFFSET)));
212  DeltaS -= (int)(OAM_K_PS_45 * psValue[PS_LEFT_45]);
213  //(((psValue[PS_LEFT_00]-PS_OFFSET)<0)?0:(psValue[PS_LEFT_00]-PS_OFFSET)));
214  DeltaS -= (int)(OAM_K_PS_00 * psValue[PS_LEFT_00]);
215  } else { // oam_side == RIGHT
216  //(((psValue[PS_RIGHT_90]-PS_OFFSET)<0)?0:(psValue[PS_RIGHT_90]-PS_OFFSET)));
217  DeltaS += (int)(OAM_K_PS_90 * psValue[PS_RIGHT_90]);
218  //(((psValue[PS_RIGHT_45]-PS_OFFSET)<0)?0:(psValue[PS_RIGHT_45]-PS_OFFSET)));
219  DeltaS += (int)(OAM_K_PS_45 * psValue[PS_RIGHT_45]);
220  //(((psValue[PS_RIGHT_00]-PS_OFFSET)<0)?0:(psValue[PS_RIGHT_00]-PS_OFFSET)));
221  DeltaS += (int)(OAM_K_PS_00 * psValue[PS_RIGHT_00]);
222  }
223  if (DeltaS > OAM_K_MAX_DELTAS)
224  DeltaS = OAM_K_MAX_DELTAS;
225  if (DeltaS < -OAM_K_MAX_DELTAS)
226  DeltaS = -OAM_K_MAX_DELTAS;
227 
228  // Set speeds
229  oam_speed[LEFT] -= DeltaS;
230  oam_speed[RIGHT] += DeltaS;
231  }
232 }
233 
235 // LLM - Line Leaving Module
236 //
237 // Since it has no output, this routine is not completely finished. It has
238 // been designed to monitor the moment while the robot is leaving the
239 // track and signal to other modules some related events. It becomes active
240 // whenever the "side" variable displays a rising edge (changing from -1 to 0 or 1).
241 
244 
245 #define LLM_THRESHOLD 800
246 
247 void LineLeavingModule(int side) {
248  // Starting the module on a rising edge of "side"
249  if (!llm_active && side != NO_SIDE && llm_past_side == NO_SIDE)
250  llm_active = TRUE;
251 
252  // Updating the memory of the "side" state at the previous call
253  llm_past_side = side;
254 
255  // Main loop
256  if (llm_active) { // Simply waiting until the line is not detected anymore
257  if (side == LEFT) {
258  if ((gsValue[GS_CENTER] + gsValue[GS_LEFT]) / 2 > LLM_THRESHOLD) { // out of line
259  llm_active = FALSE;
260  // *** PUT YOUR CODE HERE ***
262  lem_reset = TRUE;
263  } else { // still leaving the line
264  // *** PUT YOUR CODE HERE ***
266  }
267  } else { // side == RIGHT
268  if ((gsValue[GS_CENTER] + gsValue[GS_RIGHT]) / 2 > LLM_THRESHOLD) { // out of line
269  llm_active = FALSE;
270  // *** PUT YOUR CODE HERE ***
272  lem_reset = TRUE;
273  } else { // still leaving the line
274  // *** PUT YOUR CODE HERE ***
276  }
277  }
278  }
279 }
280 
282 // OFM - Obstacle Following Module
283 //
284 // This function just gives the robot a tendency to steer toward the side
285 // indicated by its argument "side". When used in competition with OAM it
286 // gives rise to an object following behavior. The output speeds are
287 // stored in ofm_speed[LEFT] and ofm_speed[RIGHT].
288 
290 int ofm_speed[2];
291 
292 #define OFM_DELTA_SPEED 150
293 
294 void ObstacleFollowingModule(int side) {
295  if (side != NO_SIDE) {
296  ofm_active = TRUE;
297  if (side == LEFT) {
300  } else {
303  }
304  } else { // side = NO_SIDE
305  ofm_active = FALSE;
306  ofm_speed[LEFT] = 0;
307  ofm_speed[RIGHT] = 0;
308  }
309 }
310 
312 // LEM - Line Entering Module
313 //
314 // This is the most complex module (and you might find easier to re-program it
315 // by yourself instead of trying to understand it ;-). Its purpose is to handle
316 // the moment when the robot must re-enter the track (after having by-passed
317 // an obstacle, e.g.). It is organized like a state machine, which state is
318 // stored in "lem_state" (see LEM_STATE_STANDBY and following #defines).
319 // The inputs are (i) the two lateral ground sensors, (ii) the argument "side"
320 // which determines the direction that the robot has to follow when detecting
321 // a black line, and (iii) the variable "lem_reset" that resets the state to
322 // standby. The output speeds are stored in lem_speed[LEFT] and
323 // lem_speed[RIGHT].
324 
326 int lem_speed[2];
329 
330 #define LEM_FORWARD_SPEED 100
331 #define LEM_K_GS_SPEED 0.8
332 #define LEM_THRESHOLD 500
333 
334 #define LEM_STATE_STANDBY 0
335 #define LEM_STATE_LOOKING_FOR_LINE 1
336 #define LEM_STATE_LINE_DETECTED 2
337 #define LEM_STATE_ON_LINE 3
338 
339 void LineEnteringModule(int side) {
340  int Side, OpSide, GS_Side, GS_OpSide;
341 
342  // Module reset
343  if (lem_reset)
345  lem_reset = FALSE;
346 
347  // Initialization
350  if (side == LEFT) { // if obstacle on left side -> enter line rightward
351  Side = RIGHT; // line entering direction
352  OpSide = LEFT;
353  GS_Side = GS_RIGHT;
354  GS_OpSide = GS_LEFT;
355  } else { // if obstacle on left side -> enter line leftward
356  Side = LEFT; // line entering direction
357  OpSide = RIGHT;
358  GS_Side = GS_LEFT;
359  GS_OpSide = GS_RIGHT;
360  }
361 
362  // Main loop (state machine)
363  switch (lem_state) {
364  case LEM_STATE_STANDBY:
365  lem_active = FALSE;
366  break;
368  if (gsValue[GS_Side] < LEM_THRESHOLD) {
369  lem_active = TRUE;
370  // set speeds for entering line
371  lem_speed[OpSide] = LEM_FORWARD_SPEED;
372  lem_speed[Side] = LEM_FORWARD_SPEED; // - LEM_K_GS_SPEED * gsValue[GS_Side];
374  // save ground sensor value
375  if (gsValue[GS_OpSide] < LEM_THRESHOLD) {
377  lem_black_counter = 1;
378  } else {
380  lem_black_counter = 0;
381  }
383  }
384  break;
386  // save the oposite ground sensor value
387  if (gsValue[GS_OpSide] < LEM_THRESHOLD) {
390  } else
392  // detect the falling edge BLACK->WHITE
393  if (prevOpGsValue == BLACK && curOpGsValue == WHITE) {
395  lem_speed[OpSide] = 0;
396  lem_speed[Side] = 0;
397  } else {
399  // set speeds for entering line
400  lem_speed[OpSide] = LEM_FORWARD_SPEED + LEM_K_GS_SPEED * (GS_WHITE - gsValue[GS_Side]);
401  lem_speed[Side] = LEM_FORWARD_SPEED - LEM_K_GS_SPEED * (GS_WHITE - gsValue[GS_Side]);
402  }
403  break;
404  case LEM_STATE_ON_LINE:
405  oam_reset = TRUE;
406  lem_active = FALSE;
408  break;
409  }
410 }
411 
412 //------------------------------------------------------------------------------
413 //
414 // CONTROLLER
415 //
416 //------------------------------------------------------------------------------
417 
419 // Main
420 int main(int argc, char **argv) {
421  int i;
422  int oamOfmSpeed[2];
423  double speed[2];
424 
425  int stepMax = 1;
426  int nStep = 0;
427 
428  // look if a limit time was given
429  if (argc < 2) {
430  ROS_INFO("Usage: $ e_puck_line [duration(seconds)].");
431  return 1;
432  }
433  stepMax = atoll(argv[1]) * 1000 / TIME_STEP;
434  ROS_INFO("Max step is %d.", stepMax);
435 
436  ros::init(argc, argv, "e_puck_line", ros::init_options::AnonymousName);
438 
439  signal(SIGINT, quit);
440 
441  // declaration of variable names used to define services and topics name dynamically
442  std::string modelName;
443 
444  // get the name of the robot
445  ros::Subscriber nameSub = n.subscribe("model_name", 100, modelNameCallback);
446  while (count == 0 || count < nameSub.getNumPublishers()) {
447  ros::spinOnce();
448  ros::spinOnce();
449  ros::spinOnce();
450  }
451  ros::spinOnce();
452  if (count == 1)
453  modelName = modelList[1];
454  else {
455  int wantedModel = 0;
456  std::cout << "Choose the # of the model you want to use:\n";
457  std::cin >> wantedModel;
458  if (1 <= wantedModel && wantedModel <= count)
459  modelName = modelList[wantedModel];
460  else {
461  ROS_ERROR("Invalid choice.");
462  return 1;
463  }
464  }
465  nameSub.shutdown();
466  count = 0;
467 
468  // send robot time step to webots
469  setTimeStepClient = n.serviceClient<webots_ros::set_int>(modelName + "/robot/time_step");
470  setTimeStepSrv.request.value = step;
471 
472  std::vector<ros::ServiceClient> enableDistSensorClient;
473  webots_ros::set_int enableDistSensorSrv;
474 
475  if (setTimeStepClient.call(setTimeStepSrv) && setTimeStepSrv.response.success)
476  nStep++;
477  else
478  ROS_ERROR("Failed to call service time_step to update robot's time step.");
479 
480  // enable ir proximity sensors
481  char deviceName[20];
482  ros::Subscriber SubDistIr[NB_DIST_SENS];
483  for (i = 0; i < NB_DIST_SENS; i++) {
484  sprintf(deviceName, "ps%d", i);
485  enableDistSensorClient.push_back(n.serviceClient<webots_ros::set_int>(modelName + '/' + deviceName + "/enable"));
486  enableDistSensorSrv.request.value = step;
487  if (enableDistSensorClient[i].call(enableDistSensorSrv) && enableDistSensorSrv.response.success) {
488  ROS_INFO("Device %s enabled.", deviceName);
489  std::ostringstream s;
490  s << step;
491  SubDistIr[i] = n.subscribe(modelName + '/' + deviceName + "/value", 1, psCallback);
492  while (SubDistIr[i].getNumPublishers() == 0) {
493  }
494  } else {
495  if (!enableDistSensorSrv.response.success)
496  ROS_ERROR("Sampling period is not valid.");
497  ROS_ERROR("Failed to enable %s.", deviceName);
498  return 1;
499  }
500  }
501 
502  // enable ir ground sensors
504  for (i = 0; i < NB_GROUND_SENS; i++) {
505  sprintf(deviceName, "gs%d", i);
506  enableDistSensorClient.push_back(n.serviceClient<webots_ros::set_int>(modelName + '/' + deviceName + "/enable"));
507  enableDistSensorSrv.request.value = step;
508  if (enableDistSensorClient[i + NB_DIST_SENS].call(enableDistSensorSrv) && enableDistSensorSrv.response.success) {
509  ROS_INFO("Device %s enabled.", deviceName);
510  std::ostringstream s;
511  s << step;
512  SubGndIr[i] = n.subscribe(modelName + '/' + deviceName + "/value", 1, gsCallback);
513  while (SubGndIr[i].getNumPublishers() == 0) {
514  }
515  } else {
516  if (!enableDistSensorSrv.response.success)
517  ROS_ERROR("Sampling period is not valid.");
518  ROS_ERROR("Failed to enable %s.", deviceName);
519  return 1;
520  }
521  }
522 
523  // set the motors to veloctiy control
524  webots_ros::set_float wheelSrv;
525  wheelSrv.request.value = INFINITY;
526  ros::ServiceClient leftWheelPositionClient =
527  n.serviceClient<webots_ros::set_float>(modelName + "/left_wheel_motor/set_position");
528  leftWheelPositionClient.call(wheelSrv);
529  ros::ServiceClient rightWheelPositionClient =
530  n.serviceClient<webots_ros::set_float>(modelName + "/right_wheel_motor/set_position");
531  rightWheelPositionClient.call(wheelSrv);
532  ros::ServiceClient leftWheelVelocityClient =
533  n.serviceClient<webots_ros::set_float>(modelName + "/left_wheel_motor/set_velocity");
534  ros::ServiceClient rightWheelVelocityClient =
535  n.serviceClient<webots_ros::set_float>(modelName + "/right_wheel_motor/set_velocity");
536 
537  // turn the leds on
538  std::vector<ros::ServiceClient> setLedClient;
539  webots_ros::set_int setLedSrv;
540  sprintf(deviceName, "led0");
541  setLedClient.push_back(n.serviceClient<webots_ros::set_int>(modelName + '/' + deviceName + "/set_led"));
542  setLedSrv.request.value = 1;
543  if (setLedClient[0].call(setLedSrv) && setLedSrv.response.success)
544  ROS_INFO("%s turned on!", deviceName);
545  else {
546  ROS_ERROR("Failed to call service set_led to enable %s.", deviceName);
547  return 1;
548  }
549  sprintf(deviceName, "led8");
550  setLedClient.push_back(n.serviceClient<webots_ros::set_int>(modelName + '/' + deviceName + "/set_led"));
551  setLedSrv.request.value = 1;
552  if (setLedClient[1].call(setLedSrv) && setLedSrv.response.success)
553  ROS_INFO("%s turned on!", deviceName);
554  else {
555  ROS_ERROR("Failed to call service set_led to enable %s.", deviceName);
556  return 1;
557  }
558 
559  llm_active = FALSE;
561  ofm_active = FALSE;
562  lem_active = FALSE;
564  oam_reset = TRUE;
565  oam_active = FALSE;
566 
567  if (setTimeStepClient.call(setTimeStepSrv) && setTimeStepSrv.response.success)
568  nStep++;
569  else
570  ROS_ERROR("Failed to call service time_step to update robot's time step.");
571 
572  // Main loop
573  while (nStep <= stepMax) {
574  if (setTimeStepClient.call(setTimeStepSrv) && setTimeStepSrv.response.success)
575  nStep++;
576  else
577  ROS_ERROR("Failed to call service time_step to update robot's time step.");
578 
579  // get sensors value
580  if (nStep % 1 == 0) {
581  countDist = 0;
582  countGnd = 0;
583  while (countDist < NB_DIST_SENS || countGnd < NB_GROUND_SENS)
584  ros::spinOnce();
585  }
586 
587  // Speed initialization
588  speed[LEFT] = 0;
589  speed[RIGHT] = 0;
590 
592  // *** START OF SUBSUMPTION ARCHITECTURE *** //
594 
595  // LFM - Line Following Module
597 
598  speed[LEFT] = lfm_speed[LEFT];
599  speed[RIGHT] = lfm_speed[RIGHT];
600 
601  // OAM - Obstacle Avoidance Module
603 
604  // LLM - Line Leaving Module
606 
607  // OFM - Obstacle Following Module
609 
610  // Inibit A
611  if (llm_inibit_ofm_speed) {
612  ofm_speed[LEFT] = 0;
613  ofm_speed[RIGHT] = 0;
614  }
615 
616  // Sum A
617  oamOfmSpeed[LEFT] = oam_speed[LEFT] + ofm_speed[LEFT];
618  oamOfmSpeed[RIGHT] = oam_speed[RIGHT] + ofm_speed[RIGHT];
619 
620  // Suppression A
621  if (oam_active || ofm_active) {
622  speed[LEFT] = oamOfmSpeed[LEFT];
623  speed[RIGHT] = oamOfmSpeed[RIGHT];
624  }
625 
626  // LEM - Line Entering Module
628 
629  // Suppression B
630  if (lem_active) {
631  speed[LEFT] = lem_speed[LEFT];
632  speed[RIGHT] = lem_speed[RIGHT];
633  }
634 
636  // *** END OF SUBSUMPTION ARCHITECTURE *** //
638 
639  // Debug display
640  if (oam_side == -1)
641  ROS_INFO("OAM %d side NO_SIDE step %d LLM %d inibitA %d OFM %d LEM %d state %d oam_reset %d.", oam_active, nStep,
643  else if (oam_side == 0)
644  ROS_INFO("OAM %d side LEFT step %d LLM %d inibitA %d OFM %d LEM %d state %d oam_reset %d.", oam_active, nStep,
646  else if (oam_side == 1)
647  ROS_INFO("OAM %d side RIGHT step %d LLM %d inibitA %d OFM %d LEM %d state %d oam_reset %d.", oam_active, nStep,
649 
650  // convert speed to rad/s and send command to the motors
651  wheelSrv.request.value = (M_PI / 1000.0) * speed[LEFT];
652  leftWheelVelocityClient.call(wheelSrv);
653  wheelSrv.request.value = (M_PI / 1000.0) * speed[RIGHT];
654  rightWheelVelocityClient.call(wheelSrv);
655  }
656 
657  // turn off leds
658  sprintf(deviceName, "led8");
659  setLedSrv.request.value = 0;
660  if (setLedClient[1].call(setLedSrv) && setLedSrv.response.success == 1)
661  ROS_INFO("%s turned off.", deviceName);
662  else {
663  ROS_ERROR("Failed to call service set_led to enable %s.", deviceName);
664  return 1;
665  }
666  sprintf(deviceName, "led0");
667  setLedSrv.request.value = 0;
668  if (setLedClient[0].call(setLedSrv) && setLedSrv.response.success == 1)
669  ROS_INFO("%s turned off.", deviceName);
670  else {
671  ROS_ERROR("Failed to call service set_led to enable %s.", deviceName);
672  return 1;
673  }
674 
675  // disable ir proximity sensors
676  for (i = 0; i < NB_DIST_SENS; i++) {
677  SubDistIr[i].shutdown();
678  ROS_INFO("ds%d disabled.", i);
679  }
680 
681  // disable ir ground sensors
682  for (i = 0; i < NB_GROUND_SENS; i++) {
683  SubGndIr[i].shutdown();
684  ROS_INFO("gs%d disabled.", i);
685  }
686 
687  // tells Webots this node will stop using time_step service
688  setTimeStepSrv.request.value = 0;
689  if (setTimeStepClient.call(setTimeStepSrv) && setTimeStepSrv.response.success)
690  ROS_INFO("Robot's time step updated to end simulation.");
691  else
692  ROS_ERROR("Failed to call service time_step to end simulation.");
693  ros::shutdown();
694  return 0;
695 }
#define PS_RIGHT_00
Definition: e_puck_line.cpp:51
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
#define BLACK
Definition: e_puck_line.cpp:44
double accelerometerValues[3]
Definition: e_puck_line.cpp:86
#define PS_RIGHT_90
Definition: e_puck_line.cpp:53
#define LEM_STATE_LINE_DETECTED
#define OAM_OBST_THRESHOLD
#define TIME_STEP
Definition: e_puck_line.cpp:47
#define WHITE
Definition: e_puck_line.cpp:43
void psCallback(const sensor_msgs::Range::ConstPtr &value)
Definition: e_puck_line.cpp:92
int lem_active
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
#define GS_RIGHT
Definition: e_puck_line.cpp:69
#define GS_CENTER
Definition: e_puck_line.cpp:68
#define NO_SIDE
Definition: e_puck_line.cpp:40
int lfm_speed[2]
#define TRUE
Definition: e_puck_line.cpp:38
#define PS_LEFT_00
Definition: e_puck_line.cpp:58
bool call(const std::string &service_name, MReq &req, MRes &res)
XmlRpcServer s
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define RIGHT
Definition: e_puck_line.cpp:42
bool call(MReq &req, MRes &res)
int prevOpGsValue
#define GS_WHITE
Definition: e_puck_line.cpp:66
void LineLeavingModule(int side)
int ofm_active
void ObstacleFollowingModule(int side)
#define NB_GROUND_SENS
Definition: e_puck_line.cpp:65
#define PS_LEFT_90
Definition: e_puck_line.cpp:56
void gsCallback(const sensor_msgs::Range::ConstPtr &value)
Definition: e_puck_line.cpp:99
const int PS_OFFSET_REALITY[NB_DIST_SENS]
Definition: e_puck_line.cpp:62
#define LFM_K_GS_SPEED
static int step
Definition: e_puck_line.cpp:85
static int count
Definition: e_puck_line.cpp:82
int lem_speed[2]
#define LEM_THRESHOLD
ros::NodeHandle * n
Definition: pioneer3at.cpp:40
#define OAM_K_MAX_DELTAS
#define LEM_K_GS_SPEED
static char modelList[10][100]
Definition: e_puck_line.cpp:81
const int PS_OFFSET_SIMULATION[NB_DIST_SENS]
Definition: e_puck_line.cpp:61
void LineFollowingModule(void)
#define FALSE
Definition: e_puck_line.cpp:39
int psValue[NB_DIST_SENS]
Definition: e_puck_line.cpp:60
#define OAM_K_PS_45
webots_ros::set_int setTimeStepSrv
Definition: e_puck_line.cpp:90
#define LEFT
Definition: e_puck_line.cpp:41
int llm_inibit_ofm_speed
#define ROS_INFO(...)
int oam_active
int lem_black_counter
#define OAM_K_PS_00
#define LEM_STATE_LOOKING_FOR_LINE
#define LLM_THRESHOLD
uint32_t getNumPublishers() const
#define LFM_FORWARD_SPEED
int ofm_speed[2]
ros::ServiceClient setTimeStepClient
Definition: e_puck_line.cpp:89
void ObstacleAvoidanceModule(void)
int llm_active
void modelNameCallback(const std_msgs::String::ConstPtr &name)
int llm_past_side
#define LEM_STATE_ON_LINE
unsigned short gsValue[NB_GROUND_SENS]
Definition: e_puck_line.cpp:70
#define PS_LEFT_45
Definition: e_puck_line.cpp:57
int lem_state
#define GS_LEFT
Definition: e_puck_line.cpp:67
int lem_reset
#define PS_RIGHT_45
Definition: e_puck_line.cpp:52
ROSCPP_DECL void shutdown()
int oam_side
#define OFM_DELTA_SPEED
std::vector< unsigned char > image
Definition: e_puck_line.cpp:87
#define LEM_FORWARD_SPEED
static int countDist
Definition: e_puck_line.cpp:83
void LineEnteringModule(int side)
ROSCPP_DECL void spinOnce()
#define ROS_ERROR(...)
void quit(int sig)
#define LEM_STATE_STANDBY
int main(int argc, char **argv)
#define NB_DIST_SENS
Definition: e_puck_line.cpp:50
int oam_speed[2]
int curOpGsValue
static int countGnd
Definition: e_puck_line.cpp:84
int oam_reset
#define OAM_K_PS_90
#define OAM_FORWARD_SPEED


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