commit 6b91ed60126a206161becbed661b29d635bb1a31 Author: Petr Sorokoumov Date: Fri Jun 19 10:48:55 2026 +0300 initial commit diff --git a/Readme.md b/Readme.md new file mode 100644 index 0000000..50729c3 --- /dev/null +++ b/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 --- что точка успешно удалена. diff --git a/gen_locations_on_mesh/gen_locations_on_mesh/__init__.py b/gen_locations_on_mesh/gen_locations_on_mesh/__init__.py new file mode 100644 index 0000000..e69de29 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 new file mode 100644 index 0000000..1ff0014 --- /dev/null +++ b/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() diff --git a/gen_locations_on_mesh/launch/mesh_loc_generator.py b/gen_locations_on_mesh/launch/mesh_loc_generator.py new file mode 100644 index 0000000..4b4257b --- /dev/null +++ b/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 diff --git a/gen_locations_on_mesh/package.xml b/gen_locations_on_mesh/package.xml new file mode 100644 index 0000000..e1bac81 --- /dev/null +++ b/gen_locations_on_mesh/package.xml @@ -0,0 +1,20 @@ + + + + gen_locations_on_mesh + 0.0.0 + Location registry on surface of a mesh + user + BSD + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + rclpy + gen_locations_on_mesh_srvs + python3-trimesh-pip + + ament_python + + diff --git a/gen_locations_on_mesh/resource/gen_locations_on_mesh b/gen_locations_on_mesh/resource/gen_locations_on_mesh new file mode 100644 index 0000000..e69de29 diff --git a/gen_locations_on_mesh/setup.cfg b/gen_locations_on_mesh/setup.cfg new file mode 100644 index 0000000..1de6aae --- /dev/null +++ b/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 diff --git a/gen_locations_on_mesh/setup.py b/gen_locations_on_mesh/setup.py new file mode 100644 index 0000000..f5fde84 --- /dev/null +++ b/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', + ], + }, +) diff --git a/gen_locations_on_mesh/share/raw_surface.ply b/gen_locations_on_mesh/share/raw_surface.ply new file mode 100644 index 0000000..e1d96de Binary files /dev/null and b/gen_locations_on_mesh/share/raw_surface.ply differ diff --git a/gen_locations_on_mesh/test/test_copyright.py b/gen_locations_on_mesh/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/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' diff --git a/gen_locations_on_mesh/test/test_flake8.py b/gen_locations_on_mesh/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/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) diff --git a/gen_locations_on_mesh/test/test_pep257.py b/gen_locations_on_mesh/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/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' diff --git a/gen_locations_on_mesh_srvs/CMakeLists.txt b/gen_locations_on_mesh_srvs/CMakeLists.txt new file mode 100644 index 0000000..8d6b57d --- /dev/null +++ b/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( 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() diff --git a/gen_locations_on_mesh_srvs/package.xml b/gen_locations_on_mesh_srvs/package.xml new file mode 100644 index 0000000..99cdcdd --- /dev/null +++ b/gen_locations_on_mesh_srvs/package.xml @@ -0,0 +1,22 @@ + + + + gen_locations_on_mesh_srvs + 0.0.0 + TODO: Package description + user + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/gen_locations_on_mesh_srvs/srv/GenerateLocation.srv b/gen_locations_on_mesh_srvs/srv/GenerateLocation.srv new file mode 100644 index 0000000..b18d936 --- /dev/null +++ b/gen_locations_on_mesh_srvs/srv/GenerateLocation.srv @@ -0,0 +1,5 @@ +--- +uint32 location_id +float64 x +float64 y +float64 z diff --git a/gen_locations_on_mesh_srvs/srv/RemoveLocation.srv b/gen_locations_on_mesh_srvs/srv/RemoveLocation.srv new file mode 100644 index 0000000..2c02522 --- /dev/null +++ b/gen_locations_on_mesh_srvs/srv/RemoveLocation.srv @@ -0,0 +1,3 @@ +uint32 location_id +--- +bool success