Add keyboard control functionality and related launch files

launch/keyboard.launch.py

@@ -0,0 +1,15 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    # Keyboard conversion node
+    start_keyboard_conv_node = Node(
+        package='joy_conv',
+        executable='keyboard_conv',
+        name='keyboard_conv',
+    )
+
+    return LaunchDescription([
+        start_keyboard_conv_node
+    ])

launch/simulation/hyflex_sim.launch.py

@@ -1,6 +1,9 @@
 import os
 import launch
 from launch import LaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.event_handlers import OnProcessExit
+from launch.events import Shutdown
 from ament_index_python.packages import get_package_share_directory
 from webots_ros2_driver.webots_launcher import WebotsLauncher
 from webots_ros2_driver.webots_controller import WebotsController
@@ -21,13 +24,21 @@ def generate_launch_description():
         ]
     )
 
+    # Include keyboard launch
+    keyboard_launch = launch.actions.IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(os.path.dirname(__file__), '..', 'keyboard.launch.py')
+        )
+    )
+
     return LaunchDescription([
         webots,
         hyflex_driver,
+        keyboard_launch,
         launch.actions.RegisterEventHandler(
-            event_handler=launch.event_handlers.OnProcessExit(
+            event_handler=OnProcessExit(
                 target_action=webots,
-                on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
+                on_exit=[launch.actions.EmitEvent(event=Shutdown())],
             )
         )
     ])

scripts/teleop_keyboard.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+# Script to run teleop_twist_keyboard with remapped topic for keyboard teleop
+
+source /opt/ros/jazzy/setup.bash
+# Assuming the workspace is sourced elsewhere, or add source /path/to/workspace/install/setup.bash if needed
+
+ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r /cmd_vel:=/keyboard_cmd_vel

src/joy_conv/joy_conv/keyboard_conv.py

@@ -0,0 +1,57 @@
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Float32, Int8
+from geometry_msgs.msg import Twist
+from utilities.tools import Tools
+
+
+class KeyboardConv(Node):
+
+    def __init__(self):
+        super().__init__('keyboard_conv')
+
+        self.current_pivot_pos = 0
+
+        self.speed_publisher = self.create_publisher(Twist, '/cmd_vel', 10)
+        self.pivot_publisher = self.create_publisher(Int8, '/vehicle/pivot', 10)
+        self.plow_publisher = self.create_publisher(Twist, '/vehicle/plow', 10)
+
+        self.pivot_sub = self.create_subscription(Float32, '/sensor/pivot', self.update_pivot, 10)
+        self.twist_sub = self.create_subscription(Twist, '/keyboard_cmd_vel', self.twist_callback, 10)
+
+    def update_pivot(self, msg: Float32):
+        self.current_pivot_pos = Tools.potentiometer_to_degrees(msg.data)
+
+    def twist_callback(self, msg: Twist):
+        self.pivot_publisher.publish(self.calculate_pivot(msg))
+        self.speed_publisher.publish(self.calculate_speed(msg))
+        self.plow_publisher.publish(self.calculate_plow(msg))
+
+    def calculate_pivot(self, twist_msg: Twist) -> Int8:
+        msg = Int8()
+        msg.data = -round(twist_msg.angular.z)
+        return msg
+
+    def calculate_speed(self, twist_msg: Twist) -> Twist:
+        msg = Twist()
+        msg.linear.x = twist_msg.linear.x
+        msg.angular.z = 0.0  # Angular z is used for pivot
+        return msg
+
+    def calculate_plow(self, twist_msg: Twist) -> Twist:
+        msg = Twist()
+        # No plow control from keyboard teleop
+        return msg
+
+def main():
+    rclpy.init()
+
+    node = KeyboardConv()
+    rclpy.spin(node)
+
+    # Destroy the node explicitly
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/joy_conv/setup.py

@@ -20,7 +20,8 @@
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
-            'joy_conv = joy_conv.joy_conv:main'
+            'joy_conv = joy_conv.joy_conv:main',
+            'keyboard_conv = joy_conv.keyboard_conv:main',
         ],
     },
 )