src/robofsm/robot_orte.c

   1 /**
   2  * @file robot_orte.c
   3  * @date 2008-2010
   4  *
   5  * @brief Module providing communication through ORTE
   6  */
   7
   8 /*
   9  * robot_orte.c                 08/04/21
  10  *
  11  * Robot's orte stuffs.
  12  *
  13  * Copyright: (c) 2008-2010 CTU Dragons
  14  *            CTU FEE - Department of Control Engineering
  15  * License: GNU GPL v.2
  16  */
  17
  18 #include <orte.h>
  19 #include <roboorte_robottype.h>
  20 #include "robodata.h"
  21 #include <robot.h>
  22 #include <movehelper.h>
  23 #include <math.h>
  24 #include <robomath.h>
  25 #include "map_handling.h"
  26 #include "match-timing.h"
  27 #include <string.h>
  28 #include <can_msg_def.h>
  29 #include <actuators.h>
  30 #include <ul_log.h>
  31 #include <lidar_params.h>
  32
  33 UL_LOG_CUST(ulogd_robot_orte); /* Log domain name = ulogd + name of the file */
  34
  35 #ifdef ORTE_DEBUG
  36    #define DBG(format, ...) ul_logdeb(format, ##__VA_ARGS__)
  37 #else
  38    #define DBG(format, ...)
  39 #endif
  40
  41 /*****FIXME:*****/
  42 extern sem_t measurement_received;
  43
  44 /* ----------------------------------------------------------------------
  45  * PUBLISHER CALLBACKS - GENERIC
  46  * ---------------------------------------------------------------------- */
  47
  48 void send_ref_pos_cb(const ORTESendInfo *info, void *vinstance,
  49                         void *sendCallBackParam)
  50 {
  51         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
  52
  53         ROBOT_LOCK(ref_pos);
  54         *instance = robot.ref_pos;
  55         ROBOT_UNLOCK(ref_pos);
  56 }
  57
  58 void send_est_pos_odo_cb(const ORTESendInfo *info, void *vinstance,
  59                         void *sendCallBackParam)
  60 {
  61         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
  62
  63         ROBOT_LOCK(est_pos_odo);
  64         *instance = robot.est_pos_odo;
  65         ROBOT_UNLOCK(est_pos_odo);
  66 }
  67
  68 void send_est_pos_indep_odo_cb(const ORTESendInfo *info, void *vinstance,
  69                         void *sendCallBackParam)
  70 {
  71         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
  72
  73         ROBOT_LOCK(est_pos_indep_odo);
  74         *instance = robot.est_pos_indep_odo;
  75         ROBOT_UNLOCK(est_pos_indep_odo);
  76 }
  77
  78 static void send_est_pos_best_cb(const ORTESendInfo *info, void *vinstance,
  79                         void *sendCallBackParam)
  80 {
  81         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
  82
  83         robot_get_est_pos(&instance->x, &instance->y, &instance->phi);
  84 }
  85
  86 void send_dummy_cb(const ORTESendInfo *info, void *vinstance,
  87                         void *sendCallBackParam)
  88 {
  89 }
  90
  91 void send_match_time_cb(const ORTESendInfo *info, void *vinstance,
  92                         void *sendCallBackParam)
  93 {
  94         struct match_time_type *instance = (struct match_time_type *)vinstance;
  95
  96         if (robot.start_state == POWER_ON || robot.start_state == START_PLUGGED_IN) {
  97                 instance->time = 600;
  98         } else {
  99                 instance->time = 600 - robot_current_time();
 100         }
 101 }
 102 /* ----------------------------------------------------------------------
 103  * SUBSCRIBER CALLBACKS - GENERIC
 104  * ---------------------------------------------------------------------- */
 105 void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
 106                         void *recvCallBackParam)
 107 {
 108         struct odo_data_type *instance = (struct odo_data_type *)vinstance;
 109         double dleft, dright, dtang, dphi;
 110         static bool first_run = true;
 111         /* spocitat prevodovy pomer */
 112         const double n = (double)(1.0 / 1.0);
 113
 114         /* vzdalenost na pulz IRC */
 115         const double c = (M_PI*2*ODOMETRY_WHEEL_RADIUS_M) / (n * 4*4096.0);
 116
 117         switch (info->status) {
 118                 case NEW_DATA:
 119                         if (first_run) {
 120                                 ROBOT_LOCK(odo_data);
 121                                 robot.odo_data = *instance;
 122                                 ROBOT_UNLOCK(odo_data);
 123                                 first_run = false;
 124                                 break;
 125                         }
 126
 127                         dleft = ((robot.odo_data.left - instance->left) >> 8) * c * robot.odo_cal_a;    // TODO >> 8 ?
 128                         dright = ((instance->right - robot.odo_data.right) >> 8) * c * robot.odo_cal_b;
 129
 130                         dtang = (dleft + dright) / 2.0;
 131                         dphi = (dright - dleft) / (2.0*ODOMETRY_ROTATION_RADIUS_M);
 132
 133                         ROBOT_LOCK(est_pos_indep_odo);
 134                         robot.odo_distance_a +=dleft;
 135                         robot.odo_distance_b +=dright;
 136                         double a = robot.est_pos_indep_odo.phi;
 137                         robot.est_pos_indep_odo.x += dtang*cos(a);
 138                         robot.est_pos_indep_odo.y += dtang*sin(a);
 139                         robot.est_pos_indep_odo.phi += dphi;
 140                         ROBOT_UNLOCK(est_pos_indep_odo);
 141
 142                         ROBOT_LOCK(odo_data);
 143                         robot.odo_data = *instance;
 144                         ROBOT_UNLOCK(odo_data);
 145
 146                         robot.indep_odometry_works = true;
 147
 148                         /* wake up motion-control/thread_trajectory_follower */
 149                         sem_post(&measurement_received);
 150
 151                         //robot.hw_status[COMPONENT_ODO] = STATUS_OK;
 152                         break;
 153                 case DEADLINE:
 154                         robot.indep_odometry_works = false;
 155                         //robot.hw_status[COMPONENT_ODO] = STATUS_FAILED;
 156                         DBG("ORTE deadline occurred - odo_data receive\n");
 157                         break;
 158         }
 159 }
 160
 161 void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
 162                         void *recvCallBackParam)
 163 {
 164         struct motion_irc_type *instance = (struct motion_irc_type *)vinstance;
 165         double dleft, dright, dtang, dphi;
 166         static bool first_run = true;
 167         /* spocitat prevodovy pomer */
 168         const double n = (double)(ROBOT_MOTOR_GEARBOX_RATIO / 1.0);
 169
 170         /* vzdalenost na pulz IRC */
 171         const double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*ROBOT_MOTOR_IRC_RESOLUTION);
 172         switch (info->status) {
 173                 case NEW_DATA:
 174                         if (first_run) {
 175                                 ROBOT_LOCK(motion_irc);
 176                                 robot.motion_irc = *instance;
 177                                 ROBOT_UNLOCK(motion_irc);
 178                                 first_run = false;
 179                                 break;
 180                         }
 181
 182
 183                         /* FIXME maybe it is not correct to do this nasty hack here (switch dleft and dright),
 184                         what is the right solution?
 185                         This was neccessary to view motor odometry correctly in robomon. */
 186                         dright = ((robot.motion_irc.left - instance->left) >> 8) * c;
 187                         dleft = ((instance->right - robot.motion_irc.right) >> 8) * c;
 188
 189                         dtang = (dleft + dright) / 2.0;
 190                         dphi = (dright - dleft) / (2.0*ROBOT_ROTATION_RADIUS_M);
 191
 192                         ROBOT_LOCK(est_pos_odo);
 193                         double a = robot.est_pos_odo.phi;
 194                         robot.est_pos_odo.x += dtang*cos(a);
 195                         robot.est_pos_odo.y += dtang*sin(a);
 196                         robot.est_pos_odo.phi += dphi;
 197                         ROBOT_UNLOCK(est_pos_odo);
 198
 199                         /* locking should not be needed, but... */
 200                         ROBOT_LOCK(motion_irc);
 201                         robot.motion_irc = *instance;
 202                         robot.motion_irc_received = 1;
 203                         ROBOT_UNLOCK(motion_irc);
 204
 205                         robot.odometry_works = true;
 206
 207                         robot.status[COMPONENT_MOTOR] = STATUS_OK;
 208                         break;
 209                 case DEADLINE:
 210                         robot.odometry_works = false;
 211                         robot.status[COMPONENT_MOTOR] = STATUS_FAILED;
 212                         DBG("ORTE deadline occurred - motion_irc receive\n");
 213                         break;
 214         }
 215 }
 216
 217 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 218                         void *recvCallBackParam)
 219 {
 220         switch (info->status) {
 221                 case NEW_DATA:
 222                         break;
 223                 case DEADLINE:
 224                         DBG("ORTE deadline occurred - motion_speed receive\n");
 225                         break;
 226         }
 227 }
 228
 229 void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
 230                         void *recvCallBackParam)
 231 {
 232         switch (info->status) {
 233                 case NEW_DATA:
 234                         break;
 235                 case DEADLINE:
 236                         DBG("ORTE deadline occurred - motion_status receive\n");
 237                         break;
 238         }
 239 }
 240
 241 void rcv_pwr_voltage_cb(const ORTERecvInfo *info, void *vinstance,
 242                         void *recvCallBackParam)
 243 {
 244         switch (info->status) {
 245                 case NEW_DATA:
 246                         robot.status[COMPONENT_POWER]=STATUS_OK;
 247                         break;
 248                 case DEADLINE:
 249                         robot.status[COMPONENT_POWER]=STATUS_FAILED;
 250                         DBG("ORTE deadline occurred - pwr_voltage receive\n");
 251                         break;
 252         }
 253 }
 254
 255 void rcv_pwr_ctrl_cb(const ORTERecvInfo *info, void *vinstance,
 256                         void *recvCallBackParam)
 257 {
 258         switch (info->status) {
 259                 case NEW_DATA:
 260                         break;
 261                 case DEADLINE:
 262                         DBG("ORTE deadline occurred - pwr_ctrl receive\n");
 263                         break;
 264         }
 265 }
 266
 267 void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
 268                         void *recvCallBackParam)
 269 {
 270         struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
 271         static struct robot_cmd_type last_instance;
 272
 273         switch (info->status) {
 274                 case NEW_DATA:
 275                         /* Stupid way of controlling the robot by
 276                          * pluggin in and out start connector. */
 277                         switch (robot.start_state) {
 278                                 case POWER_ON:
 279                                         if (!instance->start_condition) {
 280                                                 robot.status[COMPONENT_START] = STATUS_WARNING;
 281                                                 robot.start_state = START_PLUGGED_IN;
 282                                                 ul_logmsg("START_PLUGGED_IN\n");
 283                                         }
 284                                         break;
 285
 286                                 case START_PLUGGED_IN:
 287                                         robot.status[COMPONENT_START] = STATUS_OK;
 288                                         /* Competition starts when plugged out */
 289                                         if (instance->start_condition) {
 290                                                 FSM_SIGNAL(MAIN, EV_START, NULL);
 291                                                 robot.start_state = COMPETITION_STARTED;
 292                                                 ul_logmsg("STARTED\n");
 293                                         }
 294                                         break;
 295
 296                                 case COMPETITION_STARTED: {
 297                                         /* Subsequent plug-in stops the robot */
 298                                         static int num = 0;
 299                                         if (!instance->start_condition) {
 300                                                 robot.status[COMPONENT_START] = STATUS_WARNING;
 301                                                 if (++num == 10)
 302                                                         robot_exit();
 303                                         }
 304                                         break;
 305                                 }
 306                         }
 307                         last_instance = *instance;
 308                         break;
 309                 case DEADLINE:
 310                         robot.status[COMPONENT_START] = STATUS_FAILED;
 311                         DBG("ORTE deadline occurred - robot_cmd receive\n");
 312                         break;
 313         }
 314 }
 315
 316 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
 317                         void *recvCallBackParam)
 318 {
 319         struct lidar_scan_type *instance = (struct lidar_scan_type *)vinstance;
 320
 321         switch (info->status) {
 322                 case NEW_DATA: {
 323                         ROBOT_LOCK(hokuyo);
 324                         robot.hokuyo = *instance;
 325                         robot.status[COMPONENT_HOKUYO] = STATUS_OK;
 326                         ROBOT_UNLOCK(hokuyo);
 327                         if(!robot.ignore_hokuyo)
 328                         {
 329                                 //update_map_lidar(&hokuyo_params, instance);
 330                         }
 331                         break;
 332                 }
 333                 case DEADLINE:
 334                         robot.status[COMPONENT_HOKUYO] = STATUS_FAILED;
 335                         //system("killall -9 hokuyo");
 336                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 337                         break;
 338         }
 339 }
 340
 341 void rcv_sick_scan_cb(const ORTERecvInfo *info, void *vinstance,
 342                         void *recvCallBackParam)
 343 {
 344         struct lidar_scan_type *instance = (struct lidar_scan_type *)vinstance;
 345
 346         switch (info->status) {
 347                 case NEW_DATA: {
 348                         ROBOT_LOCK(sick);
 349                         robot.sick = *instance;
 350                         robot.status[COMPONENT_SICK] = STATUS_OK;
 351                         ROBOT_UNLOCK(sick);
 352                         if(!robot.ignore_sick)
 353                         {
 354                                 update_map_lidar(&sick_params, instance);
 355                         }
 356                         break;
 357                 }
 358                 case DEADLINE:
 359                         robot.status[COMPONENT_SICK] = STATUS_FAILED;
 360                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 361                         break;
 362         }
 363 }
 364
 365 void rcv_sick551_scan_cb(const ORTERecvInfo *info, void *vinstance,
 366                         void *recvCallBackParam)
 367 {
 368         struct lidar_scan_type *instance = (struct lidar_scan_type *)vinstance;
 369
 370         switch (info->status) {
 371                 case NEW_DATA: {
 372                         ROBOT_LOCK(sick551);
 373                         robot.sick551 = *instance;
 374                         robot.status[COMPONENT_SICK551] = STATUS_OK;
 375                         ROBOT_UNLOCK(sick551);
 376                         if(!robot.ignore_sick551)
 377                         {
 378                                 update_map_lidar(&sick551_params, instance);
 379                         }
 380                         break;
 381                 }
 382                 case DEADLINE:
 383                         robot.status[COMPONENT_SICK551] = STATUS_FAILED;
 384                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 385                         break;
 386         }
 387 }
 388
 389 void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
 390                         void *recvCallBackParam)
 391 {
 392         struct camera_result_type *instance = (struct camera_result_type *)vinstance;
 393         static bool last_response = false;
 394
 395         switch (info->status) {
 396                 case NEW_DATA: {
 397                         if (instance->data_valid && instance->data_valid != last_response) {
 398                                 ROBOT_LOCK(camera_result);
 399                                 robot.target_valid = instance->target_valid;
 400                                 robot.target_ang = instance->angle_deg;
 401                                 ROBOT_UNLOCK(camera_result);
 402                                 FSM_SIGNAL(MAIN, EV_CAMERA_DONE, NULL);
 403                         }
 404                         last_response = instance->data_valid;
 405                         break;
 406                 }
 407                 case DEADLINE:
 408                         if (robot.orte.camera_control.on) {
 409                                 robot.status[COMPONENT_CAMERA] = STATUS_FAILED;
 410                         }
 411
 412                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 413                         break;
 414         }
 415 }
 416 /* ----------------------------------------------------------------------
 417  * SUBSCRIBER CALLBACKS - EB2008
 418  * ---------------------------------------------------------------------- */
 419
 420 void rcv_jaws_status_cb(const ORTERecvInfo *info, void *vinstance,
 421                             void *recvCallBackParam)
 422 {
 423         struct jaws_status_type *instance = (struct jaws_status_type *)vinstance;
 424         static int last_response_left = 0;
 425         static int last_response_right = 0;
 426         switch (info->status) {
 427                 case NEW_DATA:
 428                         // new data arrived and requested position equals actual position
 429                         if (instance->flags.left == 0 &&
 430                             instance->flags.right == 0)
 431                                 robot.status[COMPONENT_JAWS] = STATUS_OK;
 432                         else
 433                                 robot.status[COMPONENT_JAWS] = STATUS_WARNING;
 434
 435                         if (instance->response.left != last_response_left &&
 436                             instance->response.right != last_response_right &&
 437                             instance->response.left == act_jaw_left_get_last_reqest() &&
 438                             instance->response.left == act_jaw_right_get_last_reqest())
 439                                 FSM_SIGNAL(MAIN, EV_JAWS_DONE, NULL);
 440
 441                         last_response_left = instance->response.left;
 442                         last_response_right = instance->response.right;
 443                         break;
 444                 case DEADLINE:
 445                         robot.status[COMPONENT_JAWS] = STATUS_FAILED;
 446                         DBG("ORTE deadline occurred - actuator_status receive\n");
 447                         break;
 448         }
 449 }
 450
 451 void rcv_lift_status_cb(const ORTERecvInfo *info, void *vinstance,
 452                             void *recvCallBackParam)
 453 {
 454         struct lift_status_type *instance = (struct lift_status_type *)vinstance;
 455         static int last_response = 0;
 456         switch (info->status) {
 457                 case NEW_DATA:
 458                         // new data arrived and requested position equals actual position
 459                         if (instance->flags == 0)
 460                                 robot.status[COMPONENT_LIFT] = STATUS_OK;
 461                         else
 462                                 robot.status[COMPONENT_LIFT] = STATUS_WARNING;
 463
 464                         if (instance->response != last_response &&
 465                             instance->response == act_lift_get_last_reqest())
 466                                 FSM_SIGNAL(MAIN, EV_LIFT_DONE, NULL);
 467                         last_response = instance->response;
 468                         break;
 469                 case DEADLINE:
 470                         robot.status[COMPONENT_LIFT] = STATUS_FAILED;
 471                         DBG("ORTE deadline occurred - actuator_status receive\n");
 472                         break;
 473         }
 474 }
 475
 476 void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance,
 477                            void *recvCallBackParam)
 478 {
 479         struct robot_switches_type *instance = (struct robot_switches_type *)vinstance;
 480         static bool last_strategy;
 481         switch (info->status) {
 482                 case NEW_DATA:
 483                         robot.team_color = instance->team_color;
 484
 485                         if (!last_strategy && instance->strategy) {
 486                                         /* strategy switching */
 487                                         FSM_SIGNAL(MAIN, EV_SWITCH_STRATEGY, NULL);
 488                         }
 489                         last_strategy = instance->strategy;
 490                         break;
 491                 case DEADLINE:
 492                         break;
 493         }
 494 }
 495
 496 void rcv_robot_bumpers_cb(const ORTERecvInfo *info, void *vinstance,
 497                            void *recvCallBackParam)
 498 {
 499         struct robot_bumpers_type *instance = (struct robot_bumpers_type *)vinstance;
 500         static bool last_left, last_right;
 501         switch (info->status) {
 502                 case NEW_DATA:
 503                         if (instance->bumper_right_across || instance->bumper_left_across || instance->bumper_rear_left || instance->bumper_rear_right)
 504                                 FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
 505
 506                         if (instance->bumper_left || instance->bumper_right) {
 507                                 FSM_SIGNAL(MOTION, EV_OBSTACLE_SIDE, NULL);
 508                         }
 509                         break;
 510                 case DEADLINE:
 511                         break;
 512         }
 513 }
 514
 515 void rcv_cl_sensor_status_cb(const ORTERecvInfo *info, void *vinstance,
 516                            void *recvCallBackParam)
 517 {
 518         struct cl_sensor_status_type *instance = (struct cl_sensor_status_type *)vinstance;
 519         static bool last_pattern_match = 0;
 520         switch (info->status) {
 521                 case NEW_DATA:
 522                         if ((last_pattern_match != instance->pattern_match) && instance->pattern_match)
 523                                 FSM_SIGNAL(MAIN, EV_OMRON_DONE, NULL);
 524
 525                         last_pattern_match = instance->pattern_match;
 526                         break;
 527                 case DEADLINE:
 528                         break;
 529         }
 530 }
 531
 532 #define HIST_CNT 5
 533 #if 0
 534 static int cmp_double(const void *v1, const void *v2)
 535 {
 536         const double *d1 = v1, *const d2 = v2;
 537         if (d1 < d2)
 538                 return -1;
 539         else if (d1 > d2)
 540                 return +1;
 541         else
 542                 return 0;
 543 }
 544 #endif
 545
 546 int robot_init_orte()
 547 {
 548         int rv = 0;
 549
 550         robot.orte.strength = 20;
 551
 552         rv = robottype_roboorte_init(&robot.orte);
 553         if (rv)
 554                 return rv;
 555
 556         /* creating publishers */
 557         robottype_publisher_motion_speed_create(&robot.orte, NULL, &robot.orte);
 558         robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte);
 559         robottype_publisher_est_pos_odo_create(&robot.orte, send_est_pos_odo_cb, &robot.orte);
 560         robottype_publisher_est_pos_indep_odo_create(&robot.orte, send_est_pos_indep_odo_cb, &robot.orte);
 561         robottype_publisher_est_pos_best_create(&robot.orte, send_est_pos_best_cb, &robot.orte);
 562         robottype_publisher_match_time_create(&robot.orte, send_match_time_cb, &robot.orte);
 563         //???robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
 564
 565         // I didn't know what was the difference between the callback function pointer
 566         // being NULL and being set to pointer to empty send_dummy_cb function. The DIFFERENCE
 567         // IS CRUCIAL!
 568         //   - NULL: message is published only when OrtePublicationSend called
 569         //   - pointer to empty function: message is published periodically
 570         robottype_publisher_fsm_main_create(&robot.orte, send_dummy_cb, &robot.orte);
 571         robottype_publisher_fsm_act_create(&robot.orte, send_dummy_cb, &robot.orte);
 572         robottype_publisher_fsm_motion_create(&robot.orte, send_dummy_cb, &robot.orte);
 573         robottype_publisher_camera_control_create(&robot.orte, send_dummy_cb, &robot.orte);
 574         robottype_publisher_jaws_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
 575         robottype_publisher_lift_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
 576
 577         /* create generic subscribers */
 578         robottype_subscriber_odo_data_create(&robot.orte, rcv_odo_data_cb, &robot.orte);
 579         robottype_subscriber_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte);
 580         robottype_subscriber_motion_speed_create(&robot.orte, rcv_motion_speed_cb, &robot.orte);
 581         robottype_subscriber_motion_status_create(&robot.orte, rcv_motion_status_cb, &robot.orte);
 582         robottype_subscriber_pwr_voltage_create(&robot.orte, rcv_pwr_voltage_cb, &robot.orte);
 583         //robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
 584         robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
 585         robottype_subscriber_sick_scan_create(&robot.orte, rcv_sick_scan_cb, &robot.orte);
 586         robottype_subscriber_jaws_status_create(&robot.orte, rcv_jaws_status_cb, &robot.orte);
 587         robottype_subscriber_lift_status_create(&robot.orte, rcv_lift_status_cb, &robot.orte);
 588         robottype_subscriber_robot_switches_create(&robot.orte, rcv_robot_switches_cb, &robot.orte);
 589         robottype_subscriber_robot_bumpers_create(&robot.orte, rcv_robot_bumpers_cb, &robot.orte);
 590         robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);
 591         robottype_subscriber_cl_sensor_status_create(&robot.orte, rcv_cl_sensor_status_cb, &robot.orte);
 592
 593         return rv;
 594 }
 595