From 08f9883e3498e9f4d658d9b7cfca3b9654579ea5 Mon Sep 17 00:00:00 2001 From: Javier Moreno Date: Thu, 15 Nov 2018 07:41:00 +0100 Subject: [PATCH] Updated to last version of Micoxrcedds Agent. --- uROS_Agent/src/main.cpp | 181 ++++++++++++++-------------------------- 1 file changed, 63 insertions(+), 118 deletions(-) diff --git a/uROS_Agent/src/main.cpp b/uROS_Agent/src/main.cpp index e9d986f..fa37425 100644 --- a/uROS_Agent/src/main.cpp +++ b/uROS_Agent/src/main.cpp @@ -37,10 +37,15 @@ void showHelp() { std::cout << "Usage: program " << std::endl; std::cout << "List of commands:" << std::endl; +#ifdef _WIN32 + std::cout << " udp " << std::endl; + std::cout << " tcp " << std::endl; +#else std::cout << " serial " << std::endl; std::cout << " pseudo-serial" << std::endl; std::cout << " udp []" << std::endl; std::cout << " tcp []" << std::endl; +#endif } void initializationError() @@ -50,7 +55,7 @@ void initializationError() std::exit(EXIT_FAILURE); } -uint16_t parsePort(const char* str_port) +uint16_t parsePort(const std::string& str_port) { uint16_t valid_port = 0; try @@ -70,73 +75,66 @@ uint16_t parsePort(const char* str_port) return valid_port; } - -class uros_agent : public rclcpp::Node -{ -public: - uros_agent() : Node("uROS_Agent") - { - } - - ~uros_agent() - { - } - -private: - -}; - - int main(int argc, char** argv) { - -#ifdef __unix__ - char exe_path[200]; - ssize_t count = readlink("/proc/self/exe", exe_path, sizeof(exe_path)); - if (count != -1) { - chdir(dirname(exe_path)); - } -#endif + eprosima::uxr::Server* server = nullptr; + std::vector cl(0); - bool initialized = false; - -#ifdef __unix__ - if(argc == 2 && (strcmp(argv[1], "-h") == 0 || strcmp(argv[1], "--help") == 0)) + if (1 == argc) { showHelp(); + std::cout << std::endl; + std::cout << "Enter command: "; + + std::string raw_cl; + std::getline(std::cin, raw_cl); + std::istringstream iss(raw_cl); + cl.insert(cl.begin(), std::istream_iterator(iss), std::istream_iterator()); + std::cout << raw_cl << std::endl; } - else if(argc >= 3 && strcmp(argv[1], "udp") == 0) + else { - std::cout << "UDP agent initialization... "; - uint16_t port = parsePort(argv[2]); - eprosima::uxr::UDPServer* udp_server = (argc == 4) //discovery port - ? new eprosima::uxr::UDPServer(port, parsePort(argv[3])) - : new eprosima::uxr::UDPServer(port); - if (udp_server->run()) + for (int i = 1; i < argc; ++i) { - initialized = true; + cl.push_back(argv[i]); } - std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl; } - else if(argc >= 3 && strcmp(argv[1], "tcp") == 0) + + if((1 == cl.size()) && (("-h" == cl[0]) || ("--help" == cl[0]))) + { + showHelp(); + } + else if((2 <= cl.size()) && ("udp" == cl[0])) + { + std::cout << "UDP agent initialization... "; + uint16_t port = parsePort(cl[1]); +#ifdef _WIN32 + server = new eprosima::uxr::UDPServer(port); +#else + server = (3 == cl.size()) //discovery port + ? new eprosima::uxr::UDPServer(port, parsePort(cl[2])) + : new eprosima::uxr::UDPServer(port); +#endif + } + else if((2 <= cl.size()) && ("tcp" == cl[0])) { std::cout << "TCP agent initialization... "; - uint16_t port = parsePort(argv[2]); - eprosima::uxr::TCPServer* tcp_server = (argc == 4) //discovery port - ? new eprosima::uxr::TCPServer(port, parsePort(argv[3])) - : new eprosima::uxr::TCPServer(port); - if (tcp_server->run()) - { - initialized = true; - } - std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl; + uint16_t port = parsePort(cl[1]); +#ifdef _WIN32 + server = new eprosima::uxr::TCPServer(port); +#else + server = (3 == cl.size()) //discovery port + ? new eprosima::uxr::TCPServer(port, parsePort(cl[2])) + : new eprosima::uxr::TCPServer(port); +#endif } - else if(argc == 3 && strcmp(argv[1], "serial") == 0) +#ifndef _WIN32 + else if((2 == cl.size()) && ("serial" == cl[0])) { std::cout << "Serial agent initialization... "; /* Open serial device. */ - int fd = open(argv[2], O_RDWR | O_NOCTTY); + int fd = open(cl[1].c_str(), O_RDWR | O_NOCTTY); if (0 < fd) { struct termios tty_config; @@ -181,18 +179,12 @@ int main(int argc, char** argv) if (0 == tcsetattr(fd, TCSANOW, &tty_config)) { - eprosima::uxr::SerialServer* serial_server = new eprosima::uxr::SerialServer(fd, 0); - if (serial_server->run()) - { - initialized = true; - } + server = new eprosima::uxr::SerialServer(fd, 0); } } } - - std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl; } - else if (argc == 2 && strcmp(argv[1], "pseudo-serial") == 0) + else if ((1 == cl.size()) && ("pseudo-serial" == cl[0])) { std::cout << "Pseudo-Serial initialization... "; @@ -208,78 +200,31 @@ int main(int argc, char** argv) cfmakeraw(&attr); tcflush(fd, TCIOFLUSH); tcsetattr(fd, TCSANOW, &attr); + std::cout << "Device: " << dev << std::endl; } } - - /* Launch server. */ - eprosima::uxr::SerialServer* serial_server = new eprosima::uxr::SerialServer(fd, 0x00); - if (serial_server->run()) - { - initialized = true; - } - - if (initialized) - { - std::cout << "OK" << std::endl; - std::cout << "Device: " << dev << std::endl; - } - else - { - std::cout << "ERROR" << std::endl; - } + server = new eprosima::uxr::SerialServer(fd, 0x00); } +#endif else { initializationError(); } -#elif _WIN32 - (void) argc; - (void) argv; - - std::string server_type = ""; - std::cout << "Select server type [udp|tcp]: "; - std::cin >> server_type; - if ("udp" == server_type) + if (nullptr != server) { - uint16_t port = 0; - std::cout << "Select port: "; - std::cin >> port; - - std::cout << "UDP agent initialization.... "; - eprosima::uxr::UDPServer* udp_server = new eprosima::uxr::UDPServer(port); - if (udp_server->run()) + /* Launch server. */ + if (server->run()) { - initialized = true; + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + server->stop(); } - std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl; - } - else if ("tcp" == server_type) - { - uint16_t port = 0; - std::cout << "Select port: "; - std::cin >> port; - - std::cout << "UDP agent initialization.... "; - eprosima::uxr::TCPServer* tcp_server = new eprosima::uxr::TCPServer(port); - if (tcp_server->run()) + else { - initialized = true; + std::cout << "ERROR" << std::endl; } - std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl; - } - else - { - std::cout << "Error server type" << std::endl; - } - -#endif - - if(initialized) - { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); } return 0;