(Рабочий отчет RWR-2014-06

Воробьев В.В.
Программа определения местоположения роботов
camercoord
Рабочий отчет RWR-2014-06-02.1
Москва, 2014
RWR-2014-06-02.1
1
Аннотация
Программа cameracoord предназначена для распознавания роботов на полигоне и
определения их координат, угла поворота и т.д. Имеется возможность задавать
произвольное начало координат и наклон осей Ox и Oy.
Принцип работы
Программа написана под ОС Kubuntu при помощи Qt Creator версии 5.0. SpotServer
запускается под ROS.
Распознавание роботов осуществляется с помощью цветовых маркеров, находящихся
на каждом роботе. Каждый робот имеет свой индивидуальный набор цветовых маркеров,
которые распознаются сервером пятна (spotserver). Полученный набор распознанных
точек от сервера пятна группируется программой по определенным признакам (расстояние
до других, размер пятна, цвет), благодаря чему осуществляется распознавание каждого
робота.
Cameracoord является узлом ROS и он получает данные от spotserver в виде
сообщений spot_msg и передает данные в определенном формате в топик rsdetpos_msg,
откуда можно получить данные о id робота, его принадлежности к определенной команде,
его координатах и угле поворота.
Входные и выходные данные
Входные данные
1. Сообщения spot_msg от сервера пятна (spotserver)
От сервера пятна cameracoord получает массив структур, которые определяют
найденные сервером пятна. Структура имеет вид:
 Координаты пятна.
 Цвет пятна
 Размер пятна
2. Файл конфигурации cameracoord_node.ini
Первая строка — число, задающая режим работы:
 1 — Учитываются искажения камеры и имеется возможность задать новые
координаты.
 2 — Учитываются искажения камеры. Координаты задаются стандартно (Верхний
левый угол изображения имеет координаты (0;0)).
 3 — Искажения камеры не учитываются. Имеется возможность задать новые
координаты.
 4 — Искажения камеры не учитываются. Координаты задаются стандартно.
Вторая строка — число, задающее режим идентификации направления движения
робота.
 0 — С красным задающим маркером
 1 — Направление определяется по 4-м оставшимся идентификационным маркерам.
Третья строка — число, задающее количество роботов для идентификации
RWR-2014-06-02.1
2
Далее идут строки, в количестве равном количеству роботов, задающие id каждого
робота, набор его идентификационных маркеров в количестве 4 штук (1 — левый
верхний, 2 — правый верхний, и т. д.), и команду робота.
После этого идет число, задающее опорные точки, которые необходимо пройти роботу.
Затем идут строки, в количестве равном количеству опорных точек, где на каждой
строке задаются координаты х и у для каждой опорной точки.
Выходные данные
Массив DPosArray типа DPosition
Структура DPosition:
nav_msgs/Odometry pos – координаты и угол поворота робота
int32 id – id робота
int32 command - команда робота
Описание цветовой кодировки
Рисунок 1. Описание размеров маркера
Цветовой маркер представлен в виде окружности диаметром 150 мм с
расположенными на нем пятнами разных цветов:
 Центральное пятно (Белый цвет на рис.1) имеет диаметр 41.5 мм. Цвета либо
желтый, либо синий. Задает координаты робота и принадлежность команде.
Относительно этого пятна ищутся другие пятна.
 Идентификационные пятна (Салатовый цвет на рис.1) имеют диаметр 35.2 мм.
Цвета либо зеленый, либо розовый. Цветовой набор для этих пятен для каждого
робота должен быть уникален, так как они кодируют id робота. Кодировка
начинается с левого верхнего пятна и идет по часовой стрелке. В случае если
режим идентификации направления движения робота равен 1, эти пятна участвуют
в определении направления движения робота.
RWR-2014-06-02.1
3
 Направляющее пятно (Розовый цвет на рис.1) имеет диаметр 35.2 мм. Используется
только для режима идентификации направления движения робота равного 0.
Определяет направление движения робота.
Рисунок 2. Комбинации идентификационных маркеров и соответствующий id
Пример работы с программой
Для работы с сервером пятна необходима ОС Kubuntu версии 12.04, установленная
библиотека компьютерной графики OpenCV версии 2.3.1 и система ROS с созданным
рабочим окружением. Инструкция по установке и настройке ROS можно найти в
DropBox'е.
После создания рабочей среды ROS скопируйте в папку src проект spotserver,
camercoord и каталоги spot_msg и rsdetpos_msg.
Затем:
1. Подключаем камеру.
2. Запускаем консоль и вводим команду roscore.
3. Не закрывая (!!!) предыдущую консоль открываем новую и вводим команду rosrun
spotserver spotserver _node.
4. Настраиваем сервер пятна на цвета (см. Документацию на SpotServer)
5. Предварительно выставляем в конфигурационном файле cameracoord.ini режим
работы. Для режимов 1 и 2 необходимо предваритльно разложить по углам
полигона цветовые маркеры.
6. В новой консоли запускаем команду rosrun cameracoord cameracoord_node. Если
были выбраны режимы 1 или 3 необходимо выложить 3 цветовых маркера,
задающие новую систему координат и нажать любую клавишу.
RWR-2014-06-02.1
4
Программные функции
1. void ReadConfig(char *fname); — Чтение из файла конфигурации
2. void error(const char *fmt, …) - Функция обработки ошибок
3. void client_Callback(const spot_msg::SpotArrayConstPtr& spotmsg) — чтение
сообщения из топика
4. void PreparationFun() - функция обработки сообщений
5. REALPOINT CalculationRealPoints(POINT Point, POINT *Massive, int Quadrant) —
расчет реальных координат с учетом линейных искажений камеры. (Для режимов
работы 1 и 2)
6. FLOATPOINT SearchTheIntersectPoint(POINT CPoint, POINT APoint, POINT Spoint)
— поиск точки пересечения прямых.
7. FLOATPOINT ClalculateTheError(FLOATPOINT IntPoints) — вычисление поправки
для учета линейных искажений
8. void NewBeginOfCoordiantes(REALPOINT *Rpoint) — вычисление нового начала
координат (Для режимов работы 2 и 3).
9. REALPOINT NewCoordinates(REALPOINT Rpoint) — расчет реальных координат с
учетом нового начала координат. (Для режимов работы 2 и 3).
10. int FindTheRobots(REALPOINT *Rpoint) — функция поиска роботов.
11. float FindTheDirection(REALPOINT FP, REALPOINT SP) — поиск направления
движения робота
12. float CheckAngle(REALPOINT FP, REALPOINT SP, int quadrant) — расчет угла
поворота робота
13. REALPOINT *FindTheIdOfRobot(REALPOINT *Dots, REALPOINT Center,
REALPOINT Dest) — поиск id робота.
14. void SortDots(REALPOINT *Dots, int FTP, int STP) — сортировка
идентификационных маркеров
Глобальные переменные
1. int Wflag — Режим работы
2. int DestinationFlag — Режим определения направления (По идентификационным
маркерам или по специальному маркеру)
3. int NumberOfRobots — количество роботов
4. int NumberOfPoints — количество опорных точек
5. ROBOTID *MassiveOfID — массив, хранящий id роботов и их принадлежность к
команде.
6. POINT *MassiveOfRepers — массив опорных точек.
7. ROBOT *MassiveOfRobots — массив найденных роботов.
8. int iterations — Флаг, определяющий нахождение опорных точек (Для режима
работы 1 и 2).
9. int SpotNum — количество точек, которые приходят в сообщении из топика
SpotMsg.
10. POINT MassiveOfPoints[MAXPOINTS] — массив, хранящий сообщение из топика
SpotMsg.
11. POINT MassiveOfStraight[4] -массив, хранящий координаты опорных маркеров (Для
режима работы 1 и 2).
RWR-2014-06-02.1
5
12. REALPOINT NewCoord[3] — массив, хранящий новое начало координат.
13. int CheckFlag — флаг отладки (0 если не нужна, 1 если нужна).
14. int flag, flag2 — флаги для поиска нового начала координат (Для режима работы 3).
Воробьев В.В. Программа определения местоположения роботов camercoord // Рабочий
отчет RWR-2014-06-02.1, -М.: Robofob Lab, 2014
RWR-2014-06-02.1
6