output in topic added
This commit is contained in:
@@ -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 --- что точка успешно удалена.
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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>
|
||||
|
||||
+2
-1
@@ -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"
|
||||
)
|
||||
@@ -0,0 +1,4 @@
|
||||
uint32[] location_id
|
||||
float64[] x
|
||||
float64[] y
|
||||
float64[] z
|
||||
@@ -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>
|
||||
|
||||
Reference in New Issue
Block a user