1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
|
#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);
}
}
|
