Merge branch 'maint-demo'

Conflicts:
	src/robomon/RobomonAtlantis.h

diff --git a/src/robomon/RobomonAtlantis.cpp b/src/robomon/RobomonAtlantis.cpp
index 63270a711a1911992a5058d9cedd9d7e6ebe86ac..9a2a30987191f88eab53e0daa39987a6d86b258d 100644 (file)
--- a/src/robomon/RobomonAtlantis.cpp
+++ b/src/robomon/RobomonAtlantis.cpp
@@ -46,7 +46,7 @@
 #include <QMessageBox>
 
 RobomonAtlantis::RobomonAtlantis(QWidget *parent)
-       : QWidget(parent)
+    : QWidget(parent), motorSimulation(orte)
 {
        QFont font;
        font.setPointSize(7);
@@ -175,8 +175,14 @@ void RobomonAtlantis::createMiscGroupBox()
 
        obstacleSimulationCheckBox = new QCheckBox(tr("&Obstacle simulation"));
        obstacleSimulationCheckBox->setShortcut(tr("o"));
+       obstacleSimulationCheckBox->setToolTip("When enabled, simulates an obstacle,\npublishes simlated hokuyo data and \ndisplays robot's map by using shared memory.");
        layout->addWidget(obstacleSimulationCheckBox);
 
+       motorSimulationCheckBox = new QCheckBox(tr("&Motor simulation"));
+       motorSimulationCheckBox->setShortcut(tr("m"));
+       motorSimulationCheckBox->setToolTip("Subscribes to motion_speed and\nbased on this publishes motion_irc.");
+       layout->addWidget(motorSimulationCheckBox);
+
        startPlug = new QCheckBox("&Start plug");
        layout->addWidget(startPlug);
 
@@ -366,6 +372,9 @@ void RobomonAtlantis::createActions()
                        playgroundScene, SLOT(showObstacle(int)));
        connect(playgroundScene, SIGNAL(obstacleChanged(QPointF)),
                        this, SLOT(changeObstacle(QPointF)));
+
+       connect(motorSimulationCheckBox, SIGNAL(stateChanged(int)),
+               this, SLOT(setMotorSimulation(int)));
 }
 
 void RobomonAtlantis::changeStrategy_1()
@@ -780,9 +789,6 @@ void RobomonAtlantis::createOrte()
                printf("RobomonAtlantis: Unable to initialize ORTE\n");
        }
 
-       /* publishers */
-       robottype_publisher_motion_speed_create(&orte, dummy_publisher_callback, NULL);
-
        robottype_publisher_pwr_ctrl_create(&orte, dummy_publisher_callback, NULL);
        robottype_publisher_robot_cmd_create(&orte, NULL, &orte);
        robottype_publisher_robot_switches_create(&orte, dummy_publisher_callback, &orte);
@@ -810,10 +816,7 @@ void RobomonAtlantis::createOrte()
                                             rcv_fsm_motion_cb, this);
        robottype_subscriber_fsm_act_create(&orte,
                                             rcv_fsm_act_cb, this);
-
-       /* motors */
-       orte.motion_speed.left = 0;
-       orte.motion_speed.right = 0;
+    robottype_subscriber_motion_speed_create(&orte, NULL, NULL);
 
        /* power management */
         orte.pwr_ctrl.voltage33 = true;
@@ -1044,6 +1047,15 @@ void RobomonAtlantis::setTeamColor(int plug)
        ORTEPublicationSend(orte.publication_robot_switches);
 }
 
+void RobomonAtlantis::setMotorSimulation(int state)
+{
+    if (state) {
+        motorSimulation.start();
+    } else {
+        motorSimulation.stop();
+    }
+}
+
 void RobomonAtlantis::resetTrails()
 {
        trailRefPos->reset();

diff --git a/src/robomon/RobomonAtlantis.h b/src/robomon/RobomonAtlantis.h
index f62fe99c9b7d28bd683abd43a54d9990ff9bd307..f7239bed5ffa13f352951673480b2017f980a124 100644 (file)
--- a/src/robomon/RobomonAtlantis.h
+++ b/src/robomon/RobomonAtlantis.h
@@ -14,6 +14,8 @@
 #define ROBOMON_ATLANTIS_H
 
 #include <QDialog>
+#include <QDateTime>
+#include <QTimer>
 
 #include <trgen.h>
 #include "PlaygroundScene.h"
@@ -40,6 +42,39 @@ class QProgressBar;
 class QFont;
 class QImage;
 
+class MotorSimulation : QObject {
+    Q_OBJECT
+
+    QTimer timer;
+    qint64 last_time;
+    struct robottype_orte_data &orte;
+public:
+    MotorSimulation(struct robottype_orte_data &orte) : QObject(), timer(this), orte(orte) {}
+    void start()
+    {
+        robottype_publisher_motion_irc_create(&orte, 0, 0);
+        connect(&timer, SIGNAL(timeout()), this, SLOT(updateIRC()));
+        timer.start(50);
+    }
+
+    void stop()
+    {
+        robottype_publisher_motion_irc_destroy(&orte);
+        timer.stop();
+        disconnect(&timer, SIGNAL(timeout()), this, SLOT(updateIRC()));
+    }
+private slots:
+    void updateIRC()
+    {
+        qint64 now = QDateTime::currentMSecsSinceEpoch();
+        orte.motion_irc.left += orte.motion_speed.left * (now - last_time);   // TODO: Find constant for speed to irc conversion
+        orte.motion_irc.right+= orte.motion_speed.right * (now - last_time);
+        ORTEPublicationSend(orte.publication_motion_irc);
+        last_time = now;
+    }
+};
+
+
 class RobomonAtlantis : public QWidget
 {
        Q_OBJECT
@@ -83,6 +118,7 @@ private slots:
        void setTeamColor(int plug);
         void changeStrategy_1();
         void changeStrategy_0();
+       void setMotorSimulation(int state);
 
        /************************************************************
         * ORTE
@@ -161,6 +197,7 @@ private:
 
        /* misc */
        QCheckBox *obstacleSimulationCheckBox;
+       QCheckBox *motorSimulationCheckBox;
        QLabel *fsm_main_state;
        QLabel *fsm_act_state;
        QLabel *fsm_motion_state;
@@ -201,6 +238,8 @@ private:
        QTimer *obstacleSimulationTimer;
        Point simulatedObstacle;
 
+       class MotorSimulation motorSimulation;
+
        /************************************************************
         * ORTE
         ************************************************************/