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 1de5758092398afce071944d0c9a96ac0bbf7a98..f64788267370434eee895e2c272ead5a35640587 100644 (file)
--- 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 e472aad9510b9f4428f57df5ef6be8c4abfedc79..a6d486ed028fc2a6bfa55f9466276c2f5752db03 100644 (file)
--- 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