[Enhancement]: Joystick Support

[winstxnhdw]

Hey, I am thinking of contributing a PR for joystick support, but I'd like to discuss how we should go about it first.

Would you prefer if it was an entirely new launch file that would launch the teleop joystick node, or should we stick with the current launch file but we pass some sort of flag as an argument?

For example,

ros2 launch sjtu_drone_bringup sjtu_drone_bringup.launch.py joystick

I haven't checked if the launch API allows you to pass arguments to the launch file, but I reckon it's likely possible.

[codeautopilot]

Potential solution

To implement joystick support, we will create a new Python script joystick_control.py that handles joystick inputs and publishes commands to control the drone. This script will be integrated into the existing launch system with a conditional argument that allows users to choose between joystick and keyboard teleoperation.

How to implement

joystick_control.py

1. Create joystick_control.py in the sjtu_drone_control/sjtu_drone_control directory.
2. Implement the JoystickControlNode class with subscriptions to the joy topic and publishers for cmd_vel, takeoff, and land.
3. Map joystick inputs to drone commands within the joy_callback function.
4. Add parameterization to allow users to configure joystick mappings.

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Joy
from geometry_msgs.msg import Twist
from std_msgs.msg import Empty

class JoystickControlNode(Node):
    def __init__(self):
        super().__init__('joystick_control_node')
        self.subscription = self.create_subscription(
            Joy,
            'joy',
            self.joy_callback,
            10)
        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)
        # TODO: Add any necessary initialization and parameters

    def joy_callback(self, msg):
        # TODO: Map joystick inputs to drone commands and publish them
        pass

def main(args=None):
    rclpy.init(args=args)
    joystick_control_node = JoystickControlNode()
    rclpy.spin(joystick_control_node)
    joystick_control_node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

sjtu_drone_bringup.launch.py

1. Add the necessary imports for handling launch arguments and conditions.
2. Declare a new launch argument joystick with a default value of 'false'.
3. Retrieve the joystick argument value using LaunchConfiguration.
4. Conditionally include the joystick_control node based on the argument value.
5. Update the LaunchDescription to include the new launch argument and the conditional node.

#!/usr/bin/env python3
# ... License and other comments ...

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch_ros.actions import Node
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch.conditions import IfCondition

def generate_launch_description():
    sjtu_drone_bringup_path = get_package_share_directory('sjtu_drone_bringup')
    # ... other setup ...

    joystick_arg = DeclareLaunchArgument(
        'joystick',
        default_value='false',
        description='Launch joystick node: true/false'
    )
    joystick_enabled = LaunchConfiguration('joystick')

    joystick_node = Node(
        package="sjtu_drone_control",
        executable="joystick_control",  # Replace with the actual name of the joystick node executable
        namespace=model_ns,
        output="screen",
        prefix="xterm -e",
        condition=IfCondition(joystick_enabled)
    )

    return LaunchDescription([
        joystick_arg,
        # ... other nodes ...
        joystick_node,  # Conditionally included
    ])

Make sure to replace "joystick_control" with the actual name of the joystick node executable if it differs.

Click here to create a Pull Request with the proposed solution

Files used for this task:

Changes on sjtu_drone_control/sjtu_drone_control/teleop.py

To add joystick support to the sjtu_drone_control package, you will need to create a new Python script that handles joystick inputs and publishes corresponding commands to control the drone. This script will be similar to teleop.py but will use joystick events instead of keyboard inputs.

Here's a practical proposal for creating the joystick control node:

1. Create a new Python script for joystick control: Name it joystick_control.py. This script will subscribe to joystick events and publish Twist messages to the cmd_vel topic, and Empty messages to the takeoff and land topics, similar to teleop.py.

2. Handle Joystick Events: Use the sensor_msgs.msg.Joy message type to receive joystick inputs. You will need to map joystick axes and buttons to drone movements. For example, one axis can control forward/backward movement, another axis for left/right, and buttons for takeoff and landing.

3. Publish Commands: Based on the joystick inputs, create and publish Twist messages to control the drone's velocity and direction.

4. Parameterize the Node: Allow users to configure the mapping between joystick axes/buttons and drone commands through ROS parameters or a configuration file.

5. Test the Joystick Control: Connect a joystick to your system, run the ROS2 node, and verify that the drone responds correctly to the joystick inputs.

6. Integrate with the Launch File: Once the joystick control node is working, you can modify the sjtu_drone_bringup.launch.py file to include an argument that determines whether to launch the joystick node.

Here is a basic outline of what the joystick_control.py script might look like:

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Joy
from geometry_msgs.msg import Twist
from std_msgs.msg import Empty

