From a909868d0a6ea6315fec8dd2fac854837ad685f1 Mon Sep 17 00:00:00 2001 From: moscowsky Date: Fri, 19 Jun 2026 14:32:32 +0300 Subject: [PATCH] target cross --- config/T-29.yaml | 2 +- launch/unity.launch.py | 2 +- .../fire_extinguisher_emulator.py | 44 ++++++++++++------- unity_robot_controller/joy_control_node.py | 4 +- 4 files changed, 31 insertions(+), 21 deletions(-) diff --git a/config/T-29.yaml b/config/T-29.yaml index 52e36e8..951403b 100644 --- a/config/T-29.yaml +++ b/config/T-29.yaml @@ -16,7 +16,7 @@ sirius_bot/joy_control: crosshair_plus_y_btn: -1 crosshair_minus_y_btn: -1 - crosshair_step: 0.05 + crosshair_step: 0.05 # where 1 is half of image crosshair_x_axis: 6 crosshair_y_axis: 7 diff --git a/launch/unity.launch.py b/launch/unity.launch.py index fce5b37..ab4a13d 100644 --- a/launch/unity.launch.py +++ b/launch/unity.launch.py @@ -57,7 +57,7 @@ def generate_launch_description(): namespace=LaunchConfiguration("namespace"), parameters=[ {"use_sim_time": LaunchConfiguration("use_sim_time")}, - {"crosshair_alpha": 0.5} + {"crosshair_alpha": 1.0} ], remappings=[ ("raw_image", "/sirius_bot/camera/image_view"), diff --git a/unity_robot_controller/fire_extinguisher_emulator.py b/unity_robot_controller/fire_extinguisher_emulator.py index c133ef6..ad40ec9 100644 --- a/unity_robot_controller/fire_extinguisher_emulator.py +++ b/unity_robot_controller/fire_extinguisher_emulator.py @@ -24,6 +24,8 @@ class FireExtinguisherEmulatorNode(Node): self.declare_parameter('robot_frame', "base_link") self.declare_parameter('fe_distance', 5.) + self.declare_parameter('show_target', True) + self.crosshair_alpha = self.get_parameter('crosshair_alpha').value self.crosshair_speed = self.get_parameter('crosshair_speed').value self.crosshair_color = self.get_parameter('crosshair_color').value @@ -31,6 +33,8 @@ class FireExtinguisherEmulatorNode(Node): self.fe_distance = self.get_parameter('fe_distance').value self.robot_frame = self.get_parameter('robot_frame').value + self.show_target = self.get_parameter('show_target').value + # Sources of Fire positions in (x, y, z, r) self.sof_poisitions = [] @@ -198,23 +202,29 @@ class FireExtinguisherEmulatorNode(Node): self.get_logger().error(f"Failed to convert image: {e}") return - crosshair_size = 20 - crosshair_thickness = 2 - color = tuple(self.crosshair_color) - - 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) - - 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) - - cv2.circle(cv_image, (int(self.current_x), int(self.current_y)), 5, color, -1) + def draw_crosshair(x, y, color): + crosshair_size = 20 + crosshair_thickness = 2 + + x1 = int(x - crosshair_size) + x2 = int(x + crosshair_size) + y1 = int(y) + y2 = int(y) + cv2.line(cv_image, (x1, y1), (x2, y2), color, crosshair_thickness) + + x1 = int(x) + x2 = int(x) + y1 = int(y - crosshair_size) + y2 = int(y + crosshair_size) + cv2.line(cv_image, (x1, y1), (x2, y2), color, crosshair_thickness) + + cv2.circle(cv_image, (int(x), int(y)), 5, color, -1) + + tx = (self.target_x + 1.0) * 0.5 * self.width + ty = self.height-(self.target_y + 1.0) * 0.5 * self.height + #self.get_logger().info(f"T: {self.target_x} {self.target_y} -> {tx} {ty}") + draw_crosshair(tx, ty, (255, 0, 0)) + draw_crosshair(self.current_x, self.current_y, (0, 0, 255)) if self.crosshair_alpha < 1.0: overlay = cv_image.copy() diff --git a/unity_robot_controller/joy_control_node.py b/unity_robot_controller/joy_control_node.py index dcb7b6d..f94ed52 100644 --- a/unity_robot_controller/joy_control_node.py +++ b/unity_robot_controller/joy_control_node.py @@ -108,8 +108,8 @@ class JoyControlNode(Node): if msg.axes[self.crosshair_x_axis] != self.prev_axes_states[self.crosshair_x_axis] or msg.axes[self.crosshair_y_axis] != self.prev_axes_states[self.crosshair_y_axis]: - self.crosshair_current_x += self.crosshair_step * -msg.axes[self.crosshair_x_axis] - self.crosshair_current_y += self.crosshair_step * msg.axes[self.crosshair_y_axis] + self.crosshair_current_x += (self.crosshair_step * -msg.axes[self.crosshair_x_axis]) + self.crosshair_current_y += (self.crosshair_step * msg.axes[self.crosshair_y_axis]) self.crosshair_current_x = max(min(1., self.crosshair_current_x), -1.) self.crosshair_current_y = max(min(1., self.crosshair_current_y), -1.)