changed on singlethread ex - better

launch/unity.launch.py

@@ -57,6 +57,7 @@ def generate_launch_description():
         namespace=LaunchConfiguration("namespace"),
         parameters=[
             {"use_sim_time": LaunchConfiguration("use_sim_time")},
+            {"crosshair_alpha": 0.5}
         ],
         remappings=[
             ("raw_image", "/arctic_bot/camera"),

unity_robot_controller/fire_extinguisher_emulator.py

@@ -2,8 +2,8 @@
 
 import rclpy
 from rclpy.node import Node
+from rclpy.executors import SingleThreadedExecutor
 from geometry_msgs.msg import Point, Vector3
-from rclpy.executors import MultiThreadedExecutor
 from sensor_msgs.msg import Image, CameraInfo
 from cv_bridge import CvBridge
 import cv2
@@ -17,7 +17,7 @@ class FireExtinguisherEmulatorNode(Node):
 
         self.declare_parameter('crosshair_alpha', 0.5)
         self.declare_parameter('crosshair_speed', 500.)  # pixel/sec
-        self.declare_parameter('crosshair_color', [255, 0, 0])  # RGB (red)
+        self.declare_parameter('crosshair_color', [255, 0, 0])  # BGR (blue)
 
         self.crosshair_alpha = self.get_parameter('crosshair_alpha').value
         self.crosshair_speed = self.get_parameter('crosshair_speed').value
@@ -74,7 +74,6 @@ class FireExtinguisherEmulatorNode(Node):
             self.current_y = (self.target_y + 1.0) * 0.5 * self.height
 
             # Remove the subscription after getting camera info
-            #self.sub_camera_info = self.sub_camera_info.__class__()  # Disable subscription
             if hasattr(self, 'sub_camera_info'):
                 self.destroy_subscription(self.sub_camera_info)
 
@@ -110,16 +109,13 @@ class FireExtinguisherEmulatorNode(Node):
 
         # Calculate 3D unit vector from camera frame
         if self.fx > 0 and self.fy > 0:
-            # Pixel coordinates to normalized coordinates
             px = self.current_x
             py = self.current_y
 
-            # Inverse projection: (px, py) -> (x, y, z) in camera frame
             x = (px - self.cx) / self.fx
             y = (py - self.cy) / self.fy
             z = 1.0
 
-            # Normalize to unit vector
             norm = np.sqrt(x**2 + y**2 + z**2)
             if norm > 0:
                 x /= norm
@@ -138,40 +134,33 @@ class FireExtinguisherEmulatorNode(Node):
             return
 
         try:
-            # Convert ROS Image to OpenCV image
             cv_image = self.bridge.imgmsg_to_cv2(raw_msg, desired_encoding='bgr8')
         except Exception as e:
             self.get_logger().error(f"Failed to convert image: {e}")
             return
 
-        # Draw crosshair
         crosshair_size = 20
         crosshair_thickness = 2
-        color = tuple(self.crosshair_color)  # RGB -> tuple for OpenCV
+        color = tuple(self.crosshair_color)
 
-        # Horizontal line
         x1 = int(self.current_x - crosshair_size)
         x2 = int(self.current_x + crosshair_size)
         y1 = int(self.current_y)
         y2 = int(self.current_y)
         cv2.line(cv_image, (x1, y1), (x2, y2), color, crosshair_thickness)
 
-        # Vertical line
         x1 = int(self.current_x)
         x2 = int(self.current_x)
         y1 = int(self.current_y - crosshair_size)
         y2 = int(self.current_y + crosshair_size)
         cv2.line(cv_image, (x1, y1), (x2, y2), color, crosshair_thickness)
 
-        # Circle at center
         cv2.circle(cv_image, (int(self.current_x), int(self.current_y)), 5, color, -1)
 
-        # Add alpha transparency effect (simple blend with background)
         if self.crosshair_alpha < 1.0:
             overlay = cv_image.copy()
             cv_image = cv2.addWeighted(cv_image, 1.0, overlay, self.crosshair_alpha, 0)
 
-        # Convert back to ROS Image
         try:
             ros_image = self.bridge.cv2_to_imgmsg(cv_image, encoding='bgr8')
             ros_image.header = raw_msg.header
@@ -179,8 +168,6 @@ class FireExtinguisherEmulatorNode(Node):
         except Exception as e:
             self.get_logger().error(f"Failed to publish image: {e}")
 
-
-
     def destroy_node(self):
         """Clean up subscriptions"""
         if hasattr(self, 'sub_camera_info'):
@@ -191,7 +178,7 @@ class FireExtinguisherEmulatorNode(Node):
 def main(args=None):
     rclpy.init(args=args)
     node = FireExtinguisherEmulatorNode()
-    executor = MultiThreadedExecutor(num_threads=4)
+    executor = SingleThreadedExecutor()
     executor.add_node(node)
     try:
         rclpy.spin(node, executor)