Updated to last version of Micoxrcedds Agent. (#3)

uROS_Agent/src/main.cpp

@@ -37,10 +37,15 @@ void showHelp()
 {
     std::cout << "Usage: program <command>" << std::endl;
     std::cout << "List of commands:" << std::endl;
+#ifdef _WIN32
+    std::cout << "    udp <local_port>" << std::endl;
+    std::cout << "    tcp <local_port>" << std::endl;
+#else
     std::cout << "    serial <device_name>" << std::endl;
     std::cout << "    pseudo-serial" << std::endl;
     std::cout << "    udp <local_port> [<discovery_port>]" << std::endl;
     std::cout << "    tcp <local_port> [<discovery_port>]" << 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)
 {
+    eprosima::uxr::Server* server = nullptr;
+    std::vector<std::string> cl(0);
 
-#ifdef __unix__
+    if (1 == argc)
-    char exe_path[200];
+    {
-    ssize_t count = readlink("/proc/self/exe", exe_path, sizeof(exe_path));
+        showHelp();
-    if (count != -1) {
+        std::cout << std::endl;
-        chdir(dirname(exe_path));
+        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<std::string>(iss), std::istream_iterator<std::string>());
+        std::cout << raw_cl << std::endl;
+    }
+    else
+    {
+        for (int i = 1; i < argc; ++i)
+        {
+            cl.push_back(argv[i]);
+        }
     }
-#endif
 
-    bool initialized = false;
+    if((1 == cl.size()) && (("-h" == cl[0]) || ("--help" == cl[0])))
-
-#ifdef __unix__
-    if(argc == 2 && (strcmp(argv[1], "-h") == 0 || strcmp(argv[1], "--help") == 0))
     {
         showHelp();
     }
-    else if(argc >= 3 && strcmp(argv[1], "udp") == 0)
+    else if((2 <= cl.size()) && ("udp" == cl[0]))
     {
         std::cout << "UDP agent initialization... ";
-        uint16_t port = parsePort(argv[2]);
+        uint16_t port = parsePort(cl[1]);
-        eprosima::uxr::UDPServer* udp_server = (argc == 4) //discovery port
+#ifdef _WIN32
-                                             ? new eprosima::uxr::UDPServer(port, parsePort(argv[3]))
+        server = new eprosima::uxr::UDPServer(port);
-                                             : new eprosima::uxr::UDPServer(port);
+#else
-        if (udp_server->run())
+        server = (3 == cl.size()) //discovery port
-        {
+                 ? new eprosima::uxr::UDPServer(port, parsePort(cl[2]))
-            initialized = true;
+                 : new eprosima::uxr::UDPServer(port);
-        }
+#endif
-        std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl;
     }
-    else if(argc >= 3 && strcmp(argv[1], "tcp") == 0)
+    else if((2 <= cl.size()) && ("tcp" == cl[0]))
     {
         std::cout << "TCP agent initialization... ";
-        uint16_t port = parsePort(argv[2]);
+        uint16_t port = parsePort(cl[1]);
-        eprosima::uxr::TCPServer* tcp_server = (argc == 4) //discovery port
+#ifdef _WIN32
-                                             ? new eprosima::uxr::TCPServer(port, parsePort(argv[3]))
+        server = new eprosima::uxr::TCPServer(port);
-                                             : new eprosima::uxr::TCPServer(port);
+#else
-        if (tcp_server->run())
+        server = (3 == cl.size()) //discovery port
-        {
+                 ? new eprosima::uxr::TCPServer(port, parsePort(cl[2]))
-            initialized = true;
+                 : new eprosima::uxr::TCPServer(port);
-        }
+#endif
-        std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl;
     }
-    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);
+                    server = new eprosima::uxr::SerialServer(fd, 0);
-                    if (serial_server->run())
-                    {
-                        initialized = true;
-                    }
                 }
             }
         }
-
-        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,79 +200,32 @@ int main(int argc, char** argv)
                 cfmakeraw(&attr);
                 tcflush(fd, TCIOFLUSH);
                 tcsetattr(fd, TCSANOW, &attr);
+                std::cout << "Device: " << dev << std::endl;
             }
         }
+        server = new eprosima::uxr::SerialServer(fd, 0x00);
+    }
+#endif
+    else
+    {
+        initializationError();
+    }
 
+    if (nullptr != server)
+    {
         /* Launch server. */
-        eprosima::uxr::SerialServer* serial_server = new eprosima::uxr::SerialServer(fd, 0x00);
+        if (server->run())
-        if (serial_server->run())
         {
-            initialized = true;
+            rclcpp::init(argc, argv);
-        }
+            rclcpp::spin(std::make_shared<uros_agent>());
-
+            rclcpp::shutdown();
-        if (initialized)
+            server->stop();
-        {
-            std::cout << "OK" << std::endl;
-            std::cout << "Device: " << dev << std::endl;
         }
         else
         {
             std::cout << "ERROR" << std::endl;
         }
     }
-    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)
-    {
-        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())
-        {
-            initialized = true;
-        }
-        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())
-        {
-            initialized = true;
-        }
-        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<uros_agent>());
-      rclcpp::shutdown();
-    }
 
     return 0;
 }