feat: add joystick support

sjtu_drone_bringup/launch/sjtu_drone_bringup.launch.py

@@ -14,33 +14,62 @@
 # limitations under the License.
 
 import os
-import yaml
 
+import yaml
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
-from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
 from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
 
+def get_teleop_controller(context, *_, **kwargs) -> Node:
+    controller = context.launch_configurations["controller"]
+    namespace = kwargs["model_ns"]
+
+    if controller == "joystick":
+        node = Node(
+            package="sjtu_drone_control",
+            executable="teleop_joystick",
+            namespace=namespace,
+            output="screen",
+        )
 
-def generate_launch_description():
+    else:
+        node = Node(
+            package="sjtu_drone_control",
+            executable="teleop",
+            namespace=namespace,
+            output="screen",
+            prefix="xterm -e",
+        )
+
+    return [node]
 
+def generate_launch_description():
     sjtu_drone_bringup_path = get_package_share_directory('sjtu_drone_bringup')
 
     rviz_path = os.path.join(
         sjtu_drone_bringup_path, "rviz", "rviz.rviz"
     )
+
     yaml_file_path = os.path.join(
         get_package_share_directory('sjtu_drone_bringup'),
         'config', 'drone.yaml'
     )
+
     model_ns = "drone"
+
     with open(yaml_file_path, 'r') as f:
         yaml_dict = yaml.load(f, Loader=yaml.FullLoader)
         model_ns = yaml_dict["namespace"]
-
 
     return LaunchDescription([
+        DeclareLaunchArgument(
+            "controller",
+            default_value="keyboard",
+            description="Type of controller: keyboard (default) or joystick",
+        ),
+
         Node(
             package="rviz2",
             executable="rviz2",
@@ -58,10 +87,15 @@ def generate_launch_description():
         ),
 
         Node(
-            package="sjtu_drone_control",
-            executable="teleop",
+            package='joy',
+            executable='joy_node',
+            name='joy',
             namespace=model_ns,
-            output="screen",
-            prefix="xterm -e"
-        )
+            output='screen',
+        ),
+
+        OpaqueFunction(
+            function=get_teleop_controller,
+            kwargs={'model_ns': model_ns},
+        ),
     ])

sjtu_drone_control/package.xml

@@ -11,10 +11,12 @@
   <depend>geometry_msgs</depend>
   <depend>rclpy</depend>
   <depend>std_msgs</depend>
+  <depend>joy</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
-  <test_depend>ament_pep257</test_depend>  <maintainer email="[email protected]">ubuntu</maintainer>
+  <test_depend>ament_pep257</test_depend>
+  <maintainer email="[email protected]">ubuntu</maintainer>
 
   <test_depend>python3-pytest</test_depend>
 

sjtu_drone_control/setup.py

@@ -21,6 +21,7 @@
     entry_points={
         'console_scripts': [
             'teleop = sjtu_drone_control.teleop:main',
+            'teleop_joystick = sjtu_drone_control.teleop_joystick:main',
             'open_loop_control = sjtu_drone_control.open_loop_control:main',
             'drone_position_control = sjtu_drone_control.drone_position_control:main'
         ],

sjtu_drone_control/sjtu_drone_control/teleop_joystick.py

@@ -0,0 +1,105 @@
+#!/usr/bin/env python3
+# Copyright 2023 Georg Novotny
+#
+# Licensed under the GNU GENERAL PUBLIC LICENSE, Version 3.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     https://www.gnu.org/licenses/gpl-3.0.en.html
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import rclpy
+from geometry_msgs.msg import Twist, Vector3
+from rclpy.node import Node
+from sensor_msgs.msg import Joy
+from std_msgs.msg import Empty
+
+
+class TeleopNode(Node):
+    def __init__(self) -> None:
+        super().__init__('teleop_node')
+
+        # Subscribers
+        self.create_subscription(Joy, 'joy', self.joy_callback, 0)
+
+        # Publishers
+        self.cmd_vel_publisher = self.create_publisher(Twist, 'cmd_vel', 10)
+        self.takeoff_publisher = self.create_publisher(Empty, 'takeoff', 10)
+        self.land_publisher = self.create_publisher(Empty, 'land', 10)
+
+    def joy_callback(self, msg: Joy) -> None:
+        """
+        Callback for joystick input
+
+        BUTTONS
+        -------
+        0 	A (CROSS)
+        1 	B (CIRCLE)
+        2 	X (SQUARE)
+        3 	Y (TRIANGLE)
+        4 	BACK (SELECT)
+        5 	GUIDE (Middle/Manufacturer Button)
+        6 	START
+        7 	LEFTSTICK
+        8 	RIGHTSTICK
+        9 	LEFTSHOULDER
+        10 	RIGHTSHOULDER
+        11 	DPAD_UP
+        12 	DPAD_DOWN
+        13 	DPAD_LEFT
+        14 	DPAD_RIGHT
+        15 	MISC1 (Depends on the controller manufacturer, but is usually at a similar location on the controller as back/start)
+        16 	PADDLE1 (Upper left, facing the back of the controller if present)
+        17 	PADDLE2 (Upper right, facing the back of the controller if present)
+        18 	PADDLE3 (Lower left, facing the back of the controller if present)
+        19 	PADDLE4 (Lower right, facing the back of the controller if present)
+        20 	TOUCHPAD (If present. Button status only)
+
+        AXES
+        ----
+        0 	LEFTX
+        1 	LEFTY
+        2 	RIGHTX
+        3 	RIGHTY
+        4 	TRIGGERLEFT
+        5 	TRIGGERRIGHT
+        """
+        linear_vec = Vector3()
+        linear_vec.x = msg.axes[1]
+        linear_vec.y = msg.axes[0]
+        linear_vec.z = msg.axes[3]
+
+        angular_vec = Vector3()
+        angular_vec.z = msg.axes[2]
+
+        self.cmd_vel_publisher.publish(Twist(linear=linear_vec, angular=angular_vec))
+
+        # Handle other keys for different movements
+        if msg.buttons[0] == 1:
+            # Takeoff
+            self.takeoff_publisher.publish(Empty())
+        elif msg.buttons[1] == 1:
+            # Land
+            self.cmd_vel_publisher.publish(Twist())
+            self.land_publisher.publish(Empty())
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    try:
+        teleop_node = TeleopNode()
+        rclpy.spin(teleop_node)
+
+    finally:
+        teleop_node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()