initial commit

Readme.md

@@ -0,0 +1,46 @@
+# Узел ROS2 для генерации точек вблизи поверхности мэша
+
+Узел содержит сервисы для создания и удаления точек вблизи поверхности указанного мэша.
+
+Мэш должен состоять только из треугольных граней; требований по непрерывности и др. хорошим свойствам нет, но обрабатываются только грани с уклоном не более 0.1 рад (5.7 градусов), чтобы не заставлять робота ездить по крутым склонам.
+
+Если в составе мэша есть несколько отдельных несвязных областей с почти горизонтальными мэшами, используется только та, в которой больше граней (независимо от площади и т.п.).
+
+По возможности точки располагаются друг от друга на расстоянии не меньше 0.01 (в единицах, в которых заданы координаты мэша); от поверхности они удалены на 0.1. Вероятность генерации на каждом треугольнике одинакова
+
+## Установка
+
+Для работы требуется нестандартный модуль Python trimesh. Чтобы поставить его, можно использовать rosdep:
+
+export PIP_BREAK_SYSTEM_PACKAGES=1
+
+rosdep install --from-paths src -y --ignore-src
+
+Собирать всё потом можно, как обычно
+
+colcon build --symlink-install
+
+## Запуск
+
+source install/local_setup.bash
+
+ros2 launch gen_locations_on_mesh mesh_loc_generator.py
+
+После первого запуска генерируется кэшированный результат предобработки мэша в течение нескольких секунд (10-20); об окончании этого говорит появившееся сообщение
+
+Cache for gen_locations_on_mesh saved successfully
+
+## Сервисы
+
+Генерация точки выполняется вызовом сервиса generate_location без параметров:
+
+ros2 service call /generate_location gen_locations_on_mesh_srvs/srv/GenerateLocation {}
+
+В ответе содержатся номер точки и её координаты:
+gen_locations_on_mesh_srvs.srv.GenerateLocation_Response(location_id=1, x=434.56764747196445, y=-969.690370275587, z=73.794516038155)
+
+Для уничтожения ранее созданной точки вызывается сервис remove_location с параметром location_id --- номером точки.
+
+ros2 service call /remove_location gen_locations_on_mesh_srvs/srv/RemoveLocation '{location_id: 2}'
+
+Возвращаемое значение False говорит о том, что такая точка неизвестна; True --- что точка успешно удалена.

gen_locations_on_mesh/gen_locations_on_mesh/gen_locations_on_mesh_node.py

