robomon2: update to use ref_pos.

diff --git a/src/robomon/src2/MainWindow.cpp b/src/robomon/src2/MainWindow.cpp
index 65a15694ca765e61e1739cd95ec8f4848a52cf1b..7488c5780d331aed17121fbcef358b7104529aba 100644 (file)
--- a/src/robomon/src2/MainWindow.cpp
+++ b/src/robomon/src2/MainWindow.cpp
@@ -203,11 +203,11 @@ void MainWindow::createTabs()
        closeToolButton->setToolTip(tr("Close tab (Ctrl+W)"));
        tabWidget->setCornerWidget(closeToolButton);
 
-       addMclTab();
+//     addMclTab();
 //     addLaserNavTab();
 //     addRobomonRecyclingTab();
 //     addRobomonTuningTab();
-//     addRobomonExplorerTab();
+       addRobomonExplorerTab();
 }
 
 int MainWindow::closeCurrentTab()

diff --git a/src/robomon/src2/RobomonExplorer.cpp b/src/robomon/src2/RobomonExplorer.cpp
index 18507d52bfbddc166bdbe26ce8cab87007e3b173..732890630beb4534b023a74bd708f07a52615400 100644 (file)
--- a/src/robomon/src2/RobomonExplorer.cpp
+++ b/src/robomon/src2/RobomonExplorer.cpp
@@ -1050,7 +1050,7 @@ void RobomonExplorer::createOrte()
                                receivePowerVoltageCallBack, this);
        generic_subscriber_motion_status_create(&orte_generic, 
                                receiveMotionStatusCallBack, this);
-       generic_subscriber_act_pos_create(&orte_generic, 
+       generic_subscriber_ref_pos_create(&orte_generic, 
                                receiveActualPositionCallBack, this);
        generic_subscriber_est_pos_create(&orte_generic, 
                                receiveEstimatedPositionCallBack, this);
@@ -1092,10 +1092,8 @@ void RobomonExplorer::createOrte()
                        this, SLOT(actualPositionReceived()));
        connect(this, SIGNAL(estimatedPositionReceivedSignal()), 
                        this, SLOT(estimatedPositionReceived()));
-       connect(this, SIGNAL(sharpLongsReceivedSignal()), 
-                       this, SLOT(sharpLongsReceived()));
-       connect(this, SIGNAL(sharpShortsReceivedSignal()), 
-                       this, SLOT(sharpShortsReceived()));
+       connect(this, SIGNAL(sharpsReceivedSignal()), 
+                       this, SLOT(sharpsReceived()));
        connect(this, SIGNAL(diReceivedSignal()), 
                        this, SLOT(diReceived()));
        connect(this, SIGNAL(accelerometerReceivedSignal()), 
@@ -1113,13 +1111,13 @@ void RobomonExplorer::motionStatusReceived()
 
 void RobomonExplorer::actualPositionReceived()
 {
-       actPosX->setText(QString("%1").arg(orte_generic.act_pos.x, 0, 'f', 3));
-       actPosY->setText(QString("%1").arg(orte_generic.act_pos.y, 0, 'f', 3));
+       actPosX->setText(QString("%1").arg(orte_generic.ref_pos.x, 0, 'f', 3));
+       actPosY->setText(QString("%1").arg(orte_generic.ref_pos.y, 0, 'f', 3));
        actPosPhi->setText(QString("%1(%2)")
-                       .arg(DEGREES(orte_generic.act_pos.phi), 0, 'f', 0)
-                       .arg(orte_generic.act_pos.phi, 0, 'f', 1));
-       robotActPos->moveRobot(orte_generic.act_pos.x, 
-               orte_generic.act_pos.y, orte_generic.act_pos.phi);
+                       .arg(DEGREES(orte_generic.ref_pos.phi), 0, 'f', 0)
+                       .arg(orte_generic.ref_pos.phi, 0, 'f', 1));
+       robotActPos->moveRobot(orte_generic.ref_pos.x, 
+               orte_generic.ref_pos.y, orte_generic.ref_pos.phi);
 }
 
 void RobomonExplorer::estimatedPositionReceived()
@@ -1262,12 +1260,12 @@ double RobomonExplorer::distanceToWall()
  */    
 double RobomonExplorer::distanceToObstacle(Point obstacle, double obstacleSize)
 {  
-       Point robot(orte_generic.act_pos.x, orte_generic.act_pos.y);
+       Point robot(orte_generic.ref_pos.x, orte_generic.ref_pos.y);
        const double sensorRange = 1.0; /*[meters]*/
                            
        double distance, angle;
            
-       angle = robot.angleTo(obstacle) - orte_generic.act_pos.phi;
+       angle = robot.angleTo(obstacle) - orte_generic.ref_pos.phi;
        angle = fmod(angle, 2.0*M_PI);
        if (angle > +M_PI) angle -= 2.0*M_PI;
        if (angle < -M_PI) angle += 2.0*M_PI;

diff --git a/src/robomon/src2/RobomonRecycling.cpp b/src/robomon/src2/RobomonRecycling.cpp
index 25a3524d8a373446a937f1acf61153111f0c2eca..6bb9cfefceea29eb8bab36c9f861d9564f695c8d 100644 (file)
--- a/src/robomon/src2/RobomonRecycling.cpp
+++ b/src/robomon/src2/RobomonRecycling.cpp
@@ -1020,13 +1020,13 @@ void RobomonRecycling::motionStatusReceived()
 
 void RobomonRecycling::actualPositionReceived()
 {
-       actPosX->setText(QString("%1").arg(orte_generic.act_pos.x, 0, 'f', 3));
-       actPosY->setText(QString("%1").arg(orte_generic.act_pos.y, 0, 'f', 3));
+       actPosX->setText(QString("%1").arg(orte_generic.ref_pos.x, 0, 'f', 3));
+       actPosY->setText(QString("%1").arg(orte_generic.ref_pos.y, 0, 'f', 3));
        actPosPhi->setText(QString("%1(%2)")
-                       .arg(DEGREES(orte_generic.act_pos.phi), 0, 'f', 0)
-                       .arg(orte_generic.act_pos.phi, 0, 'f', 1));
-       robotActPos->moveRobot(orte_generic.act_pos.x, 
-               orte_generic.act_pos.y, orte_generic.act_pos.phi);
+                       .arg(DEGREES(orte_generic.ref_pos.phi), 0, 'f', 0)
+                       .arg(orte_generic.ref_pos.phi, 0, 'f', 1));
+       robotActPos->moveRobot(orte_generic.ref_pos.x, 
+               orte_generic.ref_pos.y, orte_generic.ref_pos.phi);
 }
 
 void RobomonRecycling::estimatedPositionReceived()
@@ -1177,12 +1177,12 @@ double RobomonRecycling::distanceToWall()
  */    
 double RobomonRecycling::distanceToObstacle(Point obstacle, double obstacleSize)
 {  
-       Point robot(orte_generic.act_pos.x, orte_generic.act_pos.y);
+       Point robot(orte_generic.ref_pos.x, orte_generic.ref_pos.y);
        const double sensorRange = 1.0; /*[meters]*/
                            
        double distance, angle;
            
-       angle = robot.angleTo(obstacle) - orte_generic.act_pos.phi;
+       angle = robot.angleTo(obstacle) - orte_generic.ref_pos.phi;
        angle = fmod(angle, 2.0*M_PI);
        if (angle > +M_PI) angle -= 2.0*M_PI;
        if (angle < -M_PI) angle += 2.0*M_PI;

diff --git a/src/robomon/src2/SimMcl.cpp b/src/robomon/src2/SimMcl.cpp
index 01194c801851df79ff680acf365864c34b467362..000131f70c1fb50479a4003d0c12883d48f10801 100644 (file)
--- a/src/robomon/src2/SimMcl.cpp
+++ b/src/robomon/src2/SimMcl.cpp
@@ -282,15 +282,15 @@ void SimMcl::createMclGroupBox()
        mclYOffLineEdit->setText("0.000");
        mclGenDnoiseLineEdit->setText("0.99");
        mclGenAnoiseLineEdit->setText("360");
-       mclMovDnoiseLineEdit->setText("0.01");
-       mclMovAnoiseLineEdit->setText("0.02");
+       mclMovDnoiseLineEdit->setText("0.002");
+       mclMovAnoiseLineEdit->setText("2");
        mclBeaconCount->setText("4");
 
        mclCountLineEdit->setText("1000");
        mclWMinLineEdit->setText("0.25");
        mclWMaxLineEdit->setText("2.0");
        mclEvalSigmaLineEdit->setText("160");
-       mclAEvalSigmaLineEdit->setText("0.007");
+       mclAEvalSigmaLineEdit->setText("10");
        mclMaxavdistLineEdit->setText("0.15");
        mclMaxnoisecycleLineEdit->setText("10");
 
@@ -1004,9 +1004,9 @@ void SimMcl::initMcl()
        /* MCL laser */
        mcl_laser.width = PLAYGROUND_WIDTH_M;
        mcl_laser.height = PLAYGROUND_HEIGHT_M;
-       mcl_laser.pred_dnoise = 0.03;
-       mcl_laser.pred_anoise = DEG2RAD(5);
-       mcl_laser.aeval_sigma = DEG2RAD(10);
+       mcl_laser.pred_dnoise = mclMovDnoiseLineEdit->text().toFloat();
+       mcl_laser.pred_anoise = DEG2RAD(mclMovAnoiseLineEdit->text().toFloat());
+       mcl_laser.aeval_sigma = DEG2RAD(mclAEvalSigmaLineEdit->text().toFloat());
 
        if (mclRedBeaconsRadioButton->isChecked()) {
                mcl_laser.beacon_color = BEACON_RED;