class JoystickControlNode(Node):
    def __init__(self):
        super().__init__('joystick_control_node')
        self.subscription = self.create_subscription(
            Joy,
            'joy',
            self.joy_callback,
            10)
        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)
        # TODO: Add any necessary initialization and parameters

    def joy_callback(self, msg):
        # TODO: Map joystick inputs to drone commands and publish them
        pass

def main(args=None):
    rclpy.init(args=args)
    joystick_control_node = JoystickControlNode()
    rclpy.spin(joystick_control_node)
    joystick_control_node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Remember to add the necessary dependencies for joystick support in your package.xml and setup.py files. You may need to depend on sensor_msgs and any other ROS packages that provide joystick interfaces.

Once you have the script ready, you can test it with a joystick to ensure it works as expected. After testing, you can proceed to integrate it with the launch file as described in the task for the sjtu_drone_bringup.launch.py file.

Changes on sjtu_drone_bringup/launch/sjtu_drone_bringup.launch.py

To modify the sjtu_drone_bringup.launch.py file to accept an argument for joystick support, you can follow these steps:

1. Import the necessary module to handle launch arguments:

   from launch.substitutions import LaunchConfiguration
   from launch.actions import DeclareLaunchArgument

2. Declare a new launch argument for the joystick support:

   joystick_arg = DeclareLaunchArgument(
       'joystick',
       default_value='false',
       description='Launch joystick node: true/false'
   )

3. Use the LaunchConfiguration to get the value of the joystick argument:

   joystick_enabled = LaunchConfiguration('joystick')

4. Conditionally include the joystick node based on the argument value:

   joystick_node = Node(
       package="sjtu_drone_control",
       executable="joystick_teleop",  # This is a placeholder name for the joystick node
       namespace=model_ns,
       output="screen",
       prefix="xterm -e",
       condition=IfCondition(joystick_enabled)
   )

5. Add the launch argument and the conditional node to the LaunchDescription:

   return LaunchDescription([
       joystick_arg,
       # ... other nodes ...
       joystick_node,  # Conditionally included
   ])

6. Make sure to import IfCondition from launch.conditions to use it for the conditional node:

   from launch.conditions import IfCondition

Here is the modified sjtu_drone_bringup.launch.py file with the added joystick support argument:

#!/usr/bin/env python3
# ... License and other comments ...

import os
import yaml
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch_ros.actions import Node
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch.conditions import IfCondition

def generate_launch_description():
    sjtu_drone_bringup_path = get_package_share_directory('sjtu_drone_bringup')
    # ... other setup ...

    joystick_arg = DeclareLaunchArgument(
        'joystick',
        default_value='false',
        description='Launch joystick node: true/false'
    )
    joystick_enabled = LaunchConfiguration('joystick')

    joystick_node = Node(
        package="sjtu_drone_control",
        executable="joystick_teleop",  # This is a placeholder name for the joystick node
        namespace=model_ns,
        output="screen",
        prefix="xterm -e",
        condition=IfCondition(joystick_enabled)
    )

    return LaunchDescription([
        joystick_arg,
        Node(
            package="rviz2",
            executable="rviz2",
            name="rviz2",
            arguments=[
                "-d", rviz_path
            ],
            output="screen",
        ),
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(sjtu_drone_bringup_path, 'launch', 'sjtu_drone_gazebo.launch.py')
            )
        ),
        Node(
            package="sjtu_drone_control",
            executable="teleop",
            namespace=model_ns,
            output="screen",
            prefix="xterm -e"
        ),
        joystick_node,  # Conditionally included
    ])

Please note that the executable parameter for the joystick node is set to "joystick_teleop", which is a placeholder. You will need to replace this with the actual name of the joystick control node executable once it is created.

Disclaimer: This comment was entirely generated using AI. Be aware that the information provided may be incorrect.

Current plan usage: 49.24%

Have feedback or need help?
Discord
Documentation
support@codeautopilot.com

[NovoG93]

Hey, I am thinking of contributing a PR for joystick support, but I'd like to discuss how we should go about it first.

Would you prefer if it was an entirely new launch file that would launch the teleop joystick node, or should we stick with the current launch file but we pass some sort of flag as an argument?

For example,

ros2 launch sjtu_drone_bringup sjtu_drone_bringup.launch.py joystick

I haven't checked if the launch API allows you to pass arguments to the launch file, but I reckon it's likely possible.

Hi, that sounds awesome!
I would suggest that the best option would be to keep the current lauch file and add an argument with which it is possible to switch between keyboard (default) and the joystick.
Looking forward to your contribution!

[noraise]

Hi @NovoG93, are there still plans to follow up with this? It looks like @winstxnhdw's PR is ready.

[NovoG93]

Hi @NovoG93, are there still plans to follow up with this? It looks like @winstxnhdw's PR is ready.

Hi, already merged.
Sadly I dont have Joystick to test though

[noraise]

Tested it! Works great!