Skip to content

Commit c00f328

Browse files
author
Javier Moreno
committed
Updated to last version of Micoxrcedds Agent.
1 parent 9b615a4 commit c00f328

1 file changed

Lines changed: 63 additions & 118 deletions

File tree

‎uROS_Agent/src/main.cpp‎

Lines changed: 63 additions & 118 deletions
Original file line numberDiff line numberDiff line change
@@ -37,10 +37,15 @@ void showHelp()
3737
{
3838
std::cout << "Usage: program <command>" << std::endl;
3939
std::cout << "List of commands:" << std::endl;
40+
#ifdef _WIN32
41+
std::cout << " udp <local_port>" << std::endl;
42+
std::cout << " tcp <local_port>" << std::endl;
43+
#else
4044
std::cout << " serial <device_name>" << std::endl;
4145
std::cout << " pseudo-serial" << std::endl;
4246
std::cout << " udp <local_port> [<discovery_port>]" << std::endl;
4347
std::cout << " tcp <local_port> [<discovery_port>]" << std::endl;
48+
#endif
4449
}
4550

4651
void initializationError()
@@ -50,7 +55,7 @@ void initializationError()
5055
std::exit(EXIT_FAILURE);
5156
}
5257

53-
uint16_t parsePort(const char* str_port)
58+
uint16_t parsePort(const std::string& str_port)
5459
{
5560
uint16_t valid_port = 0;
5661
try
@@ -70,73 +75,66 @@ uint16_t parsePort(const char* str_port)
7075
return valid_port;
7176
}
7277

73-
74-
class uros_agent : public rclcpp::Node
75-
{
76-
public:
77-
uros_agent() : Node("uROS_Agent")
78-
{
79-
}
80-
81-
~uros_agent()
82-
{
83-
}
84-
85-
private:
86-
87-
};
88-
89-
9078
int main(int argc, char** argv)
9179
{
92-
93-
#ifdef __unix__
94-
char exe_path[200];
95-
ssize_t count = readlink("/proc/self/exe", exe_path, sizeof(exe_path));
96-
if (count != -1) {
97-
chdir(dirname(exe_path));
98-
}
99-
#endif
80+
eprosima::uxr::Server* server = nullptr;
81+
std::vector<std::string> cl(0);
10082

101-
bool initialized = false;
102-
103-
#ifdef __unix__
104-
if(argc == 2 && (strcmp(argv[1], "-h") == 0 || strcmp(argv[1], "--help") == 0))
83+
if (1 == argc)
10584
{
10685
showHelp();
86+
std::cout << std::endl;
87+
std::cout << "Enter command: ";
88+
89+
std::string raw_cl;
90+
std::getline(std::cin, raw_cl);
91+
std::istringstream iss(raw_cl);
92+
cl.insert(cl.begin(), std::istream_iterator<std::string>(iss), std::istream_iterator<std::string>());
93+
std::cout << raw_cl << std::endl;
10794
}
108-
else if(argc >= 3 && strcmp(argv[1], "udp") == 0)
95+
else
10996
{
110-
std::cout << "UDP agent initialization... ";
111-
uint16_t port = parsePort(argv[2]);
112-
eprosima::uxr::UDPServer* udp_server = (argc == 4) //discovery port
113-
? new eprosima::uxr::UDPServer(port, parsePort(argv[3]))
114-
: new eprosima::uxr::UDPServer(port);
115-
if (udp_server->run())
97+
for (int i = 1; i < argc; ++i)
11698
{
117-
initialized = true;
99+
cl.push_back(argv[i]);
118100
}
119-
std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl;
120101
}
121-
else if(argc >= 3 && strcmp(argv[1], "tcp") == 0)
102+
103+
if((1 == cl.size()) && (("-h" == cl[0]) || ("--help" == cl[0])))
104+
{
105+
showHelp();
106+
}
107+
else if((2 <= cl.size()) && ("udp" == cl[0]))
108+
{
109+
std::cout << "UDP agent initialization... ";
110+
uint16_t port = parsePort(cl[1]);
111+
#ifdef _WIN32
112+
server = new eprosima::uxr::UDPServer(port);
113+
#else
114+
server = (3 == cl.size()) //discovery port
115+
? new eprosima::uxr::UDPServer(port, parsePort(cl[2]))
116+
: new eprosima::uxr::UDPServer(port);
117+
#endif
118+
}
119+
else if((2 <= cl.size()) && ("tcp" == cl[0]))
122120
{
123121
std::cout << "TCP agent initialization... ";
124-
uint16_t port = parsePort(argv[2]);
125-
eprosima::uxr::TCPServer* tcp_server = (argc == 4) //discovery port
126-
? new eprosima::uxr::TCPServer(port, parsePort(argv[3]))
127-
: new eprosima::uxr::TCPServer(port);
128-
if (tcp_server->run())
129-
{
130-
initialized = true;
131-
}
132-
std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl;
122+
uint16_t port = parsePort(cl[1]);
123+
#ifdef _WIN32
124+
server = new eprosima::uxr::TCPServer(port);
125+
#else
126+
server = (3 == cl.size()) //discovery port
127+
? new eprosima::uxr::TCPServer(port, parsePort(cl[2]))
128+
: new eprosima::uxr::TCPServer(port);
129+
#endif
133130
}
134-
else if(argc == 3 && strcmp(argv[1], "serial") == 0)
131+
#ifndef _WIN32
132+
else if((2 == cl.size()) && ("serial" == cl[0]))
135133
{
136134
std::cout << "Serial agent initialization... ";
137135

138136
/* Open serial device. */
139-
int fd = open(argv[2], O_RDWR | O_NOCTTY);
137+
int fd = open(cl[1].c_str(), O_RDWR | O_NOCTTY);
140138
if (0 < fd)
141139
{
142140
struct termios tty_config;
@@ -181,18 +179,12 @@ int main(int argc, char** argv)
181179

182180
if (0 == tcsetattr(fd, TCSANOW, &tty_config))
183181
{
184-
eprosima::uxr::SerialServer* serial_server = new eprosima::uxr::SerialServer(fd, 0);
185-
if (serial_server->run())
186-
{
187-
initialized = true;
188-
}
182+
server = new eprosima::uxr::SerialServer(fd, 0);
189183
}
190184
}
191185
}
192-
193-
std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl;
194186
}
195-
else if (argc == 2 && strcmp(argv[1], "pseudo-serial") == 0)
187+
else if ((1 == cl.size()) && ("pseudo-serial" == cl[0]))
196188
{
197189
std::cout << "Pseudo-Serial initialization... ";
198190

@@ -208,78 +200,31 @@ int main(int argc, char** argv)
208200
cfmakeraw(&attr);
209201
tcflush(fd, TCIOFLUSH);
210202
tcsetattr(fd, TCSANOW, &attr);
203+
std::cout << "Device: " << dev << std::endl;
211204
}
212205
}
213-
214-
/* Launch server. */
215-
eprosima::uxr::SerialServer* serial_server = new eprosima::uxr::SerialServer(fd, 0x00);
216-
if (serial_server->run())
217-
{
218-
initialized = true;
219-
}
220-
221-
if (initialized)
222-
{
223-
std::cout << "OK" << std::endl;
224-
std::cout << "Device: " << dev << std::endl;
225-
}
226-
else
227-
{
228-
std::cout << "ERROR" << std::endl;
229-
}
206+
server = new eprosima::uxr::SerialServer(fd, 0x00);
230207
}
208+
#endif
231209
else
232210
{
233211
initializationError();
234212
}
235-
#elif _WIN32
236-
(void) argc;
237-
(void) argv;
238-
239-
std::string server_type = "";
240-
std::cout << "Select server type [udp|tcp]: ";
241-
std::cin >> server_type;
242213

