diff --git a/unity_robot_controller/fire_extinguisher_emulator.py b/unity_robot_controller/fire_extinguisher_emulator.py index ac3a781..20738f2 100644 --- a/unity_robot_controller/fire_extinguisher_emulator.py +++ b/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