diff --git a/gen_locations_on_mesh/gen_locations_on_mesh/gen_locations_on_mesh_node.py b/gen_locations_on_mesh/gen_locations_on_mesh/gen_locations_on_mesh_node.py
index 2de4222..1c2eb8c 100644
--- a/gen_locations_on_mesh/gen_locations_on_mesh/gen_locations_on_mesh_node.py
+++ b/gen_locations_on_mesh/gen_locations_on_mesh/gen_locations_on_mesh_node.py
@@ -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
diff --git a/gen_locations_on_mesh/launch/mesh_loc_generator.py b/gen_locations_on_mesh/launch/mesh_loc_generator.py
index 88e2795..9ed206c 100644
--- a/gen_locations_on_mesh/launch/mesh_loc_generator.py
+++ b/gen_locations_on_mesh/launch/mesh_loc_generator.py
@@ -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()
diff --git a/gen_locations_on_mesh/package.xml b/gen_locations_on_mesh/package.xml
index 2829ea6..2e01bb3 100644
--- a/gen_locations_on_mesh/package.xml
+++ b/gen_locations_on_mesh/package.xml
@@ -13,6 +13,7 @@
python3-pytest
rclpy
gen_locations_on_mesh_msgs
+ visualization_msgs
python3-trimesh-pip
ament_python
diff --git a/gen_locations_on_mesh/share/sirius_scene_floor.ply b/gen_locations_on_mesh/share/sirius_scene_floor.ply
new file mode 100644
index 0000000..8cfc45d
Binary files /dev/null and b/gen_locations_on_mesh/share/sirius_scene_floor.ply differ