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