src/robofsm/robot.h

   1 #ifndef ROBOT_H
   2 #define ROBOT_H
   3 #include <stdint.h>
   4 #include <stdio.h>
   5 #include <trgenconstr.h>
   6 #include <robottype.h>
   7 #include <robottype.h>
   8 #include <roboorte_robottype.h>
   9 #include <robodim.h>
  10 #include <robot_config.h>
  11 #include <actuators.h>
  12 #include <semaphore.h>
  13 #include "movehelper.h"
  14 #include "scheduler.hpp"
  15 //#include <ul_log.h>
  16 #include <boost/statechart/asynchronous_state_machine.hpp>
  17
  18 //UL_LOG_CUST(ulogd_robot_h); /* Log domain name = ulogd + name of the file */
  19 #include <guard.hpp>
  20 /**
  21  * Competition parameters
  22  */
  23 enum team_colour {
  24         VIOLET = 0,             /* Coordinate transformation does not apply */
  25         RED                     /* Coordinate transformation applies (in *_trans() functions) */
  26 };
  27
  28 enum robot_start_state {
  29         POWER_ON = 0,
  30         START_PLUGGED_IN,
  31         COMPETITION_STARTED,
  32 };
  33
  34 enum robot_status {
  35         STATUS_OK,
  36         STATUS_WARNING,
  37         STATUS_FAILED,
  38 };
  39
  40 enum robot_component {
  41         COMPONENT_MOTOR,
  42         COMPONENT_ODOMETRY,
  43         COMPONENT_CAMERA,
  44         COMPONENT_POWER,
  45         COMPONENT_HOKUYO,
  46         COMPONENT_START,
  47         COMPONENT_JAWS,
  48         COMPONENT_LIFT,
  49
  50         ROBOT_COMPONENT_NUMBER
  51 };
  52
  53 class Robot {
  54         static void int_handler(int sig);
  55         void block_signals();
  56         //static void trans_callback(struct robo_fsm *fsm) publication stringnames of states
  57     public:
  58         pthread_mutex_t lock;
  59         pthread_mutex_t lock_ref_pos;
  60         pthread_mutex_t lock_est_pos_odo;
  61         pthread_mutex_t lock_est_pos_indep_odo;
  62         pthread_mutex_t lock_meas_angles;
  63         pthread_mutex_t lock_joy_data;
  64         pthread_mutex_t lock_disp;
  65         pthread_mutex_t lock_camera;
  66         /* competition parameters */
  67         team_colour team_color;
  68
  69         /** State variable used for controlling the robot by pluggin
  70          * in and out start connector. */
  71         robot_start_state start_state;
  72
  73         /* actual position */
  74         robot_pos_type ref_pos;
  75         /* estimated position */
  76         robot_pos_type est_pos_odo;
  77         robot_pos_type est_pos_indep_odo;
  78         /** True if est_pos_odo is updated according to reception of motion_irc */
  79         bool odometry_works;
  80         /** True if est_pos_indep_odo is updated according to reception of motion_indep_odo */
  81         bool indep_odometry_works;
  82
  83         bool use_back_bumpers;
  84         bool use_left_bumper;
  85         bool use_right_bumper;
  86
  87         /** True iff at least one wheel rotates backward */
  88         bool moves_backward;
  89
  90         /*
  91          * sometimes H8S does not send queried odometry
  92          * following flag has been added for EKF estimator,
  93          * since is has been hardly disturbed by missing measurement
  94          * (taken as sudden zero velocities)
  95          */
  96         bool motion_irc_received;
  97
  98         /* orte */
  99         struct robottype_orte_data orte;
 100
 101         /* sensors */
 102         struct motion_irc_type motion_irc;      /* motor odometry */
 103         struct odo_data_type odo_data;          /* independent odometry */
 104         struct corr_distances_type corr_distances;      /* ultrasound */
 105
 106         hokuyo_scan_type hokuyo;
 107         bool ignore_hokuyo;
 108
 109         struct map *map;        /* Map for pathplanning (no locking) */
 110
 111         robot_status status[ROBOT_COMPONENT_NUMBER];
 112
 113         char corns_conf_center;
 114         char corns_conf_side;
 115         struct corns_group *corns;
 116
 117         bool obstacle_avoidance_enabled;
 118
 119         /** is set to true in separate thread when there is short time left */
 120         bool short_time_to_end;
 121         bool check_turn_safety;
 122
 123         float odo_cal_b;
 124         float odo_cal_a;
 125         float odo_distance_a;
 126         float odo_distance_b;
 127
 128         timespec startTime;
 129
 130         MoveHelper move_helper;
 131         Scheduler sched;
 132         /* State machines */
 133         Scheduler::processor_handle MAIN;
 134         Scheduler::processor_handle MOTION;
 135         /* Class methods */
 136         Robot();
 137         ~Robot();
 138         int init() __attribute__ ((warn_unused_result));
 139         int start() __attribute__ ((warn_unused_result));
 140         void robot_exit();
 141         void destroy();
 142         void set_est_pos_trans(double x, double y, double phi){set_est_pos(trans_x(x), trans_y(y), trans_angle(phi));}
 143         void set_est_pos(double x, double y, double phi);
 144         void get_est_pos_trans(double &x, double &y, double &phi);
 145         void get_est_pos(double &x, double &y, double &phi);
 146         float current_time();
 147         void set_state_name(const char* name);
 148 };
 149
 150 const unsigned THREAD_PRIO_TRAJ_FOLLOWER = 90;  /* As high as possible */
 151 const unsigned THREAD_PRIO_TRAJ_RECLAC = 18;
 152 const unsigned OBST_FORGETING_PRIO = 17;
 153
 154 extern Robot robot;
 155
 156
 157
 158
 159 #endif