From 9a617ffa6bfd036585e155dd8f6c30ac2ebafae9 Mon Sep 17 00:00:00 2001 From: moscowsky Date: Fri, 19 Jun 2026 13:59:49 +0300 Subject: [PATCH] works with unity --- config/T-29.yaml | 5 +++ config/unity.rviz | 45 ++++++++++++++----- .../fire_extinguisher_emulator.py | 16 +++++-- unity_robot_controller/joy_control_node.py | 6 ++- .../unity_robot_controller.py | 4 +- 5 files changed, 56 insertions(+), 20 deletions(-) diff --git a/config/T-29.yaml b/config/T-29.yaml index aeb190e..52e36e8 100644 --- a/config/T-29.yaml +++ b/config/T-29.yaml @@ -1,9 +1,14 @@ sirius_bot/joy_control: ros__parameters: + + max_linear_speed: 2. + max_angular_speed: 2. + speed_x_axis: 4 speed_y_axis: 3 speed_throttle_btn: 7 + fe_burst_btn: 2 crosshair_plus_x_btn: -1 diff --git a/config/unity.rviz b/config/unity.rviz index e47aae4..3f8f0aa 100644 --- a/config/unity.rviz +++ b/config/unity.rviz @@ -3,9 +3,7 @@ Panels: Help Height: 87 Name: Displays Property Tree Widget: - Expanded: - - /Grid1 - - /TF1/Tree1 + Expanded: ~ Splitter Ratio: 0.5 Tree Height: 696 - Class: rviz_common/Selection @@ -30,7 +28,7 @@ Visualization Manager: Class: "" Displays: - Alpha: 0.5 - Cell Size: 10 + Cell Size: 50 Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true @@ -90,6 +88,8 @@ Visualization Manager: Value: true camera_link: Value: true + camera_optical_link: + Value: true front_left_wheel_link: Value: true front_right_wheel_link: @@ -118,7 +118,8 @@ Visualization Manager: back_right_wheel_link: {} camera_link: - {} + camera_optical_link: + {} front_left_wheel_link: {} front_right_wheel_link: @@ -177,25 +178,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/ThirdPersonFollower - Distance: 47.24568557739258 + Distance: 61.71200180053711 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -12.875144958496094 - Y: -5.233623027801514 - Z: -0.2596208155155182 + X: -0.47304797172546387 + Y: -0.08951294422149658 + Z: -0.25962093472480774 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.49539846181869507 + Pitch: 0.5053984522819519 Target Frame: base_link Value: ThirdPersonFollower (rviz_default_plugins) - Yaw: 3.655395030975342 + Yaw: 3.5853965282440186 Saved: - Class: rviz_default_plugins/ThirdPersonFollower Distance: 2.978720188140869 @@ -217,6 +218,26 @@ Visualization Manager: Target Frame: base_link Value: ThirdPersonFollower (rviz_default_plugins) Yaw: 3.590395212173462 + - Class: rviz_default_plugins/ThirdPersonFollower + Distance: 3.1325621604919434 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.47304797172546387 + Y: -0.08951294422149658 + Z: -0.25962093472480774 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: ThirdPersonFollower + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5053984522819519 + Target Frame: base_link + Value: ThirdPersonFollower (rviz_default_plugins) + Yaw: 3.5853965282440186 Window Geometry: Crosshair: collapsed: false @@ -225,7 +246,7 @@ Window Geometry: Height: 1052 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a0000035cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004c0000035c000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000002100000035cfc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000004c000001b00000002200fffffffa000000000100000002fb000000060052004100570100000000ffffffff0000005c01000003fb0000000a005600690065007700730000000670000001100000011001000003fb0000001200430072006f00730073006800610069007201000001fd000001ab0000002201000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000004cfc0100000002fb0000000800540069006d0065010000000000000780000002bd01000003fb0000000800540069006d00650100000000000004500000000000000000000004040000035c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a0000035cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004c0000035c000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000002100000035cfc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000004c000001a80000002200fffffffa000000000100000002fb000000060052004100570100000000ffffffff0000005c01000003fb0000000a005600690065007700730000000670000001100000011001000003fb0000001200430072006f00730073006800610069007201000001f5000001b30000002201000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000004cfc0100000002fb0000000800540069006d0065010000000000000780000002bd01000003fb0000000800540069006d00650100000000000004500000000000000000000004040000035c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 RAW: collapsed: false Selection: diff --git a/unity_robot_controller/fire_extinguisher_emulator.py b/unity_robot_controller/fire_extinguisher_emulator.py index ec12cd6..c133ef6 100644 --- a/unity_robot_controller/fire_extinguisher_emulator.py +++ b/unity_robot_controller/fire_extinguisher_emulator.py @@ -2,7 +2,7 @@ import rclpy from rclpy.node import Node -from rclpy.executors import SingleThreadedExecutor +from rclpy.executors import SingleThreadedExecutor, MultiThreadedExecutor from geometry_msgs.msg import Point, Vector3 from sensor_msgs.msg import Image, CameraInfo from cv_bridge import CvBridge @@ -63,9 +63,11 @@ class FireExtinguisherEmulatorNode(Node): self.tf_listener = TransformListener(self.tf_buffer, self) # Timers and subscriptions - self.target_timer = self.create_timer(0.01, self.target_timer_cb) # 100 Hz + self.target_timer = self.create_timer(0.1, self.target_timer_cb) # 10 Hz + + self.create_subscription(Point, 'target_point', self.target_cb, 10) - self.create_subscription(CameraInfo, 'camera_info', self.cam_info_cb, 10) + self.sub_camera_info = self.create_subscription(CameraInfo, 'camera_info', self.cam_info_cb, 10) self.create_subscription(Image, 'raw_image', self.raw_image_cb, 10) self.create_service(Trigger, 'fe_burst', self.fe_burst_cb) @@ -134,6 +136,7 @@ class FireExtinguisherEmulatorNode(Node): def target_timer_cb(self): """Smooth movement of crosshair towards target with speed limit""" if self.camera_info is None: + self.get_logger().warning(f"Camera info stil not recieved!") return # Calculate target pixels @@ -161,6 +164,8 @@ class FireExtinguisherEmulatorNode(Node): self.current_x += step_x self.current_y += step_y + #self.get_logger().info(f"Target {self.target_x} {self.target_y} current {self.current_x} {self.current_y}") + # Calculate 3D unit vector from camera frame if self.fx > 0 and self.fy > 0: px = self.current_x @@ -306,9 +311,12 @@ def main(args=None): rclpy.init(args=args) node = FireExtinguisherEmulatorNode() executor = SingleThreadedExecutor() + #executor = MultiThreadedExecutor(num_threads=4) executor.add_node(node) try: - rclpy.spin(node, executor) + executor.spin() + #rclpy.spin(node, executor) + #rclpy.spin(node) except KeyboardInterrupt: pass finally: diff --git a/unity_robot_controller/joy_control_node.py b/unity_robot_controller/joy_control_node.py index 02b444d..dcb7b6d 100644 --- a/unity_robot_controller/joy_control_node.py +++ b/unity_robot_controller/joy_control_node.py @@ -111,11 +111,13 @@ class JoyControlNode(Node): 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) + self.crosshair_current_x = max(min(1., self.crosshair_current_x), -1.) + self.crosshair_current_y = max(min(1., self.crosshair_current_y), -1.) self.URC.send_fire_ext_pose_cmd(self.crosshair_current_x, self.crosshair_current_y) + #self.get_logger().info(f"Ext pose target: {self.crosshair_current_x} {self.crosshair_current_y}") + self.prev_axes_states[self.crosshair_x_axis] = msg.axes[self.crosshair_x_axis] self.prev_axes_states[self.crosshair_y_axis] = msg.axes[self.crosshair_y_axis] diff --git a/unity_robot_controller/unity_robot_controller.py b/unity_robot_controller/unity_robot_controller.py index 1dd84f6..b8355c8 100644 --- a/unity_robot_controller/unity_robot_controller.py +++ b/unity_robot_controller/unity_robot_controller.py @@ -15,8 +15,8 @@ class UnityRobotController(RobotController): self._node = node - self._node.declare_parameter('max_linear_speed', 0.5) - self._node.declare_parameter('max_angular_speed', 1.0) + self._node.declare_parameter('max_linear_speed', 1.0) + self._node.declare_parameter('max_angular_speed', 1.5) self._node.declare_parameter('fire_ext_pixel_speed', 1.0)