From cc84afb709b3df4713f88253f2e31213119ea803 Mon Sep 17 00:00:00 2001 From: TristanMcGinnis Date: Tue, 18 Feb 2025 18:32:06 +0000 Subject: [PATCH] Finish merge --- .../headless_arm_pkg/__init__.py | 0 .../headless_arm_ctrl_node.py | 258 ------------------ src/headless_arm_pkg/package.xml | 21 -- .../resource/headless_arm_pkg | 0 src/headless_arm_pkg/setup.cfg | 4 - src/headless_arm_pkg/setup.py | 26 -- src/headless_arm_pkg/test/test_copyright.py | 25 -- src/headless_arm_pkg/test/test_flake8.py | 25 -- src/headless_arm_pkg/test/test_pep257.py | 23 -- 9 files changed, 382 deletions(-) delete mode 100644 src/headless_arm_pkg/headless_arm_pkg/__init__.py delete mode 100755 src/headless_arm_pkg/headless_arm_pkg/headless_arm_ctrl_node.py delete mode 100644 src/headless_arm_pkg/package.xml delete mode 100644 src/headless_arm_pkg/resource/headless_arm_pkg delete mode 100644 src/headless_arm_pkg/setup.cfg delete mode 100644 src/headless_arm_pkg/setup.py delete mode 100644 src/headless_arm_pkg/test/test_copyright.py delete mode 100644 src/headless_arm_pkg/test/test_flake8.py delete mode 100644 src/headless_arm_pkg/test/test_pep257.py diff --git a/src/headless_arm_pkg/headless_arm_pkg/__init__.py b/src/headless_arm_pkg/headless_arm_pkg/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/src/headless_arm_pkg/headless_arm_pkg/headless_arm_ctrl_node.py b/src/headless_arm_pkg/headless_arm_pkg/headless_arm_ctrl_node.py deleted file mode 100755 index c10a73d..0000000 --- a/src/headless_arm_pkg/headless_arm_pkg/headless_arm_ctrl_node.py +++ /dev/null @@ -1,258 +0,0 @@ -import rclpy -from rclpy.node import Node - -import pygame - -import time - -import serial -import sys -import threading -import glob -import os - -from std_msgs.msg import String -from ros2_interfaces_pkg.msg import ControllerState -from ros2_interfaces_pkg.msg import ArmManual -from ros2_interfaces_pkg.msg import ArmIK - - -os.environ["SDL_AUDIODRIVER"] = "dummy" # Force pygame to use a dummy audio driver before pygame.init() -os.environ["SDL_VIDEODRIVER"] = "dummy" # Prevents pygame from trying to open a display - -class Headless(Node): - def __init__(self): - # Initalize node with name - super().__init__("headless_arm_ctrl") - - # Depricated, kept temporarily for reference - # self.create_timer(0.20, self.send_controls)#read and send controls - - self.create_timer(0.20, self.send_manual) - - - # Create a publisher to publish any output the pico sends - - # Depricated, kept temporarily for reference - #self.publisher = self.create_publisher(ControllerState, '/astra/arm/control', 10) - self.manual_pub = self.create_publisher(ArmManual, '/arm/control/manual', 10) - - # Create a subscriber to listen to any commands sent for the pico - - # Depricated, kept temporarily for reference - #self.subscriber = self.create_subscription(String, '/astra/arm/feedback', self.read_feedback, 10) - - self.debug_sub = self.create_subscription(String, '/arm/feedback/debug', self.read_feedback, 10) - - - #self.lastMsg = String() #Used to ignore sending controls repeatedly when they do not change - - - # Initialize pygame - pygame.init() - - # Initialize the gamepad module - pygame.joystick.init() - - # Check if any gamepad is connected - if pygame.joystick.get_count() == 0: - print("No gamepad found.") - pygame.quit() - exit() - for event in pygame.event.get(): - if event.type == pygame.QUIT: - pygame.quit() - exit() - - # Initialize the first gamepad, print name to terminal - self.gamepad = pygame.joystick.Joystick(0) - self.gamepad.init() - print(f'Gamepad Found: {self.gamepad.get_name()}') - # - # - - - 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(): - #Check the pico for updates - - self.read_feedback() - if pygame.joystick.get_count() == 0: #if controller disconnected, wait for it to be reconnected - print(f"Gamepad disconnected: {self.gamepad.get_name()}") - - while pygame.joystick.get_count() == 0: - #self.send_controls() #depricated, kept for reference temporarily - self.send_manual() - self.read_feedback() - self.gamepad = pygame.joystick.Joystick(0) - self.gamepad.init() #re-initialized gamepad - print(f"Gamepad reconnected: {self.gamepad.get_name()}") - - - except KeyboardInterrupt: - sys.exit(0) - - - def send_manual(self): - for event in pygame.event.get(): - if event.type == pygame.QUIT: - pygame.quit() - exit() - input = ArmManual() - - dpad_input = self.gamepad.get_hat(0) - input.axis0 = 0 - if dpad_input[0] == 1: - input.axis0 = 1 - elif dpad_input[0] == -1: - input.axis0 = -1 - - input.axis1 = round(self.gamepad.get_axis(0))#left x-axis - input.axis2 = round(self.gamepad.get_axis(1))#left y-axis - input.axis3 = round(self.gamepad.get_axis(4))#right y-axis - - #Temporary, not controlling digit. Awaiting embedded implementation - input.effector_yaw = 0 - input.effector_roll = 0 - input.gripper = 0 - input.linear_actuator = 0 - input.laser = 0 - - - if pygame.joystick.get_count() != 0: - - self.get_logger().info(f"[Ctrl] {input.axis0}, {input.axis1}, {input.axis2}, {input.axis3}\n") - self.manual_pub.publish(input) - else: - pass - - pass - - # Depricated, kept temporarily for reference - # def send_controls(self): - - # for event in pygame.event.get(): - # if event.type == pygame.QUIT: - # pygame.quit() - # exit() - # input = ControllerState() - - # input.lt = self.gamepad.get_axis(2)#left trigger - # input.rt = self.gamepad.get_axis(5)#right trigger - - # #input.lb = self.gamepad.get_button(9)#Value must be converted to bool - # if(self.gamepad.get_button(4)):#left bumper - # input.lb = True - # else: - # input.lb = False - - # #input.rb = self.gamepad.get_button(10)#Value must be converted to bool - # if(self.gamepad.get_button(5)):#right bumper - # input.rb = True - # else: - # input.rb = False - - # #input.plus = self.gamepad.get_button(6)#plus button - # if(self.gamepad.get_button(7)):#plus button - # input.plus = True - # else: - # input.plus = False - - # #input.minus = self.gamepad.get_button(4)#minus button - # if(self.gamepad.get_button(6)):#minus button - # input.minus = True - # else: - # input.minus = False - - # input.ls_x = round(self.gamepad.get_axis(0),2)#left x-axis - # input.ls_y = round(self.gamepad.get_axis(1),2)#left y-axis - # input.rs_x = round(self.gamepad.get_axis(3),2)#right x-axis - # input.rs_y = round(self.gamepad.get_axis(4),2)#right y-axis - - # #input.a = self.gamepad.get_button(1)#A button - # if(self.gamepad.get_button(0)):#A button - # input.a = True - # else: - # input.a = False - # #input.b = self.gamepad.get_button(0)#B button - # if(self.gamepad.get_button(1)):#B button - # input.b = True - # else: - # input.b = False - # #input.x = self.gamepad.get_button(3)#X button - # if(self.gamepad.get_button(2)):#X button - # input.x = True - # else: - # input.x = False - # #input.y = self.gamepad.get_button(2)#Y button - # if(self.gamepad.get_button(3)):#Y button - # input.y = True - # else: - # input.y = False - - - # dpad_input = self.gamepad.get_hat(0)#D-pad input - - # #not using up/down on DPad - # input.d_up = False - # input.d_down = False - - - # if(dpad_input[0] == 1):#D-pad right - # input.d_right = True - # else: - # input.d_right = False - # if(dpad_input[0] == -1):#D-pad left - # input.d_left = True - # else: - # input.d_left = False - - - # if pygame.joystick.get_count() != 0: - - # self.get_logger().info(f"[Ctrl] Updated Controller State\n") - - # self.publisher.publish(input) - # else: - # pass - - - - - - def read_feedback(self, msg): - - # Create a string message object - #msg = String() - - # Set message data - #msg.data = output - - # Publish data - #self.publisher.publish(msg.data) - - print(f"[MCU] {msg.data}", end="") - #print(f"[Pico] Publishing: {msg}") - - - -def main(args=None): - rclpy.init(args=args) - - node = Headless() - - rclpy.spin(node) - rclpy.shutdown() - - #tb_bs = BaseStation() - #node.run() - - -if __name__ == '__main__': - main() diff --git a/src/headless_arm_pkg/package.xml b/src/headless_arm_pkg/package.xml deleted file mode 100644 index 2d5d18a..0000000 --- a/src/headless_arm_pkg/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - headless_arm_pkg - 1.0.0 - Package used for headless rover arm control with a controller - tristan - All Rights Reserved - - rclpy - ros2_interfaces_pkg - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/src/headless_arm_pkg/resource/headless_arm_pkg b/src/headless_arm_pkg/resource/headless_arm_pkg deleted file mode 100644 index e69de29..0000000 diff --git a/src/headless_arm_pkg/setup.cfg b/src/headless_arm_pkg/setup.cfg deleted file mode 100644 index 775bd15..0000000 --- a/src/headless_arm_pkg/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/headless_arm_pkg -[install] -install_scripts=$base/lib/headless_arm_pkg diff --git a/src/headless_arm_pkg/setup.py b/src/headless_arm_pkg/setup.py deleted file mode 100644 index b692426..0000000 --- a/src/headless_arm_pkg/setup.py +++ /dev/null @@ -1,26 +0,0 @@ -from setuptools import find_packages, setup - -package_name = 'headless_arm_pkg' - -setup( - name=package_name, - version='0.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='tristan', - maintainer_email='tristanmcginnis26@gmail.com', - description='Package used for headless rover arm control with a controller', - license='All Rights Reserved', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'headless = headless_arm_pkg.headless_arm_ctrl_node:main' - ], - }, -) diff --git a/src/headless_arm_pkg/test/test_copyright.py b/src/headless_arm_pkg/test/test_copyright.py deleted file mode 100644 index 97a3919..0000000 --- a/src/headless_arm_pkg/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# 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. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/src/headless_arm_pkg/test/test_flake8.py b/src/headless_arm_pkg/test/test_flake8.py deleted file mode 100644 index 27ee107..0000000 --- a/src/headless_arm_pkg/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# 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. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/src/headless_arm_pkg/test/test_pep257.py b/src/headless_arm_pkg/test/test_pep257.py deleted file mode 100644 index b234a38..0000000 --- a/src/headless_arm_pkg/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# 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. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings'