Resolve "Добавить параметры в конфиг pose estimation узла"
This commit is contained in:
parent
4371dbdcee
commit
077872e489
22 changed files with 221906 additions and 108 deletions
|
|
@ -35,6 +35,7 @@ install(PROGRAMS
|
|||
scripts/detection_service.py
|
||||
scripts/grasp_marker_publish.py
|
||||
scripts/pose_estimation.py
|
||||
scripts/pose_estimation_lifecycle.py
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
|
|
|
|||
|
|
@ -1,16 +1,53 @@
|
|||
# Описание работы узла по оценке положения объекта
|
||||
# Оценка 6D положения объекта
|
||||
|
||||
Есть два варианта работы узла по оценке 6D позы объекта. Первый предполагает использование ROS-узла как сервиса, второй - как [lifecycle-узла](https://design.ros2.org/articles/node_lifecycle.html).
|
||||
Узел с управляемым жизненным циклом (lifecycle) позволяет лучше контролировать состояние системы ROS. Такой подход позволит системе убедиться, что все компоненты были созданы правильно, прежде чем любой компонент сможет начать выполнение своей задачи. Это также позволит перезапускать или заменять узел в режиме онлайн.
|
||||
Так как задача оценки позиции объекта требует использования больших вычислительных ресурсов, то реализация lifecycle-узла позволяет управлять загрузкой ресурсов при этом.
|
||||
|
||||
## Вариант 1. Сервис по оценке позы объекта
|
||||
|
||||
Запуск узла производится командой:
|
||||
```bash
|
||||
ros2 run rbs_perception pose_estimation.py
|
||||
```
|
||||
Запускается узел с именем 'image_sub2' по умолчанию. В нём создаётся сервис для распознавания позиции с именем 'detect6Dpose', который ожидает клиентский запрос.
|
||||
Запускается узел с именем `image_sub2` по умолчанию. В нём создаётся сервис для распознавания позиции с именем `detect6Dpose`, который ожидает клиентский запрос.
|
||||
Для получения позиции заданного объекта клиент посылает сервису запрос с параметром ObjectInfo на входе
|
||||
- id - идентификатор,
|
||||
- name - имя объекта,
|
||||
- mesh_path - путь к mesh-файлу в формате *.ply.
|
||||
- `id` - идентификатор,
|
||||
- `name` - имя объекта,
|
||||
- `mesh_path` - путь к mesh-файлу в формате *.ply.
|
||||
|
||||
При получении запроса сервис 'detect6Dpose' подписывается на Image-сообщения с камеры '/ground_true/camera_node', которые использует для запуска алгоритма 6D оценки позиции Megapose. После получения результата от Megapose сервис публикует сообщение с позицией (Quaternion) в 'pose6D_[obj]' topic.
|
||||
При получении запроса сервис `detect6Dpose` подписывается на Image-сообщения с камеры `/ground_true/camera_node`, которые использует для запуска алгоритма 6D оценки позиции Megapose. После получения результата от Megapose сервис публикует сообщение с позицией (Quaternion) в `pose6D_[obj]` topic.
|
||||
|
||||
## Вариант 2. Lifecycle-узел ROS для получения 6D-позы
|
||||
|
||||
Запуск узла производится командой:
|
||||
```bash
|
||||
ros2 run rbs_perception pose_estimation_lifecycle.py
|
||||
```
|
||||
Запускается узел с именем `image_sub2` по умолчанию. Настроечные переменные задаются в файле конфигурации `config/pose_estimation_config.json`. Пример:
|
||||
```json
|
||||
{
|
||||
"nodeName":"image_sub2",
|
||||
"topicImage":"/outer_rgbd_camera/image",
|
||||
"topicCameraInfo":"/outer_rgbd_camera/camera_info",
|
||||
"topicDepth":"/outer_rgbd_camera/depth_image",
|
||||
"topicSrv":"/image_sub2/detect6Dpose",
|
||||
"publishDelay": 3.3,
|
||||
"tf2_send_pose": 1
|
||||
}
|
||||
```
|
||||
- `"nodeName"`- это поле указывает на имя узла в контексте ROS;
|
||||
- `"topicImage"`- это поле определяет топик, по которому публикуются изображения, полученные с RGBD-камеры;
|
||||
- `"topicCameraInfo"`- это поле указывает на топик, по которому публикуется информация о камере, такая как параметры калибровки и настройки;
|
||||
- `"topicDepth"`- это поле определяет топик для изображений глубины, получаемых от RGBD-камеры. Изображения глубины предоставляют информацию о расстоянии до объектов в сцене;
|
||||
- `"topicSrv"`- это поле определяет топик, по которому публикуется 6D-поза объекта после оценки;
|
||||
- `"publishDelay"`- это поле, указывает задержку (в секундах) перед публикацией сообщений в топики;
|
||||
- `"tf2_send_pose"`- это поле связано с отправкой данных о позе (положении и ориентации) в системе tf2, которая используется в ROS для управления координатными преобразованиями. Значение 1 означает включение или активацию данной функции (0 - отключение).
|
||||
|
||||
Первым этапом работы узла будет являться его настройка вызовом `on_configure()`, перед которым необходимо установить параметр узла `"mesh_path"`. Этот строковый параметр должен содержать полный путь к файлу сетчатой модели объекта в формате *.ply.
|
||||
Если конфигурирование завершено успешно, узел перейдёт в состояние `'inactive'`. Затем нужно узел сделать активным, вызвав `on_activate()`. При этом активируется подписчик на изображения с камеры (`"topicImage"`), в обратном вызове которого будет происходить распознавание позиции объекта (megapose) и его публикация в топике `"topicSrv"`.
|
||||
Чтобы отключить распознавание нужно вызвать событие `on_deactivate()`.
|
||||
Для новой настройки узла при необходимости оценки позы другого объекта нужно вызвать событие `on_cleanup()`, а затем повторить процедуру конфигурирования и активации.
|
||||
|
||||
### Важное замечание
|
||||
---
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
{
|
||||
"topicName":"image_sub",
|
||||
"topicImage":"/ground_true/camera_node",
|
||||
"topicImage":"/outer_rgbd_camera/image",
|
||||
"weightsFile":"yolov4_objs2_final.weights"
|
||||
}
|
||||
|
|
|
|||
220438
rbs_perception/config/fork.ply
Normal file
220438
rbs_perception/config/fork.ply
Normal file
File diff suppressed because it is too large
Load diff
|
|
@ -1,5 +1,14 @@
|
|||
{
|
||||
"topicName":"image_sub2",
|
||||
"topicImage":"/ground_true/camera_node",
|
||||
"weightsFile":"yolov4_objs2_final.weights"
|
||||
"nodeName":"image_sub2",
|
||||
"topicImage":"/outer_rgbd_camera/image",
|
||||
"topicCameraInfo":"/outer_rgbd_camera/camera_info",
|
||||
"topicDepth":"/outer_rgbd_camera/depth_image",
|
||||
"publishDelay": 3.3,
|
||||
"tf2_send_pose": 1,
|
||||
"camera_info": {
|
||||
"fx": 924.855,
|
||||
"fy": 923.076,
|
||||
"width": 640,
|
||||
"height": 480
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -3,14 +3,15 @@
|
|||
detection_service
|
||||
ROS 2 program for 6D Pose Estimation
|
||||
|
||||
@shalenikol release 0.1
|
||||
@shalenikol release 0.2
|
||||
"""
|
||||
|
||||
# Import the necessary libraries
|
||||
import rclpy # Python library for ROS 2
|
||||
from rclpy.node import Node # Handles the creation of nodes
|
||||
from sensor_msgs.msg import Image # Image is the message type
|
||||
from geometry_msgs.msg import Quaternion
|
||||
from geometry_msgs.msg import Quaternion, TransformStamped
|
||||
from tf2_ros import TransformBroadcaster
|
||||
from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images
|
||||
import cv2 # OpenCV library
|
||||
from rbs_skill_interfaces.srv import DetectObject
|
||||
|
|
@ -23,9 +24,11 @@ import tempfile
|
|||
from pathlib import Path
|
||||
import numpy as np
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import megapose
|
||||
# import megapose
|
||||
from megapose.scripts.run_inference_on_example import run_inference
|
||||
|
||||
tf2_send_pose = True
|
||||
|
||||
"""
|
||||
# encoder for numpy array
|
||||
def np_encoder(object):
|
||||
|
|
@ -44,49 +47,51 @@ class ImageSubscriber(Node):
|
|||
y = json.load(f)
|
||||
|
||||
for name, val in y.items():
|
||||
if name == "topicName":
|
||||
self.topicName = val
|
||||
if name == "nodeName":
|
||||
self.nodeName = val
|
||||
elif name == "topicImage":
|
||||
self.topicImage = val
|
||||
elif name == "topicPubName":
|
||||
self.topicPubName = val
|
||||
elif name == "topicSrv":
|
||||
self.topicSrv = val
|
||||
elif name == "tf2_send_pose":
|
||||
self.tf2_send_pose = val
|
||||
elif name == "camera_info":
|
||||
self.K_, self.res_ = self._getCameraParam(val)
|
||||
|
||||
def _getCameraParam(self):
|
||||
# {"K": [[924.855, 0.0, 320.0], [0.0, 923.076, 240.0], [0.0, 0.0, 1.0]], "resolution": [480, 640]}
|
||||
def _getCameraParam(self, info):
|
||||
"""
|
||||
focal length: 25 mm (field of view: 38.1712°)
|
||||
sensor type: Micro Four Thirds System (Стандарт MFT микро 4/3)
|
||||
sensor_width: 17.3 mm
|
||||
sensor_height: 13 mm
|
||||
Returns the intrinsic matrix and resolution from the provided camera info.
|
||||
"""
|
||||
resolution = [480, 640] #np.array([480, 640])
|
||||
intrinsic_matrix = [ #np.array([
|
||||
[924.855, 0.0, resolution[1] / 2.0],
|
||||
[0.0, 923.076, resolution[0] / 2.0],
|
||||
[0.0, 0.0, 1.0]
|
||||
] #)
|
||||
intrinsic_matrix = [
|
||||
[info["fx"], 0.0, info["width"] / 2.0],
|
||||
[0.0, info["fy"], info["height"] / 2.0],
|
||||
[0.0, 0.0, 1.0]
|
||||
]
|
||||
resolution = [info["height"], info["width"]]
|
||||
return intrinsic_matrix, resolution
|
||||
|
||||
def __init__(self):
|
||||
"""
|
||||
Class constructor to set up the node
|
||||
"""
|
||||
self.topicName = "image_sub2"
|
||||
self.topicImage = "/ground_true/camera_node"
|
||||
self.topicPubName = "pose6D_images"
|
||||
self.topicSrv = "detect6Dpose"
|
||||
self.nodeName = "image_sub2"
|
||||
self.topicImage = "/outer_rgbd_camera/image"
|
||||
self.topicPubName = self.nodeName + "/pose6D_images"
|
||||
self.topicSrv = self.nodeName + "/detect6Dpose"
|
||||
self._InitService()
|
||||
|
||||
self.tmpdir = tempfile.gettempdir()
|
||||
self.mytemppath = Path(self.tmpdir) / "rbs_per"
|
||||
self.mytemppath.mkdir(exist_ok=True)
|
||||
self.K_, self.res_ = self._getCameraParam()
|
||||
#os.environ["MEGAPOSE_DATA_DIR"] = str(self.mytemppath)
|
||||
|
||||
# Initiate the Node class's constructor and give it a name
|
||||
super().__init__(self.topicName)
|
||||
super().__init__(self.nodeName)
|
||||
|
||||
# Initialize the transform broadcaster
|
||||
self.tf_broadcaster = TransformBroadcaster(self)
|
||||
|
||||
self.subscription = None
|
||||
self.objName = ""
|
||||
|
|
@ -101,14 +106,15 @@ class ImageSubscriber(Node):
|
|||
self.service = self.create_service(DetectObject, self.topicSrv, self.service_callback)
|
||||
|
||||
def service_callback(self, request, response):
|
||||
self.get_logger().info(f"Incoming request for object detection ObjectInfo(name: {request.object.name}, mesh_path: {request.object.mesh_path})")
|
||||
self.get_logger().info(f"Incoming request for pose estimation ObjectInfo(name: {request.object.name}, mesh_path: {request.object.mesh_path})")
|
||||
|
||||
if not os.path.isfile(request.object.mesh_path):
|
||||
response.call_status = False
|
||||
response.error_msg = f"{request.object.mesh_path}: no such file"
|
||||
return response
|
||||
if request.object.id == -1:
|
||||
self.subscription = None # ? сброс подписки
|
||||
self.subscription = None # ? сброс подпискиpython -m megapose.scripts.download --example_data
|
||||
|
||||
response.call_status = True
|
||||
return response
|
||||
|
||||
|
|
@ -126,7 +132,7 @@ class ImageSubscriber(Node):
|
|||
output_fn = tPath / "object_data.json"
|
||||
output_json_dict = {
|
||||
"label": self.objName,
|
||||
"bbox_modal": [2,2,self.res_[1]-1,self.res_[0]-1]
|
||||
"bbox_modal": [2,2,self.res_[1]-4,self.res_[0]-4]
|
||||
}
|
||||
data = []
|
||||
data.append(output_json_dict)
|
||||
|
|
@ -146,7 +152,7 @@ class ImageSubscriber(Node):
|
|||
}
|
||||
data = []
|
||||
data.append(output_json_dict)
|
||||
output_fn.write_text(json.dumps(data))
|
||||
output_fn.write_text(json.dumps(output_json_dict))
|
||||
|
||||
# Create the subscriber. This subscriber will receive an Image from the video_frames topic. The queue size is 3 messages.
|
||||
self.subscription = self.create_subscription(Image, self.topicImage, self.listener_callback, 3)
|
||||
|
|
@ -166,6 +172,29 @@ class ImageSubscriber(Node):
|
|||
data = "No result file: '" + str(f) + "'"
|
||||
return data
|
||||
|
||||
def tf_obj_pose(self,pose):
|
||||
"""
|
||||
Передача позиции объекта в tf2
|
||||
"""
|
||||
t = TransformStamped()
|
||||
# assign pose to corresponding tf variables
|
||||
t.header.stamp = self.get_clock().now().to_msg()
|
||||
t.header.frame_id = 'world'
|
||||
t.child_frame_id = self.objName
|
||||
# coordinates
|
||||
tr = pose[1]
|
||||
t.transform.translation.x = tr[0]
|
||||
t.transform.translation.y = tr[1]
|
||||
t.transform.translation.z = tr[2]
|
||||
# rotation
|
||||
q = pose[0]
|
||||
t.transform.rotation.x = q[1] # 0
|
||||
t.transform.rotation.y = q[2] # 1
|
||||
t.transform.rotation.z = q[3] # 2
|
||||
t.transform.rotation.w = q[0] # 3
|
||||
# Send the transformation
|
||||
self.tf_broadcaster.sendTransform(t)
|
||||
|
||||
def listener_callback(self, data):
|
||||
"""
|
||||
Callback function.
|
||||
|
|
@ -182,16 +211,19 @@ class ImageSubscriber(Node):
|
|||
|
||||
# 6D pose estimation
|
||||
self.get_logger().info(f"megapose: begin {self.cnt}")
|
||||
print(self.objPath)
|
||||
run_inference(self.objPath,"megapose-1.0-RGB-multi-hypothesis")
|
||||
|
||||
# опубликуем результат оценки позы
|
||||
data = self.load_result(self.objPath)
|
||||
if data[0] == "[":
|
||||
y = json.loads(data)[0]
|
||||
pose = y['TWO']
|
||||
pose = y["TWO"]
|
||||
quat = pose[0]
|
||||
#pose[1] - 3D перемещение
|
||||
self.publisher.publish(Quaternion(x=quat[1],y=quat[2],z=quat[3],w=quat[0]))
|
||||
if tf2_send_pose:
|
||||
self.tf_obj_pose(pose)
|
||||
|
||||
self.get_logger().info(f"megapose: end {self.cnt}")
|
||||
|
||||
|
|
|
|||
342
rbs_perception/scripts/pose_estimation_lifecycle.py
Executable file
342
rbs_perception/scripts/pose_estimation_lifecycle.py
Executable file
|
|
@ -0,0 +1,342 @@
|
|||
#!/usr/bin/env python3
|
||||
"""
|
||||
pose_estimation_lifecycle_node
|
||||
ROS 2 program for 6D Pose Estimation
|
||||
|
||||
@shalenikol release 0.3
|
||||
"""
|
||||
|
||||
from typing import Optional
|
||||
import os
|
||||
import shutil
|
||||
import json
|
||||
import tempfile
|
||||
from pathlib import Path
|
||||
|
||||
import rclpy
|
||||
from rclpy.lifecycle import Node
|
||||
from rclpy.lifecycle import Publisher
|
||||
from rclpy.lifecycle import State
|
||||
from rclpy.lifecycle import TransitionCallbackReturn
|
||||
from rclpy.timer import Timer
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from sensor_msgs.msg import Image, CameraInfo
|
||||
from geometry_msgs.msg import Pose, TransformStamped
|
||||
from tf2_ros import TransformBroadcaster
|
||||
from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images
|
||||
import cv2 # OpenCV library
|
||||
from megapose.scripts.run_inference_on_example import ModelPreload, run_inference_rbs #, run_inference
|
||||
|
||||
class PoseEstimator(Node):
|
||||
"""Our lifecycle node."""
|
||||
|
||||
def _InitService(self):
|
||||
# Initialization service data
|
||||
p = os.path.join(get_package_share_directory("rbs_perception"), "config", "pose_estimation_config.json")
|
||||
# load config
|
||||
with open(p, "r") as f:
|
||||
y = json.load(f)
|
||||
|
||||
for name, val in y.items():
|
||||
if name == "nodeName":
|
||||
self.nodeName = val
|
||||
elif name == "topicImage":
|
||||
self.topicImage = val
|
||||
elif name == "topicCameraInfo":
|
||||
self.topicCameraInfo = val
|
||||
elif name == "topicDepth":
|
||||
self.topicDepth = val
|
||||
elif name == "publishDelay":
|
||||
self.publishDelay = val
|
||||
elif name == "topicSrv":
|
||||
self.topicSrv = val
|
||||
elif name == "tf2_send_pose":
|
||||
self.tf2_send_pose = val
|
||||
|
||||
def __init__(self, node_name, **kwargs):
|
||||
"""Construct the node."""
|
||||
self._count: int = 0
|
||||
self._pub: Optional[Publisher] = None
|
||||
self._timer: Optional[Timer] = None
|
||||
self._image_cnt: int = 0
|
||||
self._sub = None
|
||||
self._sub_info = None
|
||||
self._sub_depth = None
|
||||
self._is_camerainfo = False
|
||||
self._K = [[0., 0., 0.], [0., 0., 0.], [0., 0., 0.]]
|
||||
self._res = [0, 0]
|
||||
self._pose = [[1., 0., 0., 0.], [0., 0., 0.]]
|
||||
self.tf2_send_pose = 0
|
||||
self.megapose_model = None
|
||||
|
||||
self.nodeName = node_name #"image_sub2"
|
||||
self.topicImage = "/outer_rgb_camera/image"
|
||||
self.topicCameraInfo = "/outer_rgb_camera/camera_info"
|
||||
self.topicDepth = "/outer_rgbd_camera/depth_image"
|
||||
self.publishDelay = 2.0
|
||||
self.topicSrv = self.nodeName + "/detect6Dpose"
|
||||
self._InitService()
|
||||
|
||||
self.tmpdir = tempfile.gettempdir()
|
||||
self.mytemppath = Path(self.tmpdir) / "rbs_per"
|
||||
self.mytemppath.mkdir(exist_ok=True)
|
||||
|
||||
super().__init__(self.nodeName, **kwargs)
|
||||
self.declare_parameter("mesh_path", "")
|
||||
|
||||
# Initialize the transform broadcaster
|
||||
self.tf_broadcaster = TransformBroadcaster(self)
|
||||
# Used to convert between ROS and OpenCV images
|
||||
self.br = CvBridge()
|
||||
|
||||
self.objName = ""
|
||||
self.objMeshFile = ""
|
||||
self.objPath = ""
|
||||
|
||||
def publish(self):
|
||||
"""Publish a new message when enabled."""
|
||||
self._count += 1
|
||||
|
||||
if self._pub is not None and self._pub.is_activated:
|
||||
# опубликуем результат оценки позы
|
||||
q = self._pose[0]
|
||||
t = self._pose[1]
|
||||
p = Pose()
|
||||
p.position.x = t[0]
|
||||
p.position.y = t[1]
|
||||
p.position.z = t[2]
|
||||
p.orientation.w = q[0]
|
||||
p.orientation.x = q[1]
|
||||
p.orientation.y = q[2]
|
||||
p.orientation.z = q[3]
|
||||
self._pub.publish(p)
|
||||
|
||||
if self.tf2_send_pose:
|
||||
self.tf_obj_pose(self._pose)
|
||||
|
||||
def tf_obj_pose(self,pose):
|
||||
"""
|
||||
Передача позиции объекта в tf2
|
||||
"""
|
||||
t = TransformStamped()
|
||||
# assign pose to corresponding tf variables
|
||||
t.header.stamp = self.get_clock().now().to_msg()
|
||||
t.header.frame_id = 'world'
|
||||
t.child_frame_id = self.objName
|
||||
# coordinates
|
||||
tr = pose[1]
|
||||
t.transform.translation.x = tr[0]
|
||||
t.transform.translation.y = tr[1]
|
||||
t.transform.translation.z = tr[2]
|
||||
# rotation
|
||||
q = pose[0]
|
||||
t.transform.rotation.x = q[1] # 0
|
||||
t.transform.rotation.y = q[2] # 1
|
||||
t.transform.rotation.z = q[3] # 2
|
||||
t.transform.rotation.w = q[0] # 3
|
||||
# Send the transformation
|
||||
self.tf_broadcaster.sendTransform(t)
|
||||
|
||||
def on_configure(self, state: State) -> TransitionCallbackReturn:
|
||||
"""
|
||||
Configure the node, after a configuring transition is requested.
|
||||
|
||||
:return: The state machine either invokes a transition to the "inactive" state or stays
|
||||
in "unconfigured" depending on the return value.
|
||||
TransitionCallbackReturn.SUCCESS transitions to "inactive".
|
||||
TransitionCallbackReturn.FAILURE transitions to "unconfigured".
|
||||
TransitionCallbackReturn.ERROR or any uncaught exceptions to "errorprocessing"
|
||||
"""
|
||||
mesh_path = self.get_parameter("mesh_path").get_parameter_value().string_value
|
||||
if not os.path.isfile(mesh_path):
|
||||
self.get_logger().info("Parameter 'mesh_path' not set")
|
||||
return TransitionCallbackReturn.FAILURE
|
||||
|
||||
data = os.path.basename(mesh_path)
|
||||
self.objName = os.path.splitext(data)[0]
|
||||
self.objMeshFile = mesh_path
|
||||
self.objPath = self.mytemppath / "examples"
|
||||
self.objPath.mkdir(exist_ok=True)
|
||||
self.objPath /= self.objName
|
||||
self.objPath.mkdir(exist_ok=True)
|
||||
tPath = self.objPath / "inputs"
|
||||
tPath.mkdir(exist_ok=True)
|
||||
tPath = self.objPath / "meshes"
|
||||
tPath.mkdir(exist_ok=True)
|
||||
tPath /= self.objName
|
||||
tPath.mkdir(exist_ok=True)
|
||||
shutil.copyfile(self.objMeshFile, str(tPath / (self.objName+".ply")))
|
||||
|
||||
# Create the subscribers.
|
||||
self._sub_info = self.create_subscription(CameraInfo, self.topicCameraInfo, self.listener_camera_info, 2)
|
||||
self._sub_depth = self.create_subscription(Image, self.topicDepth, self.listener_depth, 3)
|
||||
|
||||
# Create the publisher.
|
||||
self._pub = self.create_lifecycle_publisher(Pose, self.topicSrv, 10)
|
||||
self._timer = self.create_timer(self.publishDelay, self.publish)
|
||||
|
||||
# Preload Megapose model
|
||||
self.megapose_model = ModelPreload(self.objPath,"megapose-1.0-RGB-multi-hypothesis")
|
||||
|
||||
self.get_logger().info('on_configure() is called.')
|
||||
return TransitionCallbackReturn.SUCCESS
|
||||
|
||||
def on_activate(self, state: State) -> TransitionCallbackReturn:
|
||||
# Log
|
||||
self.get_logger().info('on_activate() is called.')
|
||||
# Create the main subscriber.
|
||||
self._sub = self.create_subscription(Image, self.topicImage, self.listener_callback, 3)
|
||||
return super().on_activate(state)
|
||||
|
||||
def on_deactivate(self, state: State) -> TransitionCallbackReturn:
|
||||
# Log
|
||||
self.get_logger().info('on_deactivate() is called.')
|
||||
# Destroy the main subscriber.
|
||||
self.destroy_subscription(self._sub)
|
||||
return super().on_deactivate(state)
|
||||
|
||||
def on_cleanup(self, state: State) -> TransitionCallbackReturn:
|
||||
"""
|
||||
Cleanup the node.
|
||||
|
||||
:return: The state machine either invokes a transition to the "unconfigured" state or stays
|
||||
in "inactive" depending on the return value.
|
||||
TransitionCallbackReturn.SUCCESS transitions to "unconfigured".
|
||||
TransitionCallbackReturn.FAILURE transitions to "inactive".
|
||||
TransitionCallbackReturn.ERROR or any uncaught exceptions to "errorprocessing"
|
||||
"""
|
||||
# очистим параметры
|
||||
node_param = rclpy.parameter.Parameter("mesh_path", rclpy.Parameter.Type.STRING, "")
|
||||
all_node_param = [node_param]
|
||||
self.set_parameters(all_node_param)
|
||||
|
||||
self._is_camerainfo = False
|
||||
|
||||
self.destroy_timer(self._timer)
|
||||
self.destroy_publisher(self._pub)
|
||||
self.destroy_subscription(self._sub)
|
||||
self.destroy_subscription(self._sub_info)
|
||||
self.destroy_subscription(self._sub_depth)
|
||||
|
||||
self.get_logger().info('on_cleanup() is called.')
|
||||
return TransitionCallbackReturn.SUCCESS
|
||||
|
||||
def on_shutdown(self, state: State) -> TransitionCallbackReturn:
|
||||
"""
|
||||
Shutdown the node.
|
||||
|
||||
:return: The state machine either invokes a transition to the "finalized" state or stays
|
||||
in the current state depending on the return value.
|
||||
TransitionCallbackReturn.SUCCESS transitions to "unconfigured".
|
||||
TransitionCallbackReturn.FAILURE transitions to "inactive".
|
||||
TransitionCallbackReturn.ERROR or any uncaught exceptions to "errorprocessing"
|
||||
"""
|
||||
self.destroy_timer(self._timer)
|
||||
self.destroy_publisher(self._pub)
|
||||
self.destroy_subscription(self._sub)
|
||||
self.destroy_subscription(self._sub_info)
|
||||
self.destroy_subscription(self._sub_depth)
|
||||
|
||||
self.get_logger().info('on_shutdown() is called.')
|
||||
return TransitionCallbackReturn.SUCCESS
|
||||
|
||||
def listener_camera_info(self, data):
|
||||
"""
|
||||
CameraInfo callback function.
|
||||
"""
|
||||
if self._is_camerainfo: # повторно инфо камеры не читаем
|
||||
return
|
||||
|
||||
self._res = [data.height, data.width]
|
||||
k_ = data.k
|
||||
self._K = [
|
||||
[k_[0], k_[1], k_[2]],
|
||||
[k_[3], k_[4], k_[5]],
|
||||
[k_[6], k_[7], k_[8]]
|
||||
]
|
||||
|
||||
tPath = self.objPath / "inputs"
|
||||
#{"label": "fork", "bbox_modal": [329, 189, 430, 270]}
|
||||
output_fn = tPath / "object_data.json"
|
||||
output_json_dict = {
|
||||
"label": self.objName,
|
||||
"bbox_modal": [2,2,self._res[1]-4,self._res[0]-4]
|
||||
}
|
||||
data = []
|
||||
data.append(output_json_dict)
|
||||
output_fn.write_text(json.dumps(data))
|
||||
|
||||
#{"K": [[25.0, 0.0, 8.65], [0.0, 25.0, 6.5], [0.0, 0.0, 1.0]], "resolution": [480, 640]}
|
||||
output_fn = self.objPath / "camera_data.json"
|
||||
output_json_dict = {
|
||||
"K": self._K,
|
||||
"resolution": self._res
|
||||
}
|
||||
data = []
|
||||
data.append(output_json_dict)
|
||||
output_fn.write_text(json.dumps(output_json_dict))
|
||||
|
||||
# установим признак получения инфо камеры
|
||||
self._is_camerainfo = True
|
||||
|
||||
def listener_depth(self, data):
|
||||
"""
|
||||
Depth image callback function.
|
||||
"""
|
||||
#self.get_logger().info("Receiving depth image")
|
||||
|
||||
# Convert ROS Image message to OpenCV image
|
||||
current_frame = self.br.imgmsg_to_cv2(data)
|
||||
|
||||
# Save depth image for Megapose
|
||||
cv2.imwrite(str(self.objPath / "image_depth.png"), current_frame)
|
||||
|
||||
def load_result(self, example_dir: Path, json_name = "object_data.json"):
|
||||
f = example_dir / "outputs" / json_name
|
||||
if os.path.isfile(f):
|
||||
data = f.read_text()
|
||||
else:
|
||||
data = "No result file: '" + str(f) + "'"
|
||||
return data
|
||||
|
||||
def listener_callback(self, data):
|
||||
"""
|
||||
Image Callback function.
|
||||
"""
|
||||
if not self._is_camerainfo:
|
||||
self.get_logger().warning("No data from CameraInfo")
|
||||
return
|
||||
|
||||
# Convert ROS Image message to OpenCV image
|
||||
current_frame = self.br.imgmsg_to_cv2(data)
|
||||
|
||||
# Save image for Megapose
|
||||
cv2.imwrite(str(self.objPath / "image_rgb.png"), current_frame)
|
||||
self._image_cnt += 1
|
||||
|
||||
# 6D pose estimation
|
||||
self.get_logger().info(f"megapose: begin {self._image_cnt} {self.objPath}")
|
||||
#run_inference(self.objPath,"megapose-1.0-RGB-multi-hypothesis")
|
||||
run_inference_rbs(self.megapose_model)
|
||||
|
||||
data = self.load_result(self.objPath)
|
||||
if data[0] == "[":
|
||||
y = json.loads(data)[0]
|
||||
self._pose = y["TWO"]
|
||||
|
||||
self.get_logger().info(f"megapose: end {self._image_cnt}")
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
|
||||
executor = rclpy.executors.SingleThreadedExecutor()
|
||||
lc_node = PoseEstimator("lc_pose_estimator")
|
||||
executor.add_node(lc_node)
|
||||
try:
|
||||
executor.spin()
|
||||
except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
|
||||
lc_node.destroy_node()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
Loading…
Add table
Add a link
Reference in a new issue