Enable to launch with ros 2

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()

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>

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();