emulator does smth

config/unity.rviz

@@ -0,0 +1,165 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 87
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 696
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: RAW
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /arctic_bot/camera
+      Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Crosshair
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /fire_extinguisher_emulator/fire_extinguisher_image
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 10
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.785398006439209
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.785398006439209
+    Saved: ~
+Window Geometry:
+  Crosshair:
+    collapsed: false
+  Displays:
+    collapsed: false
+  Height: 1052
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000035cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004c0000035c000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000002100000035cfc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000004c000001b50000002200fffffffa000000000100000002fb000000060052004100570100000000ffffffff0000005c01000003fb0000000a005600690065007700730000000670000001100000011001000003fb0000001200430072006f0073007300680061006900720100000202000001a60000002201000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000004cfc0100000002fb0000000800540069006d0065010000000000000780000002bd01000003fb0000000800540069006d00650100000000000004500000000000000000000004040000035c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  RAW:
+    collapsed: false
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1920
+  X: 0
+  Y: 0

launch/unity.launch.py

@@ -0,0 +1,87 @@
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.substitutions import (
+    PathJoinSubstitution,
+    LaunchConfiguration,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.substitutions import FindPackageShare
+from launch.conditions import IfCondition
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_unity_robot_controller = FindPackageShare(package="").find("unity_robot_controller")
+
+    # Launch arguments
+    launch_args = [
+        DeclareLaunchArgument(
+            "use_sim_time",
+            description="Use clock from simulation",
+            default_value="True",
+        ),
+        DeclareLaunchArgument(
+            "namespace", default_value="arctic_bot", description="Top-level namespace"
+        ),
+        DeclareLaunchArgument(
+            "start_rviz", default_value="True", description="Use basic rviz"
+        ),
+    ]
+
+    # Unity connection
+    unity_client = Node(
+        package="ros_tcp_endpoint",
+        executable="default_server_endpoint",
+        name="ros_tcp_endpoint",
+        # output="screen",
+        arguments=[
+            "--ros-args",
+            "-p",
+            "ROS_IP:=127.0.0.1",
+            "-p",
+            "ROS_TCP_PORT:=10000",
+        ],
+    )
+
+    # joy = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         PathJoinSubstitution([pkg_unity_robot_controller, "launch", "joy.launch.py"])
+    #     ),
+    #     launch_arguments={"namespace": LaunchConfiguration("namespace")}.items(),
+    # )
+
+    # fire extingusher emulator
+    fe_emulator = Node(
+        package="unity_robot_controller",
+        executable="fire_extinguisher_emulator_node",
+        parameters=[
+            {"use_sim_time": LaunchConfiguration("use_sim_time")},
+        ],
+        remappings=[
+            ("raw_image", "/arctic_bot/camera"),
+            ("camera_info", "/arctic_bot/camera/camera_info")
+        ]
+    )
+
+    rviz = Node(
+        package="rviz2",
+        executable="rviz2",
+        parameters=[
+            {"use_sim_time": LaunchConfiguration("use_sim_time")},
+        ],
+        arguments=[
+            "-d",
+            PathJoinSubstitution([pkg_unity_robot_controller, "config", "unity.rviz"]),
+        ],
+        condition=IfCondition(LaunchConfiguration('start_rviz'))
+    )
+
+    return LaunchDescription(
+        launch_args
+        + [
+            unity_client,
+            #joy,
+            fe_emulator,
+            rviz,
+        ]
+    )

setup.py

@@ -1,4 +1,6 @@
 from setuptools import find_packages, setup
+import os
+from glob import glob
 
 package_name = 'unity_robot_controller'
 
@@ -10,6 +12,8 @@ setup(
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), [f for f in glob('launch/*') if os.path.isfile(f)]) ,
+        (os.path.join('share', package_name, 'config'), [f for f in glob('config/*') if os.path.isfile(f)]) ,
     ],
     install_requires=['setuptools'],
     zip_safe=True,
@@ -24,6 +28,7 @@ setup(
     },
     entry_points={
         'console_scripts': [
+            'fire_extinguisher_emulator_node = unity_robot_controller.fire_extinguisher_emulator:main'
         ],
     },
 )

unity_robot_controller/fire_extinguisher_emulator.py

@@ -4,7 +4,10 @@ import rclpy
 from rclpy.node import Node
 from geometry_msgs.msg import Point, Vector3
 from rclpy.executors import MultiThreadedExecutor
-from sensors_msgs.msg import Image, CameraInfo
+from sensor_msgs.msg import Image, CameraInfo
+from cv_bridge import CvBridge
+import cv2
+import numpy as np
 
 
 class FireExtinguisherEmulatorNode(Node):
@@ -13,43 +16,179 @@ 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', 1.)  # pixel/sec
+        self.declare_parameter('crosshair_color', [255, 0, 0])  # RGB (red)
 
         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
 
+        # Publishers
         self.fire_ext_image_pub = self.create_publisher(Image, '~/fire_extinguisher_image', 10)
-        self.fire_ext_image_pub = self.create_publisher(Vector3, '~/fire_extinguisher_vector', 10)
+        self.fire_ext_vector_pub = self.create_publisher(Vector3, '~/fire_extinguisher_vector', 10)
 
+        # State variables
         self.target_x = 0.0
         self.target_y = 0.0
-
         self.current_x = 0.0
         self.current_y = 0.0
 
