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

reformat with black

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

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

@@ -17,17 +17,20 @@ from ros2_interfaces_pkg.msg import CoreControl
os.environ["SDL_VIDEODRIVER"] = "dummy" # Prevents pygame from trying to open a display
os.environ["SDL_AUDIODRIVER"] = "dummy" # Force pygame to use a dummy audio driver before pygame.init()
os.environ["SDL_AUDIODRIVER"] = (
"dummy" # Force pygame to use a dummy audio driver before pygame.init()
)
max_speed = 90 #Max speed as a duty cycle percentage (1-100)
max_speed = 90 # Max speed as a duty cycle percentage (1-100)
class Headless(Node):
def __init__(self):
# Initialize pygame first
pygame.init()
pygame.joystick.init()
# Wait for a gamepad to be connected
self.gamepad = None
print("Waiting for gamepad connection...")
@@ -39,23 +42,25 @@ class Headless(Node):
sys.exit(0)
time.sleep(1.0) # Check every second
print("No gamepad found. Waiting...")
# Initialize the gamepad
self.gamepad = pygame.joystick.Joystick(0)
self.gamepad.init()
print(f'Gamepad Found: {self.gamepad.get_name()}')
print(f"Gamepad Found: {self.gamepad.get_name()}")
# Now initialize the ROS2 node
super().__init__("core_headless")
self.create_timer(0.15, self.send_controls)
self.publisher = self.create_publisher(CoreControl, '/core/control', 10)
self.lastMsg = String() # Used to ignore sending controls repeatedly when they do not change
self.publisher = self.create_publisher(CoreControl, "/core/control", 10)
self.lastMsg = (
String()
) # Used to ignore sending controls repeatedly when they do not change
def run(self):
# This thread makes all the update processes run in the background
thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
thread.start()
try:
while rclpy.ok():
self.send_controls()
@@ -68,7 +73,7 @@ class Headless(Node):
if event.type == pygame.QUIT:
pygame.quit()
sys.exit(0)
# Check if controller is still connected
if pygame.joystick.get_count() == 0:
print("Gamepad disconnected. Exiting...")
@@ -82,7 +87,7 @@ class Headless(Node):
# Clean up
pygame.quit()
sys.exit(0)
input = CoreControl()
input.max_speed = max_speed
input.right_stick = -1 * round(self.gamepad.get_axis(4), 2) # right y-axis
@@ -91,15 +96,17 @@ class Headless(Node):
else:
input.left_stick = -1 * round(self.gamepad.get_axis(1), 2) # left y-axis
output = f'L: {input.left_stick}, R: {input.right_stick}, M: {max_speed}'
output = f"L: {input.left_stick}, R: {input.right_stick}, M: {max_speed}"
self.get_logger().info(f"[Ctrl] {output}")
self.publisher.publish(input)
def main(args=None):
rclpy.init(args=args)
node = Headless()
rclpy.spin(node)
rclpy.shutdown()
if __name__ == '__main__':
if __name__ == "__main__":
main()

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

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

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

