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

works somehow on unity in start config

Browse Source
This commit is contained in:
moscowsky
2026-06-15 13:42:36 +03:00
parent e7c5631de5
commit 6aa952a732
8 changed files with 185 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
config/T-29.yaml
+17
View File
@@ -0,0 +1,17 @@
arctic_bot/joy_control:
ros__parameters:
speed_x_axis: 4
speed_y_axis: 3
speed_throttle_btn: 7
crosshair_plus_x_btn: -1
crosshair_minus_x_btn: -1
crosshair_plus_y_btn: -1
crosshair_minus_y_btn: -1
crosshair_step: 0.05
crosshair_x_axis: 6
crosshair_y_axis: 7
config/joy_config.yaml
+14
View File
@@ -0,0 +1,14 @@
arctic_bot/joy:
ros__parameters:
# to list connected devices, use:
# ros2 run joy joy_enumerate_devices
device_id: 0
# device_name can used instead of device_id. device_name takes precedence
device_name: ""
# in [0.; 1.0]
deadzone: 0.3
# set autorepeat to 0 to disable
autorepeat_rate: 20.0
sticky_buttons: false
# rate control
coalesce_interval_ms: 1
config/unity.rviz
+54 -5
View File
@@ -3,9 +3,7 @@ Panels:
Help Height: 87
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
Expanded: ~
Splitter Ratio: 0.5
Tree Height: 696
- Class: rviz_common/Selection
@@ -73,7 +71,58 @@ Visualization Manager:
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /fire_extinguisher_emulator/fire_extinguisher_image
Value: /arctic_bot/crosshair_image
Value: true
- Class: rviz_default_plugins/TF
Enabled: true
Filter (blacklist): ""
Filter (whitelist): ""
Frame Timeout: 15
Frames:
All Enabled: true
back_left_wheel_link:
Value: true
back_right_wheel_link:
Value: true
base_footprint:
Value: true
base_link:
Value: true
camera_link:
Value: true
front_left_wheel_link:
Value: true
front_right_wheel_link:
Value: true
laser_link:
Value: true
map:
Value: true
odom:
Value: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: false
Tree:
map:
odom:
base_footprint:
base_link:
back_left_wheel_link:
{}
back_right_wheel_link:
{}
camera_link:
{}
front_left_wheel_link:
{}
front_right_wheel_link:
{}
laser_link:
{}
Update Interval: 0
Value: true
Enabled: true
Global Options:
@@ -121,7 +170,7 @@ Visualization Manager:
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 10
Distance: 8.9044189453125
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
launch/joy_control.launch.py
+69
View File
@@ -0,0 +1,69 @@
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import (
PathJoinSubstitution,
LaunchConfiguration,
TextSubstitution,
)
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node
from launch_ros.descriptions import ParameterFile
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(
"joy_model",
description="Used joystick model (default is T-29)",
default_value="T-29.yaml",
),
DeclareLaunchArgument(
"namespace", default_value="arctic_bot", description="Top-level namespace"
),
]
joy_config = PathJoinSubstitution([pkg_unity_robot_controller, "config", "joy_config.yaml"])
model = LaunchConfiguration("joy_model")
joy_params = PathJoinSubstitution([pkg_unity_robot_controller,
"config",
model
])
joy_node = Node(
package="joy",
executable="joy_node",
name="joy",
namespace=LaunchConfiguration("namespace"),
output="both",
parameters=[
ParameterFile(joy_config),
],
)
joy_control_node = Node(
package="unity_robot_controller",
executable="joy_control_node",
name="joy_control",
namespace=LaunchConfiguration("namespace"),
output="both",
parameters=[
ParameterFile(joy_params),
],
)
return LaunchDescription(
launch_args
+ [
joy_node,
joy_control_node,
]
)
launch/unity.launch.py
+1
View File
@@ -54,6 +54,7 @@ def generate_launch_description():
fe_emulator = Node(
package="unity_robot_controller",
executable="fire_extinguisher_emulator_node",
namespace=LaunchConfiguration("namespace"),
parameters=[
{"use_sim_time": LaunchConfiguration("use_sim_time")},
],
setup.py
+2 -1
View File
@@ -28,7 +28,8 @@ setup(
},
entry_points={
'console_scripts': [
'fire_extinguisher_emulator_node = unity_robot_controller.fire_extinguisher_emulator:main'
'fire_extinguisher_emulator_node = unity_robot_controller.fire_extinguisher_emulator:main',
'joy_control_node = unity_robot_controller.joy_control_node:main'
],
},
)
unity_robot_controller/fire_extinguisher_emulator.py
+27 -33
View File
@@ -24,8 +24,8 @@ class FireExtinguisherEmulatorNode(Node):
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_vector_pub = self.create_publisher(Vector3, '~/fire_extinguisher_vector', 10)
self.fire_ext_image_pub = self.create_publisher(Image, 'crosshair_image', 10)
self.fire_ext_vector_pub = self.create_publisher(Vector3, 'crosshair_vector', 10)
# State variables
self.target_x = 0.0
@@ -48,23 +48,15 @@ class FireExtinguisherEmulatorNode(Node):
# 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(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):
"""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 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:
@@ -116,6 +108,30 @@ class FireExtinguisherEmulatorNode(Node):
self.current_x += step_x
self.current_y += step_y
# 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 raw_image_cb(self, raw_msg):
"""Draw crosshair on image and publish 3D vector"""
if self.camera_info is None:
@@ -163,29 +179,7 @@ class FireExtinguisherEmulatorNode(Node):
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"""
unity_robot_controller/unity_robot_controller.py
+1 -1
View File
@@ -22,7 +22,7 @@ class UnityRobotController(RobotController):
self._ang_max = self._node.get_parameter('max_angular_speed').value
self._twist_pub = self._node.create_publisher(Twist, 'cmd_vel', 10)
self._target_point_pub = self._node.create_publisher(Point, '/fire_extinguisher_emulator/target_point', 10)
self._target_point_pub = self._node.create_publisher(Point, 'target_point', 10)
def send_speed_cmd(self, v, w):