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"
27 #include <can_msg_def.h>
28 #include <actuators.h>
31 #define DBG(format, ...) printf(format, ##__VA_ARGS__)
33 #define DBG(format, ...)
37 extern sem_t measurement_received;
39 /* ----------------------------------------------------------------------
40 * PUBLISHER CALLBACKS - GENERIC
41 * ---------------------------------------------------------------------- */
43 void send_ref_pos_cb(const ORTESendInfo *info, void *vinstance,
44 void *sendCallBackParam)
46 struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
49 *instance = robot.ref_pos;
50 ROBOT_UNLOCK(ref_pos);
53 void send_est_pos_odo_cb(const ORTESendInfo *info, void *vinstance,
54 void *sendCallBackParam)
56 struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
58 ROBOT_LOCK(est_pos_odo);
59 *instance = robot.est_pos_odo;
60 ROBOT_UNLOCK(est_pos_odo);
63 void send_est_pos_indep_odo_cb(const ORTESendInfo *info, void *vinstance,
64 void *sendCallBackParam)
66 struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
68 ROBOT_LOCK(est_pos_indep_odo);
69 *instance = robot.est_pos_indep_odo;
70 ROBOT_UNLOCK(est_pos_indep_odo);
73 static void send_est_pos_best_cb(const ORTESendInfo *info, void *vinstance,
74 void *sendCallBackParam)
76 struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
78 robot_get_est_pos(&instance->x, &instance->y, &instance->phi);
81 void send_dummy_cb(const ORTESendInfo *info, void *vinstance,
82 void *sendCallBackParam)
86 /* ----------------------------------------------------------------------
87 * SUBSCRIBER CALLBACKS - GENERIC
88 * ---------------------------------------------------------------------- */
89 void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
90 void *recvCallBackParam)
92 struct odo_data_type *instance = (struct odo_data_type *)vinstance;
93 double dleft, dright, dtang, dphi;
94 static bool first_run = true;
95 /* spocitat prevodovy pomer */
96 const double n = (double)(1.0 / 1.0);
98 /* vzdalenost na pulz IRC */
99 const double c = (M_PI*2*ODOMETRY_WHEEL_RADIUS_M) / (n * 4*4096.0);
101 switch (info->status) {
104 ROBOT_LOCK(odo_data);
105 robot.odo_data = *instance;
106 ROBOT_UNLOCK(odo_data);
111 dleft = ((robot.odo_data.left - instance->left) >> 8) * c; // TODO >> 8 ?
112 dright = ((instance->right - robot.odo_data.right) >> 8) * c;
114 dtang = (dleft + dright) / 2.0;
115 dphi = (dright - dleft) / (2.0*ODOMETRY_ROTATION_RADIUS_M);
117 ROBOT_LOCK(est_pos_indep_odo);
118 double a = robot.est_pos_indep_odo.phi;
119 robot.est_pos_indep_odo.x += dtang*cos(a);
120 robot.est_pos_indep_odo.y += dtang*sin(a);
121 robot.est_pos_indep_odo.phi += dphi;
122 ROBOT_UNLOCK(est_pos_indep_odo);
124 ROBOT_LOCK(odo_data);
125 robot.odo_data = *instance;
126 ROBOT_UNLOCK(odo_data);
128 robot.indep_odometry_works = true;
130 /* wake up motion-control/thread_trajectory_follower */
131 sem_post(&measurement_received);
133 //robot.hw_status[COMPONENT_ODO] = STATUS_OK;
136 robot.indep_odometry_works = false;
137 //robot.hw_status[COMPONENT_ODO] = STATUS_FAILED;
138 DBG("ORTE deadline occurred - odo_data receive\n");
143 void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
144 void *recvCallBackParam)
146 struct motion_irc_type *instance = (struct motion_irc_type *)vinstance;
147 double dleft, dright, dtang, dphi;
148 static bool first_run = true;
149 /* spocitat prevodovy pomer */
150 const double n = (double)(28.0 / 1.0);
152 /* vzdalenost na pulz IRC */
153 const double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*500.0);
155 switch (info->status) {
158 ROBOT_LOCK(motion_irc);
159 robot.motion_irc = *instance;
160 ROBOT_UNLOCK(motion_irc);
165 dleft = ((robot.motion_irc.left - instance->left) >> 8) * c;
166 dright = ((instance->right - robot.motion_irc.right) >> 8) * c;
168 dtang = (dleft + dright) / 2.0;
169 dphi = (dright - dleft) / (2.0*ROBOT_ROTATION_RADIUS_M);
171 ROBOT_LOCK(est_pos_odo);
172 double a = robot.est_pos_odo.phi;
173 robot.est_pos_odo.x += dtang*cos(a);
174 robot.est_pos_odo.y += dtang*sin(a);
175 robot.est_pos_odo.phi += dphi;
176 ROBOT_UNLOCK(est_pos_odo);
178 /* locking should not be needed, but... */
179 ROBOT_LOCK(motion_irc);
180 robot.motion_irc = *instance;
181 robot.motion_irc_received = 1;
182 ROBOT_UNLOCK(motion_irc);
184 robot.odometry_works = true;
186 robot.status[COMPONENT_MOTOR] = STATUS_OK;
189 robot.odometry_works = false;
190 robot.status[COMPONENT_MOTOR] = STATUS_FAILED;
191 DBG("ORTE deadline occurred - motion_irc receive\n");
196 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
197 void *recvCallBackParam)
199 switch (info->status) {
203 DBG("ORTE deadline occurred - motion_speed receive\n");
208 void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
209 void *recvCallBackParam)
211 switch (info->status) {
215 DBG("ORTE deadline occurred - motion_status receive\n");
220 void rcv_pwr_voltage_cb(const ORTERecvInfo *info, void *vinstance,
221 void *recvCallBackParam)
223 switch (info->status) {
225 robot.status[COMPONENT_POWER]=STATUS_OK;
228 robot.status[COMPONENT_POWER]=STATUS_FAILED;
229 DBG("ORTE deadline occurred - pwr_voltage receive\n");
234 void rcv_pwr_ctrl_cb(const ORTERecvInfo *info, void *vinstance,
235 void *recvCallBackParam)
237 switch (info->status) {
241 DBG("ORTE deadline occurred - pwr_ctrl receive\n");
246 void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
247 void *recvCallBackParam)
249 struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
250 static struct robot_cmd_type last_instance;
252 switch (info->status) {
254 /* Stupid way of controlling the robot by
255 * pluggin in and out start connector. */
256 switch (robot.start_state) {
258 if (!instance->start_condition) {
259 robot.status[COMPONENT_START] = STATUS_WARNING;
260 robot.start_state = START_PLUGGED_IN;
261 printf("START_PLUGGED_IN\n");
265 case START_PLUGGED_IN:
266 robot.status[COMPONENT_START] = STATUS_OK;
267 /* Competition starts when plugged out */
268 if (instance->start_condition) {
269 FSM_SIGNAL(MAIN, EV_START, NULL);
270 robot.start_state = COMPETITION_STARTED;
275 case COMPETITION_STARTED: {
276 /* Subsequent plug-in stops the robot */
278 if (!instance->start_condition) {
279 robot.status[COMPONENT_START] = STATUS_WARNING;
286 last_instance = *instance;
289 robot.status[COMPONENT_START] = STATUS_FAILED;
290 DBG("ORTE deadline occurred - robot_cmd receive\n");
295 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
296 void *recvCallBackParam)
298 struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
300 switch (info->status) {
303 robot.hokuyo = *instance;
304 robot.status[COMPONENT_HOKUYO] = STATUS_OK;
305 ROBOT_UNLOCK(hokuyo);
306 if(!robot.ignore_hokuyo)
308 update_map_hokuyo(instance);
313 robot.status[COMPONENT_HOKUYO] = STATUS_FAILED;
314 //system("killall -9 hokuyo");
315 DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
320 // FIXME: this is not up to date (Filip, 2010)
321 void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
322 void *recvCallBackParam)
324 struct camera_result_type *instance = (struct camera_result_type *)vinstance;
326 switch (info->status) {
328 ROBOT_LOCK(camera_result);
329 robot.corns_conf_side = instance->side;
330 robot.corns_conf_center = instance->center;
331 ROBOT_UNLOCK(camera_result);
332 robot.status[COMPONENT_CAMERA] = STATUS_OK;
336 if (robot.orte.camera_control.on) {
337 robot.status[COMPONENT_CAMERA] = STATUS_FAILED;
338 //system("killall -9 rozpuk");
341 DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
345 /* ----------------------------------------------------------------------
346 * SUBSCRIBER CALLBACKS - EB2008
347 * ---------------------------------------------------------------------- */
349 void rcv_vidle_status_cb(const ORTERecvInfo *info, void *vinstance,
350 void *recvCallBackParam)
352 struct vidle_status_type *instance = (struct vidle_status_type *)vinstance;
353 switch (info->status) {
355 // new data arrived and requested position equals actual position
356 if (instance->flags == 0)
357 robot.status[COMPONENT_VIDLE]=STATUS_OK;
359 robot.status[COMPONENT_VIDLE]=STATUS_WARNING;
361 if (instance->response == act_vidle_get_last_reqest() ||
362 instance->flags & CAN_VIDLE_TIMEOUT) {
363 //FSM_SIGNAL(MAIN, EV_VIDLE_DONE, NULL);
364 /* TODO: Uncomment the above and fix */
368 robot.status[COMPONENT_VIDLE]=STATUS_FAILED;
369 DBG("ORTE deadline occurred - actuator_status receive\n");
374 void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance,
375 void *recvCallBackParam)
377 struct robot_switches_type *instance = (struct robot_switches_type *)vinstance;
378 switch (info->status) {
380 robot.team_color = instance->team_color;
381 if (instance->bumper_pressed)
382 FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
391 static int cmp_double(const void *v1, const void *v2)
393 const double *d1 = v1, *const d2 = v2;
403 int robot_init_orte()
407 robot.orte.strength = 20;
409 rv = robottype_roboorte_init(&robot.orte);
413 /* creating publishers */
414 robottype_publisher_motion_speed_create(&robot.orte, NULL, &robot.orte);
415 robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte);
416 robottype_publisher_est_pos_odo_create(&robot.orte, send_est_pos_odo_cb, &robot.orte);
417 robottype_publisher_est_pos_indep_odo_create(&robot.orte, send_est_pos_indep_odo_cb, &robot.orte);
418 robottype_publisher_est_pos_best_create(&robot.orte, send_est_pos_best_cb, &robot.orte);
419 //???robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
421 // I didn't know what was the difference between the callback function pointer
422 // being NULL and being set to pointer to empty send_dummy_cb function. The DIFFERENCE
424 // - NULL: message is published only when OrtePublicationSend called
425 // - pointer to empty function: message is published periodically
426 robottype_publisher_fsm_main_create(&robot.orte, send_dummy_cb, &robot.orte);
427 robottype_publisher_fsm_act_create(&robot.orte, send_dummy_cb, &robot.orte);
428 robottype_publisher_fsm_motion_create(&robot.orte, send_dummy_cb, &robot.orte);
429 robottype_publisher_camera_control_create(&robot.orte, send_dummy_cb, &robot.orte);
430 robottype_publisher_vidle_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
432 /* create generic subscribers */
433 robottype_subscriber_odo_data_create(&robot.orte, rcv_odo_data_cb, &robot.orte);
434 robottype_subscriber_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte);
435 robottype_subscriber_motion_speed_create(&robot.orte, rcv_motion_speed_cb, &robot.orte);
436 robottype_subscriber_motion_status_create(&robot.orte, rcv_motion_status_cb, &robot.orte);
437 robottype_subscriber_pwr_voltage_create(&robot.orte, rcv_pwr_voltage_cb, &robot.orte);
438 //robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
439 robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
440 robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte);
441 robottype_subscriber_vidle_status_create(&robot.orte, rcv_vidle_status_cb, &robot.orte);
442 robottype_subscriber_robot_switches_create(&robot.orte, rcv_robot_switches_cb, &robot.orte);
443 robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);