5 * @brief Module providing communication through ORTE
9 * robot_orte.c 08/04/21
11 * Robot's orte stuffs.
13 * Copyright: (c) 2008-2010 CTU Dragons
14 * CTU FEE - Department of Control Engineering
15 * License: GNU GPL v.2
19 #include <roboorte_robottype.h>
22 #include <movehelper.h>
25 #include "map_handling.h"
26 #include "match-timing.h"
28 #include <can_msg_def.h>
29 #include <actuators.h>
32 UL_LOG_CUST(ulogd_robot_orte); /* Log domain name = ulogd + name of the file */
35 #define DBG(format, ...) ul_logdeb(format, ##__VA_ARGS__)
37 #define DBG(format, ...)
41 extern sem_t measurement_received;
43 /* ----------------------------------------------------------------------
44 * PUBLISHER CALLBACKS - GENERIC
45 * ---------------------------------------------------------------------- */
47 void send_ref_pos_cb(const ORTESendInfo *info, void *vinstance,
48 void *sendCallBackParam)
50 struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
53 *instance = robot.ref_pos;
54 ROBOT_UNLOCK(ref_pos);
57 void send_est_pos_odo_cb(const ORTESendInfo *info, void *vinstance,
58 void *sendCallBackParam)
60 struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
62 ROBOT_LOCK(est_pos_odo);
63 *instance = robot.est_pos_odo;
64 ROBOT_UNLOCK(est_pos_odo);
67 void send_est_pos_indep_odo_cb(const ORTESendInfo *info, void *vinstance,
68 void *sendCallBackParam)
70 struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
72 ROBOT_LOCK(est_pos_indep_odo);
73 *instance = robot.est_pos_indep_odo;
74 ROBOT_UNLOCK(est_pos_indep_odo);
77 static void send_est_pos_best_cb(const ORTESendInfo *info, void *vinstance,
78 void *sendCallBackParam)
80 struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
82 robot_get_est_pos(&instance->x, &instance->y, &instance->phi);
85 void send_dummy_cb(const ORTESendInfo *info, void *vinstance,
86 void *sendCallBackParam)
90 void send_match_time_cb(const ORTESendInfo *info, void *vinstance,
91 void *sendCallBackParam)
93 struct match_time_type *instance = (struct match_time_type *)vinstance;
95 if (robot.start_state == POWER_ON || robot.start_state == START_PLUGGED_IN) {
98 instance->time = 600 - robot_current_time();
101 /* ----------------------------------------------------------------------
102 * SUBSCRIBER CALLBACKS - GENERIC
103 * ---------------------------------------------------------------------- */
104 void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
105 void *recvCallBackParam)
107 struct odo_data_type *instance = (struct odo_data_type *)vinstance;
108 double dleft, dright, dtang, dphi;
109 static bool first_run = true;
110 /* spocitat prevodovy pomer */
111 const double n = (double)(1.0 / 1.0);
113 /* vzdalenost na pulz IRC */
114 const double c = (M_PI*2*ODOMETRY_WHEEL_RADIUS_M) / (n * 4*4096.0);
116 switch (info->status) {
119 ROBOT_LOCK(odo_data);
120 robot.odo_data = *instance;
121 ROBOT_UNLOCK(odo_data);
126 dleft = ((robot.odo_data.left - instance->left) >> 8) * c * robot.odo_cal_a; // TODO >> 8 ?
127 dright = ((instance->right - robot.odo_data.right) >> 8) * c * robot.odo_cal_b;
129 dtang = (dleft + dright) / 2.0;
130 dphi = (dright - dleft) / (2.0*ODOMETRY_ROTATION_RADIUS_M);
132 ROBOT_LOCK(est_pos_indep_odo);
133 robot.odo_distance_a +=dleft;
134 robot.odo_distance_b +=dright;
135 double a = robot.est_pos_indep_odo.phi;
136 robot.est_pos_indep_odo.x += dtang*cos(a);
137 robot.est_pos_indep_odo.y += dtang*sin(a);
138 robot.est_pos_indep_odo.phi += dphi;
139 ROBOT_UNLOCK(est_pos_indep_odo);
141 ROBOT_LOCK(odo_data);
142 robot.odo_data = *instance;
143 ROBOT_UNLOCK(odo_data);
145 robot.indep_odometry_works = true;
147 /* wake up motion-control/thread_trajectory_follower */
148 sem_post(&measurement_received);
150 //robot.hw_status[COMPONENT_ODO] = STATUS_OK;
153 robot.indep_odometry_works = false;
154 //robot.hw_status[COMPONENT_ODO] = STATUS_FAILED;
155 DBG("ORTE deadline occurred - odo_data receive\n");
160 void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
161 void *recvCallBackParam)
163 struct motion_irc_type *instance = (struct motion_irc_type *)vinstance;
164 double dleft, dright, dtang, dphi;
165 static bool first_run = true;
166 /* spocitat prevodovy pomer */
167 const double n = (double)(ROBOT_MOTOR_GEARBOX_RATIO / 1.0);
169 /* vzdalenost na pulz IRC */
170 const double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*ROBOT_MOTOR_IRC_RESOLUTION);
171 switch (info->status) {
174 ROBOT_LOCK(motion_irc);
175 robot.motion_irc = *instance;
176 ROBOT_UNLOCK(motion_irc);
182 /* FIXME maybe it is not correct to do this nasty hack here (switch dleft and dright),
183 what is the right solution?
184 This was neccessary to view motor odometry correctly in robomon. */
185 dright = ((robot.motion_irc.left - instance->left) >> 8) * c;
186 dleft = ((instance->right - robot.motion_irc.right) >> 8) * c;
188 dtang = (dleft + dright) / 2.0;
189 dphi = (dright - dleft) / (2.0*ROBOT_ROTATION_RADIUS_M);
191 ROBOT_LOCK(est_pos_odo);
192 double a = robot.est_pos_odo.phi;
193 robot.est_pos_odo.x += dtang*cos(a);
194 robot.est_pos_odo.y += dtang*sin(a);
195 robot.est_pos_odo.phi += dphi;
196 ROBOT_UNLOCK(est_pos_odo);
198 /* locking should not be needed, but... */
199 ROBOT_LOCK(motion_irc);
200 robot.motion_irc = *instance;
201 robot.motion_irc_received = 1;
202 ROBOT_UNLOCK(motion_irc);
204 robot.odometry_works = true;
206 robot.status[COMPONENT_MOTOR] = STATUS_OK;
209 robot.odometry_works = false;
210 robot.status[COMPONENT_MOTOR] = STATUS_FAILED;
211 DBG("ORTE deadline occurred - motion_irc receive\n");
216 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
217 void *recvCallBackParam)
219 switch (info->status) {
223 DBG("ORTE deadline occurred - motion_speed receive\n");
228 void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
229 void *recvCallBackParam)
231 switch (info->status) {
235 DBG("ORTE deadline occurred - motion_status receive\n");
240 void rcv_pwr_voltage_cb(const ORTERecvInfo *info, void *vinstance,
241 void *recvCallBackParam)
243 switch (info->status) {
245 robot.status[COMPONENT_POWER]=STATUS_OK;
248 robot.status[COMPONENT_POWER]=STATUS_FAILED;
249 DBG("ORTE deadline occurred - pwr_voltage receive\n");
254 void rcv_pwr_ctrl_cb(const ORTERecvInfo *info, void *vinstance,
255 void *recvCallBackParam)
257 switch (info->status) {
261 DBG("ORTE deadline occurred - pwr_ctrl receive\n");
266 void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
267 void *recvCallBackParam)
269 struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
270 static struct robot_cmd_type last_instance;
272 switch (info->status) {
274 /* Stupid way of controlling the robot by
275 * pluggin in and out start connector. */
276 switch (robot.start_state) {
278 if (!instance->start_condition) {
279 robot.status[COMPONENT_START] = STATUS_WARNING;
280 robot.start_state = START_PLUGGED_IN;
281 ul_logmsg("START_PLUGGED_IN\n");
285 case START_PLUGGED_IN:
286 robot.status[COMPONENT_START] = STATUS_OK;
287 /* Competition starts when plugged out */
288 if (instance->start_condition) {
289 FSM_SIGNAL(MAIN, EV_START, NULL);
290 robot.start_state = COMPETITION_STARTED;
291 ul_logmsg("STARTED\n");
295 case COMPETITION_STARTED: {
296 /* Subsequent plug-in stops the robot */
298 if (!instance->start_condition) {
299 robot.status[COMPONENT_START] = STATUS_WARNING;
306 last_instance = *instance;
309 robot.status[COMPONENT_START] = STATUS_FAILED;
310 DBG("ORTE deadline occurred - robot_cmd receive\n");
315 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
316 void *recvCallBackParam)
318 struct lidar_scan_type *instance = (struct lidar_scan_type *)vinstance;
320 switch (info->status) {
323 robot.hokuyo = *instance;
324 robot.status[COMPONENT_HOKUYO] = STATUS_OK;
325 ROBOT_UNLOCK(hokuyo);
326 if(!robot.ignore_hokuyo)
328 //update_map_hokuyo(instance);
333 robot.status[COMPONENT_HOKUYO] = STATUS_FAILED;
334 //system("killall -9 hokuyo");
335 DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
340 void rcv_sick_scan_cb(const ORTERecvInfo *info, void *vinstance,
341 void *recvCallBackParam)
343 struct lidar_scan_type *instance = (struct lidar_scan_type *)vinstance;
345 switch (info->status) {
348 robot.sick = *instance;
349 robot.status[COMPONENT_SICK] = STATUS_OK;
351 if(!robot.ignore_sick)
353 update_map_hokuyo(instance);
358 robot.status[COMPONENT_SICK] = STATUS_FAILED;
359 //system("killall -9 hokuyo");
360 DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
365 void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
366 void *recvCallBackParam)
368 struct camera_result_type *instance = (struct camera_result_type *)vinstance;
369 static bool last_response = false;
371 switch (info->status) {
373 if (instance->data_valid && instance->data_valid != last_response) {
374 ROBOT_LOCK(camera_result);
375 robot.target_valid = instance->target_valid;
376 robot.target_ang = instance->angle_deg;
377 ROBOT_UNLOCK(camera_result);
378 FSM_SIGNAL(MAIN, EV_CAMERA_DONE, NULL);
380 last_response = instance->data_valid;
384 if (robot.orte.camera_control.on) {
385 robot.status[COMPONENT_CAMERA] = STATUS_FAILED;
388 DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
392 /* ----------------------------------------------------------------------
393 * SUBSCRIBER CALLBACKS - EB2008
394 * ---------------------------------------------------------------------- */
397 void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance,
398 void *recvCallBackParam)
400 struct robot_switches_type *instance = (struct robot_switches_type *)vinstance;
401 static bool last_strategy;
402 switch (info->status) {
404 robot.team_color = instance->team_color;
406 if (!last_strategy && instance->strategy) {
407 /* strategy switching */
408 FSM_SIGNAL(MAIN, EV_SWITCH_STRATEGY, NULL);
410 last_strategy = instance->strategy;
417 void rcv_robot_bumpers_cb(const ORTERecvInfo *info, void *vinstance,
418 void *recvCallBackParam)
420 struct robot_bumpers_type *instance = (struct robot_bumpers_type *)vinstance;
421 static bool last_left, last_right;
422 switch (info->status) {
424 if (instance->bumper_right_across || instance->bumper_left_across || instance->bumper_rear_left || instance->bumper_rear_right)
425 FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
427 if (instance->bumper_left || instance->bumper_right) {
428 FSM_SIGNAL(MOTION, EV_OBSTACLE_SIDE, NULL);
436 void rcv_cl_sensor_status_cb(const ORTERecvInfo *info, void *vinstance,
437 void *recvCallBackParam)
439 struct cl_sensor_status_type *instance = (struct cl_sensor_status_type *)vinstance;
440 static bool last_pattern_match = 0;
441 switch (info->status) {
443 if ((last_pattern_match != instance->pattern_match) && instance->pattern_match)
444 FSM_SIGNAL(MAIN, EV_OMRON_DONE, NULL);
446 last_pattern_match = instance->pattern_match;
455 static int cmp_double(const void *v1, const void *v2)
457 const double *d1 = v1, *const d2 = v2;
467 int robot_init_orte()
471 robot.orte.strength = 20;
473 rv = robottype_roboorte_init(&robot.orte);
477 /* creating publishers */
478 robottype_publisher_motion_speed_create(&robot.orte, NULL, &robot.orte);
479 robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte);
480 robottype_publisher_est_pos_odo_create(&robot.orte, send_est_pos_odo_cb, &robot.orte);
481 robottype_publisher_est_pos_indep_odo_create(&robot.orte, send_est_pos_indep_odo_cb, &robot.orte);
482 robottype_publisher_est_pos_best_create(&robot.orte, send_est_pos_best_cb, &robot.orte);
483 robottype_publisher_match_time_create(&robot.orte, send_match_time_cb, &robot.orte);
484 //???robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
486 // I didn't know what was the difference between the callback function pointer
487 // being NULL and being set to pointer to empty send_dummy_cb function. The DIFFERENCE
489 // - NULL: message is published only when OrtePublicationSend called
490 // - pointer to empty function: message is published periodically
491 robottype_publisher_fsm_main_create(&robot.orte, send_dummy_cb, &robot.orte);
492 robottype_publisher_fsm_act_create(&robot.orte, send_dummy_cb, &robot.orte);
493 robottype_publisher_fsm_motion_create(&robot.orte, send_dummy_cb, &robot.orte);
494 robottype_publisher_camera_control_create(&robot.orte, send_dummy_cb, &robot.orte);
495 /* create generic subscribers */
496 robottype_subscriber_odo_data_create(&robot.orte, rcv_odo_data_cb, &robot.orte);
497 robottype_subscriber_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte);
498 robottype_subscriber_motion_speed_create(&robot.orte, rcv_motion_speed_cb, &robot.orte);
499 robottype_subscriber_motion_status_create(&robot.orte, rcv_motion_status_cb, &robot.orte);
500 robottype_subscriber_pwr_voltage_create(&robot.orte, rcv_pwr_voltage_cb, &robot.orte);
501 //robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
502 robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
503 robottype_subscriber_sick_scan_create(&robot.orte, rcv_sick_scan_cb, &robot.orte);
504 robottype_subscriber_robot_switches_create(&robot.orte, rcv_robot_switches_cb, &robot.orte);
505 robottype_subscriber_robot_bumpers_create(&robot.orte, rcv_robot_bumpers_cb, &robot.orte);
506 robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);
507 robottype_subscriber_cl_sensor_status_create(&robot.orte, rcv_cl_sensor_status_cb, &robot.orte);