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

fire kill in unity works

Browse Source
This commit is contained in:
moscowsky
2026-06-23 11:57:26 +03:00
parent be6398ba68
commit a32b2951f3
5 changed files with 176 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
config/unity.rviz
+49 -10
View File
@@ -28,7 +28,7 @@ Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 50
Cell Size: 10
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
@@ -38,11 +38,11 @@ Visualization Manager:
Name: Grid
Normal Cell Count: 0
Offset:
X: 20
Y: 10
X: 200
Y: 200
Z: 0
Plane: XY
Plane Cell Count: 6
Plane Cell Count: 60
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz_default_plugins/Image
@@ -132,6 +132,45 @@ Visualization Manager:
{}
Update Interval: 0
Value: true
- Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Class: rviz_default_plugins/Pose
Color: 255; 25; 0
Enabled: true
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Name: CrossHair Vector
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Shape: Arrow
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /sirius_bot/crosshair_vector
Value: true
- Class: rviz_default_plugins/MarkerArray
Enabled: true
Name: MarkerArray
Namespaces:
"": true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /locations_markers
Value: true
- Class: rviz_default_plugins/Axes
Enabled: false
Length: 10
Name: Axes
Radius: 1
Reference Frame: <Fixed Frame>
Value: false
Enabled: true
Global Options:
Background Color: 48; 48; 48
@@ -178,25 +217,25 @@ Visualization Manager:
Views:
Current:
Class: rviz_default_plugins/ThirdPersonFollower
Distance: 61.71200180053711
Distance: 72.5179443359375
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
X: 5.716884613037109
Y: 3.2075774669647217
Z: -3.7333018553908914e-05
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.5053984522819519
Pitch: 0.5053315162658691
Target Frame: base_link
Value: ThirdPersonFollower (rviz_default_plugins)
Yaw: 3.5853965282440186
Yaw: 3.1497514247894287
Saved:
- Class: rviz_default_plugins/ThirdPersonFollower
Distance: 2.978720188140869
launch/joy_control.launch.py
+1
View File
@@ -57,6 +57,7 @@ def generate_launch_description():
output="both",
parameters=[
ParameterFile(joy_params),
{"fire_ext_max_capacity": 100},
],
)
launch/unity.launch.py
+4 -2
View File
@@ -57,7 +57,9 @@ def generate_launch_description():
namespace=LaunchConfiguration("namespace"),
parameters=[
{"use_sim_time": LaunchConfiguration("use_sim_time")},
{"crosshair_alpha": 1.0}
{"crosshair_alpha": 1.0},
{"sof_cos_sim": 0.999},
{"fe_distance": 5.0},
],
remappings=[
("raw_image", "/sirius_bot/camera/image_view"),
@@ -81,7 +83,7 @@ def generate_launch_description():
pkg_gen_location_on_mesh = FindPackageShare(package="").find("gen_locations_on_mesh")
mesh_path = PathJoinSubstitution([pkg_gen_location_on_mesh, "share", "scene_init.ply"])
mesh_path = PathJoinSubstitution([pkg_gen_location_on_mesh, "share", "sirius_scene_floor.ply"])
cache_path = PathJoinSubstitution([pkg_gen_location_on_mesh, "share", "cache.pkl"])
unity_robot_controller/fire_extinguisher_emulator.py
+115 -41
View File
@@ -3,7 +3,7 @@
import rclpy
from rclpy.node import Node
from rclpy.executors import SingleThreadedExecutor, MultiThreadedExecutor
from geometry_msgs.msg import Point, Vector3
from geometry_msgs.msg import Point, Vector3, PoseStamped
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge
import cv2
@@ -12,6 +12,9 @@ from std_srvs.srv import Trigger
from tf2_ros import Buffer, TransformListener
from gen_locations_on_mesh_msgs.msg import LocationsOnMesh
from gen_locations_on_mesh_msgs.srv import RemoveLocation
from scipy.spatial.transform import Rotation
from visualization_msgs.msg import MarkerArray
class FireExtinguisherEmulatorNode(Node):
@@ -27,7 +30,7 @@ class FireExtinguisherEmulatorNode(Node):
self.declare_parameter('show_target', True)
self.declare_parameter('sof_r', 0.5)
self.declare_parameter('sof_cos_sim', 0.5)
self.crosshair_alpha = self.get_parameter('crosshair_alpha').value
self.crosshair_speed = self.get_parameter('crosshair_speed').value
@@ -37,14 +40,14 @@ class FireExtinguisherEmulatorNode(Node):
self.robot_frame = self.get_parameter('robot_frame').value
self.show_target = self.get_parameter('show_target').value
self.sof_r = self.get_parameter('sof_r').value
self.sof_cos_sim = self.get_parameter('sof_cos_sim').value
# Sources of Fire positions in (id, x, y, z, r)
self.sof_poisitions = []
# Publishers
self.fire_ext_image_pub = self.create_publisher(Image, 'crosshair_image', 10)
self.fire_ext_vector_pub = self.create_publisher(Vector3, 'crosshair_vector', 10)
self.fire_ext_vector_pub = self.create_publisher(PoseStamped, 'crosshair_vector', 10)
# State variables
self.target_x = 0.0
@@ -71,7 +74,9 @@ class FireExtinguisherEmulatorNode(Node):
self.tf_listener = TransformListener(self.tf_buffer, self)
# remove SoF
self.remove_sof_srv = self.create_client(RemoveLocation, 'remove_location')
self.remove_sof_srv = self.create_client(RemoveLocation, '/remove_location')
self.fe_vector = None
# Timers and subscriptions
self.target_timer = self.create_timer(0.1, self.target_timer_cb) # 10 Hz
@@ -80,25 +85,35 @@ class FireExtinguisherEmulatorNode(Node):
self.create_subscription(Point, 'target_point', self.target_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(LocationsOnMesh, 'locations', self.locations_cb, 10)
self.create_subscription(MarkerArray, '/locations_markers', self.locations_cb, 10)
self.create_service(Trigger, 'fe_burst', self.fe_burst_cb)
def locations_cb(self, msg):
for idx, x, y, z in zip(msg.location_id, msg.x, msg.y, msg.z):
self.sof_poisitions.append((idx, x, y, z, self.sof_r))
#for idx, x, y, z in zip(msg.location_id, msg.x, msg.y, msg.z):
self.sof_poisitions = []
for marker in msg.markers:
self.sof_poisitions.append((marker.id,
marker.pose.position.x,
marker.pose.position.y,
marker.pose.position.z,
))
def fe_burst_cb(self, req, res):
intersected_spheres = find_intersected_spheres(
spheres=self.sof_poisitions,
camera_vector=self.vector_msg, # ваш Vector3 с направлением
camera_vector=self.fe_vector, # ваш Vector3 с направлением
camera_vector_length=self.fe_distance, # длина луча
tf_buffer=self.tf_buffer,
camera_frame=self.camera_info.header.frame_id, # из CameraInfo
world_frame="map"
world_frame="map",
sof_cos_sim=self.sof_cos_sim,
print_f = self.get_logger().info
)
self.get_logger().info(f"{intersected_spheres}")
if len(intersected_spheres):
res.success = True
res.message = str(intersected_spheres[0][0]) # add ID of source of fire if success
@@ -184,8 +199,6 @@ class FireExtinguisherEmulatorNode(Node):
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
py = self.current_y
@@ -200,11 +213,20 @@ class FireExtinguisherEmulatorNode(Node):
y /= norm
z /= norm
self.vector_msg = Vector3()
self.vector_msg.x = x
self.vector_msg.y = y
self.vector_msg.z = z
self.fire_ext_vector_pub.publish(self.vector_msg)
self.fe_vector = (x, y, z)
msg = PoseStamped()
msg.header.frame_id = self.camera_info.header.frame_id
msg.header.stamp = self.get_clock().now().to_msg()
q = quaternion_from_two_vectors([1.0, 0.0, 0.0], self.fe_vector)
msg.pose.orientation.x = float(q[0])
msg.pose.orientation.y = float(q[1])
msg.pose.orientation.z = float(q[2])
msg.pose.orientation.w = float(q[3])
self.fire_ext_vector_pub.publish(msg)
def raw_image_cb(self, raw_msg):
"""Draw crosshair on image and publish 3D vector"""
@@ -260,12 +282,14 @@ class FireExtinguisherEmulatorNode(Node):
def find_intersected_spheres(
spheres: list[tuple[int, float, float, float, float]], # [(id, x, y, z, r)]
camera_vector: Vector3, # единичный вектор в координатах камеры
spheres: list[tuple[int, float, float, float]], # [(id, x, y, z)]
camera_vector: (float, float, float), # единичный вектор в координатах камеры
camera_vector_length: float, # длина луча
tf_buffer: Buffer,
camera_frame: str,
world_frame: str = "world"
world_frame: str,
sof_cos_sim,
print_f = print
) -> list[tuple[int, float, float]]:
"""
Находит все сферы, пересекаемые лучом.
@@ -275,8 +299,8 @@ def find_intersected_spheres(
# 1. Получаем преобразование камеры -> мир
transform = tf_buffer.lookup_transform(
world_frame, # target frame (мир)
camera_frame, # source frame (камера)
world_frame, # target frame
camera_frame, # source frame
rclpy.time.Time() # latest transform
)
@@ -286,10 +310,11 @@ def find_intersected_spheres(
transform.transform.translation.y,
transform.transform.translation.z
])
print_f(f"{p0}")
# 3. Преобразуем вектор камеры в координаты мира
# Вектор как точка (с нулевой координатой w)
camera_vec = np.array([camera_vector.x, camera_vector.y, camera_vector.z])
camera_vec = np.array(camera_vector)
# Матрица вращения из transform (кватернион -> матрица)
q = transform.transform.rotation
@@ -306,31 +331,80 @@ def find_intersected_spheres(
# 4. Проходим по всем сферам
intersected = []
l2 = camera_vector_length**2
for sphere in spheres:
idx, xs, ys, zs, r = sphere
p_s = np.array([xs, ys, zs]) # центр сферы в мире
idx, xs, ys, zs = sphere
# k = p₀ - pₛ (вектор от сферы до камеры)
k = p0 - p_s
dx = p0[0] - xs
dy = p0[1] - ys
D2 = (dx)**2 + (dy)**2
# Коэффициенты квадратного уравнения
# t² + 2*b*t + c = 0, где a = 1 (у нас u единичный)
b = np.dot(k, u)
c = np.dot(k, k) - r**2
if D2 <= l2:
dz = p0[2] - zs
# Дискриминант
D = b**2 - c
def cos_sim(a, b):
return np.dot(a, b) / (np.linalg.norm(a) * np.linalg.norm(b))
if D >= 0: # есть пересечение (D=0 — касание, D>0 — 2 точки)
sqrt_D = np.sqrt(D)
t1 = -b - sqrt_D
t2 = -b + sqrt_D
sim = cos_sim(u, -np.array((dx, dy, dz)))
# Проверяем, что хотя бы одна точка пересечения на луче [0, length]
if (0 <= t1 <= camera_vector_length) or (0 <= t2 <= camera_vector_length):
intersected.append((idx, t1, t2))
if sim >= sof_cos_sim:
intersected.append((idx, np.sqrt(D2), sim))
return intersected
return sorted(intersected, key=lambda x: x[1])
# for sphere in spheres:
# idx, xs, ys, zs, r = sphere
# p_s = np.array([xs, ys, zs]) # центр сферы в мире
#
# # k = p₀ - pₛ (вектор от сферы до камеры)
# k = p0 - p_s
#
# # Коэффициенты квадратного уравнения
# # t² + 2*b*t + c = 0, где a = 1 (у нас u единичный)
# b = np.dot(k, u)
# c = np.dot(k, k) - r**2
#
# # Дискриминант
# D = b**2 - c
#
# if D >= 0: # есть пересечение (D=0 — касание, D>0 — 2 точки)
# sqrt_D = np.sqrt(D)
# t1 = -b - sqrt_D
# t2 = -b + sqrt_D
#
# # Проверяем, что хотя бы одна точка пересечения на луче [0, length]
# if (0 <= t1 <= camera_vector_length) or (0 <= t2 <= camera_vector_length):
# intersected.append((idx, t1, t2))
#
# return intersected
def quaternion_from_two_vectors(a, b):
a = np.array(a, dtype=float)
b = np.array(b, dtype=float)
a /= np.linalg.norm(a)
b /= np.linalg.norm(b)
v = np.cross(a, b)
c = np.dot(a, b)
if c < -0.999999:
axis = np.array([1.0, 0.0, 0.0])
if abs(a[0]) > 0.9:
axis = np.array([0.0, 1.0, 0.0])
v = np.cross(a, axis)
v /= np.linalg.norm(v)
q = np.array([v[0], v[1], v[2], 0.0])
return q
s = np.sqrt((1.0 + c) * 2.0)
invs = 1.0 / s
qx = v[0] * invs
qy = v[1] * invs
qz = v[2] * invs
qw = 0.5 * s
return np.array([qx, qy, qz, qw], dtype=float)
def main(args=None):
unity_robot_controller/unity_robot_controller.py
+7 -2
View File
@@ -11,7 +11,7 @@ from std_srvs.srv import Trigger
class UnityRobotController(RobotController):
def __init__(self, node):
super().__init__()
self._node = node
@@ -19,10 +19,15 @@ class UnityRobotController(RobotController):
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_max_capacity', 5)
self._lin_max = self._node.get_parameter('max_linear_speed').value
self._ang_max = self._node.get_parameter('max_angular_speed').value
fire_ext_max_capacity = self._node.get_parameter('fire_ext_max_capacity').value
super().__init__(fire_ext_max_capacity = fire_ext_max_capacity)
self._twist_pub = self._node.create_publisher(Twist, 'cmd_vel', 10)
self._target_point_pub = self._node.create_publisher(Point, 'target_point', 10)
@@ -62,7 +67,7 @@ class UnityRobotController(RobotController):
def _fe_burst_done_cb(self, future):
result = future.result()
if result.success:
self._register_exted_sof(self.get_relative_time(), {'id': success.message})
self._register_exted_sof(self.get_relative_time(), {'id': result.message})
def send_fire_ext_pose_cmd(self, horisontal_pose, vertical_pose):