Merge pull request #1 from SHC-ASTRA/merge-arm-headless

Merge arm headless
This commit is contained in:
Tristan McGinnis
2025-02-18 12:32:44 -06:00
committed by GitHub
15 changed files with 5 additions and 461 deletions

View File

@@ -23,7 +23,7 @@ os.environ["SDL_VIDEODRIVER"] = "dummy" # Prevents pygame from trying to open a
class Headless(Node): class Headless(Node):
def __init__(self): def __init__(self):
# Initalize node with name # Initalize node with name
super().__init__("headless_arm_ctrl") super().__init__("arm_headless")
# Depricated, kept temporarily for reference # Depricated, kept temporarily for reference
# self.create_timer(0.20, self.send_controls)#read and send controls # self.create_timer(0.20, self.send_controls)#read and send controls

View File

@@ -18,7 +18,7 @@ thread = None
class SerialRelay(Node): class SerialRelay(Node):
def __init__(self): def __init__(self):
# Initialize node with name # Initialize node with name
super().__init__("arm_relay") super().__init__("arm_node")
# Create publishers # Create publishers

View File

@@ -1,259 +0,0 @@
import rclpy
from rclpy.node import Node
import serial
import sys
import threading
import glob
import time
from std_msgs.msg import String
from interfaces_pkg.msg import ControllerState
from interfaces_pkg.msg import ArmState
serial_pub = None
thread = None
class SerialRelay(Node):
def __init__(self):
# Initalize node with name
super().__init__("arm_relay")
# Create a publisher to publish any output the mcu sends
self.output_publisher = self.create_publisher(String, '/astra/arm/feedback', 10)#Random String feedback data
self.state_publisher = self.create_publisher(ArmState, '/astra/arm/state', 10)#Standard state feedback data
# Create a subscriber to listen to any commands sent for the mcu
self.subscriber = self.create_subscription(ControllerState, '/astra/arm/control', self.send_controls, 10)
# Loop through all serial devices on the computer to check for the mcu
self.port = None
ports = SerialRelay.list_serial_ports()
for port in ports:
try:
# connect and send a ping command
ser = serial.Serial(port, timeout=1)
ser.write(b"arm,ping\n")
response = ser.read_until("\n")
# if pong is in response, then we are talking with the mcu
if b"pong" in response:
self.port = port
print(f"Found MCU at {self.port}!")
break
except:
pass
if self.port is None:
print("Unable to find MCU... please make sure it is connected.")
#sys.exit(1) TEMPORARY TEST, UNCOMMENT THIS LINE IN THE FUTURE
self.ser = serial.Serial(self.port, 115200)
#self.mutex = threading.Lock()
def run(self):
# This thread makes all the update processes run in the background
global thread
thread = threading.Thread(target=rclpy.spin, args={self}, daemon = True)
thread.start()
try:
while rclpy.ok():
# Check the mcu for updates
#rclpy.spin_once(self, timeout_sec=0.1)
#self.mutex.acquire()
if self.ser.in_waiting:
#self.mutex.release()
self.read_mcu()
else:
time.sleep(0.1)
except KeyboardInterrupt:
#self.mutex.release()
serial_pub.ser.close()
sys.exit(0)
def read_mcu(self):
try:
#self.mutex.acquire()
output = str(self.ser.readline(), "utf8")
if output:
print(f"[MCU] {output}", end="")
# Create a string message object
msg = String()
# Set message data
msg.data = output
# Publish data
self.output_publisher.publish(msg)
#print(f"[MCU] Publishing: {msg}")
except serial.SerialException:
#self.mutex.release()
pass
#finally:
#self.mutex.release()
def send_controls(self, msg):
command = ""
ef_cmd = "" #end effector command to be apended
#self.mutex.acquire()
if(msg.b):#If B button: send ESTOP command
command = "arm,stop\n"
self.ser.write(bytes(command, "utf8"))#Send command to MCU
print(f"[Wrote] {command}", end="")#Echo command to console
#self.mutex.release()#Release mutex lock
return
if(msg.plus):#Turn EF laser on
command = "arm,endEffect,laser,1\n"
self.ser.write(bytes(command, "utf8"))
print(f"[Wrote] {command}", end="")
elif(msg.minus):#Turn EF laser off
command = "arm,endEffect,laser,0\n"
self.ser.write(bytes(command, "utf8"))
print(f"[Wrote] {command}", end="")
if(msg.rb):#If RB button: End Effector control mode
ef_cmd = "arm,endEffect,ctrl,"
if(msg.lt >= 0.5):#If LT button: Open end effector
ef_cmd += "-1,"
elif(msg.rt >= 0.5):#If RT button: Close end effector
ef_cmd += "1,"
else:
ef_cmd += "0,"
if(msg.rs_x < 0): #Check left/stop/right for tilt movement
ef_cmd += "-1,"
elif(msg.rs_x > 0):
ef_cmd += "1,"
else:
ef_cmd += "0,"
pass
if(msg.ls_x < 0): #Check left/stop/right for revolve movement
ef_cmd += "-1"
elif(msg.ls_x > 0):
ef_cmd += "1"
else:
ef_cmd += "0"
command = ef_cmd + "\n"
self.ser.write(bytes(command, "utf8"))
print(f"[Wrote] {command}", end="")
#self.mutex.release()
return
else:
ef_cmd = "arm,endEffect,ctrl,"
if(msg.lt >= 0.5):
ef_cmd += "-1,0,0"
elif(msg.rt >= 0.5):
ef_cmd += "1,0,0"
else:
ef_cmd += "0,0,0"
command = ef_cmd + "\n"
self.ser.write(bytes(command, "utf8"))
print(f"[Wrote] {command}", end="")
if(msg.lb):#If LB button: Manual control mode
#First, ensure control mode is set to manual on the MCU
self.ser.write(bytes("arm,setMode,manual\n", "utf8"))
#print(f"[Wrote] arm,setMode,manual", end="")
command = "arm,man,0.15,"#Set manual control duty cycle statically to 15% for now
if(msg.d_left):#If D-Pad left set axis_1 to -1
command += "-1,"
elif(msg.d_right):#If D-Pad right set axis_0 to 1
command += "1,"
else:
command += "0,"
#Axis_1
if(msg.ls_x < -0.5):#If LS left at least 50% set axis_1 to -1
command += "-1,"
elif(msg.ls_x > 0.5):#If LS right at least 50% set axis_1 to 1
command += "1,"
else:
command += "0,"
#Axis_2
if(msg.ls_y < -0.5):#If LS up at least 50% set axis_2 to 1
command += "1,"
elif(msg.ls_y > 0.5):#If LS down at least 50% set axis_2 to -1
command += "-1,"
else:
command += "0,"
#Axis_3
if(msg.rs_y < -0.5):#If RS up at least 50% set axis_3 to 1
command += "1"
elif(msg.rs_y > 0.5):#If RS down at least 50% set axis_3 to -1
command += "-1"
else:
command += "0"
command += "\n"
self.ser.write(bytes(command, "utf8"))
print(f"[Wrote] {command}", end="")
#self.mutex.release()
return
else:#Else normal (IK) control mode
#First, ensure control mode is set to IK on the MCU
self.ser.write(bytes("arm,setMode,ik\n", "utf8"))
#print(f"[Wrote] arm,setMode,ik", end="")
command = "arm,ik,"
if(msg.d_left):#If D-Pad left set axis zero to -1
command += "-1,"
elif(msg.d_right):#If D-Pad right set axis zero to 1
command += "1,"
else:
command += "0,"
#start with input from controller
coord_x = -1*msg.ls_y #out/in
coord_y = -1*msg.rs_y #up/down
#convert units for ik coordinate offsets
coord_x = round(coord_x * 20.4 * 2, 1) #20.4mm (1 in.) per 1 unit of input. Times 2 for two inches
coord_y = round(coord_y * 20.4 * 2, 1) #
command += f"{coord_x},{coord_y}\n"
self.ser.write(bytes(command, "utf8"))
print(f"[Wrote] {command}", end="")
#self.mutex.release()
return
@staticmethod
def list_serial_ports():
return glob.glob("/dev/tty[A-Za-z]*")
def myexcepthook(type, value, tb):
print("Uncaught exception:", type, value)
serial_pub.ser.close()
def main(args=None):
rclpy.init(args=args)
sys.excepthook = myexcepthook
global serial_pub
serial_pub = SerialRelay()
serial_pub.run()
if __name__ == '__main__':
main()

View File

@@ -16,11 +16,11 @@ setup(
maintainer='tristan', maintainer='tristan',
maintainer_email='tristanmcginnis26@gmail.com', maintainer_email='tristanmcginnis26@gmail.com',
description='TODO: Package description', description='TODO: Package description',
license='TODO: License declaration', license='All Rights Reserved',
tests_require=['pytest'],
entry_points={ entry_points={
'console_scripts': [ 'console_scripts': [
'arm = arm_pkg.arm_relay_node:main' 'arm = arm_pkg.arm_node:main',
'headless = arm_pkg.arm_headless:main'
], ],
}, },
) )

View File

@@ -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'

View File

@@ -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)

View File

@@ -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'

View File

@@ -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>

View File

@@ -1,4 +0,0 @@
[develop]
script_dir=$base/lib/headless_arm_pkg
[install]
install_scripts=$base/lib/headless_arm_pkg

View File

@@ -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'
],
},
)

View File

@@ -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'

View File

@@ -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)

View File

@@ -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'