diff --git a/config/unity.rviz b/config/unity.rviz new file mode 100644 index 0000000..3485ddc --- /dev/null +++ b/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: + 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: + 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 diff --git a/launch/unity.launch.py b/launch/unity.launch.py new file mode 100644 index 0000000..f859944 --- /dev/null +++ b/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, + ] + ) diff --git a/setup.py b/setup.py index c8d4047..c3b0999 100644 --- a/setup.py +++ b/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' ], }, ) diff --git a/unity_robot_controller/fire_extinguisher_emulator.py b/unity_robot_controller/fire_extinguisher_emulator.py index ffddcd9..ac3a781 100644 --- a/unity_robot_controller/fire_extinguisher_emulator.py +++ b/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() - -