sirius/unity_robot_controller
5
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

target cross

Browse Source
This commit is contained in:
moscowsky
2026-06-19 14:32:32 +03:00
parent 1e3abf82fe
commit a909868d0a
4 changed files with 28 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
config/T-29.yaml
+1 -1
View File
@@ -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
+1 -1
View File
@@ -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 -14
View File
@@ -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)
def draw_crosshair(x, y, color):
crosshair_size = 20
crosshair_thickness = 2
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(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(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)
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(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
+2 -2
View File
@@ -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.)