From df9202ea8c9764746434c72479ff0db4b7182b48 Mon Sep 17 00:00:00 2001
From: Jose Antonio Moral <joseantoniomoralparras@gmail.com>
Date: Mon, 14 Dec 2020 08:30:18 +0100
Subject: [PATCH] Add ros2 launch capabilities and example launch file (#47)

---
 micro_ros_agent/CMakeLists.txt                |  7 +++
 .../launch/micro_ros_agent_launch.py          | 12 ++++
 micro_ros_agent/src/main.cpp                  | 59 ++++++++++++++++++-
 3 files changed, 77 insertions(+), 1 deletion(-)
 create mode 100644 micro_ros_agent/launch/micro_ros_agent_launch.py

diff --git a/micro_ros_agent/CMakeLists.txt b/micro_ros_agent/CMakeLists.txt
index 9734ffa..d0b63c0 100644
--- a/micro_ros_agent/CMakeLists.txt
+++ b/micro_ros_agent/CMakeLists.txt
@@ -113,6 +113,13 @@ install(
     lib/${PROJECT_NAME}
   )
 
+install(
+  DIRECTORY
+    launch
+  DESTINATION
+    share/${PROJECT_NAME}
+  )
+
 if(UROSAGENT_GENERATE_PROFILE)
   set(_OUTPUT_PATH "${CMAKE_CURRENT_BINARY_DIR}/python")
 
diff --git a/micro_ros_agent/launch/micro_ros_agent_launch.py b/micro_ros_agent/launch/micro_ros_agent_launch.py
new file mode 100644
index 0000000..d9071f0
--- /dev/null
+++ b/micro_ros_agent/launch/micro_ros_agent_launch.py
@@ -0,0 +1,12 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='micro_ros_agent',
+            executable='micro_ros_agent',
+            name='micro_ros_agent',
+            arguments={'udp4 --port 8888 -v6'}
+        )
+    ])
diff --git a/micro_ros_agent/src/main.cpp b/micro_ros_agent/src/main.cpp
index 2e7e88e..c9333ab 100644
--- a/micro_ros_agent/src/main.cpp
+++ b/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;