190 lines
5.9 KiB
C++
190 lines
5.9 KiB
C++
|
|
#include "Aria.h"
|
|
#include "ArNetworking.h"
|
|
|
|
/** @example popupExample.cpp Shows how to create popup windows in a client like MobileEyes
|
|
*
|
|
* This example server program connects to a robot, and sends a message to clients (e.g. MobileEyes) to display
|
|
* in a dialog box when a sensor reading is detected in front of the robot
|
|
* within 1 meter. It also checks to see if that obstacle is not at the same
|
|
* angle as the previous detected obstacle -- it's probably the same one,
|
|
* unmoved -- to avoid sending repeated popups. The popup offers three choices,
|
|
* acknowlege and do nothing, turn the robot around 180 degrees, or exit the server.
|
|
*/
|
|
|
|
|
|
class SensorDetectPopup
|
|
{
|
|
public:
|
|
SensorDetectPopup(ArRobot *robot, ArServerHandlerPopup *popupServer);
|
|
protected:
|
|
ArRobot *myRobot;
|
|
ArServerHandlerPopup *myPopupServer;
|
|
bool myPopupDisplayed;
|
|
double myPrevObstacleAngle;
|
|
bool myPrevObstacleAngleValid;
|
|
ArFunctor2C<SensorDetectPopup, ArTypes::Byte4, int> *myPopupClosedCB;
|
|
|
|
void popupClosed(ArTypes::Byte4 popupID, int button);
|
|
void sensorTask(void) ;
|
|
};
|
|
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
Aria::init();
|
|
ArRobot robot;
|
|
ArSonarDevice sonar;
|
|
ArSick sick;
|
|
robot.addRangeDevice(&sonar);
|
|
ArServerBase server;
|
|
|
|
// Argument parser:
|
|
ArArgumentParser parser(&argc, argv);
|
|
parser.loadDefaultArguments();
|
|
|
|
// Connector and server opener:
|
|
ArRobotConnector robotConnector(&parser, &robot);
|
|
if(!robotConnector.connectRobot())
|
|
{
|
|
ArLog::log(ArLog::Terse, "popupExample: Could not connect to the robot.");
|
|
if(parser.checkHelpAndWarnUnparsed())
|
|
{
|
|
Aria::logOptions();
|
|
}
|
|
Aria::exit(1);
|
|
}
|
|
|
|
ArLaserConnector laserConnector(&parser, &robot, &robotConnector);
|
|
|
|
ArServerSimpleOpener simpleOpener(&parser);
|
|
|
|
// Get command-line and other parameters
|
|
if(!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
|
|
{
|
|
Aria::logOptions();
|
|
Aria::exit(1);
|
|
}
|
|
|
|
robot.runAsync(true);
|
|
|
|
// connect to the laser
|
|
if(!laserConnector.connectLasers())
|
|
{
|
|
ArLog::log(ArLog::Normal, "popupExample: Warning: Could not connect to lasers.");
|
|
}
|
|
|
|
|
|
// Open the server
|
|
if(!simpleOpener.open(&server))
|
|
{
|
|
ArLog::log(ArLog::Terse, "popupExample: Error, could not open server.");
|
|
return 1;
|
|
}
|
|
server.runAsync();
|
|
ArLog::log(ArLog::Normal, "popupExample: Server running. Press control-C to exit.");
|
|
ArLog::log(ArLog::Normal, "popupExample: Each time an obstacle is detected near the robot, a new popup message will be created. Connect with MobileEyes to see them.");
|
|
|
|
// Sends robot position etc.
|
|
ArServerInfoRobot robotInfoServer(&server, &robot);
|
|
|
|
// This service sends drawings e.g. showing range device positions
|
|
ArServerInfoDrawings drawingsServer(&server);
|
|
drawingsServer.addRobotsRangeDevices(&robot);
|
|
|
|
// This service can send messages to clients to display as popup dialogs:
|
|
ArServerHandlerPopup popupServer(&server);
|
|
|
|
// This object contains the robot sensor interpretation task and creates
|
|
// popups:
|
|
SensorDetectPopup(&robot, &popupServer);
|
|
|
|
robot.enableMotors();
|
|
robot.waitForRunExit();
|
|
|
|
Aria::exit(0);
|
|
}
|
|
|
|
SensorDetectPopup::SensorDetectPopup(ArRobot *robot, ArServerHandlerPopup *popupServer) :
|
|
myRobot(robot),
|
|
myPopupServer(popupServer),
|
|
myPopupDisplayed(false),
|
|
myPrevObstacleAngleValid(false)
|
|
{
|
|
myRobot->lock();
|
|
myRobot->addSensorInterpTask("sensorDetectPopup", 50, new ArFunctorC<SensorDetectPopup>(this, &SensorDetectPopup::sensorTask));
|
|
myPopupClosedCB = new ArFunctor2C<SensorDetectPopup, ArTypes::Byte4, int>(this, &SensorDetectPopup::popupClosed);
|
|
myRobot->unlock();
|
|
}
|
|
|
|
void SensorDetectPopup::sensorTask(void)
|
|
{
|
|
// Basic obstacle detection
|
|
|
|
if (myPopupDisplayed) return;
|
|
double detectAngle, detectRange;
|
|
detectRange = myRobot->checkRangeDevicesCurrentPolar(-90, 90, &detectAngle);
|
|
if (detectRange > 0 && detectRange <= 500)
|
|
{
|
|
if(myPrevObstacleAngleValid && fabs(detectAngle - myPrevObstacleAngle) < 0.0001)
|
|
return;
|
|
ArLog::log(ArLog::Normal, "popupExample: New obstacle detected at range %f, angle %f. Displaying popup dialog on client...", detectRange, detectAngle);
|
|
|
|
ArServerHandlerPopupInfo info("popupExample", // ID
|
|
"Object Detected", // Title
|
|
"A range sensor detected a reading within 0.5 meters of the robot.", // Message
|
|
ArServerHandlerPopup::INFORMATION, // Type
|
|
0, // Default button
|
|
0, // Cancel/escape button
|
|
5, // Timeout (sec.)
|
|
NULL, // Timeout String
|
|
"OK", "Acknowleged.", // Button 0 Label/Acknowlegement
|
|
"Turn Around", "Requested rotate...", // Button 1 Label/Acknowlegement
|
|
"Shut Down", "Shutting down server..." // Button 2 Label/Acknowlegement
|
|
);
|
|
int id = myPopupServer->createPopup(&info, myPopupClosedCB);
|
|
ArLog::log(ArLog::Normal, "\t...Created a popup with ID=%d", id);
|
|
myPopupDisplayed = true;
|
|
myPrevObstacleAngle = detectAngle;
|
|
myPrevObstacleAngleValid = true;
|
|
}
|
|
}
|
|
|
|
void SensorDetectPopup::popupClosed(ArTypes::Byte4 popupID, int button)
|
|
{
|
|
// A client closed the popup
|
|
ArLog::log(ArLog::Normal, "popupExample: a client closed popup dialog window with id=%d. Button=%d...", popupID, button);
|
|
myPopupDisplayed = false;
|
|
|
|
if(button < 0)
|
|
{
|
|
ArLog::log(ArLog::Normal, "\t...popup timed out or closed due to an error.");
|
|
return;
|
|
}
|
|
|
|
if (button == 0)
|
|
{
|
|
ArLog::log(ArLog::Normal, "\t...OK pressed.");
|
|
return;
|
|
}
|
|
|
|
if(button == 1)
|
|
{
|
|
ArLog::log(ArLog::Normal, "\t...180 degree rotate requested.");
|
|
myRobot->lock();
|
|
myRobot->setDeltaHeading(180);
|
|
myRobot->unlock();
|
|
return;
|
|
}
|
|
|
|
if(button == 2)
|
|
{
|
|
ArLog::log(ArLog::Normal, "\t...exit requested.");
|
|
myRobot->stopRunning();
|
|
Aria::shutdown();
|
|
Aria::exit(0);
|
|
}
|
|
}
|
|
|
|
|