Merge branch 'maint-demo'

[eurobot/public.git] / src / robomon / RobomonAtlantis.cpp

diff --git a/src/robomon/RobomonAtlantis.cpp b/src/robomon/RobomonAtlantis.cpp
index 7351c3b6cba3ea34d298cae56902ef4db9f78892..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,13 +175,22 @@ 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);
 
        colorChoser = new QCheckBox("&Team color");
        layout->addWidget(colorChoser);
+        
+        strategyButton= new QPushButton(tr("Strategy"));
+        layout->addWidget(strategyButton);
 
        miscGroupBox->setLayout(layout);
 }
@@ -350,6 +359,8 @@ void RobomonAtlantis::createActions()
 
        connect(startPlug, SIGNAL(stateChanged(int)), this, SLOT(sendStart(int)));
        connect(colorChoser, SIGNAL(stateChanged(int)), this, SLOT(setTeamColor(int)));
+        connect(strategyButton, SIGNAL(pressed()), this, SLOT(changeStrategy_1()));
+        connect(strategyButton, SIGNAL(released()), this, SLOT(changeStrategy_0()));
 
        /* obstacle simulation */
        simulationEnabled = 0;
@@ -361,6 +372,21 @@ 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()
+{
+        orte.robot_switches.strategy = true;
+        ORTEPublicationSend(orte.publication_robot_switches);
+}
+
+void RobomonAtlantis::changeStrategy_0()
+{
+        orte.robot_switches.strategy = false;
+        ORTEPublicationSend(orte.publication_robot_switches);
 }
 
 void RobomonAtlantis::setVoltage33(int state)
@@ -573,9 +599,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;
@@ -754,17 +783,12 @@ void RobomonAtlantis::createOrte()
 {
        int rv;
 
-       orte.strength = 11;
-
        memset(&orte, 0, sizeof(orte));
        rv = robottype_roboorte_init(&orte);
        if (rv) {
                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);
@@ -784,18 +808,15 @@ 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,
                                             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;
@@ -1026,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();