From 70d8d6565f820a563ef4803871be77329e0a3720 Mon Sep 17 00:00:00 2001
From: Yuki Onishi <yuki.st16@gmail.com>
Date: Fri, 1 Mar 2024 22:24:03 +0900
Subject: [PATCH] Enable to launch with ros 2

- add and install a launch file
- remove ros-dependent argv-s generated by the ros 2 launch system
---
 CMakeLists.txt                    |  5 +++++
 launch/ros2/choreonoid.launch.xml |  5 +++++
 src/node/choreonoid_ros2.cpp      | 22 +++++++++++++++++++++-
 3 files changed, 31 insertions(+), 1 deletion(-)
 create mode 100644 launch/ros2/choreonoid.launch.xml

diff --git a/CMakeLists.txt b/CMakeLists.txt
index d6030f9..453fab5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -62,5 +62,10 @@ if(DEFINED ENV{ROS_VERSION})
     )
   elseif($ENV{ROS_VERSION} EQUAL 2)
     ament_package()
+
+    install(DIRECTORY
+      launch/ros2
+      DESTINATION share/${PROJECT_NAME}/launch
+    )
   endif()
 endif()
diff --git a/launch/ros2/choreonoid.launch.xml b/launch/ros2/choreonoid.launch.xml
new file mode 100644
index 0000000..b872dd1
--- /dev/null
+++ b/launch/ros2/choreonoid.launch.xml
@@ -0,0 +1,5 @@
+<launch>
+  <arg name="choreonoid_arg" default=""/>
+  <node name="choreonoid" pkg="choreonoid_ros" exec="choreonoid"
+        args="$(var choreonoid_arg)" output="screen"/>
+</launch>
diff --git a/src/node/choreonoid_ros2.cpp b/src/node/choreonoid_ros2.cpp
index 3b710dc..65e40ec 100644
--- a/src/node/choreonoid_ros2.cpp
+++ b/src/node/choreonoid_ros2.cpp
@@ -1,15 +1,35 @@
 #include <ament_index_cpp/get_package_share_directory.hpp>
+#include <rclcpp/utilities.hpp>
+
 #include <cnoid/App>
 #include <cnoid/ExecutablePath>
 #include <cnoid/Plugin>
 #include <cnoid/PluginManager>
 #include <cnoid/ProjectManager>
 #include <cnoid/UTF8>
+
+#include <string>
+#include <vector>
 #include <cstdlib>
 
 int main(int argc, char** argv)
 {
-    cnoid::App app(argc, argv, "Choreonoid-ROS2", "Choreonoid");
+    // removes ros-dependent arguments
+    // rclcpp::remove_ros_arguments throws an error if it fails parsing
+    std::vector<std::string> nonRosArgvString = rclcpp::remove_ros_arguments(argc, argv);
+    int nonRosArgc = nonRosArgvString.size();
+    char* nonRosArgv[nonRosArgc];
+    for (int i = 0; i < nonRosArgc; ++i) {
+        nonRosArgv[i] = nonRosArgvString.at(i).data();
+    }
+    if (nonRosArgvString.at(nonRosArgc - 1).empty()) {
+        // ignores the final nonRosArgv
+        // because remove_ros_arguments potentially returns an empty string,
+        // which causes [Warning:  Input file "" was not processed.] on Choreonoid
+        --nonRosArgc;
+    }
+
+    cnoid::App app(nonRosArgc, nonRosArgv, "Choreonoid-ROS2", "Choreonoid");
 
     auto plugin_manager = cnoid::PluginManager::instance();