Add log_level

device/joystick/joystick/joystick.py

@@ -12,8 +12,10 @@ def __init__(self):
         super().__init__("joystick")
         self._timer = self.create_timer(0.01, self._timer_callback)
         self._joy_publisher = self.create_publisher(Joy, "joystick", 10)
-        # TODO: ここparameterで変えられるようにしたい
-        self._joystick = pygame.joystick.Joystick(0)
+        # Doing: ここparameterで変えられるようにしたい
+        joystick_id = self.declare_parameter("joystick_id", 0).get_parameter_value().integer_value
+        self._joystick = pygame.joystick.Joystick(joystick_id)
+        self._joystick.init()
         # logging
         name = self._joystick.get_name()
         self.get_logger().info(f"Using controller: {name}")

device/joystick/launch/joystick_launch.py

@@ -1,15 +1,51 @@
+from typing import Any
+
 from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import LaunchConfiguration, PythonExpression
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
-    return LaunchDescription(
-        [
-            Node(
-                package="joystick",
-                executable="joystick",
-                namespace="device",
-                remappings=[("/device/joystick", "/packet/joystick")],
-            ),
-        ],
+def joystick_node(**kwargs: Any) -> Node:
+    kwargs["package"] = "joystick"
+    kwargs["executable"] = "joystick"
+    kwargs["namespace"] = "device"
+    return Node(**kwargs)
+
+def generate_launch_description() -> LaunchDescription:
+    index_arg = DeclareLaunchArgument("index",default_value="-1")
+    log_level_arg = DeclareLaunchArgument(
+        "log_level",
+        default_value="info",
+        choices=["debug","info","warn","error","fatal"],
+    )
+    joystick_id_arg = DeclareLaunchArgument(
+        "joystick_id",
+        default_value="0",
     )
+    index = LaunchConfiguration("index")
+    log_level = LaunchConfiguration("log_level")
+
+    index_specified = PythonExpression([index,">= 0"])
+
+    indexed_joystick = joystick_node(
+        name=["joystick_",index],
+        remappings=[("/device/joystick","/packet/joystick")],
+        ros_arguments=["--log-level",log_level],
+        condition=IfCondition(index_specified),
+    )
+
+    unindexed_joystick = joystick_node(
+        remappings=[("/device/joystick","/packet/joystick")],
+        ros_arguments=["--log-level",log_level],
+        condition=UnlessCondition(index_specified),
+    )
+
+    return LaunchDescription([
+        index_arg,
+        log_level_arg,
+        joystick_id_arg,
+        indexed_joystick,
+        unindexed_joystick
+    ])