mirror of
https://github.com/SHC-ASTRA/rover-ros2.git
synced 2026-02-11 09:20:40 +00:00
Finish merge
This commit is contained in:
TristanMcGinnis
@@ -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()
|
||||
@@ -1,21 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>headless_arm_pkg</name>
|
||||
<version>1.0.0</version>
|
||||
<description>Package used for headless rover arm control with a controller</description>
|
||||
<maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
|
||||
<license>All Rights Reserved</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>ros2_interfaces_pkg</depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/headless_arm_pkg
|
||||
[install]
|
||||
install_scripts=$base/lib/headless_arm_pkg
|
||||
@@ -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'
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -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'
|
||||
@@ -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)
|
||||
@@ -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'
|
||||
Reference in New Issue
Block a user