#include "playgroundview.h"
#include "trail.h"
+
#include <QCoreApplication>
#include <QEvent>
#include <QKeyEvent>
#include <QDebug>
#include <QMessageBox>
-RobomonAtlantis::RobomonAtlantis(QWidget *parent)
- : QWidget(parent), motorSimulation(orte)
+RobomonAtlantis::RobomonAtlantis(QStatusBar *_statusBar)
- : QWidget(0), statusBar(_statusBar)
++ : QWidget(0), statusBar(_statusBar), motorSimulation(orte)
{
QFont font;
font.setPointSize(7);
createOrte();
createRobots();
createActions();
+ createMap();
// connect(vidle, SIGNAL(valueChanged(int)),
// robotEstPosBest, SLOT(setVidle(int)));
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);
- // startPlug = new QCheckBox("&Start plug");
- // layout->addWidget(startPlug);
+ 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);
++// startPlug = new QCheckBox("&Start plug");
++// layout->addWidget(startPlug);
+
+ colorChoser = new QCheckBox("&Team color");
+ layout->addWidget(colorChoser);
+
miscGroupBox->setLayout(layout);
}
void RobomonAtlantis::createActuatorsGroupBox()
{
- actuatorsGroupBox = new QGroupBox(tr("Actuators"));
- actuatorsGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Preferred);
- QHBoxLayout *layout = new QHBoxLayout();
-// vidle = new QDial();
-
-// vidle->setMinimum(VIDLE_VYSIP);
-// vidle->setMaximum((VIDLE_UP-VIDLE_VYSIP)+VIDLE_VYSIP);
-// vidle->setEnabled(true);
+ actuatorsGroupBox = new QGroupBox(tr("Status"));
+ actuatorsGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Maximum);
+ QGridLayout *layout = new QGridLayout();
- //createMotorsGroupBox();
+ layout->addWidget(MiscGui::createLabel("APP:"), 1, 0);
+ system_status = new QLabel();
+ system_status->setMinimumWidth(100);
+ system_status->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
+ layout->addWidget(system_status, 1, 1);
-
+
- layout->setAlignment(Qt::AlignLeft);
-// layout->addWidget(vidle);
- //layout->addWidget(enginesGroupBox);
- actuatorsGroupBox->setLayout(layout);
+ actuatorsGroupBox->setLayout(layout);
}
void RobomonAtlantis::createPowerGroupBox()
powerGroupBox->setLayout(layout);
}
-#if 0
-void RobomonAtlantis::createMotorsGroupBox()
-{
- enginesGroupBox = new QGroupBox(tr("Motors"));
- QVBoxLayout *layout = new QVBoxLayout();
- QHBoxLayout *layout1 = new QHBoxLayout();
- QHBoxLayout *layout2 = new QHBoxLayout();
-
- leftMotorSlider = new QSlider(Qt::Vertical);
- rightMotorSlider = new QSlider(Qt::Vertical);
- bothMotorsCheckBox = new QCheckBox(tr("Lock both"));
- stopMotorsPushButton = new QPushButton(tr("Stop Motors"));
-
- leftMotorSlider->setMinimum(-100);
- leftMotorSlider->setMaximum(100);
- leftMotorSlider->setTracking(false);
- leftMotorSlider->setTickPosition(QSlider::TicksLeft);
-
- rightMotorSlider->setMinimum(-100);
- rightMotorSlider->setMaximum(100);
- rightMotorSlider->setTracking(false);
- rightMotorSlider->setTickPosition(QSlider::TicksRight);
-
- stopMotorsPushButton->setMaximumWidth(90);
-
- layout1->addWidget(leftMotorSlider);
- layout1->addWidget(MiscGui::createLabel("0"));
- layout1->addWidget(rightMotorSlider);
-
- layout2->addWidget(bothMotorsCheckBox);
-
- layout->addWidget(MiscGui::createLabel("100"));
- layout->addLayout(layout1);
- layout->addWidget(MiscGui::createLabel("-100"));
- layout->addLayout(layout2);
- layout->addWidget(stopMotorsPushButton);
- enginesGroupBox->setLayout(layout);
-}
-#endif
-
void RobomonAtlantis::createRobots()
{
robotRefPos = new Robot("Ref", QPen(Qt::darkBlue), QBrush(Qt::NoBrush));
}
+void RobomonAtlantis::createMap()
+{
+ mapImage = new Map();
+ mapImage->setZValue(3);
+ mapImage->setTransform(QTransform().scale(MAP_CELL_SIZE_MM, MAP_CELL_SIZE_MM), true);
+
+
+ playgroundScene->addItem(mapImage);
+}
+
/**********************************************************************
* GUI actions
**********************************************************************/
// connect(stopMotorsPushButton, SIGNAL(clicked()),
// this, SLOT(stopMotors()));
- connect(startPlug, SIGNAL(stateChanged(int)), this, SLOT(sendStart(int)));
- connect(colorChoser, SIGNAL(stateChanged(int)), this, SLOT(setTeamColor(int)));
+// connect(startPlug, SIGNAL(stateChanged(int)), this, SLOT(sendStart(int)));
/* obstacle simulation */
simulationEnabled = 0;
playgroundScene, SLOT(showObstacle(int)));
connect(playgroundScene, SIGNAL(obstacleChanged(QPointF)),
this, SLOT(changeObstacle(QPointF)));
+
+ connect(motorSimulationCheckBox, SIGNAL(stateChanged(int)),
+ this, SLOT(setMotorSimulation(int)));
}
void RobomonAtlantis::setVoltage33(int state)
disconnect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
}
}
- playgroundScene->showMap(show);
+ mapImage->setVisible(show);
}
void RobomonAtlantis::paintMap()
if (!map) return;
- for(int i=0; i < MAP_WIDTH; i++) {
- for(int j=0; j<MAP_HEIGHT; j++) {
+ for(int i = 0; i < MAP_WIDTH; i++) {
+ for(int j = 0; j < MAP_HEIGHT; j++) {
QColor color;
struct map_cell *cell = &map->cells[j][i];
color = lightGray;
- if ((cell->flags & MAP_FLAG_WALL) &&
- (cell->flags & MAP_FLAG_INVALIDATE_WALL) == 0)
+ if (cell->flags & MAP_FLAG_WALL)
color = darkYellow;
if (cell->flags & MAP_FLAG_IGNORE_OBST)
color = darkGreen;
if (cell->flags & MAP_FLAG_DET_OBST)
color = cyan;
- playgroundScene->setMapColor(i, j, color);
+ color.setAlpha(200);
+ mapImage->setPixelColor(i, MAP_HEIGHT - j - 1, color);
}
}
}
this, SLOT(simulateObstaclesHokuyo()));
obstacleSimulationTimer->start(100);
setMouseTracking(true);
+ hokuyoScan->setVisible(true);
+
+
} else {
if (obstacleSimulationTimer)
delete obstacleSimulationTimer;
- //double distance = 0.8;
+
+ hokuyoScan->setVisible(false); /* hide hokuyo scan*/
+
}
}
case QEVENT(QEV_HOKUYO_SCAN):
hokuyoScan->newScan(&orte.hokuyo_scan);
break;
- case QEVENT(QEV_VIDLE_CMD):
- robotEstPosBest->setVidle(orte.vidle_cmd.req_pos);
+ case QEVENT(QEV_CRANE_CMD):
+ robotEstPosBest->setCrane(orte.crane_cmd.req_pos);
break;
case QEVENT(QEV_REFERENCE_POSITION):
emit actualPositionReceivedSignal();
case QEVENT(QEV_FSM_MOTION):
fsm_motion_state->setText(orte.fsm_motion.state_name);
break;
+ case QEVENT(QEV_STSTEM_STATUS):
+ if (orte.system_status.system_condition)
+ system_status->setText("WARNING");
+ else
+ system_status->setText("OK");
+ break;
default:
if (event->type() == QEvent::Close)
closeEvent((QCloseEvent *)event);
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);
generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_INDEP_ODO));
robottype_subscriber_est_pos_best_create(&orte,
generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_BEST));
+ robottype_subscriber_system_status_create(&orte,
+ generic_rcv_cb, new OrteCallbackInfo(this, QEV_STSTEM_STATUS));
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_crane_cmd_create(&orte,
+ generic_rcv_cb, new OrteCallbackInfo(this, QEV_CRANE_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 = 0xffff;
- orte.motion_speed.right = 0xffff;
- robottype_subscriber_motion_speed_create(&orte, NULL, NULL);
++ robottype_subscriber_motion_speed_create(&orte, NULL, NULL);
/* power management */
orte.pwr_ctrl.voltage33 = true;
/* Get segment identificator in a read only mode */
segmentId = shmget(SHM_MAP_KEY, sharedSegmentSize, S_IRUSR);
if(segmentId == -1) {
- QMessageBox::critical(this, "robomon",
- "Unable to open shared memory segment!");
- return;
+ statusBar->showMessage("No external map found - creating a new map.");
}
/* Init Shmap */
{
struct robot_pos_type e = orte.est_pos_best;
double sensor_a;
- struct sharp_pos s;
+ struct robot_pos_type s;
s.x = HOKUYO_CENTER_OFFSET_M;
s.y = 0.0;
- s.ang = HOKUYO_INDEX_TO_RAD(beamnum);
+ s.phi = HOKUYO_INDEX_TO_RAD(beamnum);
Point sensor(e.x + s.x*cos(e.phi) - s.y*sin(e.phi),
e.y + s.x*sin(e.phi) + s.y*cos(e.phi));
- sensor_a = e.phi + s.ang;
+ sensor_a = e.phi + s.phi;
const double sensorRange = 4.0; /*[meters]*/
ORTEPublicationSend(orte.publication_robot_cmd);
}
-void RobomonAtlantis::setTeamColor(int plug)
-{
- orte.robot_switches.team_color = plug ? 1 : 0;
- ORTEPublicationSend(orte.publication_robot_switches);
-}
-
+ void RobomonAtlantis::setMotorSimulation(int state)
+ {
+ if (state) {
+ motorSimulation.start();
+ } else {
+ motorSimulation.stop();
+ }
+ }
+
void RobomonAtlantis::resetTrails()
{
trailRefPos->reset();
#define ROBOMON_ATLANTIS_H
#include <QDialog>
+ #include <QDateTime>
+ #include <QTimer>
#include <trgen.h>
#include "PlaygroundScene.h"
#include "playgroundview.h"
#include "Robot.h"
+#include "Map.h"
#include <roboorte_robottype.h>
#include "trail.h"
#include "hokuyoscan.h"
+#include <QMainWindow>
class QHBoxLayout;
class QVBoxLayout;
class QSlider;
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
public:
- RobomonAtlantis(QWidget *parent = 0);
+ RobomonAtlantis(QStatusBar *_statusBar = 0);
protected:
bool event(QEvent *event);
void motionStatusReceivedSignal();
void actualPositionReceivedSignal();
void powerVoltageReceivedSignal();
-
+
public slots:
void showMap(bool show);
void useOpenGL(bool use);
void resetTrails();
private slots:
/************************************************************
- * GUI actions
+ * GUI actions
************************************************************/
void setVoltage33(int state);
void setVoltage50(int state);
void simulateObstaclesHokuyo();
void changeObstacle(QPointF position);
void sendStart(int plug);
- void setTeamColor(int plug);
+ void setMotorSimulation(int state);
/************************************************************
- * ORTE
+ * ORTE
************************************************************/
void motionStatusReceived();
void actualPositionReceived();
void createRobots();
void createActions();
+ void createMap();
+
+ QStatusBar *statusBar;
QVBoxLayout *leftLayout;
QVBoxLayout *rightLayout;
/* misc */
QCheckBox *obstacleSimulationCheckBox;
+ QCheckBox *motorSimulationCheckBox;
QLabel *fsm_main_state;
QLabel *fsm_act_state;
QLabel *fsm_motion_state;
+ QLabel *system_status;
QCheckBox *startPlug;
QCheckBox *colorChoser;
public:
Robot *robotEstPosBest;
Robot *robotEstPosIndepOdo;
Robot *robotEstPosOdo;
+
+ Map *mapImage;
private:
Trail *trailRefPos;
Trail *trailEstPosBest;
void openSharedMemory();
bool sharedMemoryOpened;
QTimer *mapTimer;
-
+
/* obstacle simulation */
double distanceToWallHokuyo(int beamnum);
double distanceToObstacleHokuyo(int beamnum, Point obstacle, double obstacleSize);
QTimer *obstacleSimulationTimer;
Point simulatedObstacle;
+ class MotorSimulation motorSimulation;
+
/************************************************************
- * ORTE
+ * ORTE
************************************************************/
void createOrte();
projects / eurobot / public.git / commitdiff