@@ -18,55 +18,63 @@ from core_pkg.siyi_sdk import (
DataStreamType,
DataStreamFrequency,
SingleAxis,
AttitudeData
AttitudeData,
)
class PtzNode(Node):
def __init__(self):
# Initialize node with name
super().__init__("core_ptz")
# Declare parameters
self.declare_parameter('camera_ip', '192.168.1.9')
self.declare_parameter('camera_port', 37260)
self.declare_parameter("camera_ip", "192.168.1.9")
self.declare_parameter("camera_port", 37260)
# Get parameters
self.camera_ip = self.get_parameter('camera_ip').value
self.camera_port = self.get_parameter('camera_port').value
self.get_logger().info(f"PTZ camera IP: {self.camera_ip} Port: {self.camera_port}")
self.camera_ip = self.get_parameter("camera_ip").value
self.camera_port = self.get_parameter("camera_port").value
self.get_logger().info(
f"PTZ camera IP: {self.camera_ip} Port: {self.camera_port}"
)
# Create a camera instance
self.camera = None
self.camera_connected = False # This flag is still managed but not used to gate commands
self.camera_connected = (
False # This flag is still managed but not used to gate commands
)
self.loop = None
self.thread_pool = None
# Create publishers
self.feedback_pub = self.create_publisher(PtzFeedback, '/ptz/feedback', 10)
self.debug_pub = self.create_publisher(String, '/ptz/debug', 10)
self.feedback_pub = self.create_publisher(PtzFeedback, "/ptz/feedback", 10)
self.debug_pub = self.create_publisher(String, "/ptz/debug", 10)
# Create subscribers
self.control_sub = self.create_subscription(
PtzControl, '/ptz/control', self.handle_control_command, 10)
PtzControl, "/ptz/control", self.handle_control_command, 10
)
# Create timers
self.connection_timer = self.create_timer(5.0, self.check_camera_connection)
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()
self.feedback_msg.connected = False # This will reflect the actual connection state
self.feedback_msg.connected = (
False # This will reflect the actual connection state
)
self.feedback_msg.error_msg = "Initializing"
# Flags for async operations
self.shutdown_requested = False
# Set up asyncio event loop in a separate thread
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(
self.run_async_func, self.connect_to_camera()
@@ -77,28 +85,27 @@ class PtzNode(Node):
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
DataStreamType.ATTITUDE_DATA, DataStreamFrequency.HZ_10
)
# Update connection status
self.camera_connected = True
self.feedback_msg.connected = True
self.feedback_msg.error_msg = ""
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.publish_debug(f"Camera connection failed: {str(e)}")
@@ -108,8 +115,12 @@ class PtzNode(Node):
# Update last_data_time regardless of self.camera_connected,
# as data might arrive during a brief reconnect window.
self.last_data_time = time.time()
if self.camera_connected: # Only process for feedback if we believe we are connected
if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(data, AttitudeData):
if (
self.camera_connected
): # Only process for feedback if we believe we are connected
if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(
data, AttitudeData
):
self.feedback_msg.yaw = data.yaw
self.feedback_msg.pitch = data.pitch
self.feedback_msg.roll = data.roll
@@ -123,36 +134,39 @@ class PtzNode(Node):
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.publish_debug("Attempting to reconnect to camera...")
if self.camera:
try:
if self.camera.is_connected: # SDK's internal connection state
self.run_async_func(self.camera.disconnect())
if self.camera.is_connected: # SDK's internal connection state
self.run_async_func(self.camera.disconnect())
except Exception as e:
self.get_logger().debug(f"Error during pre-reconnect disconnect: {e}")
self.get_logger().debug(
f"Error during pre-reconnect disconnect: {e}"
)
# self.camera = None # Don't nullify here, connect_to_camera will re-assign or create new
self.connect_task = self.thread_pool.submit(
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: # Only check health if we think we are 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:
self.publish_debug(f"No camera data for {time_since_last_data:.1f}s, marking as disconnected.")
self.publish_debug(
f"No camera data for {time_since_last_data:.1f}s, marking as disconnected."
)
self.camera_connected = False
self.feedback_msg.connected = False
self.feedback_msg.error_msg = "Connection stale (no data)"
@@ -161,19 +175,20 @@ class PtzNode(Node):
def handle_control_command(self, msg):
"""Handle incoming control commands."""
# Removed: if not self.camera_connected
if not self.camera: # Still check if camera object exists
self.get_logger().warning("Camera object not initialized, ignoring control command")
if not self.camera: # Still check if camera object exists
self.get_logger().warning(
"Camera object not initialized, ignoring control command"
)
return
self.thread_pool.submit(
self.run_async_func,
self.process_control_command(msg)
)
self.thread_pool.submit(self.run_async_func, self.process_control_command(msg))
async def process_control_command(self, msg):
"""Process and send the control command to the camera."""
if not self.camera:
self.get_logger().error("Process control command called but camera object is None.")
if not self.camera:
self.get_logger().error(
"Process control command called but camera object is None."
)
return
try:
# The SDK's send_... methods will raise RuntimeError if not connected.
@@ -182,43 +197,55 @@ class PtzNode(Node):
self.get_logger().info("Attempting to reset camera to center position")
await self.camera.send_attitude_angles_command(0.0, 0.0)
return
if msg.control_mode == 0:
turn_yaw = max(-100, min(100, int(msg.turn_yaw)))
turn_pitch = max(-100, min(100, int(msg.turn_pitch)))
self.get_logger().debug(f"Attempting rotation: yaw_speed={turn_yaw}, pitch_speed={turn_pitch}")
self.get_logger().debug(
f"Attempting rotation: yaw_speed={turn_yaw}, pitch_speed={turn_pitch}"
)
await self.camera.send_rotation_command(turn_yaw, turn_pitch)
elif msg.control_mode == 1:
yaw = max(-135.0, min(135.0, msg.yaw))
pitch = max(-90.0, min(90.0, msg.pitch))
self.get_logger().debug(f"Attempting absolute angles: yaw={yaw}, pitch={pitch}")
self.get_logger().debug(
f"Attempting absolute angles: yaw={yaw}, pitch={pitch}"
)
await self.camera.send_attitude_angles_command(yaw, pitch)
elif msg.control_mode == 2:
axis = SingleAxis.YAW if msg.axis_id == 0 else SingleAxis.PITCH
angle = msg.angle
self.get_logger().debug(f"Attempting single axis: axis={axis.name}, angle={angle}")
self.get_logger().debug(
f"Attempting single axis: axis={axis.name}, angle={angle}"
)
await self.camera.send_single_axis_attitude_command(angle, axis)
elif msg.control_mode == 3:
elif msg.control_mode == 3:
zoom_level = msg.zoom_level
self.get_logger().debug(f"Attempting absolute zoom: level={zoom_level}x")
self.get_logger().debug(
f"Attempting absolute zoom: level={zoom_level}x"
)
await self.camera.send_absolute_zoom_command(zoom_level)
if hasattr(msg, 'stream_type') and hasattr(msg, 'stream_freq'):
if msg.stream_type > 0 and msg.stream_freq >= 0:
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)
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().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
@@ -226,58 +253,64 @@ class PtzNode(Node):
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) # Publish for other errors
self.feedback_pub.publish(self.feedback_msg) # Publish for other errors
def publish_debug(self, message_text):
"""Publish debug message."""
msg = String()
msg.data = f"[{self.get_clock().now().nanoseconds / 1e9:.2f}] PTZ Node: {message_text}"
msg.data = (
f"[{self.get_clock().now().nanoseconds / 1e9:.2f}] PTZ Node: {message_text}"
)
self.debug_pub.publish(msg)
self.get_logger().info(message_text)
self.get_logger().info(message_text)
def run_async_func(self, coro):
"""Run an async function in the event loop."""
if self.loop and self.loop.is_running():
try:
return asyncio.run_coroutine_threadsafe(coro, self.loop).result(timeout=5.0) # Added timeout
return asyncio.run_coroutine_threadsafe(coro, self.loop).result(
timeout=5.0
) # Added timeout
except asyncio.TimeoutError:
self.get_logger().warning(f"Async function {coro.__name__} timed out.")
return None
except Exception as e:
self.get_logger().error(f"Exception in run_async_func for {coro.__name__}: {e}")
self.get_logger().error(
f"Exception in run_async_func for {coro.__name__}: {e}"
)
return None
else:
self.get_logger().warning("Asyncio loop not running, cannot execute coroutine.")
self.get_logger().warning(
"Asyncio loop not running, cannot execute coroutine."
)
return None
async def shutdown_node_async(self):
"""Perform clean shutdown of camera connection."""
self.shutdown_requested = True
self.get_logger().info("Async shutdown initiated...")
if self.camera and self.camera.is_connected: # Check SDK's connection state
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,
DataStreamFrequency.DISABLE
DataStreamType.ATTITUDE_DATA, DataStreamFrequency.DISABLE
)
await asyncio.sleep(0.1)
await asyncio.sleep(0.1)
self.get_logger().info("Disconnecting from camera...")
await self.camera.disconnect()
self.get_logger().info("Disconnected from camera successfully.")
except Exception as e:
self.get_logger().error(f"Error during camera shutdown: {e}")
self.camera_connected = False # Update node's flag
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."""
self.get_logger().info("PTZ node cleanup initiated.")
self.shutdown_requested = True
self.shutdown_requested = True
if self.connection_timer:
self.connection_timer.cancel()
@@ -287,31 +320,38 @@ class PtzNode(Node):
if self.loop and self.thread_pool:
if self.loop.is_running():
try:
future = asyncio.run_coroutine_threadsafe(self.shutdown_node_async(), self.loop)
future.result(timeout=5)
future = asyncio.run_coroutine_threadsafe(
self.shutdown_node_async(), self.loop
)
future.result(timeout=5)
except Exception as e:
self.get_logger().error(f"Error during async shutdown in cleanup: {e}")
self.get_logger().error(
f"Error during async shutdown in cleanup: {e}"
)
self.get_logger().info("Shutting down thread pool executor...")
self.thread_pool.shutdown(wait=True)
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.")
else:
self.get_logger().warning("Loop or thread_pool not initialized, skipping parts of cleanup.")
self.get_logger().warning(
"Loop or thread_pool not initialized, skipping parts of cleanup."
)
def main(args=None):
"""Main function."""
rclpy.init(args=args)
ptz_node = PtzNode()
asyncio_thread = None
if ptz_node.loop:
if ptz_node.loop:
def run_event_loop(loop):
asyncio.set_event_loop(loop)
try:
@@ -322,14 +362,12 @@ def main(args=None):
# 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
target=run_event_loop, args=(ptz_node.loop,), daemon=True
)
asyncio_thread.start()
try:
rclpy.spin(ptz_node)
except KeyboardInterrupt:
@@ -338,18 +376,18 @@ def main(args=None):
ptz_node.get_logger().info("SystemExit received, shutting down...")
finally:
ptz_node.get_logger().info("Initiating final cleanup...")
ptz_node.cleanup() # This will stop the loop and shutdown the executor
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.")
# 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__':
if __name__ == "__main__":
main()

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

@@ -110,9 +110,7 @@ class SiyiGimbalCamera:
MAX_A8_MINI_ZOOM = 6.0 # Maximum zoom for A8 mini
def __init__(
self, ip: str, port: int = 37260, *, heartbeat_interval: int = 2
):
def __init__(self, ip: str, port: int = 37260, *, heartbeat_interval: int = 2):
self.ip = ip
self.port = port
self.heartbeat_interval = heartbeat_interval
@@ -124,9 +122,7 @@ class SiyiGimbalCamera:
async def connect(self) -> None:
try:
self.reader, self.writer = await asyncio.open_connection(
self.ip, self.port
)
self.reader, self.writer = await asyncio.open_connection(self.ip, self.port)
self.is_connected = True
asyncio.create_task(self.heartbeat_loop())
asyncio.create_task(self._data_stream_listener())
@@ -158,9 +154,7 @@ class SiyiGimbalCamera:
if self.is_connected:
await self.disconnect()
def _build_packet_header(
self, cmd_id: CommandID, data_len: int
) -> bytearray:
def _build_packet_header(self, cmd_id: CommandID, data_len: int) -> bytearray:
"""Helper to build the common packet header."""
packet = bytearray()
packet.extend(b"\x55\x66") # STX
@@ -179,15 +173,11 @@ class SiyiGimbalCamera:
def _build_rotation_packet(self, turn_yaw: int, turn_pitch: int) -> bytes:
data_len = 2
packet = self._build_packet_header(
CommandID.ROTATION_CONTROL, data_len
)
packet = self._build_packet_header(CommandID.ROTATION_CONTROL, data_len)
packet.extend(struct.pack("bb", turn_yaw, turn_pitch))
return self._finalize_packet(packet)
async def send_rotation_command(
self, turn_yaw: int, turn_pitch: int
) -> None:
async def send_rotation_command(self, turn_yaw: int, turn_pitch: int) -> None:
if not self.is_connected or not self.writer:
raise RuntimeError(
"Socket is not connected or writer is None, cannot send rotation command."
@@ -199,21 +189,15 @@ class SiyiGimbalCamera:
f"Sent rotation command with yaw_speed {turn_yaw} and pitch_speed {turn_pitch}"
)
def _build_attitude_angles_packet(
self, yaw: float, pitch: float
) -> bytes:
def _build_attitude_angles_packet(self, yaw: float, pitch: float) -> bytes:
data_len = 4
packet = self._build_packet_header(
CommandID.ATTITUDE_ANGLES, data_len
)
packet = self._build_packet_header(CommandID.ATTITUDE_ANGLES, data_len)
yaw_int = int(round(yaw * 10))
pitch_int = int(round(pitch * 10))
packet.extend(struct.pack("<hh", yaw_int, pitch_int))
return self._finalize_packet(packet)
async def send_attitude_angles_command(
self, yaw: float, pitch: float
) -> None:
async def send_attitude_angles_command(self, yaw: float, pitch: float) -> None:
if not self.is_connected or not self.writer:
raise RuntimeError(
"Socket is not connected or writer is None, cannot send attitude angles command."
@@ -221,17 +205,13 @@ class SiyiGimbalCamera:
packet = self._build_attitude_angles_packet(yaw, pitch)
self.writer.write(packet)
await self.writer.drain()
logger.debug(
f"Sent attitude angles command with yaw {yaw}° and pitch {pitch}°"
)
logger.debug(f"Sent attitude angles command with yaw {yaw}° and pitch {pitch}°")
def _build_single_axis_attitude_packet(
self, angle: float, axis: SingleAxis
) -> bytes:
data_len = 3
packet = self._build_packet_header(
CommandID.SINGLE_AXIS_CONTROL, data_len
)
packet = self._build_packet_header(CommandID.SINGLE_AXIS_CONTROL, data_len)
angle_int = int(round(angle * 10))
packet.extend(struct.pack("<hB", angle_int, axis.value))
return self._finalize_packet(packet)
@@ -254,9 +234,7 @@ class SiyiGimbalCamera:
self, data_type: DataStreamType, data_freq: DataStreamFrequency
) -> bytes:
data_len = 2
packet = self._build_packet_header(
CommandID.DATA_STREAM_REQUEST, data_len
)
packet = self._build_packet_header(CommandID.DATA_STREAM_REQUEST, data_len)
packet.append(data_type.value)
packet.append(data_freq.value)
return self._finalize_packet(packet)
@@ -279,7 +257,9 @@ class SiyiGimbalCamera:
data_len = 2
packet = self._build_packet_header(CommandID.ABSOLUTE_ZOOM, data_len)
zoom_packet_value = int(round(zoom_level * 10))
if not (0 <= zoom_packet_value <= 65535): # Should be caught by clamping earlier
if not (
0 <= zoom_packet_value <= 65535
): # Should be caught by clamping earlier
raise ValueError(
"Zoom packet value out of uint16_t range after conversion."
)
@@ -329,24 +309,24 @@ class SiyiGimbalCamera:
ctrl = await self.reader.readexactly(1)
data_len_bytes = await self.reader.readexactly(2)
data_len = struct.unpack("<H", data_len_bytes)[0]
seq_bytes = await self.reader.readexactly(2) # Renamed for clarity
seq_bytes = await self.reader.readexactly(2) # Renamed for clarity
# seq_val = struct.unpack("<H", seq_bytes)[0] # If you need the sequence value
cmd_id_bytes = await self.reader.readexactly(1)
cmd_id_val = cmd_id_bytes[0] # Renamed for clarity
cmd_id_val = cmd_id_bytes[0] # Renamed for clarity
# Protect against excessively large data_len
if data_len > 2048: # Arbitrary reasonable limit
if data_len > 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("<H", crc_bytes)[0]
packet_without_crc = (
stx + ctrl + data_len_bytes + seq_bytes + cmd_id_bytes + data
)
computed_crc = Crc16.calc(packet_without_crc)
if computed_crc != received_crc:
raise ValueError(
f"CRC check failed. Expected {computed_crc:04X}, got {received_crc:04X}. "
@@ -374,10 +354,7 @@ class SiyiGimbalCamera:
self._data_callback(cmd_id_int, data)
continue
if (
cmd_id_enum == CommandID.ATTITUDE_DATA_RESPONSE
and len(data) == 12
):
if cmd_id_enum == CommandID.ATTITUDE_DATA_RESPONSE and len(data) == 12:
try:
parsed = AttitudeData.from_bytes(data)
if self._data_callback:
@@ -385,9 +362,7 @@ class SiyiGimbalCamera:
else:
logger.info(f"Received attitude data: {parsed}")
except Exception as e:
logger.exception(
f"Failed to parse attitude data: {e}"
)
logger.exception(f"Failed to parse attitude data: {e}")
if self._data_callback:
self._data_callback(cmd_id_enum, data)
else:
@@ -410,12 +385,12 @@ class SiyiGimbalCamera:
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
await asyncio.sleep(0.1) # Small delay before trying to read again
continue
except Exception as e:
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
await asyncio.sleep(0.1) # Small delay
continue
def set_data_callback(
@@ -424,12 +399,14 @@ class SiyiGimbalCamera:
self._data_callback = callback
async def main_sdk_test(): # Renamed to avoid conflict if this file is imported
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):
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}"
)
@@ -460,7 +437,7 @@ async def main_sdk_test(): # Renamed to avoid conflict if this file is imported
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)
@@ -470,16 +447,19 @@ async def main_sdk_test(): # Renamed to avoid conflict if this file is imported
await asyncio.sleep(2)
print("SDK Test: Requesting attitude data stream at 5Hz...")
await gimbal.send_data_stream_request(DataStreamType.ATTITUDE_DATA, DataStreamFrequency.HZ_5)
await gimbal.send_data_stream_request(
DataStreamType.ATTITUDE_DATA, DataStreamFrequency.HZ_5
)
print("SDK Test: Listening for data for 10 seconds...")
await asyncio.sleep(10)
print("SDK Test: Disabling attitude data stream...")
await gimbal.send_data_stream_request(DataStreamType.ATTITUDE_DATA, DataStreamFrequency.DISABLE)
await gimbal.send_data_stream_request(
DataStreamType.ATTITUDE_DATA, DataStreamFrequency.DISABLE
)
await asyncio.sleep(1)
except ConnectionRefusedError:
print(
f"SDK Test: Connection to {gimbal_ip} was refused. Is the gimbal on and accessible?"

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

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

25
src/core_pkg/setup.py
View File

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