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

wip

Browse Source
This commit is contained in:
petr-sorokoumov
2026-06-22 16:50:24 +03:00
parent f5919882bd
commit 2da387b2de
4 changed files with 72 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
gen_locations_on_mesh/gen_locations_on_mesh/gen_locations_on_mesh_node.py
+67 -31
View File
@@ -4,6 +4,7 @@ import random
import rclpy
from rclpy.node import Node
from visualization_msgs.msg import MarkerArray, Marker
from gen_locations_on_mesh_msgs.msg import LocationsOnMesh
from gen_locations_on_mesh_msgs.srv import GenerateLocation, RemoveLocation
@@ -128,15 +129,15 @@ def find_largest_piece(pieces):
max_len = max(ls)
return pieces[ls.index(max_len)]
def gen_new_point(piece, trs, vs, existing_ps, dist_limit, vert_shift):
def gen_new_point(piece, trs, vs, existing_ps, dist_limit, vert_shift,
max_z_coord, num_generation_trials):
'''создать новую точку случайным образом;
вернуть словарь точек (с новой)'''
# будем снова и снова пытаться сделать точку, удалённую от других;
# если после множества попыток это не получится, смиримся
# и примем ту, что есть
curr_trial = 0
max_trials = 500
while curr_trial < max_trials:
while curr_trial < num_generation_trials:
# выбрать случайный треугольник
tr_ind = random.randrange(len(piece))
# сформировать координаты случайной точки в нём
@@ -153,7 +154,7 @@ def gen_new_point(piece, trs, vs, existing_ps, dist_limit, vert_shift):
c2 = vs[ind2,:]
p = c0 + (c1-c0)*alpha + (c2-c0)*beta
p[2] += vert_shift
if p[2] > 1.:
if p[2] > max_z_coord:
continue
# проверить совпадение координат с существующими точками
succ = True
@@ -171,24 +172,6 @@ def gen_new_point(piece, trs, vs, existing_ps, dist_limit, vert_shift):
existing_ps[new_ind] = p
return new_ind
'''
mesh = trimesh.load_mesh("raw_surface.ply")
trs = get_all_triangles_with_correct_orientation(mesh.vertices,
mesh.faces,
0.5)
conns = calc_connections(trs)
pieces = calc_pieces(trs, conns)
the_piece = find_largest_piece(pieces)
existing_points = {}
for i in range(10):
ind = gen_new_point(the_piece, trs, mesh.vertices, existing_points, 0.01, 0.1)
print(existing_points)
'''
class GenLocationsOnMeshNode(Node):
def __init__(self):
@@ -199,16 +182,24 @@ class GenLocationsOnMeshNode(Node):
self.trs = None
self.the_piece = None
self.existing_points = {}
self.dropped_points = {}
self.declare_parameter('mesh_filename', '')
self.declare_parameter('cache_filename', '')
self.declare_parameter('init_number_of_points', 10)
self.declare_parameter('max_z_coord', 2.)
self.declare_parameter('min_distance_between_points', 2.)
self.declare_parameter('height_upon_surface', 0.1)
self.declare_parameter('num_generation_trials', 1000)
self._load_data()
# создать инфраструктуру ROS
self.state_pub_msg = LocationsOnMesh()
self.state_pub = self.create_publisher(LocationsOnMesh, 'locations', 1)
self.markers_msg = MarkerArray()
self.markers_dict = {}
self.markers_pub = self.create_publisher(MarkerArray, 'locations_markers', 1)
self.generate_location_srv = self.create_service(GenerateLocation,
'generate_location',
self.generate_location_cb)
@@ -219,13 +210,15 @@ class GenLocationsOnMeshNode(Node):
# сгенерировать нужное количество точек
num_ps = self.get_parameter('init_number_of_points').get_parameter_value().integer_value
for i in range(num_ps):
gen_new_point(self.the_piece,
self.trs,
self.vs,
self.existing_points, 0.01, 0.1)
self.gen_new_point()
def pub_timer_cb(self):
'''таймер, периодически публикующий координаты точек'''
self.publish_unity_coords()
self.publish_real_coords_markers()
def publish_unity_coords(self):
'''опубликовать результаты в левостроронней системе координат Unity'''
# собрать в сообщение все данные про объекты
self.state_pub_msg.location_id = []
self.state_pub_msg.x = []
@@ -242,6 +235,14 @@ class GenLocationsOnMeshNode(Node):
# опубликовать их
self.state_pub.publish(self.state_pub_msg)
def publish_real_coords_markers(self):
'''опубликовать данные в правосторонней системе координат реального мира'''
curr_timestamp = self.get_clock().now().to_msg()
for m in self.markers_dict.values():
m.header.stamp = curr_timestamp
self.markers_msg.markers = list(self.markers_dict.values())
self.markers_pub.publish(self.markers_msg)
def _load_data(self):
'''загрузка мэша и его предобработка'''
# сначала пытаемся загрузить кэшированный вариант
@@ -274,21 +275,56 @@ class GenLocationsOnMeshNode(Node):
def generate_location_cb(self, request, response):
#self.get_logger().info('Incoming request GEN')
ind = gen_new_point(self.the_piece,
self.trs,
self.vs,
self.existing_points, 0.01, 0.1)
'''callback for generation service'''
ind = self.gen_new_point()
response.location_id = ind
response.x = self.existing_points[ind][0]
response.y = self.existing_points[ind][1]
response.z = self.existing_points[ind][2]
return response
def gen_new_point(self):
'''create point, return its index'''
max_z_coord = self.get_parameter('max_z_coord').get_parameter_value().double_value
min_distance_between_points = self.get_parameter('min_distance_between_points').get_parameter_value().double_value
height_upon_surface = self.get_parameter('height_upon_surface').get_parameter_value().double_value
num_generation_trials = self.get_parameter('num_generation_trials').get_parameter_value().integer_value
ind = gen_new_point(self.the_piece,
self.trs,
self.vs,
self.existing_points,
min_distance_between_points,
height_upon_surface,
max_z_coord,
num_generation_trials)
# create marker for the new point
m = Marker()
m.header.frame_id = 'map'
m.id = ind
m.type = Marker.SPHERE
m.action = Marker.ADD
m.pose.position.x = self.existing_points[ind][0]
m.pose.position.y = self.existing_points[ind][1]
m.pose.position.z = self.existing_points[ind][2]
m.pose.orientation.w = 1.
m.scale.x = 1.
m.scale.y = 1.
m.scale.z = 1.
m.color.r = 1.
m.color.a = 1.
self.markers_dict[ind] = m
return ind
def remove_location_cb(self, request, response):
#self.get_logger().info('Incoming request RM')
if request.location_id in self.existing_points:
self.dropped_points[request.location_id] = self.existing_points[request.location_id]
del self.existing_points[request.location_id]
# update marker
m = self.markers_dict[request.location_id]
m.color.r = 0.
m.color.g = 0.5
m.color.b = 1.
response.success = True
else:
response.success = False
gen_locations_on_mesh/launch/mesh_loc_generator.py
+4 -2
View File
@@ -6,7 +6,8 @@ from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
package_name = 'gen_locations_on_mesh'
mesh_file = 'raw_surface.ply'
#mesh_file = 'raw_surface.ply'
mesh_file = 'sirius_scene_floor.ply'
cache_file = 'cache.pkl'
# файл карты для загрузки
@@ -20,7 +21,8 @@ def generate_launch_description():
executable='gen_locations_on_mesh',
parameters=[{'mesh_filename': mesh_path,
'cache_filename': cache_path,
'init_number_of_points': 20}])
'init_number_of_points': 20,
'max_z_coord': 2.}])
# создать описание запуска
ld = LaunchDescription()
gen_locations_on_mesh/package.xml
+1
View File
@@ -13,6 +13,7 @@
<test_depend>python3-pytest</test_depend>
<depend>rclpy</depend>
<exec_depend>gen_locations_on_mesh_msgs</exec_depend>
<exec_depend>visualization_msgs</exec_depend>
<exec_depend>python3-trimesh-pip</exec_depend>
<export>
<build_type>ament_python</build_type>
gen_locations_on_mesh/share/sirius_scene_floor.ply
BIN
View File
Binary file not shown.