diff --git a/Readme.md b/Readme.md index 50729c3..a3b4dad 100644 --- a/Readme.md +++ b/Readme.md @@ -30,17 +30,23 @@ ros2 launch gen_locations_on_mesh mesh_loc_generator.py Cache for gen_locations_on_mesh saved successfully +Если нужно использовать другой мэш, то кэшированную версию надо удалить вручную из директории install/gen_locations_on_mesh/share/gen_locations_on_mesh/share/cache.pkl + +## Топик + +Состояние постоянно спамится в топик locations типа gen_locations_on_mesh_msgs/msg/LocationsOnMesh. Каждое из его полей: location_id, x, y, z --- список, содержащий параметры точек. Все списки имеют одну длину. + ## Сервисы Генерация точки выполняется вызовом сервиса generate_location без параметров: -ros2 service call /generate_location gen_locations_on_mesh_srvs/srv/GenerateLocation {} +ros2 service call /generate_location gen_locations_on_mesh_msgs/srv/GenerateLocation {} В ответе содержатся номер точки и её координаты: -gen_locations_on_mesh_srvs.srv.GenerateLocation_Response(location_id=1, x=434.56764747196445, y=-969.690370275587, z=73.794516038155) +gen_locations_on_mesh_msgs.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}' +ros2 service call /remove_location gen_locations_on_mesh_msgs/srv/RemoveLocation '{location_id: 2}' Возвращаемое значение False говорит о том, что такая точка неизвестна; True --- что точка успешно удалена. 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 1ff0014..8bbf24c 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 @@ -5,7 +5,8 @@ import random import rclpy from rclpy.node import Node -from gen_locations_on_mesh_srvs.srv import GenerateLocation, RemoveLocation +from gen_locations_on_mesh_msgs.msg import LocationsOnMesh +from gen_locations_on_mesh_msgs.srv import GenerateLocation, RemoveLocation import numpy as np @@ -199,16 +200,43 @@ class GenLocationsOnMeshNode(Node): self.declare_parameter('mesh_filename', '') self.declare_parameter('cache_filename', '') + self.declare_parameter('init_number_of_points', 10) self._load_data() # создать инфраструктуру ROS + self.state_pub_msg = LocationsOnMesh() + self.state_pub = self.create_publisher(LocationsOnMesh, 'locations', 1) 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) + self.pub_timer = self.create_timer(0.1, self.pub_timer_cb) + # сгенерировать нужное количество точек + 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) + + def pub_timer_cb(self): + '''таймер, периодически публикующий координаты точек''' + # собрать в сообщение все данные про объекты + self.state_pub_msg.location_id = [] + self.state_pub_msg.x = [] + self.state_pub_msg.y = [] + self.state_pub_msg.z = [] + for l_id, p in self.existing_points.items(): + self.state_pub_msg.location_id.append(l_id) + self.state_pub_msg.x.append(p[0]) + self.state_pub_msg.y.append(p[1]) + self.state_pub_msg.z.append(p[2]) + # опубликовать их + self.state_pub.publish(self.state_pub_msg) + def _load_data(self): '''загрузка мэша и его предобработка''' # сначала пытаемся загрузить кэшированный вариант @@ -220,7 +248,7 @@ class GenLocationsOnMeshNode(Node): except (pickle.PickleError, FileNotFoundError): # загрузка кэша не удалась, обрабатываем файлы полностью - self.get_logger().info('Loading data from mesh file in gen_locations_on_mesh\n') + self.get_logger().info('Loading data from mesh file in gen_locations_on_mesh') mesh_filename = self.get_parameter('mesh_filename').get_parameter_value().string_value mesh = trimesh.load_mesh(mesh_filename) self.vs = mesh.vertices @@ -235,13 +263,13 @@ class GenLocationsOnMeshNode(Node): 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') + self.get_logger().info('Cache for gen_locations_on_mesh saved successfully') except pickle.PickleError: - self.get_logger().warn('Error while saving cache in gen_locations_on_mesh\n') + self.get_logger().warn('Error while saving cache in gen_locations_on_mesh') def generate_location_cb(self, request, response): - self.get_logger().info('Incoming request GEN\n') + #self.get_logger().info('Incoming request GEN') ind = gen_new_point(self.the_piece, self.trs, self.vs, @@ -253,7 +281,7 @@ class GenLocationsOnMeshNode(Node): return response def remove_location_cb(self, request, response): - self.get_logger().info('Incoming request RM\n') + #self.get_logger().info('Incoming request RM') if request.location_id in self.existing_points: del self.existing_points[request.location_id] response.success = True diff --git a/gen_locations_on_mesh/launch/mesh_loc_generator.py b/gen_locations_on_mesh/launch/mesh_loc_generator.py index 4b4257b..88e2795 100644 --- a/gen_locations_on_mesh/launch/mesh_loc_generator.py +++ b/gen_locations_on_mesh/launch/mesh_loc_generator.py @@ -19,7 +19,8 @@ def generate_launch_description(): package='gen_locations_on_mesh', executable='gen_locations_on_mesh', parameters=[{'mesh_filename': mesh_path, - 'cache_filename': cache_path}]) + 'cache_filename': cache_path, + 'init_number_of_points': 20}]) # создать описание запуска ld = LaunchDescription() diff --git a/gen_locations_on_mesh/package.xml b/gen_locations_on_mesh/package.xml index e1bac81..2829ea6 100644 --- a/gen_locations_on_mesh/package.xml +++ b/gen_locations_on_mesh/package.xml @@ -12,7 +12,7 @@ ament_pep257 python3-pytest rclpy - gen_locations_on_mesh_srvs + gen_locations_on_mesh_msgs python3-trimesh-pip ament_python diff --git a/gen_locations_on_mesh_srvs/CMakeLists.txt b/gen_locations_on_mesh_msgs/CMakeLists.txt similarity index 94% rename from gen_locations_on_mesh_srvs/CMakeLists.txt rename to gen_locations_on_mesh_msgs/CMakeLists.txt index 8d6b57d..1dab416 100644 --- a/gen_locations_on_mesh_srvs/CMakeLists.txt +++ b/gen_locations_on_mesh_msgs/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(gen_locations_on_mesh_srvs) +project(gen_locations_on_mesh_msgs) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -14,6 +14,7 @@ find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} + "msg/LocationsOnMesh.msg" "srv/GenerateLocation.srv" "srv/RemoveLocation.srv" ) diff --git a/gen_locations_on_mesh_msgs/msg/LocationsOnMesh.msg b/gen_locations_on_mesh_msgs/msg/LocationsOnMesh.msg new file mode 100644 index 0000000..1311c74 --- /dev/null +++ b/gen_locations_on_mesh_msgs/msg/LocationsOnMesh.msg @@ -0,0 +1,4 @@ +uint32[] location_id +float64[] x +float64[] y +float64[] z diff --git a/gen_locations_on_mesh_srvs/package.xml b/gen_locations_on_mesh_msgs/package.xml similarity index 81% rename from gen_locations_on_mesh_srvs/package.xml rename to gen_locations_on_mesh_msgs/package.xml index 99cdcdd..16110d9 100644 --- a/gen_locations_on_mesh_srvs/package.xml +++ b/gen_locations_on_mesh_msgs/package.xml @@ -1,11 +1,11 @@ - gen_locations_on_mesh_srvs + gen_locations_on_mesh_msgs 0.0.0 - TODO: Package description + Messages and services for gen_locatoins_on_mesh user - TODO: License declaration + BSD ament_cmake diff --git a/gen_locations_on_mesh_srvs/srv/GenerateLocation.srv b/gen_locations_on_mesh_msgs/srv/GenerateLocation.srv similarity index 100% rename from gen_locations_on_mesh_srvs/srv/GenerateLocation.srv rename to gen_locations_on_mesh_msgs/srv/GenerateLocation.srv diff --git a/gen_locations_on_mesh_srvs/srv/RemoveLocation.srv b/gen_locations_on_mesh_msgs/srv/RemoveLocation.srv similarity index 100% rename from gen_locations_on_mesh_srvs/srv/RemoveLocation.srv rename to gen_locations_on_mesh_msgs/srv/RemoveLocation.srv