target cross

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

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"),

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
+        def draw_crosshair(x, y, color):
-        crosshair_thickness = 2
+            crosshair_size = 20
-        color = tuple(self.crosshair_color)
+            crosshair_thickness = 2
 
-        x1 = int(self.current_x - crosshair_size)
+            x1 = int(x - crosshair_size)
-        x2 = int(self.current_x + crosshair_size)
+            x2 = int(x + crosshair_size)
-        y1 = int(self.current_y)
+            y1 = int(y)
-        y2 = int(self.current_y)
+            y2 = int(y)
-        cv2.line(cv_image, (x1, y1), (x2, y2), color, crosshair_thickness)
+            cv2.line(cv_image, (x1, y1), (x2, y2), color, crosshair_thickness)
 
-        x1 = int(self.current_x)
+            x1 = int(x)
-        x2 = int(self.current_x)
+            x2 = int(x)
-        y1 = int(self.current_y - crosshair_size)
+            y1 = int(y - crosshair_size)
-        y2 = int(self.current_y + crosshair_size)
+            y2 = int(y + crosshair_size)
-        cv2.line(cv_image, (x1, y1), (x2, y2), color, crosshair_thickness)
+            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)
+            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()

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_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_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.)