mirror of
https://github.com/SHC-ASTRA/rover-ros2.git
synced 2026-02-11 09:20:40 +00:00
Remove mac_headless_pkg
This commit is contained in:
Tristan McGinnis
@@ -1,157 +0,0 @@
|
|||||||
import rclpy
|
|
||||||
from rclpy.node import Node
|
|
||||||
|
|
||||||
import pygame
|
|
||||||
|
|
||||||
import time
|
|
||||||
|
|
||||||
import serial
|
|
||||||
import sys
|
|
||||||
import threading
|
|
||||||
import glob
|
|
||||||
|
|
||||||
from std_msgs.msg import String
|
|
||||||
|
|
||||||
class Mac_Headless(Node):
|
|
||||||
def __init__(self):
|
|
||||||
# Initalize node with name
|
|
||||||
super().__init__("mac_headless_ctrl")
|
|
||||||
|
|
||||||
self.create_timer(0.10, self.send_controls)
|
|
||||||
|
|
||||||
|
|
||||||
# Create a publisher to publish any output the pico sends
|
|
||||||
self.publisher = self.create_publisher(String, '/astra/core/control', 10)
|
|
||||||
|
|
||||||
# Create a subscriber to listen to any commands sent for the pico
|
|
||||||
self.subscriber = self.create_subscription(String, '/astra/core/feedback', self.read_feedback, 10)
|
|
||||||
#self.subscriber
|
|
||||||
|
|
||||||
|
|
||||||
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.send_controls()
|
|
||||||
|
|
||||||
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()
|
|
||||||
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_controls(self):
|
|
||||||
|
|
||||||
for event in pygame.event.get():
|
|
||||||
if event.type == pygame.QUIT:
|
|
||||||
pygame.quit()
|
|
||||||
exit()
|
|
||||||
|
|
||||||
|
|
||||||
#left_x = self.gamepad.get_axis(0)#left x-axis
|
|
||||||
left_y = self.gamepad.get_axis(1)#lext y-axis
|
|
||||||
#left_t = self.gamepad.get_axis(2)#left trigger
|
|
||||||
#right_x = self.gamepad.get_axis(3)#right x-axis
|
|
||||||
right_y = self.gamepad.get_axis(3)#right y-axis
|
|
||||||
right_t = self.gamepad.get_axis(4)#right trigger
|
|
||||||
|
|
||||||
|
|
||||||
if pygame.joystick.get_count() != 0:
|
|
||||||
|
|
||||||
if right_t > 0:#single-stick control mode
|
|
||||||
output = f'ctrl,{round(right_y,2)},{round(right_y,2)}'
|
|
||||||
else:
|
|
||||||
output = f'ctrl,{round(left_y,2)},{round(right_y,2)}'
|
|
||||||
else:
|
|
||||||
output = 'ctrl,0,0' #stop the rover if there is no controller connected
|
|
||||||
|
|
||||||
|
|
||||||
#print(f"[Controls] {output}", end="")
|
|
||||||
self.get_logger().info(f"[Ctrl] {output}")
|
|
||||||
# Create a string message object
|
|
||||||
msg = String()
|
|
||||||
|
|
||||||
# Set message data
|
|
||||||
msg.data = output
|
|
||||||
|
|
||||||
#only publish commands when the values are updated
|
|
||||||
if self.lastMsg != msg:
|
|
||||||
# Publish data
|
|
||||||
self.publisher.publish(msg)
|
|
||||||
|
|
||||||
self.lastMsg = msg
|
|
||||||
#print(f"[Pico] Publishing: {msg}")
|
|
||||||
|
|
||||||
|
|
||||||
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 = Mac_Headless()
|
|
||||||
|
|
||||||
rclpy.spin(node)
|
|
||||||
rclpy.shutdown()
|
|
||||||
|
|
||||||
#tb_bs = BaseStation()
|
|
||||||
#node.run()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
||||||
@@ -1,20 +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>mac_headless_pkg</name>
|
|
||||||
<version>1.0.0</version>
|
|
||||||
<description>(MAC/VM Version) Package used for headless rover control with a controller.</description>
|
|
||||||
<maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
|
|
||||||
<license>All Rights Reserved</license>
|
|
||||||
|
|
||||||
<depend>rclpy</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/mac_headless_pkg
|
|
||||||
[install]
|
|
||||||
install_scripts=$base/lib/mac_headless_pkg
|
|
||||||
@@ -1,26 +0,0 @@
|
|||||||
from setuptools import find_packages, setup
|
|
||||||
|
|
||||||
package_name = 'mac_headless_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 control with a controller',
|
|
||||||
license='All Rights Reserved',
|
|
||||||
tests_require=['pytest'],
|
|
||||||
entry_points={
|
|
||||||
'console_scripts': [
|
|
||||||
'mac_headless = mac_headless_pkg.mac_headless_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