output in topic added

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 --- что точка успешно удалена.

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

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()

gen_locations_on_mesh/package.xml

@@ -12,7 +12,7 @@
   <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>gen_locations_on_mesh_msgs</exec_depend>
   <exec_depend>python3-trimesh-pip</exec_depend>
   <export>
     <build_type>ament_python</build_type>

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"
  )

gen_locations_on_mesh_msgs/msg/LocationsOnMesh.msg

@@ -0,0 +1,4 @@
+uint32[] location_id
+float64[] x
+float64[] y
+float64[] z

gen_locations_on_mesh_msgs/package.xml

@@ -1,11 +1,11 @@
 <?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>
+  <name>gen_locations_on_mesh_msgs</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>Messages and services for gen_locatoins_on_mesh</description>
   <maintainer email="petr.sorokoumov@gmail.com">user</maintainer>
-  <license>TODO: License declaration</license>
+  <license>BSD</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>