Object detection: add YOLOv8 implementation; unified API with doc
This commit is contained in:
parent
4dbf722c73
commit
86a3f4170f
12 changed files with 582 additions and 7 deletions
|
|
@ -71,6 +71,9 @@ list(APPEND plugin_libs rbs_assemble_process_state)
|
|||
add_library(rbs_pose_estimation SHARED src/PoseEstimation.cpp)
|
||||
list(APPEND plugin_libs rbs_pose_estimation)
|
||||
|
||||
add_library(rbs_object_detection SHARED src/ObjectDetection.cpp)
|
||||
list(APPEND plugin_libs rbs_object_detection)
|
||||
|
||||
add_library(rbs_env_manager_starter SHARED src/EnvManager.cpp)
|
||||
list(APPEND plugin_libs rbs_env_manager_starter)
|
||||
|
||||
|
|
|
|||
17
rbs_bt_executor/bt_trees/test_objdet_cleanup.xml
Normal file
17
rbs_bt_executor/bt_trees/test_objdet_cleanup.xml
Normal file
|
|
@ -0,0 +1,17 @@
|
|||
<?xml version="1.0"?>
|
||||
<root main_tree_to_execute="ObjDetection">
|
||||
<BehaviorTree ID="ObjDetection">
|
||||
<Sequence>
|
||||
<Action ID="ObjectDetection"
|
||||
SettingPath=""
|
||||
ReqKind = "deactivate"
|
||||
server_name="/object_detection/change_state"
|
||||
server_timeout="1000"/>
|
||||
<Action ID="ObjectDetection"
|
||||
SettingPath=""
|
||||
ReqKind = "cleanup"
|
||||
server_name="/object_detection/change_state"
|
||||
server_timeout="1000"/>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
17
rbs_bt_executor/bt_trees/test_objdet_run.xml
Normal file
17
rbs_bt_executor/bt_trees/test_objdet_run.xml
Normal file
|
|
@ -0,0 +1,17 @@
|
|||
<?xml version="1.0"?>
|
||||
<root main_tree_to_execute="ObjDetection">
|
||||
<BehaviorTree ID="ObjDetection">
|
||||
<Sequence>
|
||||
<Action ID="ObjectDetection"
|
||||
SettingPath="!/home/shalenikol/0yolov8/test.json"
|
||||
ReqKind = "configure"
|
||||
server_name="/object_detection/change_state"
|
||||
server_timeout="1000"/>
|
||||
<Action ID="ObjectDetection"
|
||||
SettingPath=""
|
||||
ReqKind = "activate"
|
||||
server_name="/object_detection/change_state"
|
||||
server_timeout="1000"/>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
|
|
@ -7,6 +7,7 @@ bt_engine:
|
|||
"rbs_skill_move_joint_state",
|
||||
"rbs_add_planning_scene_object",
|
||||
"rbs_pose_estimation",
|
||||
"rbs_object_detection",
|
||||
"rbs_env_manager_starter",
|
||||
"rbs_skill_move_topose_array_bt_action_client"
|
||||
]
|
||||
|
|
|
|||
119
rbs_bt_executor/src/ObjectDetection.cpp
Normal file
119
rbs_bt_executor/src/ObjectDetection.cpp
Normal file
|
|
@ -0,0 +1,119 @@
|
|||
#include <ament_index_cpp/get_package_share_directory.hpp>
|
||||
#include <behavior_tree/BtService.hpp>
|
||||
|
||||
#include <behaviortree_cpp_v3/basic_types.h>
|
||||
#include <rclcpp/logging.hpp>
|
||||
#include <string>
|
||||
|
||||
#include "rcl_interfaces/msg/parameter.hpp"
|
||||
#include "rcl_interfaces/srv/set_parameters.hpp"
|
||||
|
||||
#include "lifecycle_msgs/msg/transition.hpp"
|
||||
#include "lifecycle_msgs/srv/change_state.hpp"
|
||||
|
||||
using ObjDetectionSrv = lifecycle_msgs::srv::ChangeState;
|
||||
class ObjectDetection : public BtService<ObjDetectionSrv> {
|
||||
public:
|
||||
ObjectDetection(const std::string &name, const BT::NodeConfiguration &config)
|
||||
: BtService<ObjDetectionSrv>(name, config) {
|
||||
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
|
||||
|
||||
_set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(
|
||||
_node, "/object_detection");
|
||||
|
||||
while (!_set_params_client->wait_for_service(1s)) {
|
||||
if (!rclcpp::ok()) {
|
||||
RCLCPP_ERROR(_node->get_logger(),
|
||||
"Interrupted while waiting for the service. Exiting.");
|
||||
break;
|
||||
}
|
||||
RCLCPP_WARN(_node->get_logger(),
|
||||
"service not available, waiting again...");
|
||||
}
|
||||
|
||||
_setting_path = getInput<std::string>("SettingPath").value();
|
||||
}
|
||||
|
||||
ObjDetectionSrv::Request::SharedPtr populate_request() {
|
||||
auto request = std::make_shared<ObjDetectionSrv::Request>();
|
||||
_req_type = getInput<std::string>("ReqKind").value();
|
||||
request->set__transition(transition_event(_req_type));
|
||||
return request;
|
||||
}
|
||||
|
||||
BT::NodeStatus
|
||||
handle_response(const ObjDetectionSrv::Response::SharedPtr response) {
|
||||
if (response->success) {
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
|
||||
static BT::PortsList providedPorts() {
|
||||
return providedBasicPorts({
|
||||
BT::InputPort<std::string>("ReqKind"),
|
||||
BT::InputPort<std::string>("SettingPath"),
|
||||
});
|
||||
}
|
||||
|
||||
private:
|
||||
uint8_t transition_id_{};
|
||||
std::string _setting_path{};
|
||||
// std::string _model_path{};
|
||||
std::string _req_type{};
|
||||
std::shared_ptr<rclcpp::AsyncParametersClient> _set_params_client;
|
||||
std::vector<rcl_interfaces::msg::Parameter> _params;
|
||||
rcl_interfaces::msg::Parameter _param;
|
||||
|
||||
lifecycle_msgs::msg::Transition
|
||||
transition_event(const std::string &req_type) {
|
||||
lifecycle_msgs::msg::Transition translation{};
|
||||
if (req_type == "configure") {
|
||||
set_str_param();
|
||||
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;
|
||||
} else if (req_type == "activate") {
|
||||
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE;
|
||||
} else if (req_type == "deactivate") {
|
||||
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE;
|
||||
} else if (req_type == "cleanup") {
|
||||
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP;
|
||||
}
|
||||
translation.set__id(transition_id_);
|
||||
return translation;
|
||||
}
|
||||
|
||||
// inline std::string build_model_path(const std::string &model_name,
|
||||
// const std::string &package_path) {
|
||||
// return package_path + "/config/" + model_name + ".ply";
|
||||
// }
|
||||
|
||||
// inline std::string build_model_path(const std::string &model_path) {
|
||||
// return model_path;
|
||||
// }
|
||||
|
||||
void set_str_param() {
|
||||
RCLCPP_INFO_STREAM(_node->get_logger(),
|
||||
"Set string parameter: <" + _setting_path + ">");
|
||||
|
||||
std::vector<rclcpp::Parameter> _parameters{
|
||||
rclcpp::Parameter("setting_path", _setting_path)};
|
||||
_set_params_client->set_parameters(_parameters);
|
||||
}
|
||||
|
||||
// void set_all_param() {
|
||||
// auto _package_name =
|
||||
// ament_index_cpp::get_package_share_directory("rbs_perception");
|
||||
// _model_path = build_model_path(_setting_path, _package_name);
|
||||
// RCLCPP_INFO_STREAM(_node->get_logger(), _model_path);
|
||||
|
||||
// std::vector<rclcpp::Parameter> _parameters{
|
||||
// rclcpp::Parameter("setting_path", _setting_path)};
|
||||
// _set_params_client->set_parameters(_parameters);
|
||||
// }
|
||||
};
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
|
||||
BT_REGISTER_NODES(factory) {
|
||||
factory.registerNodeType<ObjectDetection>("ObjectDetection");
|
||||
}
|
||||
|
|
@ -32,6 +32,7 @@ ament_python_install_package(${PROJECT_NAME})
|
|||
|
||||
# Install Python executables
|
||||
install(PROGRAMS
|
||||
scripts/detection_lifecycle.py
|
||||
scripts/detection_service.py
|
||||
scripts/grasp_marker_publish.py
|
||||
scripts/pose_estimation.py
|
||||
|
|
|
|||
|
|
@ -1,10 +1,155 @@
|
|||
# Оценка 6D положения объекта
|
||||
# Модуль восприятия окружения роботом rbs_perception
|
||||
## Навык обнаружения объектов (Object Detection). Описание API.
|
||||
|
||||
Вначале попытаемся описать полную последовательность действий по подготовке и использованию навыка обнаружения объектов. Задача обнаружения объектов сенсорами робота (в частности, RGB камерой в нашем случае) ставится в случае, например, когда необходимо в заданном окружении (сцене) определить наличие или отсутствие необходимых деталей для сборки изделия. Такие детали представлены в информационной среде в виде ассетов, хранимых в базе данных с заданными характеристиками. Поэтому входным параметром навыка обнаружения объектов является список ассетов, экземпляры которых в текущей задаче необходимо обнаруживать. Результатом использования навыка в информационной системе будет являться получение данных о заданном ассете на конкретном изображении, полученном с помощью RGB камеры.
|
||||
|
||||
Начальным этапом навыка является создание датасета, состоящего из синтетических изображений, полученных с использованием пакета [BlenderProc](https://github.com/DLR-RM/BlenderProc). Этот датасет представляет из себя набор файлов изображений и файлов меток к ним, а также файл аннотации, описывающий весь датасет в целом. Он имеет определённую структуру папок и будет использован для обучения нейросетевой модели обнаружения объектов на реальных изображениях в работе (runtime-режим). После создания такой датасет должен быть помещён в базу данных, как единый объект, с заданными характеристиками. В дальнейшем датасет может быть пополнен другими изображениями (например, фото из реального окружения робота), позволяющими произвести дообучение нейросети и улучшить качество работы навыка.
|
||||
|
||||
На втором этапе происходит обучение нейросетевой модели [YOLOv8](https://github.com/ultralytics/ultralytics). На выходе получаем файл весов модели, который также помещается в базу данных, с указанием версии этого файла и параметров обучения.
|
||||
|
||||
Теперь мы имеем всё необходимое для использования навыка обнаружения объектов (Object Detection) в реальном сценарии при управлении роботом в режиме runtime.
|
||||
|
||||
Рассмотрим наиболее общий вариант использования этого навыка в среде ROS2.
|
||||
|
||||
Первым шагом будет являться первоначальный запуск lifecycle-узла ROS2, отвечающего за работу навыка. Чтобы начать процесс обнаружения конкретной детали на изображении нужно выполнить стартовые действия по шаблону в дереве поведения, задав необходимые параметры процесса (топики получения изображения и выдачи результатов обнаружения, режим работы и другие). После решения поставленной задачи обнаружения конкретного объекта выполняются действия по шаблону приостановки работы навыка. Данные шаблоны деревьев поведения выполняются с помощью исполнителя [BehaviorTree](https://github.com/BehaviorTree/BehaviorTree.ROS2). Затем можно начать обнаружение другого объекта, вновь выполнив стартовый шаблон действий и подготовив новые параметры процесса.
|
||||
|
||||
Теперь перейдём к полному описанию данного API.
|
||||
|
||||
### Этап 1. Создание датасета
|
||||
|
||||
Для создания датасета используется модуль на Python для BlenderProc. Внешними параметрами для модуля являются:
|
||||
- сцена, подготовленная в Blender (файл *.blend)
|
||||
- каталог с файлами мешей в формате *.fbx
|
||||
|
||||
Сцена используется для случайного размещения в ней объектов из папки с мешами. Затем производится рендеринг полученной сцены с рандомизацией параметров освещения, текстур и размещением камеры.
|
||||
Имена файлов мешей должны совпадать с именами ассетов в нашей базе данных.
|
||||
|
||||
Пример запуска модуля генерации датасета:
|
||||
```bash
|
||||
blenderproc run objs2Yolov4dataset.py [scene] [obj_path] [output_dir] [vhacd_path] [--imgs 1]
|
||||
```
|
||||
- scene: путь к файлу описания сцены (*.blend)
|
||||
- obj_path: путь к каталогу с файлами описания детектируемых объектов *.obj
|
||||
- output_dir: выходной каталог
|
||||
- vhacd_path: каталог, в котором должен быть установлен или уже установлен vhacd (по умолчанию blenderproc_resources/vhacd)
|
||||
- --imgs 2: количество серий рендеринга (по 15 изображений каждая) на выходе (рекомендуется imgs=200, будет получено 3000 изображений)
|
||||
|
||||
В результате работы модуля в папке output_dir будет создана файловая структура с датасетом.
|
||||
|
||||
### Этап 2. Обучение модели Yolov8.
|
||||
|
||||
Для обучения модели используется модуль на Python. Внешним параметром для модуля является:
|
||||
- каталог с датасетом, сгенерированный на первом этапе.
|
||||
|
||||
Пример запуска модуля обучения:
|
||||
```bash
|
||||
python rbs_yolov8_train.py [dataset_path] [n_epoch]
|
||||
```
|
||||
- dataset_path: путь к каталогу с датасетом
|
||||
- --n_epoch 3: количество эпох обучения (пока рекомендуем 60-100)
|
||||
В результате работы создается файл весов нейросети с лучшими характеристиками обнаружения best.pt
|
||||
|
||||
Замечание. Пока этот модуль отсутствует. Подготовка параметров запуска обучения и само обучение проводились в тестовом (ручном) режиме.
|
||||
|
||||
### Этап 3. Использование навыка в ROS2 для обнаружения объекта на изображении (runtime).
|
||||
|
||||
1. Запуск lifecycle-узла ROS2, реализующего алгоритм навыка обнаружения объектов.
|
||||
|
||||
Запуск узла производится командой:
|
||||
```bash
|
||||
ros2 run rbs_perception pose_estimation_lifecycle.py
|
||||
```
|
||||
Запускается узел с именем `lc_detection` по умолчанию. Настроечные переменные задаются в файле конфигурации `rbs_perception/config/detection_config.json`. Пример:
|
||||
```json
|
||||
{
|
||||
"nodeName":"object_detection",
|
||||
"cameraLink":"inner_rgbd_camera",
|
||||
"topicImage":"/inner_rgbd_camera/image",
|
||||
"publishDelay": 0.5
|
||||
}
|
||||
```
|
||||
- `"nodeName"`- это поле указывает на имя узла в контексте ROS;
|
||||
- `"cameraLink"`- это поле определяет наименование RGB-камеры;
|
||||
- `"topicImage"`- это поле определяет топик, по которому публикуются изображения, полученные с RGB-камеры;
|
||||
- `"topicSrv"`- это поле определяет выходной топик (публикуется сообщение BoundBox);
|
||||
- `"publishDelay"`- это поле, указывает задержку (в секундах) перед публикацией сообщений в выходной топик.
|
||||
|
||||
2. Запуск процесса обнаружения заданного объекта.
|
||||
|
||||
Процесс стартует после выполнения операций конфигурирования lifecycle-узла, запущенного в пункте 1. Операции конфигурирования реализуются в дереве поведения `rbs_bt_executor/bt_trees/test_objdet_run.xml`, которое выполняется командой:
|
||||
```bash
|
||||
ros2 launch rbs_bt_executor rbs_executor.launch.py bt_file:=test_objdet_run.xml
|
||||
```
|
||||
В файле дерева поведения test_objdet_run.xml используется внешний параметр - `SettingPath`, для указания пути к json-файлу с настройками:
|
||||
```xml
|
||||
<?xml version="1.0"?>
|
||||
<root main_tree_to_execute="ObjDetection">
|
||||
<BehaviorTree ID="ObjDetection">
|
||||
<Sequence>
|
||||
<Action ID="ObjectDetection"
|
||||
SettingPath="!/home/shalenikol/0yolov8/test.json"
|
||||
ReqKind = "configure"
|
||||
server_name="/object_detection/change_state"
|
||||
server_timeout="1000"/>
|
||||
<Action ID="ObjectDetection"
|
||||
SettingPath=""
|
||||
ReqKind = "activate"
|
||||
server_name="/object_detection/change_state"
|
||||
server_timeout="1000"/>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
```
|
||||
Пример json-файла с настройками навыка обнаружения объекта:
|
||||
```json
|
||||
{
|
||||
"yolov8_weights_file":"/home/shalenikol/0yolov8/yolov8n.pt",
|
||||
"objName":"king",
|
||||
"mode":"image_res",
|
||||
}
|
||||
```
|
||||
- `"yolov8_weights_file"`- это путь к файлу весов нейросети;
|
||||
- `"objName"`- это наименование объекта для обнаружения;
|
||||
- `"mode"`- это режим работы навыка (по умолчанию - в топик `"topicSrv"` публикуется сообщение BoundBox, если `"image_res"` - то публикуется изображение с наименованными рамками обнаруженных объектов).
|
||||
|
||||
После этого узел начинает публиковать в выходной топик информацию об обнаружении объекта на каждом полученном с камеры изображении.
|
||||
|
||||
3. Прекращение процесса обнаружения объекта.
|
||||
|
||||
Для завершения навыка нужно выполнить действия из дерева поведения `rbs_bt_executor/bt_trees/test_objdet_cleanup.xml`:
|
||||
```xml
|
||||
<?xml version="1.0"?>
|
||||
<root main_tree_to_execute="ObjDetection">
|
||||
<BehaviorTree ID="ObjDetection">
|
||||
<Sequence>
|
||||
<Action ID="ObjectDetection"
|
||||
SettingPath=""
|
||||
ReqKind = "deactivate"
|
||||
server_name="/object_detection/change_state"
|
||||
server_timeout="1000"/>
|
||||
<Action ID="ObjectDetection"
|
||||
SettingPath=""
|
||||
ReqKind = "cleanup"
|
||||
server_name="/object_detection/change_state"
|
||||
server_timeout="1000"/>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
```
|
||||
|
||||
Команда запуска этого дерева:
|
||||
```bash
|
||||
ros2 launch rbs_bt_executor rbs_executor.launch.py bt_file:=test_objdet_cleanup.xml
|
||||
```
|
||||
После выполнения этих действий lifecycle-узел перейдёт в начальное состояние и можно, повторив пункт 2, вновь запустить процесс обнаружения уже с другим объектом.
|
||||
|
||||
## Оценка 6D положения объекта
|
||||
|
||||
Есть два варианта работы узла по оценке 6D позы объекта. Первый предполагает использование ROS-узла как сервиса, второй - как [lifecycle-узла](https://design.ros2.org/articles/node_lifecycle.html).
|
||||
Узел с управляемым жизненным циклом (lifecycle) позволяет лучше контролировать состояние системы ROS. Такой подход позволит системе убедиться, что все компоненты были созданы правильно, прежде чем любой компонент сможет начать выполнение своей задачи. Это также позволит перезапускать или заменять узел в режиме онлайн.
|
||||
Так как задача оценки позиции объекта требует использования больших вычислительных ресурсов, то реализация lifecycle-узла позволяет управлять загрузкой ресурсов при этом.
|
||||
|
||||
## Вариант 1. Сервис по оценке позы объекта
|
||||
### Вариант 1. Сервис по оценке позы объекта
|
||||
|
||||
Запуск узла производится командой:
|
||||
```bash
|
||||
|
|
@ -18,7 +163,7 @@ ros2 run rbs_perception pose_estimation.py
|
|||
|
||||
При получении запроса сервис `detect6Dpose` подписывается на Image-сообщения с камеры `/ground_true/camera_node`, которые использует для запуска алгоритма 6D оценки позиции Megapose. После получения результата от Megapose сервис публикует сообщение с позицией (Quaternion) в `pose6D_[obj]` topic.
|
||||
|
||||
## Вариант 2. Lifecycle-узел ROS для получения 6D-позы
|
||||
### Вариант 2. Lifecycle-узел ROS для получения 6D-позы
|
||||
|
||||
Запуск узла производится командой:
|
||||
```bash
|
||||
|
|
|
|||
|
|
@ -1,5 +1,6 @@
|
|||
{
|
||||
"topicName":"image_sub",
|
||||
"topicImage":"/outer_rgbd_camera/image",
|
||||
"weightsFile":"yolov4_objs2_final.weights"
|
||||
"nodeName":"object_detection",
|
||||
"cameraLink":"inner_rgbd_camera",
|
||||
"topicImage":"/inner_rgbd_camera/image",
|
||||
"publishDelay": 0.5
|
||||
}
|
||||
|
|
|
|||
|
|
@ -8,8 +8,13 @@ def generate_launch_description():
|
|||
package="rbs_perception",
|
||||
executable="pose_estimation_lifecycle.py",
|
||||
)
|
||||
object_detection = Node(
|
||||
package="rbs_perception",
|
||||
executable="detection_lifecycle.py",
|
||||
)
|
||||
|
||||
nodes_to_start = [
|
||||
pose_estimation
|
||||
pose_estimation,
|
||||
object_detection,
|
||||
]
|
||||
return LaunchDescription(declared_arguments + nodes_to_start)
|
||||
|
|
|
|||
258
rbs_perception/scripts/detection_lifecycle.py
Executable file
258
rbs_perception/scripts/detection_lifecycle.py
Executable file
|
|
@ -0,0 +1,258 @@
|
|||
#!/usr/bin/env python3
|
||||
"""
|
||||
object_detection_lifecycle_node
|
||||
ROS 2 program for Object Detection
|
||||
|
||||
@shalenikol release 0.1
|
||||
"""
|
||||
|
||||
from typing import Optional
|
||||
import os
|
||||
import json
|
||||
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
|
||||
from rbs_skill_interfaces.msg import BoundBox
|
||||
|
||||
from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images
|
||||
#import cv2 # OpenCV library
|
||||
|
||||
class Detection(Node):
|
||||
"""Our lifecycle node."""
|
||||
def _InitService(self):
|
||||
# Initialization service data
|
||||
p = os.path.join(get_package_share_directory("rbs_perception"), "config", "detection_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 == "cameraLink":
|
||||
self.camera_link = val
|
||||
elif name == "topicImage":
|
||||
self.topicImage = val
|
||||
elif name == "publishDelay":
|
||||
self.publishDelay = val
|
||||
elif name == "topicSrv":
|
||||
self.topicSrv = 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._is_image_mode = False
|
||||
self.yolov8_weights_file = ""
|
||||
self.model = None
|
||||
|
||||
self.nodeName = node_name
|
||||
self.camera_link = "outer_rgbd_camera"
|
||||
self.topicImage = "/outer_rgb_camera/image"
|
||||
self.publishDelay = 0.33
|
||||
self.topicSrv = self.nodeName + "/object_detect"
|
||||
self.topicDetectImage = self.nodeName + "/detect_image"
|
||||
self._InitService()
|
||||
|
||||
# for other nodes
|
||||
kwargs["allow_undeclared_parameters"] = True
|
||||
kwargs["automatically_declare_parameters_from_overrides"] = True
|
||||
super().__init__(self.nodeName, **kwargs)
|
||||
self.declare_parameter("setting_path", rclpy.Parameter.Type.STRING)
|
||||
|
||||
# Used to convert between ROS and OpenCV images
|
||||
self.br = CvBridge()
|
||||
|
||||
self.objName = ""
|
||||
self.image_det = []
|
||||
self.bbox_res = None
|
||||
|
||||
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"
|
||||
"""
|
||||
str_param = self.get_parameter("setting_path").get_parameter_value().string_value
|
||||
if str_param[0] == "!": # filename json
|
||||
json_file = str_param[1:]
|
||||
if not os.path.isfile(json_file):
|
||||
self.get_logger().info("not JSon '"+ json_file +"'")
|
||||
return TransitionCallbackReturn.FAILURE
|
||||
str_param = Path(json_file).read_text()
|
||||
try:
|
||||
y = json.loads(str_param)
|
||||
except json.JSONDecodeError as e:
|
||||
self.get_logger().info(f"JSon error: {e}")
|
||||
return TransitionCallbackReturn.FAILURE
|
||||
if "yolov8_weights_file" in y:
|
||||
self.yolov8_weights_file = y["yolov8_weights_file"]
|
||||
if "objName" in y:
|
||||
self.objName = y["objName"]
|
||||
if "mode" in y:
|
||||
self._is_image_mode = (y["mode"] == "image_res")
|
||||
else:
|
||||
self.yolov8_weights_file = str_param
|
||||
|
||||
if not os.path.isfile(self.yolov8_weights_file):
|
||||
self.get_logger().info(f"Parameter 'yolov8_weights_file' not valid: {self.yolov8_weights_file}")
|
||||
return TransitionCallbackReturn.FAILURE
|
||||
|
||||
# Create the publisher.
|
||||
if self._is_image_mode:
|
||||
self._pub = self.create_lifecycle_publisher(Image, self.topicDetectImage, 3)
|
||||
else:
|
||||
self._pub = self.create_lifecycle_publisher(BoundBox, self.topicSrv, 10)
|
||||
|
||||
self._timer = self.create_timer(self.publishDelay, self.publish)
|
||||
|
||||
from ultralytics import YOLO
|
||||
self.model = YOLO(self.yolov8_weights_file)
|
||||
|
||||
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("setting_path", rclpy.Parameter.Type.STRING, "")
|
||||
all_node_param = [node_param]
|
||||
self.set_parameters(all_node_param)
|
||||
|
||||
self._is_image_mode = False
|
||||
self.image_det = []
|
||||
self.bbox_res = None
|
||||
|
||||
self.destroy_timer(self._timer)
|
||||
self.destroy_publisher(self._pub)
|
||||
self.destroy_subscription(self._sub)
|
||||
|
||||
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.get_logger().info('on_shutdown() is called.')
|
||||
return TransitionCallbackReturn.SUCCESS
|
||||
|
||||
def publish(self):
|
||||
"""Publish a new message when enabled."""
|
||||
self._count += 1
|
||||
|
||||
if self._pub is not None and self._pub.is_activated:
|
||||
# Publish result
|
||||
if self._is_image_mode:
|
||||
if len(self.image_det) == 0:
|
||||
return
|
||||
# The 'cv2_to_imgmsg' method converts an OpenCV image to a ROS 2 image message
|
||||
msg = self.br.cv2_to_imgmsg(self.image_det, encoding="bgr8")
|
||||
else:
|
||||
if not self.bbox_res:
|
||||
return
|
||||
msg = BoundBox()
|
||||
msg.is_detection = False
|
||||
#from ultralytics.engine.results
|
||||
cconf = 0.0 # threshold
|
||||
bb = None
|
||||
for c in self.bbox_res.boxes:
|
||||
idx = int(c.cls)
|
||||
if self.bbox_res.names[idx] == self.objName:
|
||||
conf = float(c.conf)
|
||||
if cconf < conf:
|
||||
cconf = conf
|
||||
bb = c
|
||||
if bb:
|
||||
msg.is_detection = True
|
||||
msg.name = self.objName
|
||||
msg.x = float(bb.xywhn[0,0])
|
||||
msg.y = float(bb.xywhn[0,1])
|
||||
msg.w = float(bb.xywhn[0,2])
|
||||
msg.h = float(bb.xywhn[0,3])
|
||||
msg.conf = float(bb.conf)
|
||||
# Publish the message.
|
||||
self._pub.publish(msg)
|
||||
|
||||
def listener_callback(self, data):
|
||||
"""
|
||||
Image Callback function.
|
||||
"""
|
||||
if self.model:
|
||||
self._image_cnt += 1
|
||||
self.get_logger().info(f"detection: begin {self._image_cnt}")
|
||||
|
||||
# Convert ROS Image message to OpenCV image
|
||||
current_frame = self.br.imgmsg_to_cv2(data)
|
||||
# run inference
|
||||
results = self.model(current_frame)
|
||||
|
||||
if self._is_image_mode:
|
||||
for r in results:
|
||||
self.image_det = r.plot() # plot a BGR numpy array of predictions
|
||||
else:
|
||||
for r in results:
|
||||
self.bbox_res = r
|
||||
|
||||
self.get_logger().info(f"detection: end {self._image_cnt}")
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
|
||||
executor = rclpy.executors.SingleThreadedExecutor()
|
||||
lc_node = Detection("lc_detection")
|
||||
executor.add_node(lc_node)
|
||||
try:
|
||||
executor.spin()
|
||||
except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
|
||||
lc_node.destroy_node()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
|
@ -24,6 +24,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
|
|||
"msg/PropertyValuePair.msg"
|
||||
"msg/ActionFeedbackStatusConstants.msg"
|
||||
"msg/ActionResultStatusConstants.msg"
|
||||
"msg/BoundBox.msg"
|
||||
"srv/DetectObject.srv"
|
||||
"srv/BtInit.srv"
|
||||
"srv/AssembleState.srv"
|
||||
|
|
|
|||
7
rbs_skill_interfaces/msg/BoundBox.msg
Normal file
7
rbs_skill_interfaces/msg/BoundBox.msg
Normal file
|
|
@ -0,0 +1,7 @@
|
|||
bool is_detection
|
||||
string name
|
||||
float64 x
|
||||
float64 y
|
||||
float64 w
|
||||
float64 h
|
||||
float64 conf
|
||||
Loading…
Add table
Add a link
Reference in a new issue