Задача №8. Работа модуля SVCAM в режиме лидара

Как было рассмотрено ранее в задаче №4 (Преобразование облака точек в формат данных лидара), данный кейс позволяет преобразовывать облако точек, полученное стереокамерой, в формат данных лидара и передавать эти данные внешнему контроллеру через DXL-порт.

В режиме лидара сообщения, отправленные модулем SVCAM, не являются стандартными DXL пакетами, а просто пользовательские UART сообщения. Поэтому, для чтения данных в формате лидара не нужны посторонних библиотек (например Dynamixel_SDK, DxlMaster и т.п.) со стороны внешнего контроллера. Итак, данные лидара можно читать не только при подключении по DXL, но и при подключении по UART с внешним контроллером.

Таким образом, данные лидара можно отправить на внешний контроллер двумя способами: по DXL-порту или по UART (UART1 или UART2). Это делает режим лидара универсальным и удобным для любых внешних контроллеров.

Для работы в режиме лидара необходимо изменить параметр “Mode” в конфигурационном файле сервиса dxlSlave.service. Его можно изменить в файле конфигурации сервиса /usr/bin/dxl/config.json.

В файле также можно изменить:

режим работы: Mode: "lidar"
порт передачи данных:

/dev/ttyS1 — используется и DXL-шиной, и как UART1 (для подключение по UART)

/dev/ttyS2 — UART2.

Для подключения через UART рекомендуется выбрать UART2 - /dev/ttyS2 (особенно если DXL-шина внешнего контроллера занята другими устройствами).

Файл конфигурации сервиса можно открыть с помощью команды:

sudo nano /usr/bin/dxl/config.json

Рис. 44. Параметры сервиса dxlSlave.service

Сохраняем файл с помощью сочетания клавиш CTRL+O, ENTER, затем закрываем его сочетанием клавиш CTRL+X. После надо перезагрузить сервис выполняя команду:

sudo systemctl restart dxlSlave.service

Алгоритм работы программы test_laser

Исходный код программы преобразования облака точек в формат данных лидара находится по пути ~/SVCAM/cases/test_laser/src/main.cpp

Основной цикл программы проходит по всем горизонтальным направлениям изображения — 320 пикселей по ширине.
Для каждого направления:

перебираются точки вдоль вертикальной линии (ось y),
выбирается ближайшая точка,
расстояние до неё записывается в массив ranges.

Таким образом формируется массив из 320 значений, где каждое — расстояние до ближайшего препятствия в данном направлении.
Угловой диапазон соответствует примерно –1.97…+1.97 рад по горизонтали.

Максимально допустимое расстояние равно 255 см.
Если значение равно 255, это может означать что реальная дистанция > 255 см, или алгоритм не смог вычислить корректное расстояние для данного направления.

Отправка данных лидара на внешний контроллер

Передача данных выполняется с помощью библиотеки DxlSlave, вызывая метод:

slave.callMethod(pixelIndex, distanceCm);

Пример из программы:

for (int i = 1; i < ranges.size() + 1; ++i)

{

slave.callMethod(i, (int)ranges[i-1]);

}

где i – индекс направления по горизонтали (от 1 до 320).

Проект test_laser собираем таким образом:

cd ~/SVCAM/cases/test_laser/

mkdir -p build && cd build

cmake ..

make

Для запуска выполняем команду:

cd ~/SVCAM/cases/test_laser/build/

./test_laser

Чтение данных формата лидара на внешнем микроконтроллере

В режиме лидара каждое отправленное модулем SVCAM сообщение представляет собой короткий пользовательский UART-пакет, который не является стандартным пакетом Dynamixel. Поэтому внешнему контроллеру достаточно уметь читать обычные последовательные данные — никаких дополнительных библиотек не требуется. Модуль SVCAM отправляет ряд сообщений по формату: svs:194:0:sv!

svs - начала сообщения

194 - индекс направления (от 1 до 320)

0 - расстояние в см (от 0 до 255)

sv! конец сообщения

Таким образом, модуль SVCAM последовательно отправляет 320 таких сообщений, по одному для каждого направления сканирования.

Рассмотрим пример программы для контроллера OpenCM 9.04 с платой STEM Board, подключённой к SVCAM через DXL. На этом контроллере DXL-порт соответствует интерфейсу Serial3, который в коде обозначим как CAM_SERIAL.

Важно отметить, что, если бы подключение выполнялось по UART или был использован другой контроллер, код полностью сохранялся бы. Следовало бы лишь правильно указать порт куда подключена стереокамера (например, Serial1, Serial2 и т.п.).

В программе определяются основные параметры:

CAM_SERIAL - порт, к которому подключён SVCAM (Serial3 при DXL).

BAUDRATE = 115200 - скорость обмена.

NUM_MEASUREMENTS = 320 - размер массива данных

distances[] - массив для хранения значений расстояний

newDataAvailable - флаг, принимающий значение true, когда массив полностью заполнен.

Обработка сообщений осуществляется в функции parseMessage(), которая:

ловит входящие строки вида svs:индекс:расстояние:sv!
выделяет индекс и значение
записывает расстояние в массив distances[]
устанавливает флаг после получения всех 320 значений

Для вычисления расстояния до объекта прямо перед камерой используется функция computeFrontAverage(). Она берёт небольшой диапазон направлений в центре массива и усредняет значения. Если обработка невозможна (например, нет валидных данных), функция возвращает –1.

Полный код программы представлен ниже.

cpp

#define CAM_SERIAL Serial3 // Dxl порт микроконтроллера OpenCM

