diff --git a/anchor.service b/anchor.service new file mode 100644 index 0000000..b931e87 --- /dev/null +++ b/anchor.service @@ -0,0 +1,14 @@ +[Unit] +Description=Autostart the anchor node for controlling the rover and its modules +After=systemd-user-sessions.service +Requires=systemd-user-sessions.service + +[Service] +ExecStart=/home/clucky/rover-ros2/auto_start/auto_start_anchor.sh +Restart=always +RestartSec=5 +User=clucky +Environment=PYTHONUNBUFFERED=1 + +[Install] +WantedBy=multi-user.target diff --git a/auto_start/auto_start_anchor.sh b/auto_start/auto_start_anchor.sh new file mode 100755 index 0000000..724062c --- /dev/null +++ b/auto_start/auto_start_anchor.sh @@ -0,0 +1,24 @@ +#!/bin/bash + +# Wait for a network interface to be up (not necessarily online) +while ! ip link show | grep -q "state UP"; do + echo "[INFO] Waiting for active network interface..." + sleep 1 +done + +echo "[INFO] Network interface is up!" + +# Your actual ROS node start command goes here +echo "[INFO] Starting ROS node..." + +# Source ROS 2 Humble setup script +source /opt/ros/humble/setup.bash + +# Source your workspace setup script +source /home/clucky/rover-ros2/install/setup.bash + +# CD to directory +cd /home/clucky/rover-ros2/ + +# Launch the ROS 2 node with the desired mode +ros2 launch rover_launch.py mode:=anchor diff --git a/auto_start/auto_start_core_headless.sh b/auto_start/auto_start_core_headless.sh new file mode 100755 index 0000000..b59126b --- /dev/null +++ b/auto_start/auto_start_core_headless.sh @@ -0,0 +1,24 @@ +#!/bin/bash + +# Wait for a network interface to be up (not necessarily online) +while ! ip link show | grep -q "state UP"; do + echo "[INFO] Waiting for active network interface..." + sleep 1 +done + +echo "[INFO] Network interface is up!" + +# Your actual ROS node start command goes here +echo "[INFO] Starting ROS node..." + +# Source ROS 2 Humble setup script +source /opt/ros/humble/setup.bash + +# Source your workspace setup script +source /home/clucky/rover-ros2/install/setup.bash + +# CD to directory +cd /home/clucky/rover-ros2/ + +# Launch the ROS 2 node +ros2 run core_pkg headless diff --git a/auto_start/auto_start_headless_full.sh b/auto_start/auto_start_headless_full.sh new file mode 100755 index 0000000..488e033 --- /dev/null +++ b/auto_start/auto_start_headless_full.sh @@ -0,0 +1,25 @@ +#!/bin/bash + +# Wait for a network interface to be up (not necessarily online) +while ! ip link show | grep -q "state UP"; do + echo "[INFO] Waiting for active network interface..." + sleep 1 +done + +echo "[INFO] Network interface is up!" + +# Your actual ROS node start command goes here +echo "[INFO] Starting ROS node..." + +# Source ROS 2 Humble setup script +source /opt/ros/humble/setup.bash + +# Source your workspace setup script +source /home/clucky/rover-ros2/install/setup.bash + +# CD to directory +cd /home/clucky/rover-ros2/ + +# Launch the ROS 2 node +ros2 run headless_pkg headless_full + diff --git a/core_headless.service b/core_headless.service new file mode 100644 index 0000000..b43c9a3 --- /dev/null +++ b/core_headless.service @@ -0,0 +1,14 @@ +[Unit] +Description=Autostart headless core node for controlling the rover without a base station +After=systemd-user-sessions.service +Requires=systemd-user-sessions.service + +[Service] +ExecStart=/home/clucky/rover-ros2/auto_start/auto_start_core_headless.sh +Restart=always +RestartSec=10 +User=clucky +Environment=PYTHONUNBUFFERED=1 + +[Install] +WantedBy=multi-user.target diff --git a/headless_full.service b/headless_full.service new file mode 100644 index 0000000..400438d --- /dev/null +++ b/headless_full.service @@ -0,0 +1,14 @@ +[Unit] +Description=Autostart headless core node for controlling the rover without a base station +After=systemd-user-sessions.service +Requires=systemd-user-sessions.service + +[Service] +ExecStart=/home/clucky/rover-ros2/auto_start/auto_start_headless_full.sh +Restart=always +RestartSec=10 +User=clucky +Environment=PYTHONUNBUFFERED=1 + +[Install] +WantedBy=multi-user.target diff --git a/src/headless_pkg/package.xml b/src/headless_pkg/package.xml new file mode 100644 index 0000000..bcab1f8 --- /dev/null +++ b/src/headless_pkg/package.xml @@ -0,0 +1,21 @@ + + + + headless_pkg + 1.0.0 + Headless rover control package to handle command interpretation and embedded interfacing. + David Sharpe + All Rights Reserved + + rclpy + ros2_interfaces_pkg + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/headless_pkg/resource/headless_pkg b/src/headless_pkg/resource/headless_pkg new file mode 100644 index 0000000..e69de29 diff --git a/src/headless_pkg/setup.cfg b/src/headless_pkg/setup.cfg new file mode 100644 index 0000000..b9bc220 --- /dev/null +++ b/src/headless_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/headless_pkg +[install] +install_scripts=$base/lib/headless_pkg diff --git a/src/headless_pkg/setup.py b/src/headless_pkg/setup.py new file mode 100644 index 0000000..3d27cfe --- /dev/null +++ b/src/headless_pkg/setup.py @@ -0,0 +1,24 @@ +from setuptools import find_packages, setup + +package_name = 'headless_pkg' + +setup( + name=package_name, + version='1.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='David Sharpe', + maintainer_email='ds0196@uah.edu', + description='Headless rover control package to handle command interpretation and embedded interfacing.', + license='All Rights Reserved', + entry_points={ + 'console_scripts': [ + "headless_full = src.headless_node:main", + ], + }, +) diff --git a/src/headless_pkg/src/__init__.py b/src/headless_pkg/src/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/headless_pkg/src/headless_node.py b/src/headless_pkg/src/headless_node.py new file mode 100755 index 0000000..4094e60 --- /dev/null +++ b/src/headless_pkg/src/headless_node.py @@ -0,0 +1,217 @@ +import rclpy +from rclpy.node import Node + +import pygame + +import time + +import serial +import sys +import threading +import glob +import os + +import importlib +from std_msgs.msg import String +from ros2_interfaces_pkg.msg import CoreControl, ArmManual + + +os.environ["SDL_VIDEODRIVER"] = "dummy" # Prevents pygame from trying to open a display +os.environ["SDL_AUDIODRIVER"] = "dummy" # Force pygame to use a dummy audio driver before pygame.init() + + +max_speed = 90 #Max speed as a duty cycle percentage (1-100) + +core_stop_msg = CoreControl() +core_stop_msg.left_stick = 0.0 +core_stop_msg.right_stick = 0.0 +core_stop_msg.max_speed = 0 + +arm_stop_msg = ArmManual() +arm_stop_msg.axis0 = 0 +arm_stop_msg.axis1 = 0 +arm_stop_msg.axis2 = 0 +arm_stop_msg.axis3 = 0 +arm_stop_msg.effector_roll = 0 +arm_stop_msg.effector_yaw = 0 +arm_stop_msg.gripper = 0 +arm_stop_msg.linear_actuator = 0 +arm_stop_msg.laser = 0 + +ctrl_mode = "core" + + +class Headless(Node): + def __init__(self): + # Initialize pygame first + pygame.init() + pygame.joystick.init() + + # Wait for a gamepad to be connected + self.gamepad = None + print("Waiting for gamepad connection...") + while pygame.joystick.get_count() == 0: + # Process any pygame events to keep it responsive + for event in pygame.event.get(): + if event.type == pygame.QUIT: + pygame.quit() + sys.exit(0) + time.sleep(1.0) # Check every second + print("No gamepad found. Waiting...") + + # Initialize the gamepad + self.gamepad = pygame.joystick.Joystick(0) + self.gamepad.init() + print(f'Gamepad Found: {self.gamepad.get_name()}') + + # Now initialize the ROS2 node + super().__init__("headless") + self.create_timer(0.15, self.send_controls) + self.core_publisher = self.create_publisher(CoreControl, '/core/control', 10) + self.arm_publisher = self.create_publisher(ArmManual, '/arm/control/manual', 10) + + def run(self): + # This thread makes all the update processes run in the background + thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True) + thread.start() + + try: + while rclpy.ok(): + self.send_controls() + time.sleep(0.1) # Small delay to avoid CPU hogging + except KeyboardInterrupt: + sys.exit(0) + + def send_controls(self): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + pygame.quit() + sys.exit(0) + + # Check if controller is still connected + if pygame.joystick.get_count() == 0: + print("Gamepad disconnected. Exiting...") + # Send one last zero control message + self.core_publisher.publish(core_stop_msg) + self.arm_publisher.publish(arm_stop_msg) + self.get_logger().info("Final stop commands sent. Shutting down.") + # Clean up + pygame.quit() + sys.exit(0) + + + global ctrl_mode + + # Check for control mode change + dpad_input = self.gamepad.get_hat(0) + if dpad_input[1] == 1: + ctrl_mode = "arm" + elif dpad_input[1] == -1: + ctrl_mode = "core" + + + # CORE + if ctrl_mode == "core": + input = CoreControl() + input.max_speed = max_speed + + # Collect controller state + left_stick_y = deadzone(self.gamepad.get_axis(1)) + right_stick_y = deadzone(self.gamepad.get_axis(4)) + right_trigger = deadzone(self.gamepad.get_axis(5)) + + + # Right wheels + input.right_stick = float(round(-1 * right_stick_y, 2)) + + # Left wheels + if right_trigger > 0: + input.left_stick = input.right_stick + else: + input.left_stick = float(round(-1 * left_stick_y, 2)) + + + # Debug + output = f'L: {input.left_stick}, R: {input.right_stick}, M: {max_speed}' + self.get_logger().info(f"[Ctrl] {output}") + + self.core_publisher.publish(input) + self.arm_publisher.publish(arm_stop_msg) + + + # ARM + if ctrl_mode == "arm": + input = ArmManual() + + # Collect controller state + left_stick_x = deadzone(self.gamepad.get_axis(0)) + left_stick_y = deadzone(self.gamepad.get_axis(1)) + left_trigger = deadzone(self.gamepad.get_axis(2)) + right_stick_x = deadzone(self.gamepad.get_axis(3)) + right_stick_y = deadzone(self.gamepad.get_axis(4)) + right_trigger = deadzone(self.gamepad.get_axis(5)) + right_bumper = self.gamepad.get_button(5) + dpad_input = self.gamepad.get_hat(0) + + + # EF Grippers + if left_trigger > 0 and right_trigger > 0: + input.gripper = 0 + elif left_trigger > 0: + input.gripper = -1 + elif right_trigger > 0: + input.gripper = 1 + + # Axis 0 + if dpad_input[0] == 1: + input.axis0 = 1 + elif dpad_input[0] == -1: + input.axis0 = -1 + + + if right_bumper: # Control end effector + + # Effector yaw + if left_stick_x > 0: + input.effector_yaw = 1 + elif left_stick_x < 0: + input.effector_yaw = -1 + + # Effector roll + if right_stick_x > 0: + input.effector_roll = 1 + elif right_stick_x < 0: + input.effector_roll = -1 + + else: # Control arm axis + + # Axis 1 + if abs(left_stick_x) > .15: + input.axis1 = round(left_stick_x) + + # Axis 2 + if abs(left_stick_y) > .15: + input.axis2 = -1 * round(left_stick_y) + + # Axis 3 + if abs(right_stick_y) > .15: + input.axis3 = -1 * round(right_stick_y) + + self.core_publisher.publish(core_stop_msg) + self.arm_publisher.publish(input) + + +def deadzone(value: float, threshold=0.05) -> float: + if abs(value) < threshold: + return 0 + return value + + +def main(args=None): + rclpy.init(args=args) + node = Headless() + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == '__main__': + main()