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

Functional PTZ control node

Browse Source
This commit is contained in:
Tristan McGinnis
2025-05-25 20:43:49 -06:00
parent 6e4264a2a8
commit 7d9ca314a7
2 changed files with 11 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
rover_launch.py
View File

@@ -109,7 +109,7 @@ def launch_setup(context, *args, **kwargs):
package='core_pkg', package='core_pkg',
executable='ptz', executable='ptz',
name='ptz', name='ptz',
output='both' output='both',
on_exit=Shutdown(), #on fail, shutdown if this was the only node to be launched on_exit=Shutdown(), #on fail, shutdown if this was the only node to be launched
) )
) )

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

@@ -27,27 +27,26 @@ class PtzNode(Node):
super().__init__("core_ptz") super().__init__("core_ptz")
# Declare parameters # 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('camera_port', 37260)
#self.declare_parameter('launch_mode', 'core')
# Get parameters # Get parameters
self.camera_ip = self.get_parameter('camera_ip').value self.camera_ip = self.get_parameter('camera_ip').value
self.camera_port = self.get_parameter('camera_port').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}") self.get_logger().info(f"PTZ camera IP: {self.camera_ip} Port: {self.camera_port}")
# Create a camera instance # Create a camera instance
self.camera = None self.camera = None
self.camera_connected = False self.camera_connected = False
self.loop = None self.loop = None
self.executor = None self.thread_pool = None # Renamed from self.executor to avoid conflict
# Create publishers # Create publishers
self.feedback_pub = self.create_publisher(PtzFeedback, '/ptz/feedback', 10) self.feedback_pub = self.create_publisher(PtzFeedback, '/ptz/feedback', 10)
self.debug_pub = self.create_publisher(String, '/ptz/debug', 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 # Create subscribers
self.control_sub = self.create_subscription( self.control_sub = self.create_subscription(
@@ -65,11 +64,11 @@ class PtzNode(Node):
self.shutdown_requested = False self.shutdown_requested = False
# Set up asyncio event loop in a separate thread # 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() self.loop = asyncio.new_event_loop()
# Connect to camera on startup # 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() 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.""" """Periodically check camera connection and attempt to reconnect if needed."""
if not self.camera_connected and not self.shutdown_requested: if not self.camera_connected and not self.shutdown_requested:
self.get_logger().info("Attempting to reconnect to camera...") 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() self.run_async_func, self.connect_to_camera()
) )
@@ -139,7 +138,7 @@ class PtzNode(Node):
return return
# Submit the control command to the async executor # Submit the control command to the async executor
self.executor.submit( self.thread_pool.submit( # Changed from self.executor
self.run_async_func, self.run_async_func,
self.process_control_command(msg) self.process_control_command(msg)
) )
@@ -226,7 +225,7 @@ class PtzNode(Node):
def cleanup(self): def cleanup(self):
"""Clean up resources.""" """Clean up resources."""
if self.loop and self.executor: if self.loop and self.thread_pool: # Changed from self.executor
# Schedule the shutdown coroutine # Schedule the shutdown coroutine
try: try:
self.run_async_func(self.shutdown()) self.run_async_func(self.shutdown())
@@ -234,7 +233,7 @@ class PtzNode(Node):
self.get_logger().error(f"Error during cleanup: {e}") self.get_logger().error(f"Error during cleanup: {e}")
# Shut down the executor # Shut down the executor
self.executor.shutdown() self.thread_pool.shutdown() # Changed from self.executor
# Close the event loop # Close the event loop
self.loop.stop() self.loop.stop()