Added Joy Controller Teleoperation Node

* Consistent README and licensing

Signed-off-by: Ryan Friedman <[email protected]>

README.md

@@ -52,14 +52,14 @@ To launch rviz and gazebo, run:
 
 ```bash
 cd ~/ros2_ws
-source install/setup.sh
+source install/setup.bash
 ros2 launch ardupilot_gz_bringup iris_maze.launch.py
 ```
 In another terminal, with the world and copter in place, launch cartographer to generate SLAM:
 
 ```bash
 cd ~/ros2_ws
-source install/setup.sh
+source install/setup.bash
 ros2 launch ardupilot_ros cartographer.launch.py
 ```
 
@@ -80,6 +80,22 @@ The parameters above are recommended for SITL. If you plan on using this on a re
 
 Please refer to this link for more information on [Common EKF Sources](https://ardupilot.org/copter/docs/common-ekf-sources.html>) as well as this guide on [GPS / Non-GPS Transitions](https://ardupilot.org/copter/docs/common-non-gps-to-gps.html).
 
+### 2. Joystick Controller
+
+The joystick controller allows you to control ArduPilot through a ROS joy topic.
+
+```bash
+cd ~/ros2_ws
+source install/setup.bash
+ros2 run ardupilot_ros joy_controller
+```
+
+Then run the controller using,
+
+`ros2 run ardupilot_ros joy_controller`
+
+Now, using the keyboard keys you can control the drone.
+
 ## Contribution Guideline
 
 * Ensure the [pre-commit](https://github.com/pre-commit/pre-commit) hooks pass locally before creating your pull request by installing the hooks before committing.

ardupilot_ros/joy_controller.py

@@ -0,0 +1,80 @@
+#!/usr/bin/python3
+
+# Copyright 2023 ArduPilot.org.
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program. If not, see <https://www.gnu.org/licenses/>.
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Joy
+from pynput import keyboard
+
+
+class KeyboardJoyPublisher(Node):
+    def __init__(self):
+        super().__init__("keyboard_joy_publisher")
+        self.publisher_ = self.create_publisher(Joy, "/ap/joy", 10)
+
+        self.axes = [0.0, 0.0, 0.0, 0.0]
+
+        # Create a timer to publish the message every 1 second
+        self.timer = self.create_timer(1, self.publish_joy)
+
+        # Register the keyboard listener
+        self.listener = keyboard.Listener(
+            on_press=self.on_press, on_release=self.on_release
+        )
+        self.listener.start()
+
+    def on_press(self, key):
+        if key == "w":
+            self.axes[3] = 1.0
+        elif key == "a":
+            self.axes[1] = -1.0
+        elif key == "s":
+            self.axes[2] = 1.0
+        elif key == "d":
+            self.axes[0] = 1.0
+
+    def on_release(self, key):
+        if key == "w":
+            self.axes[3] = 0.0
+        elif key == "a":
+            self.axes[1] = 0.0
+        elif key == "s":
+            self.axes[2] = 0.0
+        elif key == "d":
+            self.axes[0] = 0.0
+
+    def publish_joy(self):
+        joy_msg = Joy()
+        joy_msg.axes = self.axes
+
+        now = self.get_clock().now()
+        joy_msg.header.stamp = now.to_msg()
+        joy_msg.header.stamp.nanosec = now.nanoseconds % 1000000000
+
+        self.publisher_.publish(joy_msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    keyboard_joy_publisher = KeyboardJoyPublisher()
+    rclpy.spin(keyboard_joy_publisher)
+    keyboard_joy_publisher.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

package.xml

@@ -6,6 +6,7 @@
   <description>The ardupilot ROS 2 use cases package</description>
   <maintainer email="[email protected]">Pedro Fuoco</maintainer>
   <license>GPLv3+</license>
+  <exec_depend>pynput</exec_depend>
   <exec_depend>ament_index_python</exec_depend>
   <exec_depend>ardupilot_gz_description</exec_depend>
   <exec_depend>ardupilot_gz_gazebo</exec_depend>

setup.py

@@ -32,6 +32,6 @@
     license="GPLv3+",
     tests_require=["pytest"],
     entry_points={
-        "console_scripts": [],
+        "console_scripts": ["joy_controller=ardupilot_ros.joy_controller:main"],
     },
 )