robomon: Added jaws actuator visualization.

Strategy writing and testing will be better with this visualization.

diff --git a/src/robomon/RobomonAtlantis.cpp b/src/robomon/RobomonAtlantis.cpp
index 7351c3b6cba3ea34d298cae56902ef4db9f78892..f2e9a5ff5a6cce3909b3ba46b387b57d643687a3 100644 (file)
--- a/src/robomon/RobomonAtlantis.cpp
+++ b/src/robomon/RobomonAtlantis.cpp
@@ -573,9 +573,12 @@ bool RobomonAtlantis::event(QEvent *event)
                case QEVENT(QEV_HOKUYO_SCAN):
                        hokuyoScan->newScan(&orte.hokuyo_scan);
                        break;
-//             case QEVENT(QEV_VIDLE_CMD):
-//                     robotEstPosBest->setVidle(orte.vidle_cmd.req_pos);
-//                     break;
+               case QEVENT(QEV_JAWS_CMD):
+                       robotEstPosBest->setJaws(orte.jaws_cmd.req_pos.left);
+                       robotRefPos->setJaws(orte.jaws_cmd.req_pos.left);
+                       robotEstPosIndepOdo->setJaws(orte.jaws_cmd.req_pos.left);
+                       robotEstPosOdo->setJaws(orte.jaws_cmd.req_pos.left);
+                       break;
                case QEVENT(QEV_REFERENCE_POSITION):
                        emit actualPositionReceivedSignal();
                        break;
@@ -784,8 +787,8 @@ void RobomonAtlantis::createOrte()
                        generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_BEST));
        robottype_subscriber_hokuyo_scan_create(&orte,
                        generic_rcv_cb, new OrteCallbackInfo(this, QEV_HOKUYO_SCAN));
-       //robottype_subscriber_vidle_cmd_create(&orte,
-         //              generic_rcv_cb, new OrteCallbackInfo(this, QEV_VIDLE_CMD));
+       robottype_subscriber_jaws_cmd_create(&orte,
+                      generic_rcv_cb, new OrteCallbackInfo(this, QEV_JAWS_CMD));
        robottype_subscriber_fsm_main_create(&orte,
                                             rcv_fsm_main_cb, this);
        robottype_subscriber_fsm_motion_create(&orte,

diff --git a/src/robomon/Robot.cpp b/src/robomon/Robot.cpp
index bcc4d436cfc93ce271cd3e92663591f87e9f1cfb..c2c273c98f73a72e9cef168e8f86ce983be88d03 100644 (file)
--- a/src/robomon/Robot.cpp
+++ b/src/robomon/Robot.cpp
@@ -25,8 +25,8 @@ Robot::Robot(const QString &text, const QPen &pen, const QBrush &brush) :
        this->pen = pen;
        this->brush = brush;
        setVisible(false);
-       setVidle(0);
-       moveRobot(1, 1, 0);
+       setJaws(JAW_LEFT_CLOSE);
+       moveRobot(ROBOT_START_X_M, ROBOT_START_Y_M, 0);
 }
 
 Robot::~Robot()
@@ -60,16 +60,35 @@ void Robot::paint(QPainter *painter,
                QLineF(xa, yb, xa-xd, yc),
                QLineF(xa, yb, xa+xd, yc)};
        painter->drawLines(arrow, 3);
