aaron/rover-ros2
1
0
Fork 0
mirror of https://github.com/SHC-ASTRA/rover-ros2.git synced 2026-02-11 09:20:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

reformat with black

Browse Source
This commit is contained in:
ryleu
2025-11-06 19:10:21 -06:00
parent b670bc2eda
commit c107b82a8d
15 changed files with 686 additions and 519 deletions
Download Patch File Download Diff File Expand all files Collapse all files

75
src/anchor_pkg/anchor_pkg/anchor_node.py
View File

@@ -38,6 +38,8 @@ Subscribers:
* /anchor/to_vic/relay_string
- Publish raw strings to this topic to send directly to the MCU for debugging
"""
class SerialRelay(Node):
def __init__(self):
# Initalize node with name
@@ -78,26 +80,40 @@ class SerialRelay(Node):
atexit.register(self.cleanup)
# New pub/sub with VicCAN
self.fromvic_debug_pub_ = self.create_publisher(String, '/anchor/from_vic/debug', 20)
self.fromvic_core_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/core', 20)
self.fromvic_arm_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/arm', 20)
self.fromvic_bio_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/bio', 20)
self.mock_mcu_sub_ = self.create_subscription(String, '/anchor/from_vic/mock_mcu', self.on_mock_fromvic, 20)
self.tovic_sub_ = self.create_subscription(VicCAN, '/anchor/to_vic/relay', self.on_relay_tovic_viccan, 20)
self.tovic_debug_sub_ = self.create_subscription(String, '/anchor/to_vic/relay_string', self.on_relay_tovic_string, 20)
self.fromvic_debug_pub_ = self.create_publisher(
String, "/anchor/from_vic/debug", 20
)
self.fromvic_core_pub_ = self.create_publisher(
VicCAN, "/anchor/from_vic/core", 20
)
self.fromvic_arm_pub_ = self.create_publisher(
VicCAN, "/anchor/from_vic/arm", 20
)
self.fromvic_bio_pub_ = self.create_publisher(
VicCAN, "/anchor/from_vic/bio", 20
)
self.mock_mcu_sub_ = self.create_subscription(
String, "/anchor/from_vic/mock_mcu", self.on_mock_fromvic, 20
)
self.tovic_sub_ = self.create_subscription(
VicCAN, "/anchor/to_vic/relay", self.on_relay_tovic_viccan, 20
)
self.tovic_debug_sub_ = self.create_subscription(
String, "/anchor/to_vic/relay_string", self.on_relay_tovic_string, 20
)
# Create publishers
self.arm_pub = self.create_publisher(String, '/anchor/arm/feedback', 10)
self.core_pub = self.create_publisher(String, '/anchor/core/feedback', 10)
self.bio_pub = self.create_publisher(String, '/anchor/bio/feedback', 10)
self.arm_pub = self.create_publisher(String, "/anchor/arm/feedback", 10)
self.core_pub = self.create_publisher(String, "/anchor/core/feedback", 10)
self.bio_pub = self.create_publisher(String, "/anchor/bio/feedback", 10)
self.debug_pub = self.create_publisher(String, '/anchor/debug', 10)
self.debug_pub = self.create_publisher(String, "/anchor/debug", 10)
# Create a subscriber
self.relay_sub = self.create_subscription(String, '/anchor/relay', self.on_relay_tovic_string, 10)
self.relay_sub = self.create_subscription(
String, "/anchor/relay", self.on_relay_tovic_string, 10
)
def run(self):
# This thread makes all the update processes run in the background
@@ -125,9 +141,13 @@ class SerialRelay(Node):
self.debug_pub.publish(msg)
if output.startswith("can_relay_fromvic,core"):
self.core_pub.publish(msg)
elif output.startswith("can_relay_fromvic,arm") or output.startswith("can_relay_fromvic,digit"): # digit for voltage readings
elif output.startswith("can_relay_fromvic,arm") or output.startswith(
"can_relay_fromvic,digit"
): # digit for voltage readings
self.arm_pub.publish(msg)
if output.startswith("can_relay_fromvic,citadel") or output.startswith("can_relay_fromvic,digit"): # digit for SHT sensor
if output.startswith("can_relay_fromvic,citadel") or output.startswith(
"can_relay_fromvic,digit"
): # digit for SHT sensor
self.bio_pub.publish(msg)
# msg = String()
# msg.data = output
@@ -152,13 +172,11 @@ class SerialRelay(Node):
# self.ser.close()
# exit(1)
def on_mock_fromvic(self, msg: String):
"""For testing without an actual MCU, publish strings here as if they came from an MCU"""
# self.get_logger().info(f"Got command from mock MCU: {msg}")
self.relay_fromvic(msg.data)
def on_relay_tovic_viccan(self, msg: VicCAN):
"""Relay a VicCAN message to the MCU"""
output: str = f"can_relay_tovic,{msg.mcu_name},{msg.command_id}"
@@ -181,7 +199,9 @@ class SerialRelay(Node):
malformed_reason: str = ""
if len(parts) < 3 or len(parts) > 7:
malformed = True
malformed_reason = f"invalid argument count (expected [3,7], got {len(parts)})"
malformed_reason = (
f"invalid argument count (expected [3,7], got {len(parts)})"
)
elif parts[1] not in ["core", "arm", "digit", "citadel", "broadcast"]:
malformed = True
malformed_reason = f"invalid mcu_name '{parts[1]}'"
@@ -198,7 +218,9 @@ class SerialRelay(Node):
break
if malformed:
self.get_logger().warning(f"Ignoring malformed from_vic message: '{msg.strip()}'; reason: {malformed_reason}")
self.get_logger().warning(
f"Ignoring malformed from_vic message: '{msg.strip()}'; reason: {malformed_reason}"
)
return
# Have valid VicCAN message
@@ -208,7 +230,9 @@ class SerialRelay(Node):
output.command_id = int(parts[2])
if len(parts) > 3:
output.data = [float(x) for x in parts[3:]]
output.header = Header(stamp=self.get_clock().now().to_msg(), frame_id="from_vic")
output.header = Header(
stamp=self.get_clock().now().to_msg(), frame_id="from_vic"
)
# self.get_logger().info(f"Relaying from MCU: {output}")
if output.mcu_name == "core":
@@ -218,7 +242,6 @@ class SerialRelay(Node):
elif output.mcu_name == "citadel" or output.mcu_name == "digit":
self.fromvic_bio_pub_.publish(output)
def on_relay_tovic_string(self, msg: String):
"""Relay a raw string message to the MCU for debugging"""
message = msg.data
@@ -234,6 +257,7 @@ class SerialRelay(Node):
if self.ser.is_open:
self.ser.close()
def myexcepthook(type, value, tb):
print("Uncaught exception:", type, value)
if serial_pub:
@@ -249,7 +273,10 @@ def main(args=None):
serial_pub = SerialRelay()
serial_pub.run()
if __name__ == '__main__':
if __name__ == "__main__":
# signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0)) # Catch Ctrl+Z and exit cleanly
signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0)) # Catch termination signals and exit cleanly
signal.signal(
signal.SIGTERM, lambda signum, frame: sys.exit(0)
) # Catch termination signals and exit cleanly
main()

134
src/anchor_pkg/launch/rover.launch.py
View File

@@ -2,114 +2,120 @@
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir, PathJoinSubstitution
from launch.substitutions import (
LaunchConfiguration,
ThisLaunchFileDir,
PathJoinSubstitution,
)
from launch_ros.actions import Node
# Prevent making __pycache__ directories
from sys import dont_write_bytecode
dont_write_bytecode = True
def launch_setup(context, *args, **kwargs):
# Retrieve the resolved value of the launch argument 'mode'
mode = LaunchConfiguration('mode').perform(context)
mode = LaunchConfiguration("mode").perform(context)
nodes = []
if mode == 'anchor':
if mode == "anchor":
# Launch every node and pass "anchor" as the parameter
nodes.append(
Node(
package='arm_pkg',
executable='arm', # change as needed
name='arm',
output='both',
parameters=[{'launch_mode': mode}],
on_exit=Shutdown()
package="arm_pkg",
executable="arm", # change as needed
name="arm",
output="both",
parameters=[{"launch_mode": mode}],
on_exit=Shutdown(),
)
)
nodes.append(
Node(
package='core_pkg',
executable='core', # change as needed
name='core',
output='both',
parameters=[{'launch_mode': mode}],
on_exit=Shutdown()
package="core_pkg",
executable="core", # change as needed
name="core",
output="both",
parameters=[{"launch_mode": mode}],
on_exit=Shutdown(),
)
)
nodes.append(
Node(
package='core_pkg',
executable='ptz', # change as needed
name='ptz',
output='both'
package="core_pkg",
executable="ptz", # change as needed
name="ptz",
output="both",
# Currently don't shutdown all nodes if the PTZ node fails, as it is not critical
# on_exit=Shutdown() # Uncomment if you want to shutdown on PTZ failure
)
)
nodes.append(
Node(
package='bio_pkg',
executable='bio', # change as needed
name='bio',
output='both',
parameters=[{'launch_mode': mode}],
on_exit=Shutdown()
package="bio_pkg",
executable="bio", # change as needed
name="bio",
output="both",
parameters=[{"launch_mode": mode}],
on_exit=Shutdown(),
)
)
nodes.append(
Node(
package='anchor_pkg',
executable='anchor', # change as needed
name='anchor',
output='both',
parameters=[{'launch_mode': mode}],
on_exit=Shutdown()
package="anchor_pkg",
executable="anchor", # change as needed
name="anchor",
output="both",
parameters=[{"launch_mode": mode}],
on_exit=Shutdown(),
)
)
elif mode in ['arm', 'core', 'bio', 'ptz']:
elif mode in ["arm", "core", "bio", "ptz"]:
# Only launch the node corresponding to the provided mode.
if mode == 'arm':
if mode == "arm":
nodes.append(
Node(
package='arm_pkg',
executable='arm',
name='arm',
output='both',
parameters=[{'launch_mode': mode}],
on_exit=Shutdown()
package="arm_pkg",
executable="arm",
name="arm",
output="both",
parameters=[{"launch_mode": mode}],
on_exit=Shutdown(),
)
)
elif mode == 'core':
elif mode == "core":
nodes.append(
Node(
package='core_pkg',
executable='core',
name='core',
output='both',
parameters=[{'launch_mode': mode}],
on_exit=Shutdown()
package="core_pkg",
executable="core",
name="core",
output="both",
parameters=[{"launch_mode": mode}],
on_exit=Shutdown(),
)
)
elif mode == 'bio':
elif mode == "bio":
nodes.append(
Node(
package='bio_pkg',
executable='bio',
name='bio',
output='both',
parameters=[{'launch_mode': mode}],
on_exit=Shutdown()
package="bio_pkg",
executable="bio",
name="bio",
output="both",
parameters=[{"launch_mode": mode}],
on_exit=Shutdown(),
)
)
elif mode == 'ptz':
elif mode == "ptz":
nodes.append(
Node(
package='core_pkg',
executable='ptz',
name='ptz',
output='both',
package="core_pkg",
executable="ptz",
name="ptz",
output="both",
on_exit=Shutdown(), # on fail, shutdown if this was the only node to be launched
)
)
@@ -119,14 +125,12 @@ def launch_setup(context, *args, **kwargs):
return nodes
def generate_launch_description():
declare_arg = DeclareLaunchArgument(
'mode',
default_value='anchor',
description='Launch mode: arm, core, bio, anchor, or ptz'
"mode",
default_value="anchor",
description="Launch mode: arm, core, bio, anchor, or ptz",
)
return LaunchDescription([
declare_arg,
OpaqueFunction(function=launch_setup)
])
return LaunchDescription([declare_arg, OpaqueFunction(function=launch_setup)])

27
src/anchor_pkg/setup.py
View File

@@ -2,27 +2,24 @@ from setuptools import find_packages, setup
from os import path
from glob import glob
package_name = 'anchor_pkg'
package_name = "anchor_pkg"
setup(
name=package_name,
version='0.0.0',
packages=find_packages(exclude=['test']),
version="0.0.0",
packages=find_packages(exclude=["test"]),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
(path.join("share", package_name), ['package.xml']),
(path.join("share", package_name, "launch"), glob("launch/*"))
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
(path.join("share", package_name), ["package.xml"]),
(path.join("share", package_name, "launch"), glob("launch/*")),
],
install_requires=['setuptools'],
install_requires=["setuptools"],
zip_safe=True,
maintainer='tristan',
maintainer_email='tristanmcginnis26@gmail.com',
description='Anchor node used to run all modules through a single modules MCU/Computer. Commands to all modules will be relayed through CAN',
license='All Rights Reserved',
maintainer="tristan",
maintainer_email="tristanmcginnis26@gmail.com",
description="Anchor node used to run all modules through a single modules MCU/Computer. Commands to all modules will be relayed through CAN",
license="All Rights Reserved",
entry_points={
'console_scripts': [
"anchor = anchor_pkg.anchor_node:main"
],
"console_scripts": ["anchor = anchor_pkg.anchor_node:main"],
},
)

12
src/arm_pkg/arm_pkg/arm_node.py
View File

@@ -64,7 +64,9 @@ class SerialRelay(Node):
"Wrist-EF_Roll_Joint",
"Gripper_Slider_Left",
]
self.joint_state.position = [0.0] * len(self.joint_state.name) # Initialize with zeros
self.joint_state.position = [0.0] * len(
self.joint_state.name
) # Initialize with zeros
self.joint_command_sub = self.create_subscription(
JointState, "/joint_commands", self.joint_command_callback, 10
@@ -233,8 +235,12 @@ class SerialRelay(Node):
if len(parts) >= 4:
self.digit_feedback.wrist_angle = float(parts[3])
# self.digit_feedback.wrist_roll = float(parts[4])
self.joint_state.position[4] = math.radians(float(parts[4])) # Wrist roll
self.joint_state.position[5] = math.radians(float(parts[3])) # Wrist yaw
self.joint_state.position[4] = math.radians(
float(parts[4])
) # Wrist roll
self.joint_state.position[5] = math.radians(
float(parts[3])
) # Wrist yaw
else:
return

27
src/arm_pkg/setup.py
View File

@@ -2,27 +2,26 @@ from setuptools import find_packages, setup
import os
from glob import glob
package_name = 'arm_pkg'
package_name = "arm_pkg"
setup(
name=package_name,
version='1.0.0',
packages=find_packages(exclude=['test']),
version="1.0.0",
packages=find_packages(exclude=["test"]),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml'])
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
("share/" + package_name, ["package.xml"]),
],
install_requires=['setuptools'],
install_requires=["setuptools"],
zip_safe=True,
maintainer='tristan',
maintainer_email='tristanmcginnis26@gmail.com',
description='TODO: Package description',
license='All Rights Reserved',
maintainer="tristan",
maintainer_email="tristanmcginnis26@gmail.com",
description="TODO: Package description",
license="All Rights Reserved",
entry_points={
'console_scripts': [
'arm = arm_pkg.arm_node:main',
'headless = arm_pkg.arm_headless:main'
"console_scripts": [
"arm = arm_pkg.arm_node:main",
"headless = arm_pkg.arm_headless:main",
],
},
)

85
src/bio_pkg/bio_pkg/bio_node.py
View File

@@ -15,43 +15,48 @@ from ros2_interfaces_pkg.msg import BioFeedback
serial_pub = None
thread = None
class SerialRelay(Node):
def __init__(self):
# Initialize node
super().__init__("bio_node")
# Get launch mode parameter
self.declare_parameter('launch_mode', 'bio')
self.launch_mode = self.get_parameter('launch_mode').value
self.declare_parameter("launch_mode", "bio")
self.launch_mode = self.get_parameter("launch_mode").value
self.get_logger().info(f"bio launch_mode is: {self.launch_mode}")
# Create publishers
self.debug_pub = self.create_publisher(String, '/bio/feedback/debug', 10)
self.feedback_pub = self.create_publisher(BioFeedback, '/bio/feedback', 10)
self.debug_pub = self.create_publisher(String, "/bio/feedback/debug", 10)
self.feedback_pub = self.create_publisher(BioFeedback, "/bio/feedback", 10)
# Create subscribers
self.control_sub = self.create_subscription(BioControl, '/bio/control', self.send_control, 10)
self.control_sub = self.create_subscription(
BioControl, "/bio/control", self.send_control, 10
)
# Create a publisher for telemetry
self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback)
# Topics used in anchor mode
if self.launch_mode == 'anchor':
self.anchor_sub = self.create_subscription(String, '/anchor/bio/feedback', self.anchor_feedback, 10)
self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
if self.launch_mode == "anchor":
self.anchor_sub = self.create_subscription(
String, "/anchor/bio/feedback", self.anchor_feedback, 10
)
self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
self.bio_feedback = BioFeedback()
# Search for ports IF in 'arm' (standalone) and not 'anchor' mode
if self.launch_mode == 'bio':
if self.launch_mode == "bio":
# Loop through all serial devices on the computer to check for the MCU
self.port = None
for i in range(2):
try:
# connect and send a ping command
set_port = '/dev/ttyACM0' #MCU is controlled through GPIO pins on the PI
set_port = (
"/dev/ttyACM0" # MCU is controlled through GPIO pins on the PI
)
ser = serial.Serial(set_port, 115200, timeout=1)
# print(f"Checking port {port}...")
ser.write(b"ping\n")
@@ -66,7 +71,9 @@ class SerialRelay(Node):
pass
if self.port is None:
self.get_logger().info("Unable to find MCU... please make sure it is connected.")
self.get_logger().info(
"Unable to find MCU... please make sure it is connected."
)
time.sleep(1)
sys.exit(1)
@@ -82,7 +89,7 @@ class SerialRelay(Node):
try:
while rclpy.ok():
if self.launch_mode == 'bio':
if self.launch_mode == "bio":
if self.ser.in_waiting:
self.read_mcu()
else:
@@ -92,7 +99,6 @@ class SerialRelay(Node):
finally:
self.cleanup()
# Currently will just spit out all values over the /arm/feedback/debug topic as strings
def read_mcu(self):
try:
@@ -123,26 +129,41 @@ class SerialRelay(Node):
def send_ik(self, msg):
pass
def send_control(self, msg: BioControl):
# CITADEL Control Commands
################
# Chem Pumps, only send if not zero
if msg.pump_id != 0:
command = "can_relay_tovic,citadel,27," + str(msg.pump_id) + "," + str(msg.pump_amount) + "\n"
command = (
"can_relay_tovic,citadel,27,"
+ str(msg.pump_id)
+ ","
+ str(msg.pump_amount)
+ "\n"
)
self.send_cmd(command)
# Fans, only send if not zero
if msg.fan_id != 0:
command = "can_relay_tovic,citadel,40," + str(msg.fan_id) + "," + str(msg.fan_duration) + "\n"
command = (
"can_relay_tovic,citadel,40,"
+ str(msg.fan_id)
+ ","
+ str(msg.fan_duration)
+ "\n"
)
self.send_cmd(command)
# Servos, only send if not zero
if msg.servo_id != 0:
command = "can_relay_tovic,citadel,25," + str(msg.servo_id) + "," + str(int(msg.servo_state)) + "\n"
command = (
"can_relay_tovic,citadel,25,"
+ str(msg.servo_id)
+ ","
+ str(int(msg.servo_state))
+ "\n"
)
self.send_cmd(command)
# LSS (SCYTHE)
command = "can_relay_tovic,citadel,24," + str(msg.bio_arm) + "\n"
# self.send_cmd(command)
@@ -151,7 +172,6 @@ class SerialRelay(Node):
command += "can_relay_tovic,citadel,26," + str(msg.vibration_motor) + "\n"
# self.send_cmd(command)
# FAERIE Control Commands
################
@@ -169,14 +189,14 @@ class SerialRelay(Node):
command += "can_relay_tovic,digit,42," + str(msg.drill_arm) + "\n"
self.send_cmd(command)
def send_cmd(self, msg: str):
if self.launch_mode == 'anchor': #if in anchor mode, send to anchor node to relay
if (
self.launch_mode == "anchor"
): # if in anchor mode, send to anchor node to relay
output = String()
output.data = msg
self.anchor_pub.publish(output)
elif self.launch_mode == 'bio': #if in standalone mode, send to MCU directly
elif self.launch_mode == "bio": # if in standalone mode, send to MCU directly
self.get_logger().info(f"[Bio to MCU] {msg}")
self.ser.write(bytes(msg, "utf8"))
@@ -185,7 +205,9 @@ class SerialRelay(Node):
parts = str(output.strip()).split(",")
# self.get_logger().info(f"[Bio Anchor] {msg.data}")
if output.startswith("can_relay_fromvic,citadel,54"): # bat, 12, 5, Voltage readings * 100
if output.startswith(
"can_relay_fromvic,citadel,54"
): # bat, 12, 5, Voltage readings * 100
self.bio_feedback.bat_voltage = float(parts[3]) / 100.0
self.bio_feedback.voltage_12 = float(parts[4]) / 100.0
self.bio_feedback.voltage_5 = float(parts[5]) / 100.0
@@ -209,11 +231,13 @@ class SerialRelay(Node):
except Exception as e:
exit(0)
def myexcepthook(type, value, tb):
print("Uncaught exception:", type, value)
if serial_pub:
serial_pub.cleanup()
def main(args=None):
rclpy.init(args=args)
sys.excepthook = myexcepthook
@@ -222,7 +246,10 @@ def main(args=None):
serial_pub = SerialRelay()
serial_pub.run()
if __name__ == '__main__':
if __name__ == "__main__":
# signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0)) # Catch Ctrl+Z and exit cleanly
signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0)) # Catch termination signals and exit cleanly
signal.signal(
signal.SIGTERM, lambda signum, frame: sys.exit(0)
) # Catch termination signals and exit cleanly
main()

25
src/bio_pkg/setup.py
View File

@@ -1,25 +1,22 @@
from setuptools import find_packages, setup
package_name = 'bio_pkg'
package_name = "bio_pkg"
setup(
name=package_name,
version='0.0.0',
packages=find_packages(exclude=['test']),
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']),
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
("share/" + package_name, ["package.xml"]),
],
install_requires=['setuptools'],
install_requires=["setuptools"],
zip_safe=True,
maintainer='tristan',
maintainer_email='tristanmcginnis26@gmail.com',
description='TODO: Package description',
license='TODO: License declaration',
maintainer="tristan",
maintainer_email="tristanmcginnis26@gmail.com",
description="TODO: Package description",
license="TODO: License declaration",
entry_points={
'console_scripts': [
'bio = bio_pkg.bio_node:main'
],
"console_scripts": ["bio = bio_pkg.bio_node:main"],
},
)

19
src/core_pkg/core_pkg/core_headless.py
View File

@@ -17,11 +17,14 @@ from ros2_interfaces_pkg.msg import CoreControl
os.environ["SDL_VIDEODRIVER"] = "dummy" # Prevents pygame from trying to open a display
os.environ["SDL_AUDIODRIVER"] = "dummy" # Force pygame to use a dummy audio driver before pygame.init()
os.environ["SDL_AUDIODRIVER"] = (
"dummy" # Force pygame to use a dummy audio driver before pygame.init()
)
max_speed = 90 # Max speed as a duty cycle percentage (1-100)
class Headless(Node):
def __init__(self):
# Initialize pygame first
@@ -43,13 +46,15 @@ class Headless(Node):
# Initialize the gamepad
self.gamepad = pygame.joystick.Joystick(0)
self.gamepad.init()
print(f'Gamepad Found: {self.gamepad.get_name()}')
print(f"Gamepad Found: {self.gamepad.get_name()}")
# Now initialize the ROS2 node
super().__init__("core_headless")
self.create_timer(0.15, self.send_controls)
self.publisher = self.create_publisher(CoreControl, '/core/control', 10)
self.lastMsg = String() # Used to ignore sending controls repeatedly when they do not change
self.publisher = self.create_publisher(CoreControl, "/core/control", 10)
self.lastMsg = (
String()
) # Used to ignore sending controls repeatedly when they do not change
def run(self):
# This thread makes all the update processes run in the background
@@ -91,15 +96,17 @@ class Headless(Node):
else:
input.left_stick = -1 * round(self.gamepad.get_axis(1), 2) # left y-axis
output = f'L: {input.left_stick}, R: {input.right_stick}, M: {max_speed}'
output = f"L: {input.left_stick}, R: {input.right_stick}, M: {max_speed}"
self.get_logger().info(f"[Ctrl] {output}")
self.publisher.publish(input)
def main(args=None):
rclpy.init(args=args)
node = Headless()
rclpy.spin(node)
rclpy.shutdown()
if __name__ == '__main__':
if __name__ == "__main__":
main()

182
src/core_pkg/core_pkg/core_node.py
View File

@@ -38,7 +38,7 @@ control_qos = qos.QoSProfile(
deadline=Duration(seconds=1),
lifespan=Duration(nanoseconds=500_000_000), # 500ms
liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
liveliness_lease_duration=Duration(seconds=5)
liveliness_lease_duration=Duration(seconds=5),
)
# Used to verify the length of an incoming VicCAN feedback message
@@ -52,7 +52,7 @@ viccan_msg_len_dict = {
53: 4,
54: 4,
56: 4, # really 3, but viccan
58: 4 # ditto
58: 4, # ditto
}
@@ -62,77 +62,110 @@ class SerialRelay(Node):
super().__init__("core_node")
# Launch mode -- anchor vs core
self.declare_parameter('launch_mode', 'core')
self.launch_mode = self.get_parameter('launch_mode').value
self.declare_parameter("launch_mode", "core")
self.launch_mode = self.get_parameter("launch_mode").value
self.get_logger().info(f"Core launch_mode is: {self.launch_mode}")
##################################################
# Topics
# Anchor
if self.launch_mode == 'anchor':
self.anchor_fromvic_sub_ = self.create_subscription(VicCAN, '/anchor/from_vic/core', self.relay_fromvic, 20)
self.anchor_tovic_pub_ = self.create_publisher(VicCAN, '/anchor/to_vic/relay', 20)
if self.launch_mode == "anchor":
self.anchor_fromvic_sub_ = self.create_subscription(
VicCAN, "/anchor/from_vic/core", self.relay_fromvic, 20
)
self.anchor_tovic_pub_ = self.create_publisher(
VicCAN, "/anchor/to_vic/relay", 20
)
self.anchor_sub = self.create_subscription(String, '/anchor/core/feedback', self.anchor_feedback, 10)
self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
self.anchor_sub = self.create_subscription(
String, "/anchor/core/feedback", self.anchor_feedback, 10
)
self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
# Control
# autonomy twist -- m/s and rad/s -- for autonomy, in particular Nav2
self.cmd_vel_sub_ = self.create_subscription(TwistStamped, '/cmd_vel', self.cmd_vel_callback, 1)
self.cmd_vel_sub_ = self.create_subscription(
TwistStamped, "/cmd_vel", self.cmd_vel_callback, 1
)
# manual twist -- [-1, 1] rather than real units
self.twist_man_sub_ = self.create_subscription(Twist, '/core/twist', self.twist_man_callback, qos_profile=control_qos)
self.twist_man_sub_ = self.create_subscription(
Twist, "/core/twist", self.twist_man_callback, qos_profile=control_qos
)
# manual flags -- brake mode and max duty cycle
self.control_state_sub_ = self.create_subscription(CoreCtrlState, '/core/control/state', self.control_state_callback, qos_profile=control_qos)
self.twist_max_duty = 0.5 # max duty cycle for twist commands (0.0 - 1.0); walking speed is 0.5
self.control_state_sub_ = self.create_subscription(
CoreCtrlState,
"/core/control/state",
self.control_state_callback,
qos_profile=control_qos,
)
self.twist_max_duty = (
0.5 # max duty cycle for twist commands (0.0 - 1.0); walking speed is 0.5
)
# Feedback
# Consolidated and organized core feedback
self.feedback_new_pub_ = self.create_publisher(NewCoreFeedback, '/core/feedback_new', qos_profile=qos.qos_profile_sensor_data)
self.feedback_new_pub_ = self.create_publisher(
NewCoreFeedback,
"/core/feedback_new",
qos_profile=qos.qos_profile_sensor_data,
)
self.feedback_new_state = NewCoreFeedback()
self.feedback_new_state.fl_motor.id = 1
self.feedback_new_state.bl_motor.id = 2
self.feedback_new_state.fr_motor.id = 3
self.feedback_new_state.br_motor.id = 4
self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback) # TODO: not sure about this
self.telemetry_pub_timer = self.create_timer(
1.0, self.publish_feedback
) # TODO: not sure about this
# Joint states for topic-based controller
self.joint_state_pub_ = self.create_publisher(JointState, '/core/joint_states', qos_profile=qos.qos_profile_sensor_data)
self.joint_state_pub_ = self.create_publisher(
JointState, "/core/joint_states", qos_profile=qos.qos_profile_sensor_data
)
# IMU (embedded BNO-055)
self.imu_pub_ = self.create_publisher(Imu, '/core/imu', qos_profile=qos.qos_profile_sensor_data)
self.imu_pub_ = self.create_publisher(
Imu, "/core/imu", qos_profile=qos.qos_profile_sensor_data
)
self.imu_state = Imu()
self.imu_state.header.frame_id = "core_bno055"
# GPS (embedded u-blox M9N)
self.gps_pub_ = self.create_publisher(NavSatFix, '/gps/fix', qos_profile=qos.qos_profile_sensor_data)
self.gps_pub_ = self.create_publisher(
NavSatFix, "/gps/fix", qos_profile=qos.qos_profile_sensor_data
)
self.gps_state = NavSatFix()
self.gps_state.header.frame_id = "core_gps_antenna"
self.gps_state.status.service = NavSatStatus.SERVICE_GPS
self.gps_state.status.status = NavSatStatus.STATUS_NO_FIX
self.gps_state.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
# Barometer (embedded BMP-388)
self.baro_pub_ = self.create_publisher(Barometer, '/core/baro', qos_profile=qos.qos_profile_sensor_data)
self.baro_pub_ = self.create_publisher(
Barometer, "/core/baro", qos_profile=qos.qos_profile_sensor_data
)
self.baro_state = Barometer()
self.baro_state.header.frame_id = "core_bmp388"
# Old
# /core/control
self.control_sub = self.create_subscription(CoreControl, '/core/control', self.send_controls, 10) # old control method -- left_stick, right_stick, max_speed, brake, and some other random autonomy stuff
self.control_sub = self.create_subscription(
CoreControl, "/core/control", self.send_controls, 10
) # old control method -- left_stick, right_stick, max_speed, brake, and some other random autonomy stuff
# /core/feedback
self.feedback_pub = self.create_publisher(CoreFeedback, '/core/feedback', 10)
self.feedback_pub = self.create_publisher(CoreFeedback, "/core/feedback", 10)
self.core_feedback = CoreFeedback()
# Debug
self.debug_pub = self.create_publisher(String, '/core/debug', 10)
self.ping_service = self.create_service(Empty, '/astra/core/ping', self.ping_callback)
self.debug_pub = self.create_publisher(String, "/core/debug", 10)
self.ping_service = self.create_service(
Empty, "/astra/core/ping", self.ping_callback
)
##################################################
# Find microcontroller (Non-anchor only)
# Core (non-anchor) specific
if self.launch_mode == 'core':
if self.launch_mode == "core":
# Loop through all serial devices on the computer to check for the MCU
self.port = None
ports = SerialRelay.list_serial_ports()
@@ -166,7 +199,6 @@ class SerialRelay(Node):
atexit.register(self.cleanup)
# end __init__()
def run(self):
# This thread makes all the update processes run in the background
global thread
@@ -175,7 +207,7 @@ class SerialRelay(Node):
try:
while rclpy.ok():
if self.launch_mode == 'core':
if self.launch_mode == "core":
self.read_MCU() # Check the MCU for updates
except KeyboardInterrupt:
sys.exit(0)
@@ -227,14 +259,26 @@ class SerialRelay(Node):
return str(rightMin + (valueScaled * rightSpan))
def send_controls(self, msg: CoreControl):
if(msg.turn_to_enable):
command = "can_relay_tovic,core,41," + str(msg.turn_to) + ',' + str(msg.turn_to_timeout) + '\n'
if msg.turn_to_enable:
command = (
"can_relay_tovic,core,41,"
+ str(msg.turn_to)
+ ","
+ str(msg.turn_to_timeout)
+ "\n"
)
else:
command = "can_relay_tovic,core,19," + self.scale_duty(msg.left_stick, msg.max_speed) + ',' + self.scale_duty(msg.right_stick, msg.max_speed) + '\n'
command = (
"can_relay_tovic,core,19,"
+ self.scale_duty(msg.left_stick, msg.max_speed)
+ ","
+ self.scale_duty(msg.right_stick, msg.max_speed)
+ "\n"
)
self.send_cmd(command)
# Brake mode
command = "can_relay_tovic,core,18," + str(int(msg.brake)) + '\n'
command = "can_relay_tovic,core,18," + str(int(msg.brake)) + "\n"
self.send_cmd(command)
# print(f"[Sys] Relaying: {command}")
@@ -255,13 +299,15 @@ class SerialRelay(Node):
linear = msg.linear.x # [-1 1] for forward/back from left stick y
angular = msg.angular.z # [-1 1] for left/right from right stick x
if (linear < 0): # reverse turning direction when going backwards (WIP)
if linear < 0: # reverse turning direction when going backwards (WIP)
angular *= -1
if abs(linear) > 1 or abs(angular) > 1:
# if speed is greater than 1, then there is a problem
# make it look like a problem and don't just run away lmao
linear = copysign(0.25, linear) # 0.25 duty cycle in direction of control (hopefully slow)
linear = copysign(
0.25, linear
) # 0.25 duty cycle in direction of control (hopefully slow)
angular = copysign(0.25, angular)
duty_left = linear - angular
@@ -272,8 +318,12 @@ class SerialRelay(Node):
# Apply max duty cycle
# Joysticks provide values [-1, 1] rather than real units
duty_left = map_range(duty_left, -1, 1, -self.twist_max_duty, self.twist_max_duty)
duty_right = map_range(duty_right, -1, 1, -self.twist_max_duty, self.twist_max_duty)
duty_left = map_range(
duty_left, -1, 1, -self.twist_max_duty, self.twist_max_duty
)
duty_right = map_range(
duty_right, -1, 1, -self.twist_max_duty, self.twist_max_duty
)
self.send_viccan(19, [duty_left, duty_right])
@@ -285,24 +335,24 @@ class SerialRelay(Node):
self.twist_max_duty = msg.max_duty # twist_man_callback will handle this
def send_cmd(self, msg: str):
if self.launch_mode == 'anchor':
if self.launch_mode == "anchor":
# self.get_logger().info(f"[Core to Anchor Relay] {msg}")
output = String() # Convert to std_msg string
output.data = msg
self.anchor_pub.publish(output)
elif self.launch_mode == 'core':
elif self.launch_mode == "core":
self.get_logger().info(f"[Core to MCU] {msg}")
self.ser.write(bytes(msg, "utf8"))
def send_viccan(self, cmd_id: int, data: list[float]):
self.anchor_tovic_pub_.publish(VicCAN(
self.anchor_tovic_pub_.publish(
VicCAN(
header=Header(stamp=self.get_clock().now().to_msg(), frame_id="to_vic"),
mcu_name="core",
command_id=cmd_id,
data=data
))
data=data,
)
)
def anchor_feedback(self, msg: String):
output = msg.data
@@ -376,7 +426,9 @@ class SerialRelay(Node):
if msg.command_id in viccan_msg_len_dict:
expected_len = viccan_msg_len_dict[msg.command_id]
if len(msg.data) != expected_len:
self.get_logger().warning(f"Ignoring VicCAN message with id {msg.command_id} due to unexpected data length (expected {expected_len}, got {len(msg.data)})")
self.get_logger().warning(
f"Ignoring VicCAN message with id {msg.command_id} due to unexpected data length (expected {expected_len}, got {len(msg.data)})"
)
return
match msg.command_id:
@@ -386,7 +438,11 @@ class SerialRelay(Node):
case 49: # GNSS Longitude
self.gps_state.longitude = float(msg.data[0])
case 50: # GNSS Satellite count and altitude
self.gps_state.status.status = NavSatStatus.STATUS_FIX if int(msg.data[0]) >= 3 else NavSatStatus.STATUS_NO_FIX
self.gps_state.status.status = (
NavSatStatus.STATUS_FIX
if int(msg.data[0]) >= 3
else NavSatStatus.STATUS_NO_FIX
)
self.gps_state.altitude = float(msg.data[1])
self.gps_state.header.stamp = msg.header.stamp
self.gps_pub_.publish(self.gps_state)
@@ -402,7 +458,7 @@ class SerialRelay(Node):
self.imu_state.linear_acceleration.y = float(msg.data[1])
self.imu_state.linear_acceleration.z = float(msg.data[2])
# Deal with quaternion
r = Rotation.from_euler('z', float(msg.data[3]), degrees=True)
r = Rotation.from_euler("z", float(msg.data[3]), degrees=True)
q = r.as_quat()
self.imu_state.orientation.x = q[0]
self.imu_state.orientation.y = q[1]
@@ -427,7 +483,9 @@ class SerialRelay(Node):
case 4:
motor = self.feedback_new_state.br_motor
case _:
self.get_logger().warning(f"Ignoring REV motor feedback 53 with invalid motorId {motorId}")
self.get_logger().warning(
f"Ignoring REV motor feedback 53 with invalid motorId {motorId}"
)
return
if motor:
@@ -455,9 +513,15 @@ class SerialRelay(Node):
motorId = round(float(msg.data[0]))
position = float(msg.data[1])
velocity = float(msg.data[2])
joint_state_msg = JointState() # TODO: not sure if all motors should be in each message or not
joint_state_msg.position = [position * (2 * pi) / CORE_GEAR_RATIO] # revolutions to radians
joint_state_msg.velocity = [velocity * (2 * pi / 60.0) / CORE_GEAR_RATIO] # RPM to rad/s
joint_state_msg = (
JointState()
) # TODO: not sure if all motors should be in each message or not
joint_state_msg.position = [
position * (2 * pi) / CORE_GEAR_RATIO
] # revolutions to radians
joint_state_msg.velocity = [
velocity * (2 * pi / 60.0) / CORE_GEAR_RATIO
] # RPM to rad/s
motor: RevMotorState | None = None
@@ -475,7 +539,9 @@ class SerialRelay(Node):
motor = self.feedback_new_state.br_motor
joint_state_msg.name = ["br_motor_joint"]
case _:
self.get_logger().warning(f"Ignoring REV motor feedback 58 with invalid motorId {motorId}")
self.get_logger().warning(
f"Ignoring REV motor feedback 58 with invalid motorId {motorId}"
)
return
joint_state_msg.header.stamp = msg.header.stamp
@@ -483,7 +549,6 @@ class SerialRelay(Node):
case _:
return
def publish_feedback(self):
# self.get_logger().info(f"[Core] {self.core_feedback}")
self.feedback_pub.publish(self.core_feedback)
@@ -491,7 +556,6 @@ class SerialRelay(Node):
def ping_callback(self, request, response):
return response
@staticmethod
def list_serial_ports():
return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
@@ -510,7 +574,10 @@ def myexcepthook(type, value, tb):
if serial_pub:
serial_pub.cleanup()
def map_range(value: float, in_min: float, in_max: float, out_min: float, out_max: float):
def map_range(
value: float, in_min: float, in_max: float, out_min: float, out_max: float
):
return (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
@@ -523,7 +590,10 @@ def main(args=None):
serial_pub = SerialRelay()
serial_pub.run()
if __name__ == '__main__':
if __name__ == "__main__":
# signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0)) # Catch Ctrl+Z and exit cleanly
signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0)) # Catch termination signals and exit cleanly
signal.signal(
signal.SIGTERM, lambda signum, frame: sys.exit(0)
) # Catch termination signals and exit cleanly
main()

128
src/core_pkg/core_pkg/core_ptz.py
View File

@@ -18,37 +18,43 @@ from core_pkg.siyi_sdk import (
DataStreamType,
DataStreamFrequency,
SingleAxis,
AttitudeData
AttitudeData,
)
class PtzNode(Node):
def __init__(self):
# Initialize node with name
super().__init__("core_ptz")
# Declare parameters
self.declare_parameter('camera_ip', '192.168.1.9')
self.declare_parameter('camera_port', 37260)
self.declare_parameter("camera_ip", "192.168.1.9")
self.declare_parameter("camera_port", 37260)
# Get parameters
self.camera_ip = self.get_parameter('camera_ip').value
self.camera_port = self.get_parameter('camera_port').value
self.camera_ip = self.get_parameter("camera_ip").value
self.camera_port = self.get_parameter("camera_port").value
self.get_logger().info(f"PTZ camera IP: {self.camera_ip} Port: {self.camera_port}")
self.get_logger().info(
f"PTZ camera IP: {self.camera_ip} Port: {self.camera_port}"
)
# Create a camera instance
self.camera = None
self.camera_connected = False # This flag is still managed but not used to gate commands
self.camera_connected = (
False # This flag is still managed but not used to gate commands
)
self.loop = None
self.thread_pool = None
# Create publishers
self.feedback_pub = self.create_publisher(PtzFeedback, '/ptz/feedback', 10)
self.debug_pub = self.create_publisher(String, '/ptz/debug', 10)
self.feedback_pub = self.create_publisher(PtzFeedback, "/ptz/feedback", 10)
self.debug_pub = self.create_publisher(String, "/ptz/debug", 10)
# Create subscribers
self.control_sub = self.create_subscription(
PtzControl, '/ptz/control', self.handle_control_command, 10)
PtzControl, "/ptz/control", self.handle_control_command, 10
)
# Create timers
self.connection_timer = self.create_timer(5.0, self.check_camera_connection)
@@ -57,7 +63,9 @@ class PtzNode(Node):
# Create feedback message
self.feedback_msg = PtzFeedback()
self.feedback_msg.connected = False # This will reflect the actual connection state
self.feedback_msg.connected = (
False # This will reflect the actual connection state
)
self.feedback_msg.error_msg = "Initializing"
# Flags for async operations
@@ -86,8 +94,7 @@ class PtzNode(Node):
# Request attitude data stream
await self.camera.send_data_stream_request(
DataStreamType.ATTITUDE_DATA,
DataStreamFrequency.HZ_10
DataStreamType.ATTITUDE_DATA, DataStreamFrequency.HZ_10
)
# Update connection status
@@ -108,8 +115,12 @@ class PtzNode(Node):
# Update last_data_time regardless of self.camera_connected,
# as data might arrive during a brief reconnect window.
self.last_data_time = time.time()
if self.camera_connected: # Only process for feedback if we believe we are connected
if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(data, AttitudeData):
if (
self.camera_connected
): # Only process for feedback if we believe we are connected
if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(
data, AttitudeData
):
self.feedback_msg.yaw = data.yaw
self.feedback_msg.pitch = data.pitch
self.feedback_msg.roll = data.roll
@@ -130,7 +141,6 @@ class PtzNode(Node):
debug_str += str(data)
self.get_logger().debug(debug_str)
def check_camera_connection(self):
"""Periodically check camera connection and attempt to reconnect if needed."""
if not self.camera_connected and not self.shutdown_requested:
@@ -140,7 +150,9 @@ class PtzNode(Node):
if self.camera.is_connected: # SDK's internal connection state
self.run_async_func(self.camera.disconnect())
except Exception as e:
self.get_logger().debug(f"Error during pre-reconnect disconnect: {e}")
self.get_logger().debug(
f"Error during pre-reconnect disconnect: {e}"
)
# self.camera = None # Don't nullify here, connect_to_camera will re-assign or create new
self.connect_task = self.thread_pool.submit(
@@ -152,7 +164,9 @@ class PtzNode(Node):
if self.camera_connected: # Only check health if we think we are connected
time_since_last_data = time.time() - self.last_data_time
if time_since_last_data > 5.0:
self.publish_debug(f"No camera data for {time_since_last_data:.1f}s, marking as disconnected.")
self.publish_debug(
f"No camera data for {time_since_last_data:.1f}s, marking as disconnected."
)
self.camera_connected = False
self.feedback_msg.connected = False
self.feedback_msg.error_msg = "Connection stale (no data)"
@@ -162,18 +176,19 @@ class PtzNode(Node):
"""Handle incoming control commands."""
# Removed: if not self.camera_connected
if not self.camera: # Still check if camera object exists
self.get_logger().warning("Camera object not initialized, ignoring control command")
self.get_logger().warning(
"Camera object not initialized, ignoring control command"
)
return
self.thread_pool.submit(
self.run_async_func,
self.process_control_command(msg)
)
self.thread_pool.submit(self.run_async_func, self.process_control_command(msg))
async def process_control_command(self, msg):
"""Process and send the control command to the camera."""
if not self.camera:
self.get_logger().error("Process control command called but camera object is None.")
self.get_logger().error(
"Process control command called but camera object is None."
)
return
try:
# The SDK's send_... methods will raise RuntimeError if not connected.
@@ -186,27 +201,35 @@ class PtzNode(Node):
if msg.control_mode == 0:
turn_yaw = max(-100, min(100, int(msg.turn_yaw)))
turn_pitch = max(-100, min(100, int(msg.turn_pitch)))
self.get_logger().debug(f"Attempting rotation: yaw_speed={turn_yaw}, pitch_speed={turn_pitch}")
self.get_logger().debug(
f"Attempting rotation: yaw_speed={turn_yaw}, pitch_speed={turn_pitch}"
)
await self.camera.send_rotation_command(turn_yaw, turn_pitch)
elif msg.control_mode == 1:
yaw = max(-135.0, min(135.0, msg.yaw))
pitch = max(-90.0, min(90.0, msg.pitch))
self.get_logger().debug(f"Attempting absolute angles: yaw={yaw}, pitch={pitch}")
self.get_logger().debug(
f"Attempting absolute angles: yaw={yaw}, pitch={pitch}"
)
await self.camera.send_attitude_angles_command(yaw, pitch)
elif msg.control_mode == 2:
axis = SingleAxis.YAW if msg.axis_id == 0 else SingleAxis.PITCH
angle = msg.angle
self.get_logger().debug(f"Attempting single axis: axis={axis.name}, angle={angle}")
self.get_logger().debug(
f"Attempting single axis: axis={axis.name}, angle={angle}"
)
await self.camera.send_single_axis_attitude_command(angle, axis)
elif msg.control_mode == 3:
zoom_level = msg.zoom_level
self.get_logger().debug(f"Attempting absolute zoom: level={zoom_level}x")
self.get_logger().debug(
f"Attempting absolute zoom: level={zoom_level}x"
)
await self.camera.send_absolute_zoom_command(zoom_level)
if hasattr(msg, 'stream_type') and hasattr(msg, 'stream_freq'):
if hasattr(msg, "stream_type") and hasattr(msg, "stream_freq"):
if msg.stream_type > 0 and msg.stream_freq >= 0:
try:
stream_type = DataStreamType(msg.stream_type)
@@ -214,9 +237,13 @@ class PtzNode(Node):
self.get_logger().info(
f"Attempting to set data stream: type={stream_type.name}, freq={stream_freq.name}"
)
await self.camera.send_data_stream_request(stream_type, stream_freq)
await self.camera.send_data_stream_request(
stream_type, stream_freq
)
except ValueError:
self.get_logger().error("Invalid stream type or frequency values in control message")
self.get_logger().error(
"Invalid stream type or frequency values in control message"
)
except RuntimeError as e: # Catch SDK's "not connected" errors
self.get_logger().warning(f"SDK command failed (likely not connected): {e}")
@@ -231,7 +258,9 @@ class PtzNode(Node):
def publish_debug(self, message_text):
"""Publish debug message."""
msg = String()
msg.data = f"[{self.get_clock().now().nanoseconds / 1e9:.2f}] PTZ Node: {message_text}"
msg.data = (
f"[{self.get_clock().now().nanoseconds / 1e9:.2f}] PTZ Node: {message_text}"
)
self.debug_pub.publish(msg)
self.get_logger().info(message_text)
@@ -239,18 +268,23 @@ class PtzNode(Node):
"""Run an async function in the event loop."""
if self.loop and self.loop.is_running():
try:
return asyncio.run_coroutine_threadsafe(coro, self.loop).result(timeout=5.0) # Added timeout
return asyncio.run_coroutine_threadsafe(coro, self.loop).result(
timeout=5.0
) # Added timeout
except asyncio.TimeoutError:
self.get_logger().warning(f"Async function {coro.__name__} timed out.")
return None
except Exception as e:
self.get_logger().error(f"Exception in run_async_func for {coro.__name__}: {e}")
self.get_logger().error(
f"Exception in run_async_func for {coro.__name__}: {e}"
)
return None
else:
self.get_logger().warning("Asyncio loop not running, cannot execute coroutine.")
self.get_logger().warning(
"Asyncio loop not running, cannot execute coroutine."
)
return None
async def shutdown_node_async(self):
"""Perform clean shutdown of camera connection."""
self.shutdown_requested = True
@@ -259,8 +293,7 @@ class PtzNode(Node):
try:
self.get_logger().info("Disabling data stream...")
await self.camera.send_data_stream_request(
DataStreamType.ATTITUDE_DATA,
DataStreamFrequency.DISABLE
DataStreamType.ATTITUDE_DATA, DataStreamFrequency.DISABLE
)
await asyncio.sleep(0.1)
@@ -287,10 +320,14 @@ class PtzNode(Node):
if self.loop and self.thread_pool:
if self.loop.is_running():
try:
future = asyncio.run_coroutine_threadsafe(self.shutdown_node_async(), self.loop)
future = asyncio.run_coroutine_threadsafe(
self.shutdown_node_async(), self.loop
)
future.result(timeout=5)
except Exception as e:
self.get_logger().error(f"Error during async shutdown in cleanup: {e}")
self.get_logger().error(
f"Error during async shutdown in cleanup: {e}"
)
self.get_logger().info("Shutting down thread pool executor...")
self.thread_pool.shutdown(wait=True)
@@ -301,7 +338,9 @@ class PtzNode(Node):
self.get_logger().info("PTZ node resources cleaned up.")
else:
self.get_logger().warning("Loop or thread_pool not initialized, skipping parts of cleanup.")
self.get_logger().warning(
"Loop or thread_pool not initialized, skipping parts of cleanup."
)
def main(args=None):
@@ -312,6 +351,7 @@ def main(args=None):
asyncio_thread = None
if ptz_node.loop:
def run_event_loop(loop):
asyncio.set_event_loop(loop)
try:
@@ -324,9 +364,7 @@ def main(args=None):
loop.close()
asyncio_thread = threading.Thread(
target=run_event_loop,
args=(ptz_node.loop,),
daemon=True
target=run_event_loop, args=(ptz_node.loop,), daemon=True
)
asyncio_thread.start()
@@ -351,5 +389,5 @@ def main(args=None):
ptz_node.get_logger().info("ROS shutdown complete.")
if __name__ == '__main__':
if __name__ == "__main__":
main()

70
src/core_pkg/core_pkg/siyi_sdk/siyi.py
View File

@@ -110,9 +110,7 @@ class SiyiGimbalCamera:
MAX_A8_MINI_ZOOM = 6.0 # Maximum zoom for A8 mini
def __init__(
self, ip: str, port: int = 37260, *, heartbeat_interval: int = 2
):
def __init__(self, ip: str, port: int = 37260, *, heartbeat_interval: int = 2):
self.ip = ip
self.port = port
self.heartbeat_interval = heartbeat_interval
@@ -124,9 +122,7 @@ class SiyiGimbalCamera:
async def connect(self) -> None:
try:
self.reader, self.writer = await asyncio.open_connection(
self.ip, self.port
)
self.reader, self.writer = await asyncio.open_connection(self.ip, self.port)
self.is_connected = True
asyncio.create_task(self.heartbeat_loop())
asyncio.create_task(self._data_stream_listener())
@@ -158,9 +154,7 @@ class SiyiGimbalCamera:
if self.is_connected:
await self.disconnect()
def _build_packet_header(
self, cmd_id: CommandID, data_len: int
) -> bytearray:
def _build_packet_header(self, cmd_id: CommandID, data_len: int) -> bytearray:
"""Helper to build the common packet header."""
packet = bytearray()
packet.extend(b"\x55\x66") # STX
@@ -179,15 +173,11 @@ class SiyiGimbalCamera:
def _build_rotation_packet(self, turn_yaw: int, turn_pitch: int) -> bytes:
data_len = 2
packet = self._build_packet_header(
CommandID.ROTATION_CONTROL, data_len
)
packet = self._build_packet_header(CommandID.ROTATION_CONTROL, data_len)
packet.extend(struct.pack("bb", turn_yaw, turn_pitch))
return self._finalize_packet(packet)
async def send_rotation_command(
self, turn_yaw: int, turn_pitch: int
) -> None:
async def send_rotation_command(self, turn_yaw: int, turn_pitch: int) -> None:
if not self.is_connected or not self.writer:
raise RuntimeError(
"Socket is not connected or writer is None, cannot send rotation command."
@@ -199,21 +189,15 @@ class SiyiGimbalCamera:
f"Sent rotation command with yaw_speed {turn_yaw} and pitch_speed {turn_pitch}"
)
def _build_attitude_angles_packet(
self, yaw: float, pitch: float
) -> bytes:
def _build_attitude_angles_packet(self, yaw: float, pitch: float) -> bytes:
data_len = 4
packet = self._build_packet_header(
CommandID.ATTITUDE_ANGLES, data_len
)
packet = self._build_packet_header(CommandID.ATTITUDE_ANGLES, data_len)
yaw_int = int(round(yaw * 10))
pitch_int = int(round(pitch * 10))
packet.extend(struct.pack("<hh", yaw_int, pitch_int))
return self._finalize_packet(packet)
async def send_attitude_angles_command(
self, yaw: float, pitch: float
) -> None:
async def send_attitude_angles_command(self, yaw: float, pitch: float) -> None:
if not self.is_connected or not self.writer:
raise RuntimeError(
"Socket is not connected or writer is None, cannot send attitude angles command."
@@ -221,17 +205,13 @@ class SiyiGimbalCamera:
packet = self._build_attitude_angles_packet(yaw, pitch)
self.writer.write(packet)
await self.writer.drain()
logger.debug(
f"Sent attitude angles command with yaw {yaw}° and pitch {pitch}°"
)
logger.debug(f"Sent attitude angles command with yaw {yaw}° and pitch {pitch}°")
def _build_single_axis_attitude_packet(
self, angle: float, axis: SingleAxis
) -> bytes:
data_len = 3
packet = self._build_packet_header(
CommandID.SINGLE_AXIS_CONTROL, data_len
)
packet = self._build_packet_header(CommandID.SINGLE_AXIS_CONTROL, data_len)
angle_int = int(round(angle * 10))
packet.extend(struct.pack("<hB", angle_int, axis.value))
return self._finalize_packet(packet)
@@ -254,9 +234,7 @@ class SiyiGimbalCamera:
self, data_type: DataStreamType, data_freq: DataStreamFrequency
) -> bytes:
data_len = 2
packet = self._build_packet_header(
CommandID.DATA_STREAM_REQUEST, data_len
)
packet = self._build_packet_header(CommandID.DATA_STREAM_REQUEST, data_len)
packet.append(data_type.value)
packet.append(data_freq.value)
return self._finalize_packet(packet)
@@ -279,7 +257,9 @@ class SiyiGimbalCamera:
data_len = 2
packet = self._build_packet_header(CommandID.ABSOLUTE_ZOOM, data_len)
zoom_packet_value = int(round(zoom_level * 10))
if not (0 <= zoom_packet_value <= 65535): # Should be caught by clamping earlier
if not (
0 <= zoom_packet_value <= 65535
): # Should be caught by clamping earlier
raise ValueError(
"Zoom packet value out of uint16_t range after conversion."
)
@@ -374,10 +354,7 @@ class SiyiGimbalCamera:
self._data_callback(cmd_id_int, data)
continue
if (
cmd_id_enum == CommandID.ATTITUDE_DATA_RESPONSE
and len(data) == 12
):
if cmd_id_enum == CommandID.ATTITUDE_DATA_RESPONSE and len(data) == 12:
try:
parsed = AttitudeData.from_bytes(data)
if self._data_callback:
@@ -385,9 +362,7 @@ class SiyiGimbalCamera:
else:
logger.info(f"Received attitude data: {parsed}")
except Exception as e:
logger.exception(
f"Failed to parse attitude data: {e}"
)
logger.exception(f"Failed to parse attitude data: {e}")
if self._data_callback:
self._data_callback(cmd_id_enum, data)
else:
@@ -429,7 +404,9 @@ async def main_sdk_test(): # Renamed to avoid conflict if this file is imported
gimbal = SiyiGimbalCamera(gimbal_ip)
def my_data_handler(cmd_id, data):
if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(data, AttitudeData):
if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(
data, AttitudeData
):
print(
f"Attitude: Yaw={data.yaw:.1f}, Pitch={data.pitch:.1f}, Roll={data.roll:.1f}"
)
@@ -470,16 +447,19 @@ async def main_sdk_test(): # Renamed to avoid conflict if this file is imported
await asyncio.sleep(2)
print("SDK Test: Requesting attitude data stream at 5Hz...")
await gimbal.send_data_stream_request(DataStreamType.ATTITUDE_DATA, DataStreamFrequency.HZ_5)
await gimbal.send_data_stream_request(
DataStreamType.ATTITUDE_DATA, DataStreamFrequency.HZ_5
)
print("SDK Test: Listening for data for 10 seconds...")
await asyncio.sleep(10)
print("SDK Test: Disabling attitude data stream...")
await gimbal.send_data_stream_request(DataStreamType.ATTITUDE_DATA, DataStreamFrequency.DISABLE)
await gimbal.send_data_stream_request(
DataStreamType.ATTITUDE_DATA, DataStreamFrequency.DISABLE
)
await asyncio.sleep(1)
except ConnectionRefusedError:
print(
f"SDK Test: Connection to {gimbal_ip} was refused. Is the gimbal on and accessible?"

12
src/core_pkg/core_pkg/siyi_sdk/test.py
View File

@@ -24,7 +24,9 @@ async def main():
await asyncio.sleep(2)
# Command 1: Move all the way to the right (using set angles)
logger.info("Command 1: Move all the way to the right (using absolute angle control)")
logger.info(
"Command 1: Move all the way to the right (using absolute angle control)"
)
await camera.send_attitude_angles_command(135.0, 0.0)
await asyncio.sleep(5)
@@ -35,13 +37,17 @@ async def main():
await asyncio.sleep(5)
# Command 3: Stop looking down, then look up (with the single axis)
logger.info("Command 3: Stop looking down and start looking up (single axis control)")
logger.info(
"Command 3: Stop looking down and start looking up (single axis control)"
)
await camera.send_rotation_command(0, 0)
await camera.send_single_axis_attitude_command(135, SingleAxis.PITCH)
await asyncio.sleep(5)
# Command 4: Reset and move all the way to the left (Absolute value).
logger.info("Command 4: Move back to the center, and start moving all the way left")
logger.info(
"Command 4: Move back to the center, and start moving all the way left"
)
await camera.send_attitude_angles_command(-135.0, 0.0)
await asyncio.sleep(5)

25
src/core_pkg/setup.py
View File

@@ -1,27 +1,26 @@
from setuptools import find_packages, setup
package_name = 'core_pkg'
package_name = "core_pkg"
setup(
name=package_name,
version='0.0.0',
packages=find_packages(exclude=['test']),
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']),
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
("share/" + package_name, ["package.xml"]),
],
install_requires=['setuptools'],
install_requires=["setuptools"],
zip_safe=True,
maintainer='tristan',
maintainer_email='tristanmcginnis26@gmail.com',
description='Core rover control package to handle command interpretation and embedded interfacing.',
license='All Rights Reserved',
maintainer="tristan",
maintainer_email="tristanmcginnis26@gmail.com",
description="Core rover control package to handle command interpretation and embedded interfacing.",
license="All Rights Reserved",
entry_points={
'console_scripts': [
"console_scripts": [
"core = core_pkg.core_node:main",
"headless = core_pkg.core_headless:main",
"ptz = core_pkg.core_ptz:main"
"ptz = core_pkg.core_ptz:main",
],
},
)

22
src/headless_pkg/setup.py
View File

@@ -1,23 +1,23 @@
from setuptools import find_packages, setup
package_name = 'headless_pkg'
package_name = "headless_pkg"
setup(
name=package_name,
version='1.0.0',
packages=find_packages(exclude=['test']),
version="1.0.0",
packages=find_packages(exclude=["test"]),
data_files=[
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
("share/" + package_name, ["package.xml"]),
],
install_requires=['setuptools'],
install_requires=["setuptools"],
zip_safe=True,
maintainer='David Sharpe',
maintainer_email='ds0196@uah.edu',
description='Headless rover control package to handle command interpretation and embedded interfacing.',
license='All Rights Reserved',
maintainer="David Sharpe",
maintainer_email="ds0196@uah.edu",
description="Headless rover control package to handle command interpretation and embedded interfacing.",
license="All Rights Reserved",
entry_points={
'console_scripts': [
"console_scripts": [
"headless_full = src.headless_node:main",
],
},

74
src/headless_pkg/src/headless_node.py
View File

@@ -21,7 +21,9 @@ from ros2_interfaces_pkg.msg import CoreCtrlState
import pygame
os.environ["SDL_VIDEODRIVER"] = "dummy" # Prevents pygame from trying to open a display
os.environ["SDL_AUDIODRIVER"] = "dummy" # Force pygame to use a dummy audio driver before pygame.init()
os.environ["SDL_AUDIODRIVER"] = (
"dummy" # Force pygame to use a dummy audio driver before pygame.init()
)
CORE_STOP_MSG = CoreControl() # All zeros by default
@@ -37,7 +39,7 @@ control_qos = qos.QoSProfile(
deadline=Duration(seconds=1),
lifespan=Duration(nanoseconds=500_000_000), # 500ms
liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
liveliness_lease_duration=Duration(seconds=5)
liveliness_lease_duration=Duration(seconds=5),
)
CORE_MODE = "twist" # "twist" or "duty"
@@ -51,11 +53,11 @@ class Headless(Node):
super().__init__("headless")
# Wait for anchor to start
pub_info = self.get_publishers_info_by_topic('/anchor/from_vic/debug')
pub_info = self.get_publishers_info_by_topic("/anchor/from_vic/debug")
while len(pub_info) == 0:
self.get_logger().info("Waiting for anchor to start...")
time.sleep(1.0)
pub_info = self.get_publishers_info_by_topic('/anchor/from_vic/debug')
pub_info = self.get_publishers_info_by_topic("/anchor/from_vic/debug")
# Wait for a gamepad to be connected
print("Waiting for gamepad connection...")
@@ -71,17 +73,20 @@ class Headless(Node):
# Initialize the gamepad
self.gamepad = pygame.joystick.Joystick(0)
self.gamepad.init()
print(f'Gamepad Found: {self.gamepad.get_name()}')
print(f"Gamepad Found: {self.gamepad.get_name()}")
self.create_timer(0.15, self.send_controls)
self.core_publisher = self.create_publisher(CoreControl, '/core/control', 2)
self.arm_publisher = self.create_publisher(ArmManual, '/arm/control/manual', 2)
self.bio_publisher = self.create_publisher(BioControl, '/bio/control', 2)
self.core_twist_pub_ = self.create_publisher(Twist, '/core/twist', qos_profile=control_qos)
self.core_state_pub_ = self.create_publisher(CoreCtrlState, '/core/control/state', qos_profile=control_qos)
self.core_publisher = self.create_publisher(CoreControl, "/core/control", 2)
self.arm_publisher = self.create_publisher(ArmManual, "/arm/control/manual", 2)
self.bio_publisher = self.create_publisher(BioControl, "/bio/control", 2)
self.core_twist_pub_ = self.create_publisher(
Twist, "/core/twist", qos_profile=control_qos
)
self.core_state_pub_ = self.create_publisher(
CoreCtrlState, "/core/control/state", qos_profile=control_qos
)
self.ctrl_mode = "core" # Start in core mode
self.core_brake_mode = False
@@ -90,7 +95,6 @@ class Headless(Node):
# Rumble when node is ready (returns False if rumble not supported)
self.gamepad.rumble(0.7, 0.8, 150)
def run(self):
# This thread makes all the update processes run in the background
thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
@@ -122,7 +126,6 @@ class Headless(Node):
pygame.quit()
sys.exit(0)
new_ctrl_mode = self.ctrl_mode # if "" then inequality will always be true
# Check for control mode change
@@ -137,7 +140,6 @@ class Headless(Node):
self.ctrl_mode = new_ctrl_mode
self.get_logger().info(f"Switched to {self.ctrl_mode} control mode")
# CORE
if self.ctrl_mode == "core" and CORE_MODE == "duty":
input = CoreControl()
@@ -148,7 +150,6 @@ class Headless(Node):
right_stick_y = deadzone(self.gamepad.get_axis(4))
right_trigger = deadzone(self.gamepad.get_axis(5))
# Right wheels
input.right_stick = float(round(-1 * right_stick_y, 2))
@@ -158,9 +159,8 @@ class Headless(Node):
else:
input.left_stick = float(round(-1 * left_stick_y, 2))
# Debug
output = f'L: {input.left_stick}, R: {input.right_stick}'
output = f"L: {input.left_stick}, R: {input.right_stick}"
self.get_logger().info(f"[Ctrl] {output}")
self.core_publisher.publish(input)
@@ -179,13 +179,17 @@ class Headless(Node):
# Forward/back and Turn
input.linear.x = -1.0 * left_stick_y
input.angular.z = -1.0 * copysign(right_stick_x ** 2, right_stick_x) # Exponent for finer control (curve)
input.angular.z = -1.0 * copysign(
right_stick_x**2, right_stick_x
) # Exponent for finer control (curve)
# Publish
self.core_twist_pub_.publish(input)
self.arm_publisher.publish(ARM_STOP_MSG)
# self.bio_publisher.publish(BIO_STOP_MSG)
self.get_logger().info(f"[Core Ctrl] Linear: {round(input.linear.x, 2)}, Angular: {round(input.angular.z, 2)}")
self.get_logger().info(
f"[Core Ctrl] Linear: {round(input.linear.x, 2)}, Angular: {round(input.angular.z, 2)}"
)
# Brake mode
new_brake_mode = button_a
@@ -198,15 +202,19 @@ class Headless(Node):
new_max_duty = 0.5
# Only publish if needed
if new_brake_mode != self.core_brake_mode or new_max_duty != self.core_max_duty:
if (
new_brake_mode != self.core_brake_mode
or new_max_duty != self.core_max_duty
):
self.core_brake_mode = new_brake_mode
self.core_max_duty = new_max_duty
state_msg = CoreCtrlState()
state_msg.brake_mode = bool(self.core_brake_mode)
state_msg.max_duty = float(self.core_max_duty)
self.core_state_pub_.publish(state_msg)
self.get_logger().info(f"[Core State] Brake: {self.core_brake_mode}, Max Duty: {self.core_max_duty}")
self.get_logger().info(
f"[Core State] Brake: {self.core_brake_mode}, Max Duty: {self.core_max_duty}"
)
# ARM and BIO
if self.ctrl_mode == "arm":
@@ -222,7 +230,6 @@ class Headless(Node):
right_bumper = self.gamepad.get_button(5)
dpad_input = self.gamepad.get_hat(0)
# EF Grippers
if left_trigger > 0 and right_trigger > 0:
arm_input.gripper = 0
@@ -237,7 +244,6 @@ class Headless(Node):
elif dpad_input[0] == -1:
arm_input.axis0 = -1
if right_bumper: # Control end effector
# Effector yaw
@@ -255,27 +261,28 @@ class Headless(Node):
else: # Control arm axis
# Axis 1
if abs(left_stick_x) > .15:
if abs(left_stick_x) > 0.15:
arm_input.axis1 = round(left_stick_x)
# Axis 2
if abs(left_stick_y) > .15:
if abs(left_stick_y) > 0.15:
arm_input.axis2 = -1 * round(left_stick_y)
# Axis 3
if abs(right_stick_y) > .15:
if abs(right_stick_y) > 0.15:
arm_input.axis3 = -1 * round(right_stick_y)
# BIO
bio_input = BioControl(
bio_arm=int(left_stick_y * -100),
drill_arm=int(round(right_stick_y) * -100)
drill_arm=int(round(right_stick_y) * -100),
)
# Drill motor (FAERIE)
if deadzone(left_trigger) > 0 or deadzone(right_trigger) > 0:
bio_input.drill = int(30 * (right_trigger - left_trigger)) # Max duty cycle 30%
bio_input.drill = int(
30 * (right_trigger - left_trigger)
) # Max duty cycle 30%
self.core_publisher.publish(CORE_STOP_MSG)
self.arm_publisher.publish(arm_input)
@@ -295,6 +302,9 @@ def main(args=None):
rclpy.spin(node)
rclpy.shutdown()
if __name__ == '__main__':
signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0)) # Catch termination signals and exit cleanly
if __name__ == "__main__":
signal.signal(
signal.SIGTERM, lambda signum, frame: sys.exit(0)
) # Catch termination signals and exit cleanly
main()