From ff68d53e24672cf025df292f347134430ae70f14 Mon Sep 17 00:00:00 2001 From: David Date: Fri, 23 May 2025 05:46:19 +0000 Subject: [PATCH 01/17] feat: update for new faerie arm --- src/anchor_pkg/anchor_pkg/anchor_node.py | 16 ++++++++-------- src/bio_pkg/bio_pkg/bio_node.py | 21 +++++++++++---------- src/ros2_interfaces_pkg | 2 +- 3 files changed, 20 insertions(+), 19 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 5267ccb..071a71a 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -90,9 +90,9 @@ class SerialRelay(Node): msg.data = output if output.startswith("can_relay_fromvic,core"): self.core_pub.publish(msg) - elif output.startswith("can_relay_fromvic,arm"): + elif output.startswith("can_relay_fromvic,arm") or output.startswith("can_relay_fromvic,digit"): # digit for voltage readings self.arm_pub.publish(msg) - elif output.startswith("can_relay_fromvic,bio"): + elif 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 @@ -135,16 +135,16 @@ 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 + rclpy.init(args=args) + sys.excepthook = myexcepthook - global serial_pub + global serial_pub - serial_pub = SerialRelay() - serial_pub.run() + serial_pub = SerialRelay() + serial_pub.run() if __name__ == '__main__': signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0)) # Catch Ctrl+Z and exit cleanly diff --git a/src/bio_pkg/bio_pkg/bio_node.py b/src/bio_pkg/bio_pkg/bio_node.py index 1523ecd..f4b582f 100644 --- a/src/bio_pkg/bio_pkg/bio_node.py +++ b/src/bio_pkg/bio_pkg/bio_node.py @@ -9,6 +9,7 @@ import atexit import signal from std_msgs.msg import String from ros2_interfaces_pkg.msg import BioControl +from ros2_interfaces_pkg.msg import BioFeedback serial_pub = None thread = None @@ -25,10 +26,10 @@ class SerialRelay(Node): # Create publishers self.debug_pub = self.create_publisher(String, '/bio/feedback/debug', 10) - #self.socket_pub = self.create_publisher(SocketFeedback, '/arm/feedback/socket', 10) + self.bio_pub = self.create_publisher(BioFeedback, '/bio/feedback', 10) - # Create subscribers\ + # Create subscribers self.control_sub = self.create_subscription(BioControl, '/bio/control', self.send_control, 10) # Topics used in anchor mode @@ -136,7 +137,7 @@ class SerialRelay(Node): self.send_cmd(command) - # LSS + # LSS (SCYTHE) command = "can_relay_tovic,citadel,24," + str(msg.lss_direction) + "\n" self.send_cmd(command) # Vibration Motor @@ -150,18 +151,18 @@ class SerialRelay(Node): # To be reviewed before use# # Laser - command = "can_relay_tovic,faerie,28," + str(msg.laser) + "\n" + command = "can_relay_tovic,digit,28," + str(msg.laser) + "\n" self.send_cmd(command) - - # # UV Light - # command = "can_relay_tovic,faerie,38," + str(msg.uvLight) + "\n" - # self.send_cmd(command) - # Drill - command = f"can_relay_tovic,faerie,19,{msg.drill_duty:.2f}\n" + # Drill (SCABBARD) + command = f"can_relay_tovic,digit,19,{msg.drill_duty:.2f}\n" print(msg.drill_duty) self.send_cmd(command) + # Bio linear actuator + command = "can_relay_tovic,digit,42," + str(msg.drill_arm) + "\n" + self.send_cmd(command) + def send_cmd(self, msg): diff --git a/src/ros2_interfaces_pkg b/src/ros2_interfaces_pkg index f7f39a6..3db6c94 160000 --- a/src/ros2_interfaces_pkg +++ b/src/ros2_interfaces_pkg @@ -1 +1 @@ -Subproject commit f7f39a635287f242a8feacb1792332c0521fc5a7 +Subproject commit 3db6c9483300ddec637c8745bcde3d1017686470 From fa057bb4ace60d8b9e4b4744d7a117daabf16c3d Mon Sep 17 00:00:00 2001 From: David Date: Fri, 23 May 2025 06:19:20 +0000 Subject: [PATCH 02/17] feat: add digit voltage feedback --- src/arm_pkg/arm_pkg/arm_node.py | 16 +++++++++++++--- src/ros2_interfaces_pkg | 2 +- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index c736685..45ab4b2 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -10,7 +10,7 @@ import signal from std_msgs.msg import String from ros2_interfaces_pkg.msg import ArmManual from ros2_interfaces_pkg.msg import ArmIK -from ros2_interfaces_pkg.msg import SocketFeedback +from ros2_interfaces_pkg.msg import ArmFeedback serial_pub = None thread = None @@ -27,7 +27,7 @@ class SerialRelay(Node): # Create publishers self.debug_pub = self.create_publisher(String, '/arm/feedback/debug', 10) - self.socket_pub = self.create_publisher(SocketFeedback, '/arm/feedback/socket', 10) + self.socket_pub = self.create_publisher(ArmFeedback, '/arm/feedback/socket', 10) self.feedback_timer = self.create_timer(1.0, self.publish_feedback) # Create subscribers @@ -39,7 +39,7 @@ class SerialRelay(Node): self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10) self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10) - self.arm_feedback = SocketFeedback() + self.arm_feedback = ArmFeedback() # Search for ports IF in 'arm' (standalone) and not 'anchor' mode if self.launch_mode == 'arm': @@ -182,6 +182,16 @@ class SerialRelay(Node): elif output.startswith("can_relay_fromvic,arm,53"): #pass self.updateMotorFeedback(output) + elif output.startswith("can_relay_fromvic,digit,54"): + parts = msg.split(",") + if len(parts) >= 7: + # Extract the voltage from the string + voltages_in = parts[3:7] + # Convert the voltages to floats + self.arm_feedback.digit_bat_voltage = float(voltages_in[0]) / 100.0 + self.arm_feedback.digit_voltage_12 = float(voltages_in[1]) / 100.0 + self.arm_feedback.digit_voltage_5 = float(voltages_in[2]) / 100.0 + self.arm_feedback.digit_voltage_3 = float(voltages_in[3]) / 100.0 else: return diff --git a/src/ros2_interfaces_pkg b/src/ros2_interfaces_pkg index 3db6c94..fac196b 160000 --- a/src/ros2_interfaces_pkg +++ b/src/ros2_interfaces_pkg @@ -1 +1 @@ -Subproject commit 3db6c9483300ddec637c8745bcde3d1017686470 +Subproject commit fac196b5d6efb52db9490c1326629ac2d8f63c4e From 02b8904d631d52bde9c8d8babac3e18c9a1d6ffc Mon Sep 17 00:00:00 2001 From: David Date: Fri, 23 May 2025 06:37:14 +0000 Subject: [PATCH 03/17] feat: add bio feedback --- src/bio_pkg/bio_pkg/bio_node.py | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/src/bio_pkg/bio_pkg/bio_node.py b/src/bio_pkg/bio_pkg/bio_node.py index f4b582f..bf84cde 100644 --- a/src/bio_pkg/bio_pkg/bio_node.py +++ b/src/bio_pkg/bio_pkg/bio_node.py @@ -26,17 +26,22 @@ class SerialRelay(Node): # Create publishers self.debug_pub = self.create_publisher(String, '/bio/feedback/debug', 10) - self.bio_pub = self.create_publisher(BioFeedback, '/bio/feedback', 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) + + # 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) + self.bio_feedback = BioFeedback() + # Search for ports IF in 'arm' (standalone) and not 'anchor' mode if self.launch_mode == 'bio': @@ -175,9 +180,20 @@ class SerialRelay(Node): self.ser.write(bytes(msg, "utf8")) def anchor_feedback(self, msg): + output = msg.data + parts = str(output.strip()).split(",") self.get_logger().info(f"[Bio Anchor] {msg.data}") - #self.send_cmd(msg.data) + if output.startswith("can_relay_fromvic,citadel,54"):#bat, 12, 5, 3, 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 + if output.startswith("can_relay_fromvic,digit,57"): + self.bio_feedback.drill_temp = float(parts[3]) + self.bio_feedback.drill_humidity = float(parts[4]) + + def publish_feedback(self): + self.feedback_pub.publish(self.bio_feedback) @staticmethod def list_serial_ports(): From 76ca9d81b227e1255099cdf4b922459c2ef75f61 Mon Sep 17 00:00:00 2001 From: David Date: Fri, 23 May 2025 22:42:12 +0000 Subject: [PATCH 04/17] feat: actually add digit feedback --- src/arm_pkg/arm_pkg/arm_node.py | 21 ++++++++++++++------- src/bio_pkg/bio_pkg/bio_node.py | 2 +- src/ros2_interfaces_pkg | 2 +- 3 files changed, 16 insertions(+), 9 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 45ab4b2..0536590 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -10,7 +10,8 @@ import signal from std_msgs.msg import String from ros2_interfaces_pkg.msg import ArmManual from ros2_interfaces_pkg.msg import ArmIK -from ros2_interfaces_pkg.msg import ArmFeedback +from ros2_interfaces_pkg.msg import SocketFeedback +from ros2_interfaces_pkg.msg import DigitFeedback serial_pub = None thread = None @@ -27,7 +28,8 @@ class SerialRelay(Node): # Create publishers self.debug_pub = self.create_publisher(String, '/arm/feedback/debug', 10) - self.socket_pub = self.create_publisher(ArmFeedback, '/arm/feedback/socket', 10) + self.socket_pub = self.create_publisher(SocketFeedback, '/arm/feedback/socket', 10) + self.digit_pub = self.create_publisher(DigitFeedback, '/arm/feedback/digit', 10) self.feedback_timer = self.create_timer(1.0, self.publish_feedback) # Create subscribers @@ -39,7 +41,8 @@ class SerialRelay(Node): self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10) self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10) - self.arm_feedback = ArmFeedback() + self.arm_feedback = SocketFeedback() + self.digit_feedback = DigitFeedback() # Search for ports IF in 'arm' (standalone) and not 'anchor' mode if self.launch_mode == 'arm': @@ -188,15 +191,19 @@ class SerialRelay(Node): # Extract the voltage from the string voltages_in = parts[3:7] # Convert the voltages to floats - self.arm_feedback.digit_bat_voltage = float(voltages_in[0]) / 100.0 - self.arm_feedback.digit_voltage_12 = float(voltages_in[1]) / 100.0 - self.arm_feedback.digit_voltage_5 = float(voltages_in[2]) / 100.0 - self.arm_feedback.digit_voltage_3 = float(voltages_in[3]) / 100.0 + self.digit_feedback.bat_voltage = float(voltages_in[0]) / 100.0 + self.digit_feedback.voltage_12 = float(voltages_in[1]) / 100.0 + self.digit_feedback.voltage_5 = float(voltages_in[2]) / 100.0 + elif output.startswith("can_relay_fromvic,digit,55"): + parts = msg.split(",") + if len(parts) >= 4: + self.digit_feedback.wrist_angle = float(parts[3]) else: return def publish_feedback(self): self.socket_pub.publish(self.arm_feedback) + self.digit_pub.publish(self.digit_feedback) def updateAngleFeedback(self, msg): # Angle feedbacks, diff --git a/src/bio_pkg/bio_pkg/bio_node.py b/src/bio_pkg/bio_pkg/bio_node.py index bf84cde..2f5d580 100644 --- a/src/bio_pkg/bio_pkg/bio_node.py +++ b/src/bio_pkg/bio_pkg/bio_node.py @@ -184,7 +184,7 @@ 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, 3, 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 diff --git a/src/ros2_interfaces_pkg b/src/ros2_interfaces_pkg index fac196b..3db6c94 160000 --- a/src/ros2_interfaces_pkg +++ b/src/ros2_interfaces_pkg @@ -1 +1 @@ -Subproject commit fac196b5d6efb52db9490c1326629ac2d8f63c4e +Subproject commit 3db6c9483300ddec637c8745bcde3d1017686470 From 6b0ea58e952173a68b4168982af7fd9da4c556f3 Mon Sep 17 00:00:00 2001 From: David Date: Sat, 24 May 2025 00:02:28 +0000 Subject: [PATCH 05/17] feat: add imu calibration for core --- src/core_pkg/core_pkg/core_node.py | 1 + src/ros2_interfaces_pkg | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index a2b5cd3..95228ea 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -203,6 +203,7 @@ class SerialRelay(Node): self.core_feedback.bno_gyro.x = float(parts[3]) self.core_feedback.bno_gyro.y = float(parts[4]) self.core_feedback.bno_gyro.z = float(parts[5]) + self.core_feedback.imu_calib = float(parts[6]) elif output.startswith("can_relay_fromvic,core,52"):#Accel x,y,z, heading *10 self.core_feedback.bno_accel.x = float(parts[3]) self.core_feedback.bno_accel.y = float(parts[4]) diff --git a/src/ros2_interfaces_pkg b/src/ros2_interfaces_pkg index 3db6c94..021dd0c 160000 --- a/src/ros2_interfaces_pkg +++ b/src/ros2_interfaces_pkg @@ -1 +1 @@ -Subproject commit 3db6c9483300ddec637c8745bcde3d1017686470 +Subproject commit 021dd0c3dd0bdbad0e653388fa6c73188257dd9b From f34ef88d6ce2ef4f46ea3871bfb40abab33c5bfe Mon Sep 17 00:00:00 2001 From: David Date: Sat, 24 May 2025 16:03:33 +0000 Subject: [PATCH 06/17] feat: add brake mode to core --- src/core_pkg/core_pkg/core_node.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index 95228ea..3cc1974 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -172,12 +172,15 @@ class SerialRelay(Node): def send_controls(self, msg): #can_relay_tovic,core,19, left_stick, right_stick if(msg.turn_to_enable): - command = "can_relay_tovic,core,41," + msg.turn_to + ',' + msg.turn_to_timeout + '\n' + 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' - self.send_cmd(command) - + + # Brake mode + command = "can_relay_tovic,core,18," + str(int(msg.brake)) + '\n' + self.send_cmd(command) + #print(f"[Sys] Relaying: {command}") def send_cmd(self, msg): if self.launch_mode == 'anchor': From 1d0292b4c926a3f9fceb235139af0df751f70d87 Mon Sep 17 00:00:00 2001 From: David Date: Sun, 25 May 2025 23:25:23 +0000 Subject: [PATCH 07/17] refactor: change servo_position to servo_state wiggle --- src/bio_pkg/bio_pkg/bio_node.py | 4 ++-- src/ros2_interfaces_pkg | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/bio_pkg/bio_pkg/bio_node.py b/src/bio_pkg/bio_pkg/bio_node.py index 2f5d580..8785d3a 100644 --- a/src/bio_pkg/bio_pkg/bio_node.py +++ b/src/bio_pkg/bio_pkg/bio_node.py @@ -138,8 +138,8 @@ class SerialRelay(Node): 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(msg.servo_position) + "\n" - self.send_cmd(command) + command = "can_relay_tovic,citadel,25," + str(msg.servo_id) + "," + str(int(msg.servo_state)) + "\n" + self.send_cmd(command) # LSS (SCYTHE) diff --git a/src/ros2_interfaces_pkg b/src/ros2_interfaces_pkg index 021dd0c..aec2208 160000 --- a/src/ros2_interfaces_pkg +++ b/src/ros2_interfaces_pkg @@ -1 +1 @@ -Subproject commit 021dd0c3dd0bdbad0e653388fa6c73188257dd9b +Subproject commit aec22081e3deb6f7092bb79393ecb234193a75ab From 3ed821f4fbb2643bea7c3b2aded630645fccc60f Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sun, 25 May 2025 18:08:56 -0600 Subject: [PATCH 08/17] ai draft of ptz control code --- rover_launch.py | 27 +- src/core_pkg/core_pkg/core_ptz.py | 281 ++++++++++++++++ src/core_pkg/core_pkg/siyi_sdk/.gitignore | 174 ++++++++++ src/core_pkg/core_pkg/siyi_sdk/README.md | 26 ++ src/core_pkg/core_pkg/siyi_sdk/__init__.py | 1 + src/core_pkg/core_pkg/siyi_sdk/siyi.py | 363 +++++++++++++++++++++ src/core_pkg/core_pkg/siyi_sdk/test.py | 61 ++++ src/core_pkg/setup.py | 3 +- 8 files changed, 931 insertions(+), 5 deletions(-) create mode 100644 src/core_pkg/core_pkg/core_ptz.py create mode 100644 src/core_pkg/core_pkg/siyi_sdk/.gitignore create mode 100644 src/core_pkg/core_pkg/siyi_sdk/README.md create mode 100644 src/core_pkg/core_pkg/siyi_sdk/__init__.py create mode 100644 src/core_pkg/core_pkg/siyi_sdk/siyi.py create mode 100644 src/core_pkg/core_pkg/siyi_sdk/test.py diff --git a/rover_launch.py b/rover_launch.py index 90b4485..1e24f2a 100644 --- a/rover_launch.py +++ b/rover_launch.py @@ -38,6 +38,16 @@ def launch_setup(context, *args, **kwargs): on_exit=Shutdown() ) ) + nodes.append( + Node( + 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', @@ -58,7 +68,7 @@ def launch_setup(context, *args, **kwargs): on_exit=Shutdown() ) ) - elif mode in ['arm', 'core', 'bio']: + elif mode in ['arm', 'core', 'bio', 'ptz']: # Only launch the node corresponding to the provided mode. if mode == 'arm': nodes.append( @@ -93,10 +103,19 @@ def launch_setup(context, *args, **kwargs): on_exit=Shutdown() ) ) + elif mode == 'ptz': + nodes.append( + Node( + package='core_pkg', + executable='ptz', + name='ptz', + output='both' + on_exit=Shutdown(), #on fail, shutdown if this was the only node to be launched + ) + ) else: # If an invalid mode is provided, print an error. - # (You might want to raise an exception or handle it differently.) - print("Invalid mode provided. Choose one of: arm, core, bio, anchor.") + print("Invalid mode provided. Choose one of: arm, core, bio, anchor, ptz.") return nodes @@ -104,7 +123,7 @@ def generate_launch_description(): declare_arg = DeclareLaunchArgument( 'mode', default_value='anchor', - description='Launch mode: arm, core, bio, or anchor' + description='Launch mode: arm, core, bio, anchor, or ptz' ) return LaunchDescription([ diff --git a/src/core_pkg/core_pkg/core_ptz.py b/src/core_pkg/core_pkg/core_ptz.py new file mode 100644 index 0000000..be145dc --- /dev/null +++ b/src/core_pkg/core_pkg/core_ptz.py @@ -0,0 +1,281 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +import asyncio +from concurrent.futures import ThreadPoolExecutor +import signal +import sys +import threading +import time + +from std_msgs.msg import String +from ros2_interfaces_pkg.msg import PtzControl, PtzFeedback + +# Import the SIYI SDK +from core_pkg.siyi_sdk import ( + SiyiGimbalCamera, + CommandID, + DataStreamType, + DataStreamFrequency, + SingleAxis, + 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.17') + self.declare_parameter('camera_port', 37260) + #self.declare_parameter('launch_mode', 'core') + + # Get parameters + self.camera_ip = self.get_parameter('camera_ip').value + self.camera_port = self.get_parameter('camera_port').value + #self.launch_mode = self.get_parameter('launch_mode').value + + #self.get_logger().info(f"PTZ mode: {self.launch_mode}") + 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 + self.loop = None + self.executor = None + + # Create publishers + 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) + + # Create timers + self.connection_timer = self.create_timer(5.0, self.check_camera_connection) + + # Create feedback message + self.feedback_msg = PtzFeedback() + self.feedback_msg.connected = False + self.feedback_msg.error_msg = "Initializing" + + # Flags for async operations + self.shutdown_requested = False + + # Set up asyncio event loop in a separate thread + self.executor = ThreadPoolExecutor(max_workers=1) + self.loop = asyncio.new_event_loop() + + # Connect to camera on startup + self.connect_task = self.executor.submit( + self.run_async_func, self.connect_to_camera() + ) + + async def connect_to_camera(self): + """Connect to the SIYI camera.""" + try: + # Create a new camera instance + self.camera = SiyiGimbalCamera(ip=self.camera_ip, port=self.camera_port) + + # Connect to the camera + await self.camera.connect() + + # Set up data callback + self.camera.set_data_callback(self.camera_data_callback) + + # Request attitude data stream + await self.camera.send_data_stream_request( + DataStreamType.ATTITUDE_DATA, + DataStreamFrequency.HZ_10 + ) + + # Update connection status + self.camera_connected = True + self.feedback_msg.connected = True + self.feedback_msg.error_msg = "" + + self.get_logger().info("Connected to PTZ camera") + self.publish_debug("Camera connected successfully") + + except Exception as e: + self.camera_connected = False + self.feedback_msg.connected = False + self.feedback_msg.error_msg = f"Connection error: {str(e)}" + self.get_logger().error(f"Failed to connect to camera: {e}") + self.publish_debug(f"Camera connection failed: {str(e)}") + + def camera_data_callback(self, cmd_id, data): + """Handle data received from the camera.""" + if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(data, AttitudeData): + # Update feedback message with attitude data + self.feedback_msg.yaw = data.yaw + self.feedback_msg.pitch = data.pitch + self.feedback_msg.roll = data.roll + self.feedback_msg.yaw_velocity = data.yaw_velocity + self.feedback_msg.pitch_velocity = data.pitch_velocity + self.feedback_msg.roll_velocity = data.roll_velocity + + # Publish feedback + self.feedback_pub.publish(self.feedback_msg) + else: + # Log other data for debugging + debug_str = f"Camera data: CMD_ID={cmd_id}, Data={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: + self.get_logger().info("Attempting to reconnect to camera...") + self.connect_task = self.executor.submit( + self.run_async_func, self.connect_to_camera() + ) + + def handle_control_command(self, msg): + """Handle incoming control commands.""" + if not self.camera_connected: + self.get_logger().warning("Camera not connected, ignoring control command") + return + + # Submit the control command to the async executor + self.executor.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.""" + try: + # Check if reset command + if msg.reset: + self.get_logger().info("Resetting camera to center position") + await self.camera.send_attitude_angles_command(0.0, 0.0) + return + + # Process based on control mode + if msg.control_mode == 0: # Relative speed control + # Clamp values between -100 and 100 + turn_yaw = max(-100, min(100, msg.turn_yaw)) + turn_pitch = max(-100, min(100, msg.turn_pitch)) + + self.get_logger().debug(f"Sending rotation command: yaw={turn_yaw}, pitch={turn_pitch}") + await self.camera.send_rotation_command(turn_yaw, turn_pitch) + + elif msg.control_mode == 1: # Absolute angle control + # Clamp angles to valid ranges + yaw = max(-135.0, min(135.0, msg.yaw)) + pitch = max(-90.0, min(90.0, msg.pitch)) + + self.get_logger().debug(f"Sending absolute angles: yaw={yaw}, pitch={pitch}") + await self.camera.send_attitude_angles_command(yaw, pitch) + + elif msg.control_mode == 2: # Single axis control + axis = SingleAxis.YAW if msg.axis_id == 0 else SingleAxis.PITCH + self.get_logger().debug(f"Sending single axis command: axis={axis.name}, angle={msg.angle}") + await self.camera.send_single_axis_attitude_command(msg.angle, axis) + + # Handle data streaming configuration if requested + if msg.stream_type > 0 and msg.stream_freq >= 0: + try: + stream_type = DataStreamType(msg.stream_type) + stream_freq = DataStreamFrequency(msg.stream_freq) + + self.get_logger().info( + f"Setting data stream: type={stream_type.name}, freq={stream_freq.name}" + ) + + await self.camera.send_data_stream_request(stream_type, stream_freq) + + except ValueError: + self.get_logger().error("Invalid stream type or frequency values") + + except Exception as e: + self.get_logger().error(f"Error processing control command: {e}") + self.feedback_msg.error_msg = f"Control error: {str(e)}" + self.feedback_pub.publish(self.feedback_msg) + + def publish_debug(self, message): + """Publish debug message.""" + msg = String() + msg.data = message + self.debug_pub.publish(msg) + + def run_async_func(self, coro): + """Run an async function in the event loop.""" + return asyncio.run_coroutine_threadsafe(coro, self.loop).result() + + async def shutdown(self): + """Perform clean shutdown.""" + self.shutdown_requested = True + + if self.camera and self.camera_connected: + try: + # Stop any data streams + await self.camera.send_data_stream_request( + DataStreamType.ATTITUDE_DATA, + DataStreamFrequency.DISABLE + ) + + # Disconnect from camera + await self.camera.disconnect() + self.get_logger().info("Disconnected from camera") + + except Exception as e: + self.get_logger().error(f"Error during shutdown: {e}") + + def cleanup(self): + """Clean up resources.""" + if self.loop and self.executor: + # Schedule the shutdown coroutine + try: + self.run_async_func(self.shutdown()) + except Exception as e: + self.get_logger().error(f"Error during cleanup: {e}") + + # Shut down the executor + self.executor.shutdown() + + # Close the event loop + self.loop.stop() + self.loop.close() + + self.get_logger().info("PTZ node resources cleaned up") + +def main(args=None): + """Main function.""" + # Initialize ROS + rclpy.init(args=args) + + # Create the node + ptz_node = PtzNode() + + # Create and start event loop for the camera + def run_event_loop(loop): + asyncio.set_event_loop(loop) + loop.run_forever() + + # Start the asyncio loop in a separate thread + asyncio_thread = threading.Thread( + target=run_event_loop, + args=(ptz_node.loop,), + daemon=True + ) + asyncio_thread.start() + + try: + # Spin the node to process callbacks + rclpy.spin(ptz_node) + except KeyboardInterrupt: + pass + finally: + # Clean up + ptz_node.cleanup() + rclpy.shutdown() + +if __name__ == '__main__': + # Register signal handler for clean shutdown + signal.signal(signal.SIGINT, lambda signum, frame: sys.exit(0)) + signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0)) + main() + diff --git a/src/core_pkg/core_pkg/siyi_sdk/.gitignore b/src/core_pkg/core_pkg/siyi_sdk/.gitignore new file mode 100644 index 0000000..0a19790 --- /dev/null +++ b/src/core_pkg/core_pkg/siyi_sdk/.gitignore @@ -0,0 +1,174 @@ +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ +cover/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +.pybuilder/ +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +# For a library or package, you might want to ignore these files since the code is +# intended to run in multiple environments; otherwise, check them in: +# .python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +#Pipfile.lock + +# UV +# Similar to Pipfile.lock, it is generally recommended to include uv.lock in version control. +# This is especially recommended for binary packages to ensure reproducibility, and is more +# commonly ignored for libraries. +#uv.lock + +# poetry +# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. +# This is especially recommended for binary packages to ensure reproducibility, and is more +# commonly ignored for libraries. +# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control +#poetry.lock + +# pdm +# Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. +#pdm.lock +# pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it +# in version control. +# https://pdm.fming.dev/latest/usage/project/#working-with-version-control +.pdm.toml +.pdm-python +.pdm-build/ + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm +__pypackages__/ + +# Celery stuff +celerybeat-schedule +celerybeat.pid + +# SageMath parsed files +*.sage.py + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +# pytype static type analyzer +.pytype/ + +# Cython debug symbols +cython_debug/ + +# PyCharm +# JetBrains specific template is maintained in a separate JetBrains.gitignore that can +# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore +# and can be added to the global gitignore or merged into this file. For a more nuclear +# option (not recommended) you can uncomment the following to ignore the entire idea folder. +#.idea/ + +# Ruff stuff: +.ruff_cache/ + +# PyPI configuration file +.pypirc diff --git a/src/core_pkg/core_pkg/siyi_sdk/README.md b/src/core_pkg/core_pkg/siyi_sdk/README.md new file mode 100644 index 0000000..7277a6f --- /dev/null +++ b/src/core_pkg/core_pkg/siyi_sdk/README.md @@ -0,0 +1,26 @@ +# SIYI Gimbal Camera Python Library + +This library provides a Python interface for controlling SIYI gimbal cameras over TCP. It allows you to: + +- Connect to the camera and maintain a stable connection. +- Control the camera's movement (pan, tilt). +- Request a continuous stream of data (attitude, etc.). + +> [!NOTE] +> The code for this library (including this README) was written almost entirely by an AI assistant, with +> the assistance and direction of a human developer. It probably sucks, but I need sleep, so I'm not going +> to redo it. Sorry. + +## Core Features + +- **Asynchronous communication:** Utilizes `asyncio` for non-blocking operation. +- **Command handling:** Easy-to-use functions for sending control commands. +- **Data streaming:** Ability to receive continuous data streams for real-time monitoring. + +## Key Components + +- `SiyiGimbalCamera` class: Main class for interfacing with the camera. +- Enums: `DataStreamType`, `DataStreamFrequency`, `SingleAxis`, `CommandID` to manage protocol values. +- Logging: Uses the Python `logging` library. + +Refer to the code comments and docstrings for specific function details. diff --git a/src/core_pkg/core_pkg/siyi_sdk/__init__.py b/src/core_pkg/core_pkg/siyi_sdk/__init__.py new file mode 100644 index 0000000..d48c636 --- /dev/null +++ b/src/core_pkg/core_pkg/siyi_sdk/__init__.py @@ -0,0 +1 @@ +from .siyi import * diff --git a/src/core_pkg/core_pkg/siyi_sdk/siyi.py b/src/core_pkg/core_pkg/siyi_sdk/siyi.py new file mode 100644 index 0000000..2704d65 --- /dev/null +++ b/src/core_pkg/core_pkg/siyi_sdk/siyi.py @@ -0,0 +1,363 @@ +import asyncio +import socket +import struct +import logging +from enum import Enum +from dataclasses import dataclass +from typing import Optional, Callable, Any +from crccheck.crc import Crc16 + +# Set up logging +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) # You can adjust the logging level as needed +handler = logging.StreamHandler() # Output to console +formatter = logging.Formatter("%(asctime)s - %(name)s - %(levelname)s - %(message)s") +handler.setFormatter(formatter) +logger.addHandler(handler) + +# Global constant for the heartbeat packet (used in TCP keep-alive) +HEARTBEAT_PACKET = bytes.fromhex("55 66 01 01 00 00 00 00 00 59 8B") + + +class DataStreamType(Enum): + """ + Enumeration for data stream types in the 0x25 command. + + Allows users to select which type of data to stream from the gimbal camera. + """ + + ATTITUDE_DATA = 1 + LASER_RANGE_DATA = 2 + MAGNETIC_ENCODER_ANGLE_DATA = 3 + MOTOR_VOLTAGE_DATA = 4 + + +class DataStreamFrequency(Enum): + """ + Enumeration for data stream frequencies in the 0x25 command. + + Allows the user to select the output (streaming) frequency. + """ + + DISABLE = 0 + HZ_2 = 1 + HZ_4 = 2 + HZ_5 = 3 + HZ_10 = 4 + HZ_20 = 5 + HZ_50 = 6 + HZ_100 = 7 + + +class SingleAxis(Enum): + """ + Enumeration for specifying the controlled axis in the 0x41 single-axis control command. + + YAW corresponds to 0 and PITCH corresponds to 1. + """ + + YAW = 0 + PITCH = 1 + + +class CommandID(Enum): + """ + Enumeration for command identifiers used in the protocol. + + These identifiers are sent as the CMD_ID in a packet and help in matching + responses or processing incoming packets. + """ + + ROTATION_CONTROL = 0x07 + ATTITUDE_ANGLES = 0x0E + SINGLE_AXIS_CONTROL = 0x41 + DATA_STREAM_REQUEST = 0x25 + ATTITUDE_DATA_RESPONSE = 0x0D # Expected response packet for attitude data + + +@dataclass +class AttitudeData: + """ + Class representing the parsed attitude data from the gimbal. + + Angles (yaw, pitch, roll) are in degrees and are scaled using one decimal point precision. + Angular velocities are raw int16 values. + """ + + yaw: float + pitch: float + roll: float + yaw_velocity: int + pitch_velocity: int + roll_velocity: int + + @classmethod + def from_bytes(cls, data: bytes) -> "AttitudeData": + """Create an AttitudeData instance from a byte string.""" + if len(data) != 12: + raise ValueError("Attitude data should be exactly 12 bytes.") + # Unpack six little-endian int16 values + values = struct.unpack(" None: + """Establish a TCP connection with the gimbal camera.""" + try: + self.reader, self.writer = await asyncio.open_connection( + self.ip, self.port + ) + self.is_connected = True + logger.info(f"Connected to {self.ip}:{self.port}") + asyncio.create_task(self.heartbeat_loop()) + # Start the data stream listener in the background. + asyncio.create_task(self._data_stream_listener()) + except (socket.gaierror, ConnectionRefusedError) as e: + self.is_connected = False + logger.error(f"Could not connect: {e}") + raise + + async def disconnect(self) -> None: + """Gracefully disconnect from the gimbal camera.""" + if self.is_connected: + self.is_connected = False + self.writer.close() + await self.writer.wait_closed() + logger.info("Disconnected") + else: + logger.warning("Not connected, cannot disconnect.") + + async def heartbeat_loop(self) -> None: + """Periodically send heartbeat packets to maintain the TCP connection.""" + if not self.is_connected: + logger.warning("Heartbeat loop started before connection was established.") + return + + try: + while self.is_connected: + try: + self.writer.write(HEARTBEAT_PACKET) + await self.writer.drain() + logger.debug("Sent heartbeat packet") + await asyncio.sleep(self.heartbeat_interval) + except (socket.error, BrokenPipeError) as e: + logger.error(f"Connection error in heartbeat loop: {e}") + break + finally: + logger.info("Heartbeat loop stopped.") + if self.is_connected: + await self.disconnect() + + def _build_rotation_packet(self, turn_yaw: int, turn_pitch: int) -> bytes: + """Build a full packet for the 0x07 Gimbal Rotation Control command.""" + packet = bytearray() + packet.extend(b"\x55\x66") # STX bytes + packet.append(0x01) # CTRL (request ACK) + packet.extend(struct.pack(" None: + """Send a rotation control command (0x07) with the given yaw and pitch values.""" + if not self.is_connected: + raise RuntimeError("Socket is not connected, cannot send rotation command.") + packet = self._build_rotation_packet(turn_yaw, turn_pitch) + self.writer.write(packet) + await self.writer.drain() + logger.debug(f"Sent rotation command with yaw {turn_yaw} and pitch {turn_pitch}") + + def _build_attitude_angles_packet(self, yaw: float, pitch: float) -> bytes: + """Build a full packet for the 0x0E 'Set Gimbal Attitude Angles' command.""" + packet = bytearray() + packet.extend(b"\x55\x66") + packet.append(0x01) # CTRL (request ACK) + packet.extend(struct.pack(" None: + """Send the 0x0E 'Set Gimbal Attitude Angles' command to set absolute angles.""" + if not self.is_connected: + raise RuntimeError("Socket is not connected, cannot send attitude angles command.") + 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}°") + + def _build_single_axis_attitude_packet(self, angle: float, axis: SingleAxis) -> bytes: + """Build a full packet for the 0x41 'Single-Axis Attitude Control' command.""" + packet = bytearray() + packet.extend(b"\x55\x66") + packet.append(0x01) # CTRL + packet.extend(struct.pack(" None: + """Send the 0x41 single-axis attitude control command.""" + if not self.is_connected: + raise RuntimeError("Socket is not connected, cannot send single-axis attitude command.") + packet = self._build_single_axis_attitude_packet(angle, axis) + self.writer.write(packet) + await self.writer.drain() + logger.debug( + f"Sent single-axis attitude command for {axis.name.lower()} with angle {angle}°" + ) + + def _build_data_stream_packet(self, data_type: DataStreamType, data_freq: DataStreamFrequency) -> bytes: + """Build a packet for the 0x25 'Request Gimbal to Send Data Stream' command.""" + packet = bytearray() + packet.extend(b"\x55\x66") + packet.append(0x01) # CTRL, request ACK + packet.extend(struct.pack(" None: + """Send the 0x25 command to request a continuous data stream from the gimbal.""" + if not self.is_connected: + raise RuntimeError("Socket is not connected, cannot request data stream.") + packet = self._build_data_stream_packet(data_type, data_freq) + self.writer.write(packet) + await self.writer.drain() + logger.info(f"Sent data stream request: data_type {data_type}, data_freq {data_freq}") + + async def _read_packet(self): + """Read a full packet from the TCP stream following the protocol format.""" + stx = await self.reader.readexactly(2) + if stx != b"\x55\x66": + raise ValueError("Invalid packet start bytes.") + ctrl = await self.reader.readexactly(1) + data_len_bytes = await self.reader.readexactly(2) + data_len = struct.unpack(" AttitudeData: + """Parse the attitude data (12 bytes) into human-readable values.""" + if len(data) != 12: + raise ValueError("Attitude data should be exactly 12 bytes.") + return AttitudeData.from_bytes(data) + + async def _data_stream_listener(self) -> None: + """ + Continuously listen for incoming packets from the gimbal and process them. + + If a data callback is set via set_data_callback(), the callback is called + with the CommandID and the corresponding data (parsed or raw). + """ + while self.is_connected: + try: + cmd_id_int, data = await self._read_packet() + cmd_id = CommandID(cmd_id_int) + except Exception as e: + logger.error(f"Error reading packet: {e}") + continue + + # Process attitude data packets if applicable. + if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and len(data) == 12: + try: + parsed = AttitudeData.from_bytes(data) + except Exception as e: + logger.exception(f"Failed to parse attitude data: {e}") + parsed = None + if self._data_callback: + self._data_callback(cmd_id, parsed) + else: + logger.info(f"Received attitude data: {parsed}") + else: + if self._data_callback: + self._data_callback(cmd_id, data) + else: + logger.info(f"Received packet with CMD_ID {cmd_id}: {data}") + + def set_data_callback(self, callback: Callable[[CommandID, Any], None]) -> None: + """ + Change the callback function used by the data stream listener. + + The callback should be a function that accepts two arguments: + - A CommandID indicating the type of packet received. + - The parsed data (for attitude data, an AttitudeData instance) or raw data. + + :param callback: A callable to be used as the data callback. + """ + self._data_callback = callback diff --git a/src/core_pkg/core_pkg/siyi_sdk/test.py b/src/core_pkg/core_pkg/siyi_sdk/test.py new file mode 100644 index 0000000..24e7693 --- /dev/null +++ b/src/core_pkg/core_pkg/siyi_sdk/test.py @@ -0,0 +1,61 @@ +import asyncio +import logging + +from siyi import ( + SiyiGimbalCamera, + CommandID, + DataStreamType, + DataStreamFrequency, + SingleAxis, +) + +# Set up basic logging for this test script +logging.basicConfig(level=logging.INFO) +logger = logging.getLogger(__name__) + + +async def main(): + # Setup: Define your camera instance and connection parameters + camera = SiyiGimbalCamera(ip="192.168.1.17", port=37260) + + try: + await camera.connect() + # Initial Connection Established: Wait briefly before sending the commands + 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)") + await camera.send_attitude_angles_command(135.0, 0.0) + await asyncio.sleep(5) + + # Command 2: Look down (using relative control) + logger.info("Command 2: Start looking down (relative speed control)") + # The numbers used are example numbers and may be different from the numbers you use. + await camera.send_rotation_command(0, 50) + 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)") + 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") + await camera.send_attitude_angles_command(-135.0, 0.0) + await asyncio.sleep(5) + + # Command 5: Final rotation to the center (using set angles) + logger.info("Command 5: Moving back to the center.") + await camera.send_attitude_angles_command(0, 0) + await asyncio.sleep(5) + except Exception as e: + logger.exception(f"An exception occurred: {e}") + finally: + # Disconnect and cleanup after commands have run. + await camera.disconnect() + logger.info("Test script completed and shutdown") + + +if __name__ == "__main__": + asyncio.run(main()) diff --git a/src/core_pkg/setup.py b/src/core_pkg/setup.py index 77bda6d..0d3d80b 100644 --- a/src/core_pkg/setup.py +++ b/src/core_pkg/setup.py @@ -20,7 +20,8 @@ setup( entry_points={ 'console_scripts': [ "core = core_pkg.core_node:main", - "headless = core_pkg.core_headless:main" + "headless = core_pkg.core_headless:main", + "ptz = core_pkg.core_ptz:main" ], }, ) From 6e4264a2a882021c71a11c454afebc44190ee82b Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sun, 25 May 2025 18:11:51 -0600 Subject: [PATCH 09/17] swap ros2_interfaces submodule to ptz_controller branch while testing --- src/ros2_interfaces_pkg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ros2_interfaces_pkg b/src/ros2_interfaces_pkg index f7f39a6..3965e0f 160000 --- a/src/ros2_interfaces_pkg +++ b/src/ros2_interfaces_pkg @@ -1 +1 @@ -Subproject commit f7f39a635287f242a8feacb1792332c0521fc5a7 +Subproject commit 3965e0f650fc8fe5a59e16073996f084d01bd554 From 9f3ff0f8df28b382d287c53d30773443a569cd55 Mon Sep 17 00:00:00 2001 From: David Date: Mon, 26 May 2025 02:34:16 +0000 Subject: [PATCH 10/17] fix: interface names --- src/bio_pkg/bio_pkg/bio_node.py | 4 ++-- src/core_pkg/core_pkg/core_node.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/bio_pkg/bio_pkg/bio_node.py b/src/bio_pkg/bio_pkg/bio_node.py index 8785d3a..3aa2691 100644 --- a/src/bio_pkg/bio_pkg/bio_node.py +++ b/src/bio_pkg/bio_pkg/bio_node.py @@ -123,7 +123,7 @@ class SerialRelay(Node): pass - def send_control(self, msg): + def send_control(self, msg: BioControl): # CITADEL Control Commands ################ @@ -143,7 +143,7 @@ class SerialRelay(Node): # LSS (SCYTHE) - command = "can_relay_tovic,citadel,24," + str(msg.lss_direction) + "\n" + command = "can_relay_tovic,citadel,24," + str(msg.bio_arm) + "\n" self.send_cmd(command) # Vibration Motor command = "can_relay_tovic,citadel,26," + str(msg.vibration_motor) + "\n" diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index 3cc1974..ffa2d11 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -206,7 +206,7 @@ class SerialRelay(Node): self.core_feedback.bno_gyro.x = float(parts[3]) self.core_feedback.bno_gyro.y = float(parts[4]) self.core_feedback.bno_gyro.z = float(parts[5]) - self.core_feedback.imu_calib = float(parts[6]) + self.core_feedback.imu_calib = int(parts[6]) elif output.startswith("can_relay_fromvic,core,52"):#Accel x,y,z, heading *10 self.core_feedback.bno_accel.x = float(parts[3]) self.core_feedback.bno_accel.y = float(parts[4]) From 7027900620d29a169446e5150cb721d560ba3ec3 Mon Sep 17 00:00:00 2001 From: David Date: Mon, 26 May 2025 02:36:51 +0000 Subject: [PATCH 11/17] fix: round float imu_calib --- src/core_pkg/core_pkg/core_node.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index ffa2d11..0f1052f 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -192,7 +192,7 @@ class SerialRelay(Node): self.get_logger().info(f"[Core to MCU] {msg}") self.ser.write(bytes(msg, "utf8")) - def anchor_feedback(self, msg): + def anchor_feedback(self, msg: String): output = msg.data parts = str(output.strip()).split(",") #self.get_logger().info(f"[ANCHOR FEEDBACK parts] {parts}") @@ -206,7 +206,7 @@ class SerialRelay(Node): self.core_feedback.bno_gyro.x = float(parts[3]) self.core_feedback.bno_gyro.y = float(parts[4]) self.core_feedback.bno_gyro.z = float(parts[5]) - self.core_feedback.imu_calib = int(parts[6]) + self.core_feedback.imu_calib = round(float(parts[6])) elif output.startswith("can_relay_fromvic,core,52"):#Accel x,y,z, heading *10 self.core_feedback.bno_accel.x = float(parts[3]) self.core_feedback.bno_accel.y = float(parts[4]) From 1486b87156435e9c14f53c059eda26e458049e37 Mon Sep 17 00:00:00 2001 From: David Date: Mon, 26 May 2025 02:38:55 +0000 Subject: [PATCH 12/17] fix: drill_duty --- src/bio_pkg/bio_pkg/bio_node.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/bio_pkg/bio_pkg/bio_node.py b/src/bio_pkg/bio_pkg/bio_node.py index 3aa2691..be0103d 100644 --- a/src/bio_pkg/bio_pkg/bio_node.py +++ b/src/bio_pkg/bio_pkg/bio_node.py @@ -160,8 +160,8 @@ class SerialRelay(Node): self.send_cmd(command) # Drill (SCABBARD) - command = f"can_relay_tovic,digit,19,{msg.drill_duty:.2f}\n" - print(msg.drill_duty) + command = f"can_relay_tovic,digit,19,{msg.drill:.2f}\n" + print(msg.drill) self.send_cmd(command) # Bio linear actuator From 7d9ca314a71270bcca651eaaece1c351b91972e9 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sun, 25 May 2025 20:43:49 -0600 Subject: [PATCH 13/17] Functional PTZ control node --- rover_launch.py | 2 +- src/core_pkg/core_pkg/core_ptz.py | 21 ++++++++++----------- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/rover_launch.py b/rover_launch.py index 1e24f2a..73f9316 100644 --- a/rover_launch.py +++ b/rover_launch.py @@ -109,7 +109,7 @@ def launch_setup(context, *args, **kwargs): package='core_pkg', executable='ptz', name='ptz', - output='both' + output='both', on_exit=Shutdown(), #on fail, shutdown if this was the only node to be launched ) ) diff --git a/src/core_pkg/core_pkg/core_ptz.py b/src/core_pkg/core_pkg/core_ptz.py index be145dc..9ce0148 100644 --- a/src/core_pkg/core_pkg/core_ptz.py +++ b/src/core_pkg/core_pkg/core_ptz.py @@ -27,27 +27,26 @@ class PtzNode(Node): super().__init__("core_ptz") # Declare parameters - self.declare_parameter('camera_ip', '192.168.1.17') + self.declare_parameter('camera_ip', '192.168.1.9') self.declare_parameter('camera_port', 37260) - #self.declare_parameter('launch_mode', 'core') # Get parameters self.camera_ip = self.get_parameter('camera_ip').value self.camera_port = self.get_parameter('camera_port').value - #self.launch_mode = self.get_parameter('launch_mode').value - #self.get_logger().info(f"PTZ mode: {self.launch_mode}") 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 self.loop = None - self.executor = None + self.thread_pool = None # Renamed from self.executor to avoid conflict # Create publishers self.feedback_pub = self.create_publisher(PtzFeedback, '/ptz/feedback', 10) self.debug_pub = self.create_publisher(String, '/ptz/debug', 10) + #create timer to publish feedback at a regular interval + self.create_timer(1.0, self.publish_feedback) # Create subscribers self.control_sub = self.create_subscription( @@ -65,11 +64,11 @@ class PtzNode(Node): self.shutdown_requested = False # Set up asyncio event loop in a separate thread - self.executor = ThreadPoolExecutor(max_workers=1) + self.thread_pool = ThreadPoolExecutor(max_workers=1) # Renamed from self.executor self.loop = asyncio.new_event_loop() # Connect to camera on startup - self.connect_task = self.executor.submit( + self.connect_task = self.thread_pool.submit( # Use thread_pool instead of executor self.run_async_func, self.connect_to_camera() ) @@ -128,7 +127,7 @@ class PtzNode(Node): """Periodically check camera connection and attempt to reconnect if needed.""" if not self.camera_connected and not self.shutdown_requested: self.get_logger().info("Attempting to reconnect to camera...") - self.connect_task = self.executor.submit( + self.connect_task = self.thread_pool.submit( # Changed from self.executor self.run_async_func, self.connect_to_camera() ) @@ -139,7 +138,7 @@ class PtzNode(Node): return # Submit the control command to the async executor - self.executor.submit( + self.thread_pool.submit( # Changed from self.executor self.run_async_func, self.process_control_command(msg) ) @@ -226,7 +225,7 @@ class PtzNode(Node): def cleanup(self): """Clean up resources.""" - if self.loop and self.executor: + if self.loop and self.thread_pool: # Changed from self.executor # Schedule the shutdown coroutine try: self.run_async_func(self.shutdown()) @@ -234,7 +233,7 @@ class PtzNode(Node): self.get_logger().error(f"Error during cleanup: {e}") # Shut down the executor - self.executor.shutdown() + self.thread_pool.shutdown() # Changed from self.executor # Close the event loop self.loop.stop() From 39f2aa3a18bd35a65461b7345711c002e4baecff Mon Sep 17 00:00:00 2001 From: David Date: Mon, 26 May 2025 02:55:14 +0000 Subject: [PATCH 14/17] fix: msg.split to msg.data.split --- src/arm_pkg/arm_pkg/arm_node.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 0536590..ab8260f 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -186,7 +186,7 @@ class SerialRelay(Node): #pass self.updateMotorFeedback(output) elif output.startswith("can_relay_fromvic,digit,54"): - parts = msg.split(",") + parts = msg.data.split(",") if len(parts) >= 7: # Extract the voltage from the string voltages_in = parts[3:7] @@ -195,7 +195,7 @@ class SerialRelay(Node): self.digit_feedback.voltage_12 = float(voltages_in[1]) / 100.0 self.digit_feedback.voltage_5 = float(voltages_in[2]) / 100.0 elif output.startswith("can_relay_fromvic,digit,55"): - parts = msg.split(",") + parts = msg.data.split(",") if len(parts) >= 4: self.digit_feedback.wrist_angle = float(parts[3]) else: @@ -205,10 +205,10 @@ class SerialRelay(Node): self.socket_pub.publish(self.arm_feedback) self.digit_pub.publish(self.digit_feedback) - def updateAngleFeedback(self, msg): + def updateAngleFeedback(self, msg: String): # Angle feedbacks, #split the msg.data by commas - parts = msg.split(",") + parts = msg.data.split(",") if len(parts) >= 7: # Extract the angles from the string @@ -241,9 +241,9 @@ class SerialRelay(Node): self.get_logger().info("Invalid angle feedback input format") - def updateBusVoltage(self, msg): + def updateBusVoltage(self, msg: String): # Bus Voltage feedbacks - parts = msg.split(",") + parts = msg.data.split(",") if len(parts) >= 7: # Extract the voltage from the string voltages_in = parts[3:7] @@ -259,7 +259,7 @@ class SerialRelay(Node): def updateMotorFeedback(self, msg): # Motor voltage/current/temperature feedback return - # parts = msg.split(",") + # parts = msg.data.split(",") # if len(parts) >= 7: # # Extract the voltage/current/temperature from the string # values_in = parts[3:7] From 9d10607b2e9f0aa2233ffa2b1ea98acf04506d2f Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sun, 25 May 2025 21:21:28 -0600 Subject: [PATCH 15/17] Remove logger() outputs and assume always connected to prevent breaking --- src/core_pkg/core_pkg/core_ptz.py | 35 ++++++++++++++++++++------ src/core_pkg/core_pkg/siyi_sdk/siyi.py | 25 +++++++++++------- 2 files changed, 44 insertions(+), 16 deletions(-) diff --git a/src/core_pkg/core_pkg/core_ptz.py b/src/core_pkg/core_pkg/core_ptz.py index 9ce0148..47dd2c5 100644 --- a/src/core_pkg/core_pkg/core_ptz.py +++ b/src/core_pkg/core_pkg/core_ptz.py @@ -46,7 +46,7 @@ class PtzNode(Node): self.feedback_pub = self.create_publisher(PtzFeedback, '/ptz/feedback', 10) self.debug_pub = self.create_publisher(String, '/ptz/debug', 10) #create timer to publish feedback at a regular interval - self.create_timer(1.0, self.publish_feedback) + #self.create_timer(1.0, self.publish_feedback) # Create subscribers self.control_sub = self.create_subscription( @@ -54,6 +54,8 @@ class PtzNode(Node): # Create timers self.connection_timer = self.create_timer(5.0, self.check_camera_connection) + self.last_data_time = time.time() + self.health_check_timer = self.create_timer(2.0, self.check_camera_health) # Create feedback message self.feedback_msg = PtzFeedback() @@ -95,19 +97,21 @@ class PtzNode(Node): self.feedback_msg.connected = True self.feedback_msg.error_msg = "" - self.get_logger().info("Connected to PTZ camera") + #self.get_logger().info("Connected to PTZ camera") self.publish_debug("Camera connected successfully") except Exception as e: - self.camera_connected = False + #self.camera_connected = False self.feedback_msg.connected = False self.feedback_msg.error_msg = f"Connection error: {str(e)}" - self.get_logger().error(f"Failed to connect to camera: {e}") + #self.get_logger().error(f"Failed to connect to camera: {e}") self.publish_debug(f"Camera connection failed: {str(e)}") def camera_data_callback(self, cmd_id, data): """Handle data received from the camera.""" if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(data, AttitudeData): + self.last_data_time = time.time() # Update timestamp on successful data + # Update feedback message with attitude data self.feedback_msg.yaw = data.yaw self.feedback_msg.pitch = data.pitch @@ -126,15 +130,32 @@ class PtzNode(Node): 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: - self.get_logger().info("Attempting to reconnect to camera...") - self.connect_task = self.thread_pool.submit( # Changed from self.executor + #self.get_logger().info("Attempting to reconnect to camera...") + if self.camera: + # Fully clean up old connection first + try: + self.run_async_func(self.camera.disconnect()) + except Exception: + pass # Ignore errors during cleanup + self.camera = None + + self.connect_task = self.thread_pool.submit( self.run_async_func, self.connect_to_camera() ) + def check_camera_health(self): + """Check if we're still receiving data from the camera""" + if self.camera_connected: + time_since_last_data = time.time() - self.last_data_time + if time_since_last_data > 5.0: # No data for 5 seconds + #self.get_logger().warning(f"No camera data for {time_since_last_data:.1f}s, connection may be stale") + self.camera_connected = False + # Force reconnection on next check_camera_connection timer + def handle_control_command(self, msg): """Handle incoming control commands.""" if not self.camera_connected: - self.get_logger().warning("Camera not connected, ignoring control command") + #self.get_logger().warning("Camera not connected, ignoring control command") return # Submit the control command to the async executor diff --git a/src/core_pkg/core_pkg/siyi_sdk/siyi.py b/src/core_pkg/core_pkg/siyi_sdk/siyi.py index 2704d65..954da86 100644 --- a/src/core_pkg/core_pkg/siyi_sdk/siyi.py +++ b/src/core_pkg/core_pkg/siyi_sdk/siyi.py @@ -153,7 +153,7 @@ class SiyiGimbalCamera: self.ip, self.port ) self.is_connected = True - logger.info(f"Connected to {self.ip}:{self.port}") + #logger.info(f"Connected to {self.ip}:{self.port}") asyncio.create_task(self.heartbeat_loop()) # Start the data stream listener in the background. asyncio.create_task(self._data_stream_listener()) @@ -168,14 +168,15 @@ class SiyiGimbalCamera: self.is_connected = False self.writer.close() await self.writer.wait_closed() - logger.info("Disconnected") + #logger.info("Disconnected") else: - logger.warning("Not connected, cannot disconnect.") + #logger.warning("Not connected, cannot disconnect.") + pass async def heartbeat_loop(self) -> None: """Periodically send heartbeat packets to maintain the TCP connection.""" if not self.is_connected: - logger.warning("Heartbeat loop started before connection was established.") + #logger.warning("Heartbeat loop started before connection was established.") return try: @@ -183,13 +184,13 @@ class SiyiGimbalCamera: try: self.writer.write(HEARTBEAT_PACKET) await self.writer.drain() - logger.debug("Sent heartbeat packet") + #logger.debug("Sent heartbeat packet") await asyncio.sleep(self.heartbeat_interval) except (socket.error, BrokenPipeError) as e: - logger.error(f"Connection error in heartbeat loop: {e}") + #logger.error(f"Connection error in heartbeat loop: {e}") break finally: - logger.info("Heartbeat loop stopped.") + #logger.info("Heartbeat loop stopped.") if self.is_connected: await self.disconnect() @@ -289,7 +290,7 @@ class SiyiGimbalCamera: packet = self._build_data_stream_packet(data_type, data_freq) self.writer.write(packet) await self.writer.drain() - logger.info(f"Sent data stream request: data_type {data_type}, data_freq {data_freq}") + #logger.info(f"Sent data stream request: data_type {data_type}, data_freq {data_freq}") async def _read_packet(self): """Read a full packet from the TCP stream following the protocol format.""" @@ -329,8 +330,14 @@ class SiyiGimbalCamera: try: cmd_id_int, data = await self._read_packet() cmd_id = CommandID(cmd_id_int) + # Process data as before... + except (ConnectionResetError, BrokenPipeError, ConnectionError) as e: + logger.error(f"Connection error: {e}") + self.is_connected = False + break # Exit the loop to allow reconnection except Exception as e: - logger.error(f"Error reading packet: {e}") + #logger.error(f"Error reading packet: {e}") + ## Continue trying for other errors, but count failures continue # Process attitude data packets if applicable. From a4d3f0dfbe088628c09770a20e3ccd848f7c8991 Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Tue, 27 May 2025 00:39:52 -0500 Subject: [PATCH 16/17] no idea if this works, thanks gemini --- src/core_pkg/core_pkg/core_ptz.py | 296 +++++++++------- src/core_pkg/core_pkg/siyi_sdk/siyi.py | 452 ++++++++++++++++--------- src/ros2_interfaces_pkg | 2 +- 3 files changed, 468 insertions(+), 282 deletions(-) diff --git a/src/core_pkg/core_pkg/core_ptz.py b/src/core_pkg/core_pkg/core_ptz.py index 47dd2c5..15df4ec 100644 --- a/src/core_pkg/core_pkg/core_ptz.py +++ b/src/core_pkg/core_pkg/core_ptz.py @@ -38,15 +38,13 @@ class PtzNode(Node): # Create a camera instance self.camera = None - self.camera_connected = False + self.camera_connected = False # This flag is still managed but not used to gate commands self.loop = None - self.thread_pool = None # Renamed from self.executor to avoid conflict + 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) - #create timer to publish feedback at a regular interval - #self.create_timer(1.0, self.publish_feedback) # Create subscribers self.control_sub = self.create_subscription( @@ -59,18 +57,18 @@ class PtzNode(Node): # Create feedback message self.feedback_msg = PtzFeedback() - self.feedback_msg.connected = False + self.feedback_msg.connected = False # This will reflect the actual connection state self.feedback_msg.error_msg = "Initializing" # Flags for async operations self.shutdown_requested = False # Set up asyncio event loop in a separate thread - self.thread_pool = ThreadPoolExecutor(max_workers=1) # Renamed from self.executor + self.thread_pool = ThreadPoolExecutor(max_workers=1) self.loop = asyncio.new_event_loop() # Connect to camera on startup - self.connect_task = self.thread_pool.submit( # Use thread_pool instead of executor + self.connect_task = self.thread_pool.submit( self.run_async_func, self.connect_to_camera() ) @@ -97,47 +95,53 @@ class PtzNode(Node): self.feedback_msg.connected = True self.feedback_msg.error_msg = "" - #self.get_logger().info("Connected to PTZ camera") self.publish_debug("Camera connected successfully") except Exception as e: - #self.camera_connected = False + self.camera_connected = False self.feedback_msg.connected = False self.feedback_msg.error_msg = f"Connection error: {str(e)}" - #self.get_logger().error(f"Failed to connect to camera: {e}") self.publish_debug(f"Camera connection failed: {str(e)}") def camera_data_callback(self, cmd_id, data): """Handle data received from the camera.""" - if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(data, AttitudeData): - self.last_data_time = time.time() # Update timestamp on successful data - - # Update feedback message with attitude data - self.feedback_msg.yaw = data.yaw - self.feedback_msg.pitch = data.pitch - self.feedback_msg.roll = data.roll - self.feedback_msg.yaw_velocity = data.yaw_velocity - self.feedback_msg.pitch_velocity = data.pitch_velocity - self.feedback_msg.roll_velocity = data.roll_velocity - - # Publish feedback - self.feedback_pub.publish(self.feedback_msg) - else: - # Log other data for debugging - debug_str = f"Camera data: CMD_ID={cmd_id}, Data={data}" - self.get_logger().debug(debug_str) + # 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): + self.feedback_msg.yaw = data.yaw + self.feedback_msg.pitch = data.pitch + self.feedback_msg.roll = data.roll + self.feedback_msg.yaw_velocity = data.yaw_velocity + self.feedback_msg.pitch_velocity = data.pitch_velocity + self.feedback_msg.roll_velocity = data.roll_velocity + self.feedback_pub.publish(self.feedback_msg) + else: + debug_str = "" + if isinstance(cmd_id, CommandID): + debug_str = f"Camera data: CMD_ID={cmd_id.name}, Data=" + else: + debug_str = f"Camera data: CMD_ID={cmd_id}, Data=" + + if isinstance(data, bytes): + debug_str += data.hex() + else: + 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: - #self.get_logger().info("Attempting to reconnect to camera...") + self.publish_debug("Attempting to reconnect to camera...") if self.camera: - # Fully clean up old connection first try: - self.run_async_func(self.camera.disconnect()) - except Exception: - pass # Ignore errors during cleanup - self.camera = None + 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.camera = None # Don't nullify here, connect_to_camera will re-assign or create new self.connect_task = self.thread_pool.submit( self.run_async_func, self.connect_to_camera() @@ -145,157 +149,207 @@ class PtzNode(Node): def check_camera_health(self): """Check if we're still receiving data from the camera""" - if self.camera_connected: + 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: # No data for 5 seconds - #self.get_logger().warning(f"No camera data for {time_since_last_data:.1f}s, connection may be stale") + 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.camera_connected = False - # Force reconnection on next check_camera_connection timer + self.feedback_msg.connected = False + self.feedback_msg.error_msg = "Connection stale (no data)" + self.feedback_pub.publish(self.feedback_msg) def handle_control_command(self, msg): """Handle incoming control commands.""" - if not self.camera_connected: - #self.get_logger().warning("Camera not connected, ignoring control command") + # 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") return - # Submit the control command to the async executor - self.thread_pool.submit( # Changed from self.executor + 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.") + return try: - # Check if reset command + # The SDK's send_... methods will raise RuntimeError if not connected. + # This try-except block will catch those. if msg.reset: - self.get_logger().info("Resetting camera to center position") + self.get_logger().info("Attempting to reset camera to center position") await self.camera.send_attitude_angles_command(0.0, 0.0) return - # Process based on control mode - if msg.control_mode == 0: # Relative speed control - # Clamp values between -100 and 100 - turn_yaw = max(-100, min(100, msg.turn_yaw)) - turn_pitch = max(-100, min(100, msg.turn_pitch)) - - self.get_logger().debug(f"Sending rotation command: yaw={turn_yaw}, pitch={turn_pitch}") + 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}") await self.camera.send_rotation_command(turn_yaw, turn_pitch) - elif msg.control_mode == 1: # Absolute angle control - # Clamp angles to valid ranges + 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"Sending 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: # Single axis control + elif msg.control_mode == 2: axis = SingleAxis.YAW if msg.axis_id == 0 else SingleAxis.PITCH - self.get_logger().debug(f"Sending single axis command: axis={axis.name}, angle={msg.angle}") - await self.camera.send_single_axis_attitude_command(msg.angle, axis) + angle = msg.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") + await self.camera.send_absolute_zoom_command(zoom_level) - # Handle data streaming configuration if requested - if msg.stream_type > 0 and msg.stream_freq >= 0: - try: - stream_type = DataStreamType(msg.stream_type) - stream_freq = DataStreamFrequency(msg.stream_freq) - - self.get_logger().info( - f"Setting data stream: type={stream_type.name}, freq={stream_freq.name}" - ) - - await self.camera.send_data_stream_request(stream_type, stream_freq) - - except ValueError: - self.get_logger().error("Invalid stream type or frequency values") - + 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) + stream_freq = DataStreamFrequency(msg.stream_freq) + 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) + except ValueError: + 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}") + # self.camera_connected will be updated by health/connection checks + # self.feedback_msg.error_msg = f"Command failed: {str(e)}" # Already set by health check + # self.feedback_pub.publish(self.feedback_msg) except Exception as e: self.get_logger().error(f"Error processing control command: {e}") self.feedback_msg.error_msg = f"Control error: {str(e)}" - self.feedback_pub.publish(self.feedback_msg) + self.feedback_pub.publish(self.feedback_msg) # Publish for other errors - def publish_debug(self, message): + def publish_debug(self, message_text): """Publish debug message.""" msg = String() - msg.data = message + 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) def run_async_func(self, coro): """Run an async function in the event loop.""" - return asyncio.run_coroutine_threadsafe(coro, self.loop).result() - - async def shutdown(self): - """Perform clean shutdown.""" - self.shutdown_requested = True - - if self.camera and self.camera_connected: + if self.loop and self.loop.is_running(): try: - # Stop any data streams + 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}") + return None + else: + 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 + self.get_logger().info("Async shutdown initiated...") + if self.camera and self.camera.is_connected: # Check SDK's connection state + try: + self.get_logger().info("Disabling data stream...") await self.camera.send_data_stream_request( - DataStreamType.ATTITUDE_DATA, + DataStreamType.ATTITUDE_DATA, DataStreamFrequency.DISABLE ) + await asyncio.sleep(0.1) - # Disconnect from camera + self.get_logger().info("Disconnecting from camera...") await self.camera.disconnect() - self.get_logger().info("Disconnected from camera") + self.get_logger().info("Disconnected from camera successfully.") except Exception as e: - self.get_logger().error(f"Error during shutdown: {e}") + self.get_logger().error(f"Error during camera shutdown: {e}") + self.camera_connected = False # Update node's flag + self.feedback_msg.connected = False + self.feedback_msg.error_msg = "Shutting down" def cleanup(self): """Clean up resources.""" - if self.loop and self.thread_pool: # Changed from self.executor - # Schedule the shutdown coroutine - try: - self.run_async_func(self.shutdown()) - except Exception as e: - self.get_logger().error(f"Error during cleanup: {e}") + self.get_logger().info("PTZ node cleanup initiated.") + self.shutdown_requested = True + + if self.connection_timer: + self.connection_timer.cancel() + if self.health_check_timer: + self.health_check_timer.cancel() + + 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.result(timeout=5) + except Exception as e: + self.get_logger().error(f"Error during async shutdown in cleanup: {e}") - # Shut down the executor - self.thread_pool.shutdown() # Changed from self.executor + self.get_logger().info("Shutting down thread pool executor...") + self.thread_pool.shutdown(wait=True) - # Close the event loop - self.loop.stop() - self.loop.close() + if self.loop.is_running(): + self.get_logger().info("Stopping asyncio event loop...") + self.loop.call_soon_threadsafe(self.loop.stop) - self.get_logger().info("PTZ node resources cleaned up") + self.get_logger().info("PTZ node resources cleaned up.") + else: + self.get_logger().warning("Loop or thread_pool not initialized, skipping parts of cleanup.") + def main(args=None): """Main function.""" - # Initialize ROS rclpy.init(args=args) - # Create the node ptz_node = PtzNode() - # Create and start event loop for the camera - def run_event_loop(loop): - asyncio.set_event_loop(loop) - loop.run_forever() - - # Start the asyncio loop in a separate thread - asyncio_thread = threading.Thread( - target=run_event_loop, - args=(ptz_node.loop,), - daemon=True - ) - asyncio_thread.start() + asyncio_thread = None + if ptz_node.loop: + def run_event_loop(loop): + asyncio.set_event_loop(loop) + try: + loop.run_forever() + finally: + # This ensures the loop is closed when the thread running it exits. + # This is important if run_forever() exits due to loop.stop() + # or an unhandled exception within a task scheduled on the loop. + if not loop.is_closed(): + loop.close() + + asyncio_thread = threading.Thread( + target=run_event_loop, + args=(ptz_node.loop,), + daemon=True + ) + asyncio_thread.start() try: - # Spin the node to process callbacks rclpy.spin(ptz_node) except KeyboardInterrupt: - pass + ptz_node.get_logger().info("KeyboardInterrupt received, shutting down...") + except SystemExit: + ptz_node.get_logger().info("SystemExit received, shutting down...") finally: - # Clean up - ptz_node.cleanup() + ptz_node.get_logger().info("Initiating final cleanup...") + ptz_node.cleanup() # This will stop the loop and shutdown the executor + + if asyncio_thread and asyncio_thread.is_alive(): + # The loop should have been stopped by cleanup. We just join the thread. + ptz_node.get_logger().info("Waiting for asyncio thread to join...") + asyncio_thread.join(timeout=5) + if asyncio_thread.is_alive(): + ptz_node.get_logger().warning("Asyncio thread did not join cleanly.") + rclpy.shutdown() + ptz_node.get_logger().info("ROS shutdown complete.") + if __name__ == '__main__': - # Register signal handler for clean shutdown - signal.signal(signal.SIGINT, lambda signum, frame: sys.exit(0)) - signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0)) main() - diff --git a/src/core_pkg/core_pkg/siyi_sdk/siyi.py b/src/core_pkg/core_pkg/siyi_sdk/siyi.py index 954da86..a5e426a 100644 --- a/src/core_pkg/core_pkg/siyi_sdk/siyi.py +++ b/src/core_pkg/core_pkg/siyi_sdk/siyi.py @@ -9,11 +9,6 @@ from crccheck.crc import Crc16 # Set up logging logger = logging.getLogger(__name__) -logger.setLevel(logging.INFO) # You can adjust the logging level as needed -handler = logging.StreamHandler() # Output to console -formatter = logging.Formatter("%(asctime)s - %(name)s - %(levelname)s - %(message)s") -handler.setFormatter(formatter) -logger.addHandler(handler) # Global constant for the heartbeat packet (used in TCP keep-alive) HEARTBEAT_PACKET = bytes.fromhex("55 66 01 01 00 00 00 00 00 59 8B") @@ -69,10 +64,11 @@ class CommandID(Enum): """ ROTATION_CONTROL = 0x07 + ABSOLUTE_ZOOM = 0x08 ATTITUDE_ANGLES = 0x0E SINGLE_AXIS_CONTROL = 0x41 DATA_STREAM_REQUEST = 0x25 - ATTITUDE_DATA_RESPONSE = 0x0D # Expected response packet for attitude data + ATTITUDE_DATA_RESPONSE = 0x0D @dataclass @@ -96,7 +92,6 @@ class AttitudeData: """Create an AttitudeData instance from a byte string.""" if len(data) != 12: raise ValueError("Attitude data should be exactly 12 bytes.") - # Unpack six little-endian int16 values values = struct.unpack(" None: - """Establish a TCP connection with the gimbal camera.""" try: self.reader, self.writer = await asyncio.open_connection( self.ip, self.port ) self.is_connected = True - #logger.info(f"Connected to {self.ip}:{self.port}") asyncio.create_task(self.heartbeat_loop()) - # Start the data stream listener in the background. asyncio.create_task(self._data_stream_listener()) except (socket.gaierror, ConnectionRefusedError) as e: self.is_connected = False @@ -163,104 +136,113 @@ class SiyiGimbalCamera: raise async def disconnect(self) -> None: - """Gracefully disconnect from the gimbal camera.""" - if self.is_connected: + if self.is_connected and self.writer: self.is_connected = False self.writer.close() await self.writer.wait_closed() - #logger.info("Disconnected") else: - #logger.warning("Not connected, cannot disconnect.") pass async def heartbeat_loop(self) -> None: - """Periodically send heartbeat packets to maintain the TCP connection.""" - if not self.is_connected: - #logger.warning("Heartbeat loop started before connection was established.") + if not self.is_connected or not self.writer: return - try: while self.is_connected: try: self.writer.write(HEARTBEAT_PACKET) await self.writer.drain() - #logger.debug("Sent heartbeat packet") await asyncio.sleep(self.heartbeat_interval) except (socket.error, BrokenPipeError) as e: - #logger.error(f"Connection error in heartbeat loop: {e}") break finally: - #logger.info("Heartbeat loop stopped.") if self.is_connected: await self.disconnect() - def _build_rotation_packet(self, turn_yaw: int, turn_pitch: int) -> bytes: - """Build a full packet for the 0x07 Gimbal Rotation Control command.""" + 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 bytes - packet.append(0x01) # CTRL (request ACK) - packet.extend(struct.pack(" bytes: + """Helper to add CRC and increment sequence number.""" crc_value = Crc16.calc(packet) packet.extend(struct.pack(" None: - """Send a rotation control command (0x07) with the given yaw and pitch values.""" - if not self.is_connected: - raise RuntimeError("Socket is not connected, cannot send rotation command.") + 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.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: + if not self.is_connected or not self.writer: + raise RuntimeError( + "Socket is not connected or writer is None, cannot send rotation command." + ) packet = self._build_rotation_packet(turn_yaw, turn_pitch) self.writer.write(packet) await self.writer.drain() - logger.debug(f"Sent rotation command with yaw {turn_yaw} and pitch {turn_pitch}") + logger.debug( + 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: - """Build a full packet for the 0x0E 'Set Gimbal Attitude Angles' command.""" - packet = bytearray() - packet.extend(b"\x55\x66") - packet.append(0x01) # CTRL (request ACK) - packet.extend(struct.pack(" bytes: + data_len = 4 + 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(" None: - """Send the 0x0E 'Set Gimbal Attitude Angles' command to set absolute angles.""" - if not self.is_connected: - raise RuntimeError("Socket is not connected, cannot send attitude angles command.") + 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." + ) 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: - """Build a full packet for the 0x41 'Single-Axis Attitude Control' command.""" - packet = bytearray() - packet.extend(b"\x55\x66") - packet.append(0x01) # CTRL - packet.extend(struct.pack(" bytes: + data_len = 3 + packet = self._build_packet_header( + CommandID.SINGLE_AXIS_CONTROL, data_len + ) angle_int = int(round(angle * 10)) packet.extend(struct.pack(" None: - """Send the 0x41 single-axis attitude control command.""" - if not self.is_connected: - raise RuntimeError("Socket is not connected, cannot send single-axis attitude command.") + async def send_single_axis_attitude_command( + self, angle: float, axis: SingleAxis + ) -> None: + if not self.is_connected or not self.writer: + raise RuntimeError( + "Socket is not connected or writer is None, cannot send single-axis attitude command." + ) packet = self._build_single_axis_attitude_packet(angle, axis) self.writer.write(packet) await self.writer.drain() @@ -268,103 +250,253 @@ class SiyiGimbalCamera: f"Sent single-axis attitude command for {axis.name.lower()} with angle {angle}°" ) - def _build_data_stream_packet(self, data_type: DataStreamType, data_freq: DataStreamFrequency) -> bytes: - """Build a packet for the 0x25 'Request Gimbal to Send Data Stream' command.""" - packet = bytearray() - packet.extend(b"\x55\x66") - packet.append(0x01) # CTRL, request ACK - packet.extend(struct.pack(" bytes: + data_len = 2 + 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) - async def send_data_stream_request(self, data_type: DataStreamType, data_freq: DataStreamFrequency) -> None: - """Send the 0x25 command to request a continuous data stream from the gimbal.""" - if not self.is_connected: - raise RuntimeError("Socket is not connected, cannot request data stream.") + async def send_data_stream_request( + self, data_type: DataStreamType, data_freq: DataStreamFrequency + ) -> None: + if not self.is_connected or not self.writer: + raise RuntimeError( + "Socket is not connected or writer is None, cannot request data stream." + ) packet = self._build_data_stream_packet(data_type, data_freq) self.writer.write(packet) await self.writer.drain() - #logger.info(f"Sent data stream request: data_type {data_type}, data_freq {data_freq}") + logger.info( + f"Sent data stream request: data_type {data_type.name}, data_freq {data_freq.name}" + ) + + def _build_absolute_zoom_packet(self, zoom_level: float) -> bytes: + 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 + raise ValueError( + "Zoom packet value out of uint16_t range after conversion." + ) + packet.extend(struct.pack(" None: + """ + Send an absolute zoom command (0x08) to the gimbal. + + :param zoom_level: The desired zoom level as a float (e.g., 1.0 for 1x, + 2.5 for 2.5x). For A8 mini, this will be clamped + between 1.0 and MAX_A8_MINI_ZOOM (6.0). + """ + if not self.is_connected or not self.writer: + raise RuntimeError( + "Socket is not connected or writer is None, cannot send absolute zoom command." + ) + + original_requested_zoom = zoom_level # For logging clarity + + if zoom_level < 1.0: + logger.warning( + f"Requested zoom level {original_requested_zoom:.1f} is less than 1.0. Clamping to 1.0x." + ) + zoom_level = 1.0 + elif zoom_level > self.MAX_A8_MINI_ZOOM: # Check against max zoom + logger.warning( + f"Requested zoom level {original_requested_zoom:.1f} exceeds maximum {self.MAX_A8_MINI_ZOOM:.1f}x for A8 mini. " + f"Clamping to {self.MAX_A8_MINI_ZOOM:.1f}x." + ) + zoom_level = self.MAX_A8_MINI_ZOOM + + packet = self._build_absolute_zoom_packet(zoom_level) + self.writer.write(packet) + await self.writer.drain() + logger.debug( + f"Sent absolute zoom command with level {zoom_level:.1f}x (original request: {original_requested_zoom:.1f}x)" + ) async def _read_packet(self): - """Read a full packet from the TCP stream following the protocol format.""" + if not self.reader: + raise RuntimeError("Reader is not initialized.") stx = await self.reader.readexactly(2) if stx != b"\x55\x66": - raise ValueError("Invalid packet start bytes.") + raise ValueError(f"Invalid packet start bytes: {stx.hex()}") ctrl = await self.reader.readexactly(1) data_len_bytes = await self.reader.readexactly(2) data_len = struct.unpack(" 2048: # Arbitrary reasonable limit + raise ValueError(f"Excessive data length received: {data_len}") + data = await self.reader.readexactly(data_len) crc_bytes = await self.reader.readexactly(2) received_crc = struct.unpack(" AttitudeData: - """Parse the attitude data (12 bytes) into human-readable values.""" if len(data) != 12: raise ValueError("Attitude data should be exactly 12 bytes.") return AttitudeData.from_bytes(data) async def _data_stream_listener(self) -> None: - """ - Continuously listen for incoming packets from the gimbal and process them. - - If a data callback is set via set_data_callback(), the callback is called - with the CommandID and the corresponding data (parsed or raw). - """ while self.is_connected: try: cmd_id_int, data = await self._read_packet() - cmd_id = CommandID(cmd_id_int) - # Process data as before... - except (ConnectionResetError, BrokenPipeError, ConnectionError) as e: - logger.error(f"Connection error: {e}") + try: + cmd_id_enum = CommandID(cmd_id_int) + except ValueError: + logger.warning( + f"Received unknown CMD_ID {cmd_id_int:02X}, data: {data.hex()}" + ) + if self._data_callback: + self._data_callback(cmd_id_int, data) + continue + + if ( + cmd_id_enum == CommandID.ATTITUDE_DATA_RESPONSE + and len(data) == 12 + ): + try: + parsed = AttitudeData.from_bytes(data) + if self._data_callback: + self._data_callback(cmd_id_enum, parsed) + else: + logger.info(f"Received attitude data: {parsed}") + except Exception as e: + logger.exception( + f"Failed to parse attitude data: {e}" + ) + if self._data_callback: + self._data_callback(cmd_id_enum, data) + else: + if self._data_callback: + self._data_callback(cmd_id_enum, data) + else: + logger.info( + f"Received packet with CMD_ID {cmd_id_enum.name} ({cmd_id_enum.value:02X}): {data.hex()}" + ) + + except ( + ConnectionResetError, + BrokenPipeError, + ConnectionError, + asyncio.IncompleteReadError, + ) as e: + logger.error(f"Connection error in listener: {e}") self.is_connected = False - break # Exit the loop to allow reconnection + break + except ValueError as e: + logger.error(f"Packet error in listener: {e}") + # Consider adding a small delay or a mechanism to resync if this happens frequently + await asyncio.sleep(0.1) # Small delay before trying to read again + continue except Exception as e: - #logger.error(f"Error reading packet: {e}") - ## Continue trying for other errors, but count failures + logger.exception(f"Unexpected error in data stream listener: {e}") + # Depending on the error, you might want to break or continue + await asyncio.sleep(0.1) # Small delay continue - # Process attitude data packets if applicable. - if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and len(data) == 12: - try: - parsed = AttitudeData.from_bytes(data) - except Exception as e: - logger.exception(f"Failed to parse attitude data: {e}") - parsed = None - if self._data_callback: - self._data_callback(cmd_id, parsed) - else: - logger.info(f"Received attitude data: {parsed}") - else: - if self._data_callback: - self._data_callback(cmd_id, data) - else: - logger.info(f"Received packet with CMD_ID {cmd_id}: {data}") - - def set_data_callback(self, callback: Callable[[CommandID, Any], None]) -> None: - """ - Change the callback function used by the data stream listener. - - The callback should be a function that accepts two arguments: - - A CommandID indicating the type of packet received. - - The parsed data (for attitude data, an AttitudeData instance) or raw data. - - :param callback: A callable to be used as the data callback. - """ + def set_data_callback( + self, callback: Callable[[CommandID | int, Any], None] + ) -> None: self._data_callback = callback + + +async def main_sdk_test(): # Renamed to avoid conflict if this file is imported + gimbal_ip = "192.168.144.25" + gimbal = SiyiGimbalCamera(gimbal_ip) + + def my_data_handler(cmd_id, data): + 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}" + ) + elif isinstance(cmd_id, CommandID): + print( + f"Received {cmd_id.name}: {data.hex() if isinstance(data, bytes) else data}" + ) + else: + print( + f"Received Unknown CMD_ID {cmd_id:02X}: {data.hex() if isinstance(data, bytes) else data}" + ) + + gimbal.set_data_callback(my_data_handler) + + try: + await gimbal.connect() + if gimbal.is_connected: + print("SDK Test: Successfully connected to the gimbal.") + + print("SDK Test: Setting zoom to 1.0x") + await gimbal.send_absolute_zoom_command(1.0) + await asyncio.sleep(2) + + print("SDK Test: Setting zoom to 3.5x") + await gimbal.send_absolute_zoom_command(3.5) + await asyncio.sleep(2) + + print("SDK Test: Setting zoom to 6.0x (A8 mini max)") + await gimbal.send_absolute_zoom_command(6.0) + await asyncio.sleep(2) + + print("SDK Test: Attempting zoom to 7.0x (should be clamped to 6.0x)") + await gimbal.send_absolute_zoom_command(7.0) + await asyncio.sleep(2) + + print("SDK Test: Attempting zoom to 0.5x (should be clamped to 1.0x)") + await gimbal.send_absolute_zoom_command(0.5) + 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) + + 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 asyncio.sleep(1) + + + except ConnectionRefusedError: + print( + f"SDK Test: Connection to {gimbal_ip} was refused. Is the gimbal on and accessible?" + ) + except Exception as e: + print(f"SDK Test: An error occurred: {e}") + finally: + if gimbal.is_connected: + print("SDK Test: Disconnecting...") + await gimbal.disconnect() + print("SDK Test: Program finished.") + + +if __name__ == "__main__": + logging.basicConfig( + level=logging.INFO, format="%(asctime)s [%(levelname)s] %(name)s: %(message)s" + ) + # To see debug logs from this SDK specifically: + # logger.setLevel(logging.DEBUG) + asyncio.run(main_sdk_test()) diff --git a/src/ros2_interfaces_pkg b/src/ros2_interfaces_pkg index 3965e0f..a412af5 160000 --- a/src/ros2_interfaces_pkg +++ b/src/ros2_interfaces_pkg @@ -1 +1 @@ -Subproject commit 3965e0f650fc8fe5a59e16073996f084d01bd554 +Subproject commit a412af5017e73c4090bc5ebe709546561afcbebe From bff729f25a5cde22d837ab86797c0f50bd7dfca3 Mon Sep 17 00:00:00 2001 From: David Date: Tue, 27 May 2025 19:59:13 +0000 Subject: [PATCH 17/17] style: add a few type annotations --- src/bio_pkg/bio_pkg/bio_node.py | 6 +++--- src/core_pkg/core_pkg/core_node.py | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/bio_pkg/bio_pkg/bio_node.py b/src/bio_pkg/bio_pkg/bio_node.py index be0103d..e901e5b 100644 --- a/src/bio_pkg/bio_pkg/bio_node.py +++ b/src/bio_pkg/bio_pkg/bio_node.py @@ -170,7 +170,7 @@ class SerialRelay(Node): - def send_cmd(self, msg): + def send_cmd(self, msg: str): if self.launch_mode == 'anchor': #if in anchor mode, send to anchor node to relay output = String() output.data = msg @@ -179,7 +179,7 @@ class SerialRelay(Node): self.get_logger().info(f"[Bio to MCU] {msg}") self.ser.write(bytes(msg, "utf8")) - def anchor_feedback(self, msg): + def anchor_feedback(self, msg: String): output = msg.data parts = str(output.strip()).split(",") self.get_logger().info(f"[Bio Anchor] {msg.data}") @@ -188,7 +188,7 @@ class SerialRelay(Node): 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 - if output.startswith("can_relay_fromvic,digit,57"): + elif output.startswith("can_relay_fromvic,digit,57"): self.bio_feedback.drill_temp = float(parts[3]) self.bio_feedback.drill_humidity = float(parts[4]) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index 0f1052f..ca3555b 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -152,7 +152,7 @@ class SerialRelay(Node): self.ser.close() self.exit(1) - def scale_duty(self, value, max_speed): + def scale_duty(self, value: float, max_speed: float): leftMin = -1 leftMax = 1 rightMin = -max_speed/100.0