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