-//     if (vidle != 0) {
-//             const float vidle_angle = (float)(vidle - VIDLE_DOWN)/(VIDLE_UP-VIDLE_DOWN)*M_PI/2.0;
-//             const float y = -ROBOT_VIDLE_LENGTH_M*1000*cos(vidle_angle);
-//             QLineF vidle[] = {
-//                     QLineF(0*ROBOT_WIDTH_MM/3, 0, 0*ROBOT_WIDTH_MM/3, y),
-//                     QLineF(1*ROBOT_WIDTH_MM/3, 0, 1*ROBOT_WIDTH_MM/3, y),
-//                     QLineF(2*ROBOT_WIDTH_MM/3, 0, 2*ROBOT_WIDTH_MM/3, y),
-//                     QLineF(3*ROBOT_WIDTH_MM/3, 0, 3*ROBOT_WIDTH_MM/3, y)};
-//             painter->drawLines(vidle, 4);
-//     }
+
+       QLineF jawsLines[2];    /* field witch jaws lines */
+       const int offset = 40;  /* offset of point of rotation from robot sides [mm] */
+       const int hold = 40;    /* offset from open position [mm] */
+       const int close = 110;  /* offset from open positon [mm] */
+       const double y_hold = sqrt(pow(ROBOT_JAWS_LENGHT_MM, 2) - pow(hold, 2));
+       const double y_close = sqrt(pow(ROBOT_JAWS_LENGHT_MM, 2) - pow(close, 2));
+
+       switch (jawsPosition) {
+               case JAW_LEFT_OPEN:
+                       jawsLines[0] = QLineF(offset, ROBOT_HEIGHT_MM, offset, ROBOT_HEIGHT_MM + ROBOT_JAWS_LENGHT_MM);
+                       jawsLines[1] = QLineF(ROBOT_WIDTH_MM - offset , ROBOT_HEIGHT_MM, ROBOT_WIDTH_MM - offset, ROBOT_HEIGHT_MM + ROBOT_JAWS_LENGHT_MM);
+                       break;
+
+               case JAW_LEFT_CATCH:
+                       jawsLines[0] = QLineF(offset, ROBOT_HEIGHT_MM, offset + hold, ROBOT_HEIGHT_MM + ROBOT_JAWS_LENGHT_MM);
+                       jawsLines[1] = QLineF(ROBOT_WIDTH_MM - offset , ROBOT_HEIGHT_MM, ROBOT_WIDTH_MM - offset - hold, ROBOT_HEIGHT_MM + y_hold);
+                       break;
+
+               case JAW_LEFT_CLOSE:
+                       jawsLines[0] = QLineF(offset, ROBOT_HEIGHT_MM, offset + close, ROBOT_HEIGHT_MM + y_close);
+                       jawsLines[1] = QLineF(ROBOT_WIDTH_MM - offset , ROBOT_HEIGHT_MM, ROBOT_WIDTH_MM - offset - close, ROBOT_HEIGHT_MM + y_close);
+                       break;
+       }
+
+       QPen penThick(Qt::black, 20, Qt::SolidLine);    /* new pen with 20px thickness for jaws drawing */
+       painter->setPen(penThick);
+       painter->drawLines(jawsLines, 2);       /* draw jaws */
+       painter->setPen(pen);   /* set pen to previous slim pen */
 
        painter->setFont(QFont("Arial", 40, 0, 0));
        QTransform mirror;
@@ -94,10 +113,10 @@ void Robot::mySetVisible(bool show)
        setVisible(show);
 }
 
-void Robot::setVidle(int value)
+void Robot::setJaws(int value)
 {
        QRectF r = boundingRect();
        //r.setBottom(0);
-       vidle = value;
+       jawsPosition = value;
        update(r);
 }

diff --git a/src/robomon/Robot.h b/src/robomon/Robot.h
index 9d89413915ab4259b180610fd0d90e362d1cb4e1..3e68a8ac3784ce4eb4d30650c4a17944d1459a7e 100644 (file)
--- a/src/robomon/Robot.h
+++ b/src/robomon/Robot.h
@@ -29,9 +29,9 @@ public:
        void moveRobot(double x, double y, double angle);
 public slots:
        void mySetVisible(bool show);
-       void setVidle(int value);
+       void setJaws(int value);
 private:
-       int vidle;
+       int jawsPosition;
        QString text;
        QPen pen;
        QBrush brush;

diff --git a/src/robomon/robomon_orte.h b/src/robomon/robomon_orte.h
index f1c9d851018e60709f1d8380010ace2b98aee82d..02e47eff5388fdba9a2d7fd47ac1407f98b590fa 100644 (file)
--- a/src/robomon/robomon_orte.h
+++ b/src/robomon/robomon_orte.h
@@ -35,7 +35,7 @@ enum robomon_qev {
        QEV_FSM_ACT,
        QEV_FSM_MOTION,
        QEV_HOKUYO_SCAN,
-       QEV_VIDLE_CMD,
+       QEV_JAWS_CMD,
 };
 
 class OrteCallbackInfo {