Merge pull request #18 from ichiro-its/feature/adapt-main-to-ros2-launch

[Feature - PD-420] Add ROS2 Launch for Ninshiki and Shisen

CMakeLists.txt

@@ -90,6 +90,10 @@ target_link_libraries(${PROJECT_NAME}
 
 install(DIRECTORY "include" DESTINATION ".")
 
+install(DIRECTORY
+  launch
+  DESTINATION "share/${PROJECT_NAME}")
+
 install(TARGETS ${PROJECT_NAME}
   EXPORT export_${PROJECT_NAME}
   ARCHIVE DESTINATION "lib"

launch/ninshiki_cpp_launch.py

@@ -0,0 +1,50 @@
+# Copyright (c) 2024 ICHIRO ITS
+# 
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+# 
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+# 
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+import os
+import socket
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    hostname = socket.gethostname()
+    ninshiki_config_path = os.path.expanduser(f'~/ros2-ws/configuration/{hostname}/color_classifier/')
+    shisen_config_path = os.path.expanduser(f'~/ros2-ws/configuration/{hostname}/camera/')
+
+    return LaunchDescription([
+        Node(
+            package='shisen_cpp',
+            executable='camera',
+            name='camera',
+            output='screen',
+            arguments=[shisen_config_path],
+            respawn=True,
+            respawn_delay=1
+        ),
+        Node(
+            package='ninshiki_cpp',
+            executable='detector',
+            name='detector',
+            output='screen',
+            arguments=[ninshiki_config_path],
+            respawn=True,
+            respawn_delay=1
+        ),
+    ])

src/ninshiki_cpp_detector.cpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2021 ICHIRO ITS
+// Copyright (c) 2021-2024 ICHIRO ITS
 //
 // Permission is hereby granted, free of charge, to any person obtaining a copy
 // of this software and associated documentation files (the "Software"), to deal
@@ -26,7 +26,7 @@
 
 int main(int argc, char ** argv)
 {
-  rclcpp::init(argc, argv);
+  auto args = rclcpp::init_and_remove_ros_arguments(argc, argv);
   shisen_cpp::Options options;
 
   // Default Value
@@ -54,56 +54,58 @@ int main(int argc, char ** argv)
 
   // Handle arguments
   try {
-    if (argc < 2) {
-      std::cerr << "Argument needed!\n\n" << help_message << std::endl;
+    if (args.size() < 2) {
+      RCLCPP_ERROR_STREAM(rclcpp::get_logger("ninshiki_cpp"), "Argument needed!\n\n" << help_message);
       return 1;
     }
     int i = 1;
     int pos = 0;
-    while (i < argc) {
-      std::string arg = argv[i++];
+    while (i < args.size()) {
+      const std::string& arg = args[i++];
       if (arg[0] == '-') {
         if (arg == "-h" || arg == "--help") {
-          std::cout << help_message << std::endl;
+          RCLCPP_INFO_STREAM(rclcpp::get_logger("ninshiki_cpp"), help_message);
           return 1;
         } else if (arg == "--detector") {
-          std::string value = argv[i++];
+          const std::string& value = args[i++];
           if (value == "yolo") {
             detection_method = value;
           } else {
-            std::cout << "Unknown detector `" << arg << "`!\n\n" << help_message << std::endl;
+            RCLCPP_ERROR_STREAM(rclcpp::get_logger("ninshiki_cpp"), "No value provided for `--detector`!\n\n" << help_message);
             return 1;
           }
         } else if (arg == "--GPU") {
-          int value = atoi(argv[i++]);
+          int value = std::stoi(args[i++]);
           if (value == 0 || value == 1) {
             gpu = value;
           } else {
-            std::cout << "Unknown option for GPU `" << arg << "`!\n\n" << help_message << std::endl;
+            RCLCPP_ERROR_STREAM(rclcpp::get_logger("ninshiki_cpp"), "No value provided for `--GPU`!\n\n" << help_message);
             return 1;
           }
         } else if (arg == "--MYRIAD") {
-          int value = atoi(argv[i++]);
+          int value = std::stoi(args[i++]);
           if (value == 0 || value == 1) {
             myriad = value;
           } else {
-            std::cout << "Unknown option for MYRIAD `" << arg << "`!\n\n" <<
-              help_message << std::endl;
+            RCLCPP_ERROR_STREAM(rclcpp::get_logger("ninshiki_cpp"), "No value provided for `--MYRIAD`!\n\n" << help_message);
             return 1;
           }
         } else if (arg == "--frequency") {
-          frequency = atoi(argv[i++]);
+          frequency = std::stoi(args[i++]);
         } else {
-          std::cout << "Unknown argument `" << arg << "`!\n\n" << help_message << std::endl;
+          RCLCPP_ERROR_STREAM(rclcpp::get_logger("ninshiki_cpp"), "Unknown argument `" << arg << "`!\n\n" << help_message);
           return 1;
         }
       } else if (pos == 0) {
         path = arg;
         ++pos;
+      } else {
+        RCLCPP_ERROR_STREAM(rclcpp::get_logger("ninshiki_cpp"), "Unexpected positional argument `" << arg << "`!\n\n" << help_message);
+        return 1;
       }
     }
-  } catch (...) {
-    std::cout << "Invalid arguments!\n\n" << help_message << std::endl;
+  } catch (const std::exception &e) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("ninshiki_cpp"), "Invalid arguments: `" << e.what() << "`!\n\n" << help_message);
     return 1;
   }