robomon shows lift and pusher positions

diff --git a/src/robomon/RobomonAtlantis.cpp b/src/robomon/RobomonAtlantis.cpp
index e4aecfad7348784abaf1d3976f9f53a6eec406b5..e7cbc2aea0c27b1e9926eaff082f1d7dccbbec49 100644 (file)
--- a/src/robomon/RobomonAtlantis.cpp
+++ b/src/robomon/RobomonAtlantis.cpp
@@ -309,6 +309,20 @@ void RobomonAtlantis::createSensorsGroupBox()
        sharpPuck->setMaximum(250);
        layout->addWidget(sharpPuck);
 
+       layout->addWidget(MiscGui::createLabel("Lift"));
+       liftPos = new QProgressBar();
+       liftPos->setFormat("%v IRC");
+       liftPos->setTextVisible(true);
+       liftPos->setMaximum(1000);
+       layout->addWidget(liftPos);
+       
+       layout->addWidget(MiscGui::createLabel("Pusher"));
+       pusherPos = new QProgressBar();
+       pusherPos->setFormat("%v IRC");
+       pusherPos->setTextVisible(true);
+       pusherPos->setMaximum(250);
+       layout->addWidget(pusherPos);
+       
        sensorsGroupBox->setLayout(layout);
 }
 
@@ -728,6 +742,10 @@ bool RobomonAtlantis::event(QEvent *event)
                case QEVENT(QEV_PUCK_DISTANCE):
                        sharpPuck->setValue(orte.puck_distance.distance*1000);
                        break;
+               case QEVENT(QEV_LIFT_PUSHER):
+                       liftPos->setValue(orte.actuator_status.lift_pos);
+                       pusherPos->setValue(orte.actuator_status.pusher_pos);
+                       break;
                default:
                        if (event->type() == QEvent::Close)
                                closeEvent((QCloseEvent *)event);
@@ -897,8 +915,8 @@ void RobomonAtlantis::createOrte()
                                             rcv_fsm_motion_cb, this);
        robottype_subscriber_fsm_act_create(&orte, 
                                             rcv_fsm_act_cb, this);
-       robottype_subscriber_puck_distance_create(&orte, rcv_puck_distance_cb, this);
-       //robottype_subscriber_actuator_status_create(orte, receiveActuatorStatusCallBack, this);
+       robottype_subscriber_puck_distance_create(&orte, generic_rcv_cb, new OrteCallbackInfo(this, QEV_PUCK_DISTANCE));
+       robottype_subscriber_actuator_status_create(&orte, generic_rcv_cb, new OrteCallbackInfo(this, QEV_LIFT_PUSHER));
 
 
        /*createDISubscriber(this, &orteData);*/

diff --git a/src/robomon/RobomonAtlantis.h b/src/robomon/RobomonAtlantis.h
index 9eaf9b0765e88f7b70a03eb5f26de27593a8bbd6..29053d8beead7303362c80504a34975913e92a2d 100644 (file)
--- a/src/robomon/RobomonAtlantis.h
+++ b/src/robomon/RobomonAtlantis.h
@@ -112,8 +112,6 @@ private:
        void createPickerGroupBox();
        void createFSMGroupBox();
 
-       void createSharpSensorsLayout();
-
        void createRobots();
        void createActions();
 
@@ -199,6 +197,8 @@ private:
        
        /* sensors */
        QProgressBar *sharpPuck;
+       QProgressBar *liftPos;
+       QProgressBar *pusherPos;
 
        /* obstacle simulation */
        double distanceToWallHokuyo(int beamnum);

diff --git a/src/robomon/robomon_orte.cpp b/src/robomon/robomon_orte.cpp
index cd372f9b45792f0da23f73720cd79a1ec41d579c..904da7f66c33567a37c35e7c39f501204baa1e7e 100644 (file)
--- a/src/robomon/robomon_orte.cpp
+++ b/src/robomon/robomon_orte.cpp
@@ -27,7 +27,6 @@
 
 #include "robomon_orte.h"
 
-
 /* ---------------------------------------------------------------------- 
  * PUBLISHERS
  * ---------------------------------------------------------------------- */
@@ -348,16 +347,15 @@ void rcv_fsm_motion_cb(const ORTERecvInfo *info, void *vinstance,
        }
 }
 
-void rcv_puck_distance_cb(const ORTERecvInfo *info, void *vinstance,
-                         void *arg)
+void generic_rcv_cb(const ORTERecvInfo *info, void *vinstance, void *arg)
 {
+       OrteCallbackInfo *i = (OrteCallbackInfo*)arg;
        Q_UNUSED(vinstance);
        switch (info->status) {
                case NEW_DATA:
-                       POST_QEVENT(arg, QEV_PUCK_DISTANCE);
+                       POST_QEVENT(i->receiver, i->qev);
                        break;
                case DEADLINE:
-                       printf("fsm_motion deadline occurred\n");
                        break;
        }
 }

diff --git a/src/robomon/robomon_orte.h b/src/robomon/robomon_orte.h
index d5c2a2b8cce0d3ca44b43c72e3efc5e9a3589b83..237b2cb463c905f7c1db6c5741ca40f199af8fc2 100644 (file)
--- a/src/robomon/robomon_orte.h
+++ b/src/robomon/robomon_orte.h
@@ -23,7 +23,7 @@
                QCoreApplication::postEvent((QObject *)(receiver), \
                new QEvent(QEVENT(event)))
 
-enum {
+enum robomon_qev {
        QEV_MOTION_STATUS = 0,
        QEV_MOTION_IRC,
        QEV_ACTUAL_POSITION,
@@ -41,8 +41,19 @@ enum {
        QEV_FSM_ACT,
        QEV_FSM_MOTION,
        QEV_PUCK_DISTANCE,
+       QEV_LIFT_PUSHER,
+};
+
+class OrteCallbackInfo {
+public:
+       QObject *receiver;
+       enum robomon_qev qev;
+       
+OrteCallbackInfo(QObject *areceiver, enum robomon_qev aqev) :
+               receiver(areceiver), qev(aqev) {};
 };
 
+
 #define FRONT_DOOR             1
 #define INNER_DOOR             4
 #define BACK_DOOR              0
@@ -119,7 +130,6 @@ void rcv_fsm_act_cb(const ORTERecvInfo *info,
                     void *vinstance, void *arg);
 void rcv_fsm_motion_cb(const ORTERecvInfo *info, 
                     void *vinstance, void *arg);
-void rcv_puck_distance_cb(const ORTERecvInfo *info, void *vinstance,
-                         void *arg);
+void generic_rcv_cb(const ORTERecvInfo *info, void *vinstance, void *arg);
 
 #endif /* ROBOMON_ORTE_H */