2 #define _XOPEN_SOURCE 500
4 #include <movehelper.h>
10 #include <robot_orte.h>
15 #include "map_handling.h"
22 /* Subtract the `struct timespec' values X and Y,
23 storing the result in RESULT (result = x - y).
24 Return 1 if the difference is negative, otherwise 0. */
26 int timespec_subtract (timespec &result, const timespec &x, timespec &y) {
27 /* Perform the carry for the later subtraction by updating Y. */
28 if (x.tv_nsec < y.tv_nsec) {
29 int num_sec = (y.tv_nsec - x.tv_nsec) / 1000000000 + 1;
30 y.tv_nsec -= 1000000000 * num_sec;
33 if (x.tv_nsec - y.tv_nsec > 1000000000) {
34 int num_sec = (x.tv_nsec - y.tv_nsec) / 1000000000;
35 y.tv_nsec += 1000000000 * num_sec;
39 /* Compute the time remaining to wait.
40 `tv_nsec' is certainly positive. */
41 result.tv_sec = x.tv_sec - y.tv_sec;
42 result.tv_nsec = x.tv_nsec - y.tv_nsec;
44 /* Return 1 if result is negative. */
45 return x.tv_sec < y.tv_sec;
51 void Robot::int_handler(int sig) {
55 void Robot::block_signals() {
60 for (i=SIGRTMIN; i<=SIGRTMAX; i++)
61 sigaddset(&sigset, i);
63 pthread_sigmask(SIG_BLOCK, &sigset, (sigset_t*)NULL);
74 * Initializes the robot.
75 * Setup fields in robot structure, initializes FSMs and ORTE.
81 pthread_mutexattr_t mattr;
82 rv = pthread_mutexattr_init(&mattr);
83 #ifdef HAVE_PRIO_INHERIT
84 rv = pthread_mutexattr_setprotocol(&mattr, PTHREAD_PRIO_INHERIT);
86 pthread_mutex_init(&robot.lock, &mattr);
87 pthread_mutex_init(&robot.lock_ref_pos, &mattr);
88 pthread_mutex_init(&robot.lock_est_pos_odo, &mattr);
89 pthread_mutex_init(&robot.lock_est_pos_indep_odo, &mattr);
90 pthread_mutex_init(&robot.lock_meas_angles, &mattr);
91 pthread_mutex_init(&robot.lock_joy_data, &mattr);
92 pthread_mutex_init(&robot.lock_disp, &mattr);
93 MOTION = sched.create_processor<FSMMotion>();
97 if (team_color == VIOLET) {
98 printf("We are VIOLET!\n");
100 ul_loginf("We are RED!\n");
103 set_est_pos_trans(ROBOT_START_X_M, ROBOT_START_Y_M, DEG2RAD(ROBOT_START_ANGLE_DEG));
105 ignore_hokuyo = false;
108 // fill_in_known_areas_in_map();
110 signal(SIGINT, int_handler);
111 signal(SIGTERM, int_handler);
115 orte.motion_speed.left = 0;
116 orte.motion_speed.right = 0;
118 orte.pwr_ctrl.voltage33 = 1;
119 orte.pwr_ctrl.voltage50 = 1;
120 orte.pwr_ctrl.voltage80 = 1;
122 orte.camera_control.on = true;
125 /* Only activate display if it is configured */
127 robot.sercom = uoled_init(serial_comm);
128 if (strcmp(robot.sercom->devname, "/dev/null") != 0)
129 robot.fsm.display.state = &fsm_state_disp_init;
132 obstacle_avoidance_enabled = true;
133 use_back_bumpers = true;
134 use_left_bumper = true;
135 use_right_bumper = true;
136 start_state = POWER_ON;
137 check_turn_safety = true;
139 /* init ORTE domain, create publishers, subscribers, .. */
140 rv = robot_init_orte();
146 * Starts the robot FSMs and threads.
153 pthread_attr_t tattr;
154 struct sched_param param;
155 pthread_t thr_obstacle_forgeting;
158 ret = motion_control_init();
160 perror("motion_control_init");
165 /* Obstacle forgeting thread */
166 pthread_attr_init (&tattr);
167 pthread_attr_getschedparam (&tattr, ¶m);
168 pthread_attr_getschedparam (&tattr, ¶m);
169 pthread_attr_setschedpolicy(&tattr, SCHED_FIFO);
170 param.sched_priority = OBST_FORGETING_PRIO;
171 rv = pthread_attr_setschedparam (&tattr, ¶m);
173 perror("robot_start: pthread_attr_setschedparam()");
176 rv = pthread_create(&thr_obstacle_forgeting,
177 &tattr, thread_obstacle_forgeting, NULL);
179 perror("robot_start: pthread_create");
183 sched(); // start main loop
190 * Signals all the robot threads to finish.
192 void Robot::robot_exit() {
198 * Stops the robot. All resources alocated by robot_init() or
199 * robot_start() are dealocated here.
201 void Robot::destroy() {
202 motion_control_done();
204 robottype_roboorte_destroy(&orte);
211 void Robot::get_est_pos_trans(double &x, double &y, double &phi) {
212 get_est_pos(x, y, phi);
215 phi = trans_angle(phi);
218 void Robot::get_est_pos(double &x, double &y, double &phi) {
219 if (indep_odometry_works) {
220 Guard g(lock_est_pos_indep_odo);
221 x = est_pos_indep_odo.x;
222 y = est_pos_indep_odo.y;
223 phi = est_pos_indep_odo.phi;
224 } else if (odometry_works) {
225 Guard g(lock_est_pos_odo);
228 phi = est_pos_odo.phi;
230 Guard g(lock_ref_pos);
237 /** Sets actual position of the robot and with respoect to color of
238 * the team. Should be used for setting initial position of the
240 void Robot::set_est_pos(double x, double y, double phi) {
242 if (x>PLAYGROUND_WIDTH_M) x=PLAYGROUND_WIDTH_M;
244 if (y>PLAYGROUND_HEIGHT_M) y=PLAYGROUND_HEIGHT_M;
247 Guard g(lock_est_pos_indep_odo);
248 est_pos_indep_odo.x = x;
249 est_pos_indep_odo.y = y;
250 est_pos_indep_odo.phi = phi;
254 Guard g(lock_est_pos_odo);
257 est_pos_odo.phi = phi;
261 Guard g(lock_ref_pos);
268 float Robot::current_time() {
269 timespec now, diff, start_local;
270 start_local = startTime;
271 clock_gettime(CLOCK_MONOTONIC, &now);
272 timespec_subtract(diff, now, start_local);
273 return diff.tv_sec + diff.tv_nsec/1000000000.0;
276 void Robot::set_state_name(const char* name)
278 sched.get_actual_handle()->state_name = name;
282 void Robot::trans_callback()
284 if (MAIN == sched.get_actual_handle()) {
285 strncpy(robot.orte.fsm_main.state_name, MAIN->state_name, sizeof(robot.orte.fsm_main.state_name));
286 ORTEPublicationSend(robot.orte.publication_fsm_main);
287 } else if (MOTION == sched.get_actual_handle()) {
288 strncpy(robot.orte.fsm_motion.state_name, MOTION->state_name, sizeof(robot.orte.fsm_motion.state_name));
289 ORTEPublicationSend(robot.orte.publication_fsm_motion);