#define BAUDRATE 115200

#define NUM_MEASUREMENTS 320 // кол-во измерений

int distances[NUM_MEASUREMENTS]; // Массив для сохранения результатов

bool newDataAvailable = false; // Прочитан ли весь массив данных лидара
(320 значений)

void parseMessage(String msg)

{

msg.trim();

if (!msg.startsWith("svs:")) return;

msg.remove(0, 4); // удалим "svs:"

int firstColon = msg.indexOf(':'); // позиция первого “:”

int secondColon = msg.indexOf(':', firstColon + 1); // позиция второго “:”

if (firstColon < 0 || secondColon < 0) return;

int index = msg.substring(0, firstColon).toInt(); // номер пикеля

int value = msg.substring(firstColon + 1, secondColon).toInt(); //само значение

distances[index - 1] = value; // Запись данных лидара в массив

if (index == NUM_MEASUREMENTS) {

newDataAvailable = true; // Все значения прочитаны

}

}

float computeFrontAverage(int *data)

{

int center = NUM_MEASUREMENTS / 2; // Индекс среднего пикселя в массиве

int window = 10; // пиксели, которые сканируем слева/справа от среднего

long sum = 0;

int count = 0;

for (int i = center - window; i < center + window + 1; ++i)

{

if (data[i] \< 255) {

sum += data[i];

count++;

}

}

if (count == 0) return -1; // Если не удалось считать расстояние, то возвращает -1

return (float)sum / count;

}

void setup()

{

CAM_SERIAL.begin(BAUDRATE);

Serial.begin(115200);

for (int i = 0; i < NUM_MEASUREMENTS; i++) distances[i] = 255;

Serial.println("Ready to read data");

}

void loop()

{

static String incomingMessage = "";

while (CAM_SERIAL.available())

{

char c = CAM_SERIAL.read();

if (c == '!') // конец сообщения

{

parseMessage(incomingMessage);

incomingMessage = "";

}

else {

incomingMessage += c;

}

}

if (newDataAvailable)

{ // Если заполнена таблица то считаем расстояние

float frontDistance = computeFrontAverage(distances);

Serial.print("Front distance (cm): ");

Serial.println(frontDistance);

newDataAvailable = false;

}

}

Подключаем камеру к OpenCM по Dxl порту и запускаем программу на микроконтроллере и камере. Для запуска кейса на микрокомпьютере камеры выполните команды:

cd ~/SVCAM/cases/test_laser/build/

./test_laser

Рис. 45. Расчёт расстояния до объекта перед камерой с визуализацией изображений с левой и правой камеры

Запуск службы для автоматического запуска алгоритмов ТЗ и отправки данных на внешний контроллер

Задача обеспечения полностью автономной работы модуля SVCAM и непрерывного функционирования робототехнической системы может решаться с помощью автоматической службы.
Автоматическая служба - это специализированная программа или процесс, который запускается автоматически при включении системы и работает в фоновом режиме без участия пользователя.

Такая служба может отвечать за запуск алгоритма и передачу данных, например информации о распознанных объектах, расстояниях, данных лидара, состоянии алгоритмов и т.д. В данной главе рассматривается принцип работы такой службы на примере запуска процесса передачи данных лидара на внешний контроллер.

Сначала создаём файл конфигурации службы. Для этого выполняем команду:
sudo nano /etc/systemd/system/lidar.service

Откроется текстовый редактор, в котором необходимо написать содержимое, показанное на рис. 47.

Рис. 47. Файл конфигурации службы lidar.service

Сохраняем файл с помощью сочетания клавиш CTRL+O, затем ENTER, после чего закрываем редактор сочетанием CTRL+X.

Этот файл описывает systemd-службу, которая автоматически запускает указанную программу после включения системы. Ниже приведено объяснение параметров службы:

Description - краткое описание службы. Здесь указано, что это сервис для работы с лидаром.

ExecStartPre=/bin/sleep 7 - перед запуском службы система сделает паузу 7 секунд. Это нужно, чтобы дождаться загрузки необходимых модулей или периферии.

ExecStart=/home/pi/SVCAM/cases/test_laser/build/test_laser - команда, которая будет выполнена при старте службы. В данном случае запускается исполняемый файл test_laser, расположенный в каталоге проекта. Нужно указать полный путь к исполняемому файлу.

WantedBy=multi-user.target — указывает, что служба должна запускаться автоматически при достижении стандартного режима многопользовательской системы (обычный режим работы без графического интерфейса).

Для запуска сервиса выполняем следующие команды:

sudo sytemctl daemon-reload

sudo systemctl enable lidar.service

sudo systemctl start lidar.service

После этого служба будет запущена и будет работать в фоновом режиме. Она также будет автоматически запускаться при включении или перезагрузке системы. Теперь внешний контроллер может непрерывно получать данные лидара, как описано в предыдущей главе.

Для проверки статуса службы используем команду:

sudo systemctl status lidar.service

Как видно на рис. 48, статус службы “active (running)”, что означает корректную работу без ошибок.

Рис. 48. Проверка статуса работы сервиса lidar.service

Если необходимо остановить службу, используем команду:

sudo systemctl stop lidar.service

При этом служба будет остановлена, но автоматически запустится при следующей перезагрузке системы.

Чтобы полностью отключить автозапуск службы, выполняем:
sudo systemctl disable lidar.service

Рис. 49. Статус службы lidar.service после отключения

Задача №8. Работа модуля SVCAM в режиме лидара ​

Задача №8. Работа модуля SVCAM в режиме лидара