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

works with unity

Browse Source
main
moscowsky 1 month ago
parent 3739fd6859
commit 9a617ffa6b
5 changed files with 56 additions and 20 deletions
Show Stats Download Patch File Download Diff File

5
config/T-29.yaml
Unescape Escape View File

@ -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

45
config/unity.rviz
Unescape Escape View File

@ -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:

16
unity_robot_controller/fire_extinguisher_emulator.py
Unescape Escape View File

@ -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:

6
unity_robot_controller/joy_control_node.py
Unescape Escape 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_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]

4
unity_robot_controller/unity_robot_controller.py
Unescape Escape View File

@ -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)

Write Preview
Loading…
Cancel
Save