From: Michal Sojka Date: Sat, 24 Apr 2010 20:19:44 +0000 (+0200) Subject: robofsm: Publish the best available estimated position X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/927287878f2981fafdaecdea989b3c2b05eabe8b robofsm: Publish the best available estimated position Robot's motion control algorithms needs estimated position of the robot. Depending on whether various peripherals work or not, this position is either the position calculated from independent odometry or calculated from motors or the reference position. For robomon, it is useful to know, which estimated position is currently being used. Therefore, we publish the position under topic est_pos_best. --- diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c index 1de57580..f6478826 100644 --- a/src/robofsm/robot_orte.c +++ b/src/robofsm/robot_orte.c @@ -69,6 +69,14 @@ void send_est_pos_indep_odo_cb(const ORTESendInfo *info, void *vinstance, ROBOT_UNLOCK(est_pos_indep_odo); } +static void send_est_pos_best_cb(const ORTESendInfo *info, void *vinstance, + void *sendCallBackParam) +{ + struct robot_pos_type *instance = (struct robot_pos_type *)vinstance; + + robot_get_est_pos(&instance->x, &instance->y, &instance->phi); +} + void send_dummy_cb(const ORTESendInfo *info, void *vinstance, void *sendCallBackParam) { @@ -382,6 +390,7 @@ int robot_init_orte() robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte); robottype_publisher_est_pos_odo_create(&robot.orte, send_est_pos_odo_cb, &robot.orte); robottype_publisher_est_pos_indep_odo_create(&robot.orte, send_est_pos_indep_odo_cb, &robot.orte); + robottype_publisher_est_pos_best_create(&robot.orte, send_est_pos_best_cb, &robot.orte); //???robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte); // I didn't know what was the difference between the callback function pointer diff --git a/src/types/robottype.ortegen b/src/types/robottype.ortegen index e472aad9..a6d486ed 100644 --- a/src/types/robottype.ortegen +++ b/src/types/robottype.ortegen @@ -5,6 +5,7 @@ type=binary_data topic=binary_data type=can_msg topic=can_msg type=robot_pos topic=est_pos_odo type=robot_pos topic=est_pos_indep_odo +type=robot_pos topic=est_pos_best type=hokuyo_pitch topic=hokuyo_pitch type=hokuyo_scan topic=hokuyo_scan deadline=1 type=odo_data topic=odo_data deadline=0.3