@@ -0,0 +1,274 @@
+'''узел программы генерации точек возле поверхности мэшей'''
+import pickle
+import random
+
+import rclpy
+from rclpy.node import Node
+
+from gen_locations_on_mesh_srvs.srv import GenerateLocation, RemoveLocation
+
+import numpy as np
+
+import trimesh
+# Библиотека работы с мэшами.
+# Мэш должен состоять только из треугольников.
+# Совпадение координат вершин не допускается.
+# доступ к частям мэша:
+# mesh = trimesh.load_mesh(filename)
+# mesh.edges  # (ne,2)
+# mesh.faces  # (nf,3)
+# mesh.vertices  # (nv,3)
+
+
+def get_all_triangles_with_correct_orientation(vs, fs, max_a):
+    '''создать список всех треугольных граней, ориентированных
+    одинаково: при виде сверху точки проходятся по часовой стрелке.
+    Грани с углом наклона к вертикали больше max_a радиан не рассматриваются'''
+    min_cos = np.cos(max_a)
+    res_faces = []  # [(ind0, ind1, ind2, normal)]
+    # для каждой грани
+    for i in range(fs.shape[0]):
+        # извлечь индексы вершин
+        ind0 = fs[i,0]
+        ind1 = fs[i,1]
+        ind2 = fs[i,2]
+        # извлечь координаты вершин
+        v0 = vs[ind0,:]
+        v1 = vs[ind1,:]
+        v2 = vs[ind2,:]
+        # сформировать векторы сторон
+        sd1 = v1 - v0
+        sd2 = v2 - v0
+        # рассчитать нормаль
+        nrm = np.cross(sd2,sd1)
+        nrm /= np.linalg.norm(nrm)
+        # если грань слишком наклонена, она не рассматривается
+        if abs(nrm[2]) < min_cos:
+            continue
+        # определяем порядок вершин
+        # по направлению нормали
+        if nrm[2] > 0:
+            res = (ind0, ind1, ind2, nrm)
+        else:
+            res = (ind0, ind2, ind1, -nrm)
+        res_faces.append(res)
+        #if i % 10000 == 0:
+        #    print(f'{i/fs.shape[0]*100:3.1f}%')
+    return res_faces
+
+def calc_connections(fs):
+    '''рассчитать для каждой грани её соединения
+    --- список граней, с которой она связана'''
+    connections = {}  # face_id -> [face_id]
+    unknown_conns = {}  # (v1, v2) -> face_id
+    for i,(ind0, ind1, ind2, _) in enumerate(fs):
+        # для каждого ребра: если оно уже встречалось,
+        # добавим связь; если нет, вставим в список ожидания
+        for edg in [(ind1, ind0), (ind2, ind1), (ind0, ind2)]:
+            if edg in unknown_conns:
+                # добавить связь между i и unknown_conns[edg]
+                if i in connections:
+                    connections[i].append(unknown_conns[edg])
+                else:
+                    connections[i] = [unknown_conns[edg]]
+                if unknown_conns[edg] in connections:
+                    connections[unknown_conns[edg]].append(i)
+                else:
+                    connections[unknown_conns[edg]] = [i]
+                # убрать ребро
+                del unknown_conns[edg]
+            else:  # связи нет, добавляем её в список ожидания
+                unknown_conns[(edg[1], edg[0])] = i
+    return connections
+
+def calc_pieces(faces, conns):
+    '''найти непрерывные куски поверхности'''
+    part_used = [False] * len(faces)
+    parts = []
+    curr_index = 0
+    # обходим все грани
+    while curr_index < len(faces):
+        if part_used[curr_index]:
+            # пропустить уже использованную грань
+            curr_index += 1
+            continue
+        # начинаем новый кусок с текущей грани
+        new_part = set([curr_index])
+        part_used[curr_index] = True
+        # поместим в очередь все грани, которые надо искать на текущем шаге
+        queue = [curr_index]
+        # пока очередь не закончится, будем искать грани
+        while len(queue) > 0:
+            curr_f = queue[0]
+            del queue[0]
+            # пропустить грань без соседей
+            if curr_f not in conns:
+                continue
+            # для всех соседей:
+            for nb in conns[curr_f]:
+                if not part_used[nb]:
+                    # добавляем грань в кусок
+                    part_used[nb] = True
+                    # ставим её в очередь
+                    queue.append(nb)
+                    new_part.add(nb)
+        # очередь закончена, кусок сформирован
+        parts.append(new_part)
+        curr_index += 1
+
+    return parts
+
+def find_largest_piece(pieces):
+    '''найти самый большой кусок мэшей'''
+    # TODO учитывать не количество, а площадь
+    if len(pieces) == 0:
+        raise ValueError('No pieces found')
+    ls = [len(v) for v in pieces]
+    max_len = max(ls)
+    return pieces[ls.index(max_len)]
+
+def gen_new_point(piece, trs, vs, existing_ps, dist_limit, vert_shift):
+    '''создать новую точку случайным образом;
+    вернуть словарь точек (с новой)'''
+    # будем снова и снова пытаться сделать точку, удалённую от других;
+    # если после множества попыток это не получится, смиримся
+    # и примем ту, что есть
+    curr_trial = 0
+    max_trials = 100
+    while curr_trial < max_trials:
+        # выбрать случайный треугольник
+        tr_ind = random.randrange(len(piece))
+        # сформировать координаты случайной точки в нём
+        alpha = random.random()
+        beta = random.random()
+        if alpha + beta > 1:
+            kappa = (alpha + beta - 1) * 0.5
+            alpha -= 2 * kappa
+            beta -= 2 * kappa
+        # получить её абсолютные координаты
+        ind0, ind1, ind2, nrm = trs[tr_ind]
+        c0 = vs[ind0,:]
+        c1 = vs[ind1,:]
+        c2 = vs[ind2,:]
+        p = c0 + (c1-c0)*alpha + (c2-c0)*beta
+        p[2] += vert_shift
+        # проверить совпадение координат с существующими точками
+        succ = True
+        for ep in existing_ps.values():
+            if np.linalg.norm(p - ep) < dist_limit:
+                succ = False
+                break
+        if succ or curr_trial == max_trial - 1:
+            break
+    # выдать найденную точку
+    if len(existing_ps) > 0:
+        new_ind = max(existing_ps.keys()) + 1
+    else:
+        new_ind = 1
+    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):
+        super().__init__('gen_locations_on_mesh_node')
+
+        # состояние
+        self.vs = None
+        self.trs = None
+        self.the_piece = None
+        self.existing_points = {}
+
+        self.declare_parameter('mesh_filename', '')
+        self.declare_parameter('cache_filename', '')
+
+        self._load_data()
+
+        # создать инфраструктуру ROS
+        self.generate_location_srv = self.create_service(GenerateLocation,
+                                                         'generate_location',
+                                                         self.generate_location_cb)
+        self.remove_location_srv = self.create_service(RemoveLocation,
+                                                       'remove_location',
+                                                       self.remove_location_cb)
+    def _load_data(self):
+        '''загрузка мэша и его предобработка'''
+        # сначала пытаемся загрузить кэшированный вариант
+        cache_filename = self.get_parameter('cache_filename').get_parameter_value().string_value
+        try:
+            with open(cache_filename, 'rb') as f:
+                dt = pickle.load(f)
+                self.vs, self.trs, self.the_piece = dt
+        except (pickle.PickleError,
+                FileNotFoundError):
+            # загрузка кэша не удалась, обрабатываем файлы полностью
+            self.get_logger().info('Loading data from mesh file in gen_locations_on_mesh\n')
+            mesh_filename = self.get_parameter('mesh_filename').get_parameter_value().string_value
+            mesh = trimesh.load_mesh(mesh_filename)
+            self.vs = mesh.vertices
+            self.trs = get_all_triangles_with_correct_orientation(mesh.vertices,
+                                                                mesh.faces,
+                                                                0.1)
+            conns = calc_connections(self.trs)
+            pieces = calc_pieces(self.trs, conns)
+            self.the_piece = find_largest_piece(pieces)
+            # сохраняем кэш
+            try:
+                with open(cache_filename, 'wb') as f:
+                    dt = (self.vs, self.trs, self.the_piece)
+                    pickle.dump(dt, f)
+                self.get_logger().info('Cache for gen_locations_on_mesh saved successfully\n')
+            except pickle.PickleError:
+                self.get_logger().warn('Error while saving cache in gen_locations_on_mesh\n')
+
+
+    def generate_location_cb(self, request, response):
+        self.get_logger().info('Incoming request GEN\n')
+        ind = gen_new_point(self.the_piece,
+                            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 remove_location_cb(self, request, response):
+        self.get_logger().info('Incoming request RM\n')
+        if request.location_id in self.existing_points:
+            del self.existing_points[request.location_id]
+            response.success = True
+        else:
+            response.success = False
+        return response
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    gen_node = GenLocationsOnMeshNode()
+    try:
+        rclpy.spin(gen_node)
+    except KeyboardInterrupt:
+        pass
+
+if __name__ == '__main__':
+    main()

gen_locations_on_mesh/launch/mesh_loc_generator.py

@@ -0,0 +1,28 @@
+import os
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+def generate_launch_description():
+    package_name = 'gen_locations_on_mesh'
+    mesh_file = 'raw_surface.ply'
+    cache_file = 'cache.pkl'
+
+    # файл карты для загрузки
+    pkg_share = FindPackageShare(package=package_name).find(package_name)
+    mesh_path = os.path.join(pkg_share, 'share', mesh_file)
+    cache_path = os.path.join(pkg_share, 'share', cache_file)
+
+    start_gen_location_on_mesh_cmd = Node(
+      name='gen_locations_on_mesh',
+      package='gen_locations_on_mesh',
+      executable='gen_locations_on_mesh',
+      parameters=[{'mesh_filename': mesh_path,
+                   'cache_filename': cache_path}])
+
+    # создать описание запуска
+    ld = LaunchDescription()
+    ld.add_action(start_gen_location_on_mesh_cmd)
+    # завершить создание
+    return ld

gen_locations_on_mesh/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>gen_locations_on_mesh</name>
+  <version>0.0.0</version>
+  <description>Location registry on surface of a mesh</description>
+  <maintainer email="petr.sorokoumov@gmail.com">user</maintainer>
+  <license>BSD</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+  <depend>rclpy</depend>
+  <exec_depend>gen_locations_on_mesh_srvs</exec_depend>
+  <exec_depend>python3-trimesh-pip</exec_depend>
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

gen_locations_on_mesh/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/gen_locations_on_mesh
+[install]
+install_scripts=$base/lib/gen_locations_on_mesh

gen_locations_on_mesh/setup.py

@@ -0,0 +1,35 @@
+from glob import glob
+import os
+from setuptools import find_packages, setup
+
+package_name = 'gen_locations_on_mesh'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'),
+           glob('launch/*')),
+        (os.path.join('share', package_name, 'share'),
+           glob('share/*'))
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='user',
+    maintainer_email='petr.sorokoumov@gmail.com',
+    description='Location registry on surface of a mesh',
+    license='BSD',
+    extras_require={
+        'test': [
+            'pytest',
+        ],
+    },
+    entry_points={
+        'console_scripts': ['gen_locations_on_mesh = gen_locations_on_mesh.gen_locations_on_mesh_node:main',
+        ],
+    },
+)

gen_locations_on_mesh/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

gen_locations_on_mesh/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

gen_locations_on_mesh/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

gen_locations_on_mesh_srvs/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.8)
+project(gen_locations_on_mesh_srvs)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/GenerateLocation.srv"
+  "srv/RemoveLocation.srv"
+ )
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

gen_locations_on_mesh_srvs/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>gen_locations_on_mesh_srvs</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="petr.sorokoumov@gmail.com">user</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

gen_locations_on_mesh_srvs/srv/GenerateLocation.srv

@@ -0,0 +1,5 @@
+---
+uint32 location_id
+float64 x
+float64 y
+float64 z

gen_locations_on_mesh_srvs/srv/RemoveLocation.srv

@@ -0,0 +1,3 @@
+uint32 location_id
+---
+bool success