-        self.target_timer = self.create_timer(0.1, self.target_timer_cb)
+        # Camera info
+        self.camera_info = None
+        self.width = 0
+        self.height = 0
+        self.fx = 0.0
+        self.fy = 0.0
+        self.cx = 0.0
+        self.cy = 0.0
+
+        # Image buffer
+        self.current_image = None
+        self.bridge = CvBridge()
+
+        # Timers and subscriptions
+        self.target_timer = self.create_timer(0.01, self.target_timer_cb)  # 100 Hz
         self.create_subscription(Point, '~/target_point', self.target_cb, 10)
         self.create_subscription(CameraInfo, 'camera_info', self.cam_info_cb, 10)
         self.create_subscription(Image, 'raw_image', self.raw_image_cb, 10)
 
+        # UI control subscription (optional)
+        self.create_subscription(Vector3, '~/ui_control', self.ui_control_cb, 10)
 
     def target_cb(self, msg):
-        self.target_x = min( max(-1., msg.x), 1.)
-        self.target_y = min( max(-1., msg.y), 1.)
+        """Received target point in normalized coordinates [-1, 1]"""
+        self.target_x = min(max(-1.0, msg.x), 1.0)
+        self.target_y = min(max(-1.0, msg.y), 1.0)
 
-    def cam_info_cb(self, msg):
-        # save info and shutdown that subscription
+    def ui_control_cb(self, msg):
+        """Manual UI control for crosshair (normalized coordinates)"""
+        self.target_x = min(max(-1.0, msg.x), 1.0)
+        self.target_y = min(max(-1.0, msg.y), 1.0)
 
+    def cam_info_cb(self, msg):
+        """Save camera info and shutdown subscription"""
+        if self.camera_info is None:
+            self.camera_info = msg
+            self.width = msg.width
+            self.height = msg.height
+            self.fx = msg.k[0]  # fx
+            self.fy = msg.k[4]  # fy
+            self.cx = msg.k[2]  # cx
+            self.cy = msg.k[5]  # cy
+
+            self.get_logger().info(f"Camera info received: {self.width}x{self.height}, fx={self.fx}, fy={self.fy}")
+
+            # Remove the subscription after getting camera info
+            #self.sub_camera_info = self.sub_camera_info.__class__()  # Disable subscription
+            if hasattr(self, 'sub_camera_info'):
+                self.destroy_subscription(self.sub_camera_info)
 
     def target_timer_cb(self):
-        if self.current_x != self.target_x or self.current_y != self.target_x:
-            # do target smooth movement to target assumint crosshair_speed limits
+        """Smooth movement of crosshair towards target with speed limit"""
+        if self.camera_info is None:
+            return
+
+        # 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
+
+        # Calculate distance to target
+        dx = target_pixel_x - self.current_x
+        dy = target_pixel_y - self.current_y
+        distance = np.sqrt(dx**2 + dy**2)
+
+        # Move towards target with speed limit
+        if distance > 0.1:
+            speed_pixels = self.crosshair_speed  # pixels per second
+            dt = 0.01  # 100 Hz timer
+            max_step = speed_pixels * dt
+
+            if distance > max_step:
+                step_x = max_step * dx / distance
+                step_y = max_step * dy / distance
+            else:
+                step_x = dx
+                step_y = dy
+
+            self.current_x += step_x
+            self.current_y += step_y
 
     def raw_image_cb(self, raw_msg):
-        # draw some kind of simple crosshair from current x and y
-
-        # using saved info calculate 3d unit vector from camera frame and publish it
-
+        """Draw crosshair on image and publish 3D vector"""
+        if self.camera_info is None:
+            return
+
+        try:
+            # Convert ROS Image to OpenCV image
+            cv_image = self.bridge.imgmsg_to_cv2(raw_msg, desired_encoding='bgr8')
+        except Exception as e:
+            self.get_logger().error(f"Failed to convert image: {e}")
+            return
+
+        # Draw crosshair
+        crosshair_size = 20
+        crosshair_thickness = 2
+        color = tuple(self.crosshair_color)  # RGB -> tuple for OpenCV
+
+        # Horizontal line
+        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)
+
+        # Vertical line
+        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)
+
+        # Circle at center
+        cv2.circle(cv_image, (int(self.current_x), int(self.current_y)), 5, color, -1)
+
+        # Add alpha transparency effect (simple blend with background)
+        if self.crosshair_alpha < 1.0:
+            overlay = cv_image.copy()
+            cv_image = cv2.addWeighted(cv_image, 1.0, overlay, self.crosshair_alpha, 0)
+
+        # Convert back to ROS Image
+        try:
+            ros_image = self.bridge.cv2_to_imgmsg(cv_image, encoding='bgr8')
+            ros_image.header = raw_msg.header
+            self.fire_ext_image_pub.publish(ros_image)
+        except Exception as e:
+            self.get_logger().error(f"Failed to publish image: {e}")
+
+        # Calculate 3D unit vector from camera frame
+        if self.fx > 0 and self.fy > 0:
+            # Pixel coordinates to normalized coordinates
+            px = self.current_x
+            py = self.current_y
+
+            # Inverse projection: (px, py) -> (x, y, z) in camera frame
+            x = (px - self.cx) / self.fx
+            y = (py - self.cy) / self.fy
+            z = 1.0
+
+            # Normalize to unit vector
+            norm = np.sqrt(x**2 + y**2 + z**2)
+            if norm > 0:
+                x /= norm
+                y /= norm
+                z /= norm
+
+                vector_msg = Vector3()
+                vector_msg.x = x
+                vector_msg.y = y
+                vector_msg.z = z
+                self.fire_ext_vector_pub.publish(vector_msg)
+
+    def destroy_node(self):
+        """Clean up subscriptions"""
+        if hasattr(self, 'sub_camera_info'):
+            self.sub_camera_info.close()
+        super().destroy_node()
 
 
 def main(args=None):
@@ -65,7 +204,6 @@ def main(args=None):
         node.destroy_node()
         rclpy.shutdown()
 
+
 if __name__ == '__main__':
     main()
-
-