wip

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 < num_generation_trials:
-    while curr_trial < max_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.gen_new_point()
-                          self.trs,
-                          self.vs,
-                          self.existing_points, 0.01, 0.1)
 
     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')
+        '''callback for generation service'''
-        ind = gen_new_point(self.the_piece,
+        ind = self.gen_new_point()
-                            self.trs,
-                            self.vs,
-                            self.existing_points, 0.01, 0.1)
         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

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

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