upvise

unity_robot_controller/fire_extinguisher_emulator.py

@@ -16,7 +16,7 @@ class FireExtinguisherEmulatorNode(Node):
         super().__init__('fire_extinguisher_emulator')
 
         self.declare_parameter('crosshair_alpha', 0.5)
-        self.declare_parameter('crosshair_speed', 1.)  # pixel/sec
+        self.declare_parameter('crosshair_speed', 500.)  # pixel/sec
         self.declare_parameter('crosshair_color', [255, 0, 0])  # RGB (red)
 
         self.crosshair_alpha = self.get_parameter('crosshair_alpha').value
@@ -30,8 +30,8 @@ class FireExtinguisherEmulatorNode(Node):
         # State variables
         self.target_x = 0.0
         self.target_y = 0.0
-        self.current_x = 0.0
-        self.current_y = 0.0
+        self.current_x = None
+        self.current_y = None
 
         # Camera info
         self.camera_info = None
@@ -78,6 +78,9 @@ class FireExtinguisherEmulatorNode(Node):
 
             self.get_logger().info(f"Camera info received: {self.width}x{self.height}, fx={self.fx}, fy={self.fy}")
 
+            self.current_x = (self.target_x + 1.0) * 0.5 * self.width
+            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'):
@@ -90,7 +93,7 @@ class FireExtinguisherEmulatorNode(Node):
 
         # Calculate target pixels
         target_pixel_x = (self.target_x + 1.0) * 0.5 * self.width
-        target_pixel_y = (self.target_y + 1.0) * 0.5 * self.height
+        target_pixel_y = self.height - (self.target_y + 1.0) * 0.5 * self.height
 
         # Calculate distance to target
         dx = target_pixel_x - self.current_x