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

works with unity

Browse Source
This commit is contained in:
moscowsky
2026-06-19 13:59:49 +03:00
parent 3739fd6859
commit 9a617ffa6b
5 changed files with 56 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
config/T-29.yaml
+5
View File
@@ -1,9 +1,14 @@
sirius_bot/joy_control: sirius_bot/joy_control:
ros__parameters: ros__parameters:
max_linear_speed: 2.
max_angular_speed: 2.
speed_x_axis: 4 speed_x_axis: 4
speed_y_axis: 3 speed_y_axis: 3
speed_throttle_btn: 7 speed_throttle_btn: 7
fe_burst_btn: 2 fe_burst_btn: 2
crosshair_plus_x_btn: -1 crosshair_plus_x_btn: -1
config/unity.rviz
+32 -11
View File
@@ -3,9 +3,7 @@ Panels:
Help Height: 87 Help Height: 87
Name: Displays Name: Displays
Property Tree Widget: Property Tree Widget:
Expanded: Expanded: ~
- /Grid1
- /TF1/Tree1
Splitter Ratio: 0.5 Splitter Ratio: 0.5
Tree Height: 696 Tree Height: 696
- Class: rviz_common/Selection - Class: rviz_common/Selection
@@ -30,7 +28,7 @@ Visualization Manager:
Class: "" Class: ""
Displays: Displays:
- Alpha: 0.5 - Alpha: 0.5
Cell Size: 10 Cell Size: 50
Class: rviz_default_plugins/Grid Class: rviz_default_plugins/Grid
Color: 160; 160; 164 Color: 160; 160; 164
Enabled: true Enabled: true
@@ -90,6 +88,8 @@ Visualization Manager:
Value: true Value: true
camera_link: camera_link:
Value: true Value: true
camera_optical_link:
Value: true
front_left_wheel_link: front_left_wheel_link:
Value: true Value: true
front_right_wheel_link: front_right_wheel_link:
@@ -118,6 +118,7 @@ Visualization Manager:
back_right_wheel_link: back_right_wheel_link:
{} {}
camera_link: camera_link:
camera_optical_link:
{} {}
front_left_wheel_link: front_left_wheel_link:
{} {}
@@ -177,25 +178,25 @@ Visualization Manager:
Views: Views:
Current: Current:
Class: rviz_default_plugins/ThirdPersonFollower Class: rviz_default_plugins/ThirdPersonFollower
Distance: 47.24568557739258 Distance: 61.71200180053711
Enable Stereo Rendering: Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549 Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1 Stereo Focal Distance: 1
Swap Stereo Eyes: false Swap Stereo Eyes: false
Value: false Value: false
Focal Point: Focal Point:
X: -12.875144958496094 X: -0.47304797172546387
Y: -5.233623027801514 Y: -0.08951294422149658
Z: -0.2596208155155182 Z: -0.25962093472480774
Focal Shape Fixed Size: true Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806 Focal Shape Size: 0.05000000074505806
Invert Z Axis: false Invert Z Axis: false
Name: Current View Name: Current View
Near Clip Distance: 0.009999999776482582 Near Clip Distance: 0.009999999776482582
Pitch: 0.49539846181869507 Pitch: 0.5053984522819519
Target Frame: base_link Target Frame: base_link
Value: ThirdPersonFollower (rviz_default_plugins) Value: ThirdPersonFollower (rviz_default_plugins)
Yaw: 3.655395030975342 Yaw: 3.5853965282440186
Saved: Saved:
- Class: rviz_default_plugins/ThirdPersonFollower - Class: rviz_default_plugins/ThirdPersonFollower
Distance: 2.978720188140869 Distance: 2.978720188140869
@@ -217,6 +218,26 @@ Visualization Manager:
Target Frame: base_link Target Frame: base_link
Value: ThirdPersonFollower (rviz_default_plugins) Value: ThirdPersonFollower (rviz_default_plugins)
Yaw: 3.590395212173462 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: Window Geometry:
Crosshair: Crosshair:
collapsed: false collapsed: false
@@ -225,7 +246,7 @@ Window Geometry:
Height: 1052 Height: 1052
Hide Left Dock: false Hide Left Dock: false
Hide Right Dock: false Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a0000035cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004c0000035c000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000002100000035cfc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000004c000001b00000002200fffffffa000000000100000002fb000000060052004100570100000000ffffffff0000005c01000003fb0000000a005600690065007700730000000670000001100000011001000003fb0000001200430072006f00730073006800610069007201000001fd000001ab0000002201000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000004cfc0100000002fb0000000800540069006d0065010000000000000780000002bd01000003fb0000000800540069006d00650100000000000004500000000000000000000004040000035c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 QMainWindow State: 000000ff00000000fd00000004000000000000016a0000035cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004c0000035c000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000002100000035cfc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000004c000001a80000002200fffffffa000000000100000002fb000000060052004100570100000000ffffffff0000005c01000003fb0000000a005600690065007700730000000670000001100000011001000003fb0000001200430072006f00730073006800610069007201000001f5000001b30000002201000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000004cfc0100000002fb0000000800540069006d0065010000000000000780000002bd01000003fb0000000800540069006d00650100000000000004500000000000000000000004040000035c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
RAW: RAW:
collapsed: false collapsed: false
Selection: Selection:
unity_robot_controller/fire_extinguisher_emulator.py
+12 -4
View File
@@ -2,7 +2,7 @@
import rclpy import rclpy
from rclpy.node import Node from rclpy.node import Node
from rclpy.executors import SingleThreadedExecutor from rclpy.executors import SingleThreadedExecutor, MultiThreadedExecutor
from geometry_msgs.msg import Point, Vector3 from geometry_msgs.msg import Point, Vector3
from sensor_msgs.msg import Image, CameraInfo from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge from cv_bridge import CvBridge
@@ -63,9 +63,11 @@ class FireExtinguisherEmulatorNode(Node):
self.tf_listener = TransformListener(self.tf_buffer, self) self.tf_listener = TransformListener(self.tf_buffer, self)
# Timers and subscriptions # 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(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_subscription(Image, 'raw_image', self.raw_image_cb, 10)
self.create_service(Trigger, 'fe_burst', self.fe_burst_cb) self.create_service(Trigger, 'fe_burst', self.fe_burst_cb)
@@ -134,6 +136,7 @@ class FireExtinguisherEmulatorNode(Node):
def target_timer_cb(self): def target_timer_cb(self):
"""Smooth movement of crosshair towards target with speed limit""" """Smooth movement of crosshair towards target with speed limit"""
if self.camera_info is None: if self.camera_info is None:
self.get_logger().warning(f"Camera info stil not recieved!")
return return
# Calculate target pixels # Calculate target pixels
@@ -161,6 +164,8 @@ class FireExtinguisherEmulatorNode(Node):
self.current_x += step_x self.current_x += step_x
self.current_y += step_y 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 # Calculate 3D unit vector from camera frame
if self.fx > 0 and self.fy > 0: if self.fx > 0 and self.fy > 0:
px = self.current_x px = self.current_x
@@ -306,9 +311,12 @@ def main(args=None):
rclpy.init(args=args) rclpy.init(args=args)
node = FireExtinguisherEmulatorNode() node = FireExtinguisherEmulatorNode()
executor = SingleThreadedExecutor() executor = SingleThreadedExecutor()
#executor = MultiThreadedExecutor(num_threads=4)
executor.add_node(node) executor.add_node(node)
try: try:
rclpy.spin(node, executor) executor.spin()
#rclpy.spin(node, executor)
#rclpy.spin(node)
except KeyboardInterrupt: except KeyboardInterrupt:
pass pass
finally: finally:
unity_robot_controller/joy_control_node.py
+4 -2
View File
@@ -111,11 +111,13 @@ class JoyControlNode(Node):
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_x = max(min(1., self.crosshair_current_x), -1.)
self.crosshair_current_y = max(min(1, self.crosshair_current_y), -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.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_x_axis] = msg.axes[self.crosshair_x_axis]
self.prev_axes_states[self.crosshair_y_axis] = msg.axes[self.crosshair_y_axis] self.prev_axes_states[self.crosshair_y_axis] = msg.axes[self.crosshair_y_axis]
unity_robot_controller/unity_robot_controller.py
+2 -2
View File
@@ -15,8 +15,8 @@ class UnityRobotController(RobotController):
self._node = node self._node = node
self._node.declare_parameter('max_linear_speed', 0.5) self._node.declare_parameter('max_linear_speed', 1.0)
self._node.declare_parameter('max_angular_speed', 1.0) self._node.declare_parameter('max_angular_speed', 1.5)
self._node.declare_parameter('fire_ext_pixel_speed', 1.0) self._node.declare_parameter('fire_ext_pixel_speed', 1.0)