marker.launch.py added

Author: CamelPoem32

.vscode/settings.json

@@ -13,6 +13,7 @@
         "./src/stingray/src/stingray_utils",
         "./src/stingray/src/stingray_core/src/stingray_core_interfaces",
         "./src/stingray/src/stingray_missions",
+        "./src/marker_finder",
     ],
     "files.associations": {
         "cctype": "cpp",

src/marker_finder/launch/marker.launch.py

@@ -0,0 +1,84 @@
+from pathlib import Path
+
+from ament_index_python import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    calibration_config_dir = Path(get_package_share_directory('welt_cam')) / "configs"
+    camera_calibration_path = calibration_config_dir / "bottom_camera.yaml"
+
+    # bottom camera
+    camera_topic_arg = DeclareLaunchArgument(
+        "camera_bottom_topic_name", default_value='/stingray/topics/camera_bottom'
+    )
+    camera_path_arg = DeclareLaunchArgument(
+        "camera_bottom_path", default_value='/dev/video0'
+    )
+    camera_calibration_path_arg = DeclareLaunchArgument(
+        "camera_bottom_calibration_path", default_value=str(camera_calibration_path)
+    )
+    box_topic_arg = DeclareLaunchArgument(
+        "box_topic_name", default_value='/stingray/topics/marker_box'
+    )
+    debug_arg = DeclareLaunchArgument(
+        "debug", default_value='True'
+    )
+    debug_topic_arg = DeclareLaunchArgument(
+        "debug_image_name", default_value='/stingray/topics/marker_debug_image'
+    )
+    kernel_arg = DeclareLaunchArgument(
+        "kernel", default_value='5'
+    )
+    sigma_arg = DeclareLaunchArgument(
+        "sigma", default_value='3'
+    )
+    closure_arg = DeclareLaunchArgument(
+        "closure", default_value='10'
+    )
+    light_arg = DeclareLaunchArgument(
+        "light", default_value='0.6'
+    )
+    grayness_arg = DeclareLaunchArgument(
+        "grayness", default_value='0.1'
+    )
+ 
+    # load ros config
+    return LaunchDescription([
+        camera_topic_arg,
+        camera_path_arg,
+        camera_calibration_path_arg,
+        box_topic_arg,
+        debug_arg,
+        debug_topic_arg,
+        kernel_arg,
+        sigma_arg,
+        closure_arg,
+        light_arg,
+        grayness_arg,
+        # bottom camera
+        Node(
+            package='marker_finder',
+            executable='marker_detector',
+            name='marker_finder_node',
+            remappings=[
+                ('/image_raw', LaunchConfiguration("camera_bottom_topic")),
+            ],
+            parameters=[
+                {'video_device': LaunchConfiguration("camera_bottom_path")},
+                {'params-file': LaunchConfiguration("camera_bottom_calibration_path")},
+                # {'image_width': 640},
+                # {'image_height': 480},
+                # {'camera_name': 'camera'},
+            ],
+            respawn=True,
+            respawn_delay=1,
+        ),
+    ])

src/marker_finder/marker_finder/marker_detector.py

@@ -20,32 +20,32 @@
 class MarkerDetector(Node):
     def __init__(self):
         super().__init__("marker_finder")
-        self.declare_parameter("~image_topic_name")
-        self.declare_parameter("~box_topic_name")
-        self.declare_parameter("~debug")
-        self.declare_parameter("~debug_image_name")
-        self.declare_parameter("~kernel")
-        self.declare_parameter("~sigma")
-        self.declare_parameter("~closure")
-        self.declare_parameter("~light")
-        self.declare_parameter("~grayness")
+        self.declare_parameter("camera_bottom_topic_name")
+        self.declare_parameter("box_topic_name")
+        self.declare_parameter("debug")
+        self.declare_parameter("debug_image_name")
+        self.declare_parameter("kernel")
+        self.declare_parameter("sigma")
+        self.declare_parameter("closure")
+        self.declare_parameter("light")
+        self.declare_parameter("grayness")
 
         self.image_sub = self.create_subscription(Image, 
-                                                  self.get_parameter("~image_topic_name").get_parameter_value().string_value, 
+                                                  self.get_parameter("image_topic_name").get_parameter_value().string_value, 
                                                   self.image_callback, 
                                                   1)
         self.box_topic = self.create_publisher(ObjectsArray, 
-                                               self.get_parameter("~box_topic_name").get_parameter_value().string_value, 
+                                               self.get_parameter("box_topic_name").get_parameter_value().string_value, 
                                                1)
 
         self.service = self.create_service(detector_toggle_srv, 
                                            "marker_detection_switch", 
                                            self.enable_detector)
         self.detection_enabled = False
 
-        if self.get_parameter("~debug").get_parameter_value().bool_value:
+        if self.get_parameter("debug").get_parameter_value().bool_value:
             self.image_pub = self.create_publisher(Image, 
-                                                   self.get_parameter("~debug_image_name").get_parameter_value().string_value, 
+                                                   self.get_parameter("debug_image_name").get_parameter_value().string_value, 
                                                    1)
 
 

src/marker_finder/package.xml

@@ -7,6 +7,8 @@
   <maintainer email="[email protected]">super_ubuntu22</maintainer>
   <license>TODO: License declaration</license>
 
+  <exec_depend>ros2launch</exec_depend>
+
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>

src/marker_finder/setup.py

@@ -1,4 +1,6 @@
 from setuptools import find_packages, setup
+import os
+from glob import glob
 
 package_name = 'marker_finder'
 
@@ -10,6 +12,7 @@
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name), glob('launch/*.launch.py'))
     ],
     install_requires=['setuptools'],
     zip_safe=True,