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
|
#include <string>
#include <iostream>
#include <signal.h>
#include <stdlib.h>
#include <stdio.h>
#include <sstream>
#include <unistd.h>
#include <yarp/os/Network.h>
#include <yarp/os/BufferedPort.h>
#include <yarp/os/Bottle.h>
#include <yarp/os/Time.h>
#define X 0
#define Y 1
#define Z 2
using namespace std;
using namespace yarp::os;
BufferedPort<Bottle> LocalAdminInPort; // Port to receive data from Blender
BufferedPort<Bottle> LocalAdminOutPort; // Port to receive data from Blender
void sigproc(int);
void usage (char* program_name);
int main(int argc, char* argv[])
{
// No parameter needed (for the moment)
if (argc != 1)
{
usage (argv[0]);
exit (1);
}
string command;
string robot_list;
bool waiting = true;
Bottle *incomingBottle;
// Define the names of the ports opened by this program
string local_admin_in_port = "/scene/admin/in/";
string local_admin_out_port = "/scene/admin/out/";
// Define the names for the comunication ports opened by Blender
string ors_admin_in_port = "/ors/admin/in";
string ors_admin_out_port = "/ors/admin/out";
cout << "********* Scene Initialization Client *********" << endl;
cout << "Get the list of robots provided by the ORS" << endl;
cout << "Press ctrl+c to exit." << endl;
//We catch ctrl+c to cleanly close the application
signal( SIGINT,sigproc);
//setvbuf(stdin, NULL, _IONBF, 0);
//Initialization of Yarp network
Network::init();
cout << "\n * YARP network initialized." << endl;
// Connect to Open Robots Simulator
LocalAdminInPort.open(local_admin_in_port.c_str());
LocalAdminOutPort.open(local_admin_out_port.c_str());
Network::connect(LocalAdminOutPort.getName().c_str(), ors_admin_in_port.c_str());
Network::connect( ors_admin_out_port.c_str(), LocalAdminInPort.getName().c_str());
cout << " * Receiving ORS communication port: " << ors_admin_in_port << endl;
cout << " * Transmitting ORS communication port: " << ors_admin_out_port << endl;
cout << " * Requesting list of robots..." << endl;
// Prepare the data bottle to send
Bottle& cmdBottle = LocalAdminOutPort.prepare();
cmdBottle.clear();
cmdBottle.addString("list_robots");
// cin.ignore();
LocalAdminOutPort.write();
cout << " * Waiting for the reply..." << endl;
while(waiting)
{
// Read on the Blender output port, but don't wait.
incomingBottle = LocalAdminInPort.read(false);
if (incomingBottle != NULL)
{
cout << " RESPONSE: " << incomingBottle->toString().c_str() << endl;
robot_list = (std::string) incomingBottle->toString();
if (! (robot_list == "") )
waiting = false;
}
}
cout << " * Got the list of scene elements: " << robot_list << endl;
}
void sigproc(int sig){
signal(SIGINT, sigproc); /* */
/* NOTE some versions of UNIX will reset signal to default
after each call. So for portability reset signal each time */
cout << " * Exiting now!" << endl;
LocalAdminOutPort.close();
LocalAdminInPort.close();
Network::fini();
cout << "*******************************" << endl;
exit(0);
}
void usage (char* program_name)
{
printf ("Usage: %s [robot_name] [number_of_cameras]\n", program_name);
exit (1);
}
|
