robofsm: Add odometry data to ORTE

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index c736bd8b6ed20c460ec31d2a57bcda506bbd6100..c06a0782438d236848fbd1f83a7229f576d4b5e0 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -79,6 +79,64 @@ void send_dummy_cb(const ORTESendInfo *info, void *vinstance,
  * SUBSCRIBER CALLBACKS - GENERIC
  * ---------------------------------------------------------------------- */
 
+void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
+                       void *recvCallBackParam)
+{
+#if 0
+       struct motion_irc_type *instance = (struct motion_irc_type *)vinstance;
+       double dleft, dright, dtang, dphi;
+       static bool first_run = true;
+       /* spocitat prevodovy pomer */
+       const double n = (double)(28.0 / 1.0); 
+
+       /* vzdalenost na pulz IRC */
+       const double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*500.0);
+
+       /* TODO: Michal - dodelat */
+       switch (info->status) {
+               case NEW_DATA:
+                       if (first_run) {
+                               ROBOT_LOCK(motion_irc);
+                               robot.motion_irc = *instance;
+                               ROBOT_UNLOCK(motion_irc);
+                               first_run = false;
+                               break;
+                       }
+                       
+                       dleft = ((robot.motion_irc.left - instance->left) >> 8) * c;
+                       dright = ((instance->right - robot.motion_irc.right) >> 8) * c;
+
+                       dtang = (dleft + dright) / 2.0;
+                       dphi = (dright - dleft) / (2.0*ROBOT_ROTATION_RADIUS_M);
+
+                       ROBOT_LOCK(est_pos_odo);
+                       double a = robot.est_pos_odo.phi;
+                       robot.est_pos_odo.x += dtang*cos(a);
+                       robot.est_pos_odo.y += dtang*sin(a);
+                       robot.est_pos_odo.phi += dphi;
+                       ROBOT_UNLOCK(est_pos_odo);
+
+                       /* locking should not be needed, but... */
+                       ROBOT_LOCK(motion_irc);
+                       robot.motion_irc = *instance;
+                       robot.motion_irc_received = 1;
+                       ROBOT_UNLOCK(motion_irc);
+
+                       robot.odometry_works = true;
+
+                       robot.hw_status[STATUS_MOTION] = HW_STATUS_OK;
+                       break;
+               case DEADLINE:
+                       robot.odometry_works = false;
+                       robot.hw_status[STATUS_MOTION] = HW_STATUS_FAILED;
+                       DBG("ORTE deadline occurred - motion_irc receive\n");
+                       break;
+       }
+#endif
+}
+
+
+
 void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
                        void *recvCallBackParam)
 {
@@ -368,6 +426,7 @@ int robot_init_orte()
        robottype_publisher_camera_control_create(&robot.orte, send_dummy_cb, &robot.orte);
 
        /* create generic subscribers */
+       robottype_subscriber_odo_data_create(&robot.orte, rcv_odo_data_cb, &robot.orte);
        robottype_subscriber_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte);
        robottype_subscriber_motion_speed_create(&robot.orte, rcv_motion_speed_cb, &robot.orte);
        robottype_subscriber_motion_status_create(&robot.orte, rcv_motion_status_cb, &robot.orte);

diff --git a/src/types/robottype.idl b/src/types/robottype.idl
index a4ff40383b7afdd8937f084c162f0e69cfe7418e..01ba68947433fb410b3f8b9fd66fc97f9f1525b9 100644 (file)
--- a/src/types/robottype.idl
+++ b/src/types/robottype.idl
@@ -4,6 +4,11 @@ struct motion_irc {
        octet seq;              // Sequence number from the last received CAN_CORR_TRIG
 };
 
+struct odo_data {
+       long left;              // PXMC's actual position (pxms.ap)
+       long right;             // PXMC's actual position (pxms.ap)
+};
+
 struct motion_speed {
        short left;
        short right;    

diff --git a/src/types/robottype.ortegen b/src/types/robottype.ortegen
index 0c629c25ce45c0abd2e68439608de58c05537c25..64483ace59dacc7a2df7796807535aec052be808 100644 (file)
--- a/src/types/robottype.ortegen
+++ b/src/types/robottype.ortegen
@@ -9,6 +9,7 @@ type=robot_pos          topic=est_pos_uzv
 type=robot_pos         topic=est_pos_odo
 type=hokuyo_pitch      topic=hokuyo_pitch
 type=hokuyo_scan       topic=hokuyo_scan       deadline=1
+type=odo_data          topic=odo_data          deadline=0.3
 type=motion_irc                topic=motion_irc        deadline=0.3
 type=motion_speed      topic=motion_speed      deadline=0.3    pubdelay=0.1
 type=motion_status     topic=motion_status     deadline=1.5