Add ros2 launch capabilities and example launch file (#47)

micro_ros_agent/src/main.cpp

@@ -18,10 +18,67 @@ int main(int argc, char** argv)
 {
     uros::agent::Agent micro_ros_agent;
 
-    if (!micro_ros_agent.create(argc, argv))
+    /** Bypass '--ros-args' flag, as we use our own CLI parser.
+      * As a workaround for launch files, arguments will be passed from
+      * ros2launch as a single token with spaces,
+      * to preserve the correct argument order; split it here.
+      * TODO(jamoralp): investigate ROS2 tools for properly parsing params from launch
+      **/
+    std::vector<std::string> params;
+    for (int i = 0; i < argc; ++i)
+    {
+        params.emplace_back(std::string(argv[i]));
+    }
+
+    auto it = std::find(params.begin(), params.end(), "--ros-args");
+    if (params.end() != it)
+    {
+        if ((it - params.begin()) != 2)
+        {
+            std::ostringstream ss;
+            ss << "Error: when using ros2 launch, please specify ";
+            ss << "your arguments in a single variable." << std::endl;
+            ss << "Instead of 'arguments: {'udp4', '-p', '8888'}', do ";
+            ss << "'arguments: {'udp4 -p 8888'}" << std::endl;
+
+            std::cerr << ss.str();
+            return 1;
+        }
+        const std::string& agent_args(params.at(1));
+        std::istringstream iss(agent_args);
+        std::vector<std::string> agent_args_split(
+            std::istream_iterator<std::string>{iss},
+            std::istream_iterator<std::string>());
+
+        char** agent_argv = new char*[agent_args_split.size() + 1];
+        agent_argv[0] = argv[0];
+        char** agent_argv_it = &agent_argv[1];
+
+        for (const auto& arg : agent_args_split)
+        {
+            *agent_argv_it = new char[arg.length() + 1];
+            strcpy(*agent_argv_it, arg.c_str());
+            agent_argv_it++;
+        }
+
+        bool success = micro_ros_agent.create(agent_args_split.size() + 1, agent_argv);
+
+        for (size_t i = 1; i <= agent_args_split.size(); ++i)
+        {
+            delete [] agent_argv[i];
+        }
+        delete [] agent_argv;
+
+        if (!success)
+        {
+            return 1;
+        }
+    }
+    else if (!micro_ros_agent.create(argc, argv))
     {
         return 1;
     }
+
     micro_ros_agent.run();
 
     return 0;