243-
if ("udp" == server_type)
214+
if (nullptr != server)
244215
{
245-
uint16_t port = 0;
246-
std::cout << "Select port: ";
247-
std::cin >> port;
248-
249-
std::cout << "UDP agent initialization.... ";
250-
eprosima::uxr::UDPServer* udp_server = new eprosima::uxr::UDPServer(port);
251-
if (udp_server->run())
216+
/* Launch server. */
217+
if (server->run())
252218
{
253-
initialized = true;
219+
rclcpp::init(argc, argv);
220+
rclcpp::spin(std::make_shared<uros_agent>());
221+
rclcpp::shutdown();
222+
server->stop();
254223
}
255-
std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl;
256-
}
257-
else if ("tcp" == server_type)
258-
{
259-
uint16_t port = 0;
260-
std::cout << "Select port: ";
261-
std::cin >> port;
262-
263-
std::cout << "UDP agent initialization.... ";
264-
eprosima::uxr::TCPServer* tcp_server = new eprosima::uxr::TCPServer(port);
265-
if (tcp_server->run())
224+
else
266225
{
267-
initialized = true;
226+
std::cout << "ERROR" << std::endl;
268227
}
269-
std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl;
270-
}
271-
else
272-
{
273-
std::cout << "Error server type" << std::endl;
274-
}
275-
276-
#endif
277-
278-
if(initialized)
279-
{
280-
rclcpp::init(argc, argv);
281-
rclcpp::spin(std::make_shared<uros_agent>());
282-
rclcpp::shutdown();
283228
}
284229

285230
return 0;

0 commit comments

Comments
 (0)