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