src/robofsm/robot_orte.cc

   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 <string.h>
  27 #include <can_msg_def.h>
  28 #include <actuators.h>
  29 #include <ul_log.h>
  30
  31 UL_LOG_CUST(ulogd_robot_orte); /* Log domain name = ulogd + name of the file */
  32
  33 #ifdef ORTE_DEBUG
  34    #define DBG(format, ...) ul_logdeb(format, ##__VA_ARGS__)
  35 #else
  36    #define DBG(format, ...)
  37 #endif
  38
  39 /*****FIXME:*****/
  40 extern sem_t measurement_received;
  41
  42 /**
  43  * Check rrobot component status
  44  * @return zero if system OK, else 1
  45  */
  46 int check_prestart_status()
  47 {
  48         if ((robot.status[COMPONENT_MOTOR] == STATUS_FAILED) || (robot.status[COMPONENT_MOTOR] == STATUS_WARNING)) {
  49                 return 1;
  50         } else if (robot.status[COMPONENT_ODOMETRY] == STATUS_FAILED) {
  51                 return 1;
  52         } else if ((robot.status[COMPONENT_CAMERA] == STATUS_FAILED) || (robot.status[COMPONENT_CAMERA] == STATUS_WARNING)) {
  53                 return 1;
  54         } else if (robot.status[COMPONENT_POWER] == STATUS_FAILED) {
  55                 return 1;
  56         } else if (robot.status[COMPONENT_HOKUYO] == STATUS_FAILED) {
  57                 return 1;
  58         } else if (robot.status[COMPONENT_CRANE] == STATUS_FAILED) {
  59                 return 1;
  60         } else if (robot.status[COMPONENT_START] == STATUS_FAILED || robot.status[COMPONENT_START] == STATUS_WARNING) {
  61                 return 1;
  62         } else {
  63                 return 0;
  64         }
  65 }
  66 /* ----------------------------------------------------------------------
  67  * PUBLISHER CALLBACKS - GENERIC
  68  * ---------------------------------------------------------------------- */
  69
  70 void send_ref_pos_cb(const ORTESendInfo *info, void *vinstance,
  71                         void *sendCallBackParam)
  72 {
  73         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
  74
  75         ROBOT_LOCK(ref_pos);
  76         *instance = robot.ref_pos;
  77         ROBOT_UNLOCK(ref_pos);
  78 }
  79
  80 void send_est_pos_odo_cb(const ORTESendInfo *info, void *vinstance,
  81                         void *sendCallBackParam)
  82 {
  83         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
  84
  85         ROBOT_LOCK(est_pos_odo);
  86         *instance = robot.est_pos_odo;
  87         ROBOT_UNLOCK(est_pos_odo);
  88 }
  89
  90 void send_est_pos_indep_odo_cb(const ORTESendInfo *info, void *vinstance,
  91                         void *sendCallBackParam)
  92 {
  93         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
  94
  95         ROBOT_LOCK(est_pos_indep_odo);
  96         *instance = robot.est_pos_indep_odo;
  97         ROBOT_UNLOCK(est_pos_indep_odo);
  98 }
  99
 100 static void send_est_pos_best_cb(const ORTESendInfo *info, void *vinstance,
 101                         void *sendCallBackParam)
 102 {
 103         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
 104
 105         robot_get_est_pos(&instance->x, &instance->y, &instance->phi);
 106 }
 107
 108 static void send_system_status_cb(const ORTESendInfo *info, void *vinstance,
 109                         void *sendCallBackParam)
 110 {
 111         system_status *instance = (system_status *)vinstance;
 112
 113         /* if any component not ready, send warning for display to show APP yellow */
 114         if (check_prestart_status()) {
 115                 instance->system_condition = 1; // system warning
 116         } else {
 117                 instance->system_condition = 0; // system OK
 118         }
 119 }
 120
 121 void send_dummy_cb(const ORTESendInfo *info, void *vinstance,
 122                         void *sendCallBackParam)
 123 {
 124 }
 125
 126 /* ----------------------------------------------------------------------
 127  * SUBSCRIBER CALLBACKS - GENERIC
 128  * ---------------------------------------------------------------------- */
 129 void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
 130                         void *recvCallBackParam)
 131 {
 132         struct odo_data_type *instance = (struct odo_data_type *)vinstance;
 133         double dleft, dright, dtang, dphi;
 134         static bool first_run = true;
 135         /* spocitat prevodovy pomer */
 136         const double n = (double)(1.0 / 1.0);
 137
 138         /* vzdalenost na pulz IRC */
 139         const double c = (M_PI*2*ODOMETRY_WHEEL_RADIUS_M) / (n * 4*4096.0);
 140
 141         switch (info->status) {
 142                 case NEW_DATA:
 143                         if (first_run) {
 144                                 ROBOT_LOCK(odo_data);
 145                                 robot.odo_data = *instance;
 146                                 ROBOT_UNLOCK(odo_data);
 147                                 first_run = false;
 148                                 break;
 149                         }
 150
 151                         dleft = ((robot.odo_data.left - instance->left) >> 8) * c;      // TODO >> 8 ?
 152                         dright = ((instance->right - robot.odo_data.right) >> 8) * c;
 153
 154                         dtang = (dleft + dright) / 2.0;
 155                         dphi = (dright - dleft) / (2.0*ODOMETRY_ROTATION_RADIUS_M);
 156
 157                         ROBOT_LOCK(est_pos_indep_odo);
 158                         double a = robot.est_pos_indep_odo.phi;
 159                         robot.est_pos_indep_odo.x += dtang*cos(a);
 160                         robot.est_pos_indep_odo.y += dtang*sin(a);
 161                         robot.est_pos_indep_odo.phi += dphi;
 162                         ROBOT_UNLOCK(est_pos_indep_odo);
 163
 164                         ROBOT_LOCK(odo_data);
 165                         robot.odo_data = *instance;
 166                         ROBOT_UNLOCK(odo_data);
 167
 168                         robot.indep_odometry_works = true;
 169
 170                         /* wake up motion-control/thread_trajectory_follower */
 171                         sem_post(&measurement_received);
 172
 173                         robot.status[COMPONENT_ODOMETRY] = STATUS_OK;
 174                         break;
 175                 case DEADLINE:
 176                         robot.indep_odometry_works = false;
 177                         robot.status[COMPONENT_ODOMETRY] = STATUS_FAILED;
 178                         DBG("ORTE deadline occurred - odo_data receive\n");
 179                         break;
 180         }
 181 }
 182
 183 void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
 184                         void *recvCallBackParam)
 185 {
 186         struct motion_irc_type *instance = (struct motion_irc_type *)vinstance;
 187         double dleft, dright, dtang, dphi;
 188         static bool first_run = true;
 189         /* spocitat prevodovy pomer */
 190         const double n = (double)(28.0 / 1.0);
 191
 192         /* vzdalenost na pulz IRC */
 193         const double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*500.0);
 194
 195         switch (info->status) {
 196                 case NEW_DATA:
 197                         if (first_run) {
 198                                 ROBOT_LOCK(motion_irc);
 199                                 robot.motion_irc = *instance;
 200                                 ROBOT_UNLOCK(motion_irc);
 201                                 first_run = false;
 202                                 break;
 203                         }
 204
 205                         dleft = ((robot.motion_irc.left - instance->left) >> 8) * c;
 206                         dright = ((instance->right - robot.motion_irc.right) >> 8) * c;
 207
 208                         dtang = (dleft + dright) / 2.0;
 209                         dphi = (dright - dleft) / (2.0*ROBOT_ROTATION_RADIUS_M);
 210
 211                         ROBOT_LOCK(est_pos_odo);
 212                         double a = robot.est_pos_odo.phi;
 213                         robot.est_pos_odo.x += dtang*cos(a);
 214                         robot.est_pos_odo.y += dtang*sin(a);
 215                         robot.est_pos_odo.phi += dphi;
 216                         ROBOT_UNLOCK(est_pos_odo);
 217
 218                         /* locking should not be needed, but... */
 219                         ROBOT_LOCK(motion_irc);
 220                         robot.motion_irc = *instance;
 221                         robot.motion_irc_received = 1;
 222                         ROBOT_UNLOCK(motion_irc);
 223
 224                         robot.odometry_works = true;
 225
 226                         //robot.status[COMPONENT_MOTOR] = STATUS_OK;
 227                         break;
 228                 case DEADLINE:
 229                         robot.odometry_works = false;
 230                         robot.status[COMPONENT_MOTOR] = STATUS_FAILED;
 231                         DBG("ORTE deadline occurred - motion_irc receive\n");
 232                         break;
 233         }
 234 }
 235
 236 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 237                         void *recvCallBackParam)
 238 {
 239         switch (info->status) {
 240                 case NEW_DATA:
 241                         break;
 242                 case DEADLINE:
 243                         DBG("ORTE deadline occurred - motion_speed receive\n");
 244                         break;
 245         }
 246 }
 247
 248 void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
 249                         void *recvCallBackParam)
 250 {
 251         struct motion_status_type *m = (struct motion_status_type *)vinstance;
 252
 253         switch (info->status) {
 254                 case NEW_DATA:
 255                         if (m->err_left == 0 && m->err_right == 0)
 256                                 robot.status[COMPONENT_MOTOR] = STATUS_OK;
 257                         else
 258                                 robot.status[COMPONENT_MOTOR] = STATUS_WARNING;
 259                         break;
 260                 case DEADLINE:
 261                         DBG("ORTE deadline occurred - motion_status receive\n");
 262                         break;
 263         }
 264 }
 265
 266 void rcv_pwr_voltage_cb(const ORTERecvInfo *info, void *vinstance,
 267                         void *recvCallBackParam)
 268 {
 269         switch (info->status) {
 270                 case NEW_DATA:
 271                         robot.status[COMPONENT_POWER]=STATUS_OK;
 272                         break;
 273                 case DEADLINE:
 274                         robot.status[COMPONENT_POWER]=STATUS_FAILED;
 275                         DBG("ORTE deadline occurred - pwr_voltage receive\n");
 276                         break;
 277         }
 278 }
 279
 280 void rcv_pwr_ctrl_cb(const ORTERecvInfo *info, void *vinstance,
 281                         void *recvCallBackParam)
 282 {
 283         switch (info->status) {
 284                 case NEW_DATA:
 285                         break;
 286                 case DEADLINE:
 287                         DBG("ORTE deadline occurred - pwr_ctrl receive\n");
 288                         break;
 289         }
 290 }
 291
 292 void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
 293                         void *recvCallBackParam)
 294 {
 295         struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
 296         static struct robot_cmd_type last_instance;
 297
 298         switch (info->status) {
 299                 case NEW_DATA:
 300                         /* Stupid way of controlling the robot by
 301                          * pluggin in and out start connector. */
 302                         switch (robot.start_state) {
 303                                 case POWER_ON:
 304                                         robot.status[COMPONENT_START] = STATUS_WARNING;
 305                                         if (instance->start_condition == START_PLUGGED_IN) {
 306                                                 robot.start_state = START_PLUGGED_IN;
 307                                                 ul_logmsg("START_PLUGGED_IN\n");
 308                                         }
 309                                         break;
 310
 311                                 case START_PLUGGED_IN:
 312                                         robot.status[COMPONENT_START] = STATUS_OK;
 313                                         /* Competition starts when plugged out, check component status before start */
 314                                         if ((instance->start_condition == COMPETITION_STARTED) && !check_prestart_status()) {
 315                                                 FSM_SIGNAL(MAIN, EV_START, NULL);
 316                                                 robot.start_state = COMPETITION_STARTED;
 317                                                 ul_logmsg("STARTED\n");
 318                                         }
 319                                         break;
 320
 321                                 case COMPETITION_STARTED: {
 322                                         robot.status[COMPONENT_START] = STATUS_WARNING;
 323                                         /* Subsequent plug-in stops the robot */
 324                                         static int num = 0;
 325                                         if (instance->start_condition == START_PLUGGED_IN) {
 326                                                 if (++num == 10)
 327                                                         robot_exit();
 328                                         }
 329                                         break;
 330                                 }
 331                         }
 332                         last_instance = *instance;
 333                         break;
 334                 case DEADLINE:
 335                         robot.status[COMPONENT_START] = STATUS_FAILED;
 336                         DBG("ORTE deadline occurred - robot_cmd receive\n");
 337                         break;
 338         }
 339 }
 340
 341 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
 342                         void *recvCallBackParam)
 343 {
 344         struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
 345
 346         switch (info->status) {
 347                 case NEW_DATA: {
 348                         ROBOT_LOCK(hokuyo);
 349                         robot.hokuyo = *instance;
 350                         robot.status[COMPONENT_HOKUYO] = STATUS_OK;
 351                         ROBOT_UNLOCK(hokuyo);
 352                         if(!robot.ignore_hokuyo)
 353                         {
 354                                 update_map_hokuyo(instance);
 355                         }
 356                         break;
 357                 }
 358                 case DEADLINE:
 359                         robot.status[COMPONENT_HOKUYO] = STATUS_FAILED;
 360                         //system("killall -9 hokuyo");
 361                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 362                         break;
 363         }
 364 }
 365
 366 void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
 367                         void *recvCallBackParam)
 368 {
 369         struct camera_result_type *instance = (struct camera_result_type *)vinstance;
 370         static bool last_response = false;
 371
 372         switch (info->status) {
 373                 case NEW_DATA: {
 374                         if (instance->error) {
 375                                 robot.status[COMPONENT_CAMERA] = STATUS_FAILED;
 376                         } else if (instance->data_valid && instance->data_valid != last_response) {
 377                                 ROBOT_LOCK(camera_result);
 378                                 robot.target_valid = instance->target_valid;
 379                                 ROBOT_UNLOCK(camera_result);
 380                                 FSM_SIGNAL(MAIN, EV_CAMERA_DONE, NULL);
 381                                 printf("robot_orte: valid: %d\n", instance->target_valid);
 382
 383                                 robot.status[COMPONENT_CAMERA] = STATUS_OK;
 384                         }
 385                         last_response = instance->data_valid;
 386                         break;
 387                 }
 388                 case DEADLINE:
 389                         if (robot.orte.camera_control.on) {
 390                                 robot.status[COMPONENT_CAMERA] = STATUS_FAILED;
 391                                 //system("killall -9 rozpuk");
 392                         }
 393
 394                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 395                         break;
 396         }
 397 }
 398
 399 void rcv_crane_status_cb(const ORTERecvInfo *info, void *vinstance,
 400                             void *recvCallBackParam)
 401 {
 402         struct crane_status_type *instance = (struct crane_status_type *)vinstance;
 403         static int last_response = 0;
 404         switch (info->status) {
 405                 case NEW_DATA:
 406                         // new data arrived and requested position equals actual position
 407                         if (instance->flags == 0)
 408                                 robot.status[COMPONENT_CRANE]=STATUS_OK;
 409                         else
 410                                 robot.status[COMPONENT_CRANE]=STATUS_WARNING;
 411
 412                         if (instance->response != last_response &&
 413                             instance->response == act_crane_get_last_reqest())
 414                                 FSM_SIGNAL(MAIN, EV_CRANE_DONE, NULL);
 415                         last_response = instance->response;
 416                         break;
 417                 case DEADLINE:
 418                         robot.status[COMPONENT_CRANE]=STATUS_FAILED;
 419                         DBG("ORTE deadline occurred - actuator_status receive\n");
 420                         break;
 421         }
 422 }
 423
 424 void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance,
 425                            void *recvCallBackParam)
 426 {
 427         struct robot_switches_type *instance = (struct robot_switches_type *)vinstance;
 428         static bool last_strategy;
 429         switch (info->status) {
 430                 case NEW_DATA:
 431                         if ((!last_strategy && instance->strategy) && (robot.start_state == POWER_ON)) {
 432                                 FSM_SIGNAL(MAIN, EV_SWITCH_STRATEGY, NULL);
 433                         }
 434                         last_strategy = instance->strategy;
 435                         break;
 436                 case DEADLINE:
 437                         break;
 438         }
 439 }
 440
 441 void rcv_robot_bumpers_cb(const ORTERecvInfo *info, void *vinstance,
 442                            void *recvCallBackParam)
 443 {
 444         struct robot_bumpers_type *instance = (struct robot_bumpers_type *)vinstance;
 445         switch (info->status) {
 446                 case NEW_DATA:
 447                         if (!instance->bumper_rear) {
 448                                 FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
 449                         }
 450                         if ((!instance->bumper_left) ||
 451                             (!instance->bumper_right)) {
 452                                 FSM_SIGNAL(MOTION, EV_OBSTACLE_SIDE, NULL);
 453                         }
 454                         break;
 455                 case DEADLINE:
 456                         break;
 457         }
 458 }
 459
 460
 461 #define HIST_CNT 5
 462 #if 0
 463 static int cmp_double(const void *v1, const void *v2)
 464 {
 465         const double *d1 = v1, *const d2 = v2;
 466         if (d1 < d2)
 467                 return -1;
 468         else if (d1 > d2)
 469                 return +1;
 470         else
 471                 return 0;
 472 }
 473 #endif
 474
 475 int robot_init_orte()
 476 {
 477         int rv = 0;
 478
 479         robot.orte.strength = 20;
 480
 481         rv = robottype_roboorte_init(&robot.orte);
 482         if (rv)
 483                 return rv;
 484
 485         /* creating publishers */
 486         robottype_publisher_motion_speed_create(&robot.orte, NULL, &robot.orte);
 487         robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte);
 488         robottype_publisher_est_pos_odo_create(&robot.orte, send_est_pos_odo_cb, &robot.orte);
 489         robottype_publisher_est_pos_indep_odo_create(&robot.orte, send_est_pos_indep_odo_cb, &robot.orte);
 490         robottype_publisher_est_pos_best_create(&robot.orte, send_est_pos_best_cb, &robot.orte);
 491         robottype_publisher_system_status_create(&robot.orte, send_system_status_cb, &robot.orte);
 492         //???robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
 493
 494         // I didn't know what was the difference between the callback function pointer
 495         // being NULL and being set to pointer to empty send_dummy_cb function. The DIFFERENCE
 496         // IS CRUCIAL!
 497         //   - NULL: message is published only when OrtePublicationSend called
 498         //   - pointer to empty function: message is published periodically
 499         robottype_publisher_fsm_main_create(&robot.orte, send_dummy_cb, &robot.orte);
 500         robottype_publisher_fsm_act_create(&robot.orte, send_dummy_cb, &robot.orte);
 501         robottype_publisher_fsm_motion_create(&robot.orte, send_dummy_cb, &robot.orte);
 502         robottype_publisher_camera_control_create(&robot.orte, send_dummy_cb, &robot.orte);
 503         robottype_publisher_crane_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
 504         robottype_publisher_magnet_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
 505
 506         /* create generic subscribers */
 507         robottype_subscriber_odo_data_create(&robot.orte, rcv_odo_data_cb, &robot.orte);
 508         robottype_subscriber_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte);
 509         robottype_subscriber_motion_speed_create(&robot.orte, rcv_motion_speed_cb, &robot.orte);
 510         robottype_subscriber_motion_status_create(&robot.orte, rcv_motion_status_cb, &robot.orte);
 511         robottype_subscriber_pwr_voltage_create(&robot.orte, rcv_pwr_voltage_cb, &robot.orte);
 512         //robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
 513         robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
 514         robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte);
 515         robottype_subscriber_crane_status_create(&robot.orte, rcv_crane_status_cb, &robot.orte);
 516         robottype_subscriber_robot_switches_create(&robot.orte, rcv_robot_switches_cb, &robot.orte);
 517         robottype_subscriber_robot_bumpers_create(&robot.orte, rcv_robot_bumpers_cb, &robot.orte);
 518         robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);
 519
 520         return rv;
 521 }
 522