Создание драйвера для робота в системе URBI

Создание драйвера для робота в системе URBI

Сообщение erbol » 24 мар 2013, 15:17

Создание драйвера для робота в системе URBI

Прочитал статью интересную URBI - введение в программирование UObject
http://robocraft.ru/blog/algorithm/294.html

В ней описывается процесс создания dll, которая подключается к серверу и взаимодействует с роботом.

Скачал нужные программы по ссылкам и попробовал пройти весь процесс, не получилось.

Разница в том что у автора статьи VC полный, а у меня Express. В Express не понятно как создать шаблон проекта UObject

Пришлось настраивать проект самостоятельно

Весь процесс поиска решения описывать не буду, а расскажу только про установки проекта, которые были найдены методом "тыка".

1. Создаем проект. Тип проекта - консольное приложение Win32. В поле "Имя" ввел MyAdder.
Далее следуем за "мастером". В параметрах приложения выбираем Тип приложения - Консольное, дополнительные параметры - Пустой проект. Создаем приложение - Ок

2. Слева, в окне обозревателя выбираем папку "Файлы исходного кода" и создаем новый элемент - Файл С++. Называем MyAdder и копируем туда текст класса из статьи. Работа с кодом на этом закончена

3. Теперь делаем необходимые установки для проекта. Для проекта определены две конфигурации - debug и release.

Сначала сконфигурируем debug

a. Свойства конфигурации -> C/C++ -> Общие -> Дополнительные каталоги включения : "C:\Program Files\Gostai Engine Runtime\include"

b. Свойства конфигурации -> Компоновщик -> Общие -> Дополнительные каталоги библиотек : С:\Program Files\Gostai Engine Runtime\bin; С:\Program Files\Gostai Engine Runtime\bin\gostai\engine

c. Свойства конфигурации -> Компоновщик -> Ввод -> Дополнительные зависимости : libjpeg4urbi-vc90-d.lib libport-vc90-d.lib libsched-vc90-d.lib libuobject-vc90-d.lib

d. Свойства конфигурации -> C/C++ -> Определения препроцессора : WIN32;_DEBUG;_WINDOWS;_USRDLL;MYADDER_EXPORTS;BOOST_ALL_DYN_LINK. То есть три определения там уже было, я добавил BOOST_ALL_DYN_LINK

Конфигурируем release

a. Свойства конфигурации -> C/C++ -> Общие -> Дополнительные каталоги включения : "C:\Program Files\Gostai Engine Runtime\include"

b. Свойства конфигурации -> Компоновщик -> Общие -> Дополнительные каталоги библиотек : С:\Program Files\Gostai Engine Runtime\bin; С:\Program Files\Gostai Engine Runtime\bin\gostai\engine

c. Свойства конфигурации -> Компоновщик -> Ввод -> Дополнительные зависимости : libjpeg4urbi-vc90.lib libport-vc90.lib libsched-vc90.lib libuobject-vc90.lib

d. Свойства конфигурации -> C/C++ -> Определения препроцессора : WIN32;_DEBUG;_WINDOWS;_USRDLL;MYADDER_EXPORTS;BOOST_ALL_DYN_LINK. То есть три определения там уже было, я добавил BOOST_ALL_DYN_LINK

Строим проект
erbol
 
Сообщения: 19
Зарегистрирован: 10 мар 2013, 07:06
programming: java

Re: Создание драйвера для робота в системе URBI

Сообщение noonv » 27 мар 2013, 00:06

отличная работа :)
Аватара пользователя
noonv
Администратор
 
Сообщения: 557
Зарегистрирован: 05 май 2011, 15:44
Откуда: Калининград
programming: С++

Re: Создание драйвера для робота в системе URBI

Сообщение erbol » 27 мар 2013, 07:35

noonv,

Спасибо большое.
erbol
 
Сообщения: 19
Зарегистрирован: 10 мар 2013, 07:06
programming: java

Re: Создание драйвера для робота в системе URBI

Сообщение erbol » 28 мар 2013, 11:47

Код драйвера для URBI с помощью которого можно поморгать лампочками AR.Drone

Код: Выделить всё
#include <urbi/uobject.hh>
#include <stdio.h>
#include <string.h>
#include <winsock2.h>
#include <windows.h>

#define PORT 5556
#define SERVERADDR "192.168.1.1"

class DroneLed: public urbi::UObject
{
  public:
   // The class must have a single constructor taking a string.
   DroneLed(const std::string& str);
   int init();
 
   // Our variable.
   urbi::UVar v;
 
   // Our method.
   double add (double);

   int led();
};


// the constructor defines what is available from Urbi
DroneLed::DroneLed(const std::string& s)
 : urbi::UObject(s)  // required
{
  // Bind the variable.
  UBindVar (DroneLed, v);
 
  // Bind the function.
  UBindFunction (DroneLed, add);

  UBindFunction (DroneLed, led);

  UBindFunction(DroneLed, init);
};

int DroneLed::init()
{
  return 0;
};

 
double
DroneLed::add (double rhs)
{
  return ((double)v + rhs);
}

int
DroneLed::led() {

   //Инициализируем библиотеку WinSock с промощью вызова функции WSAStartup
   
   int my_sock;

   char buff[10 * 1014];
   printf("UDP DEMO Client\nType quit to quit\n");
 
   // Шаг 1 - иницилизация библиотеки Winsocks
   if (WSAStartup(0x202, (WSADATA *)&buff[0]))
   {
      printf("WSAStartup error: %d\n", WSAGetLastError());
      return -1;
   }


   /* Создание сокета и присвоение значения дескриптору сокета для UDP пакетов
    * AF_INET   - protocol family
    * SOCK_DGRAM - Raw protocol interface */
   if (( my_sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) {
      printf("socket");
      return -1;
   }

   // Объявляем переменные dest_addr и hst

   HOSTENT *hst;
   sockaddr_in dest_addr;

   dest_addr.sin_family = AF_INET;
   dest_addr.sin_port = htons(PORT);

   // определение IP-адреса узла
   if (inet_addr(SERVERADDR) != INADDR_NONE)
      dest_addr.sin_addr.s_addr = inet_addr(SERVERADDR);
   else
   {
      if (hst = gethostbyname(SERVERADDR))
         dest_addr.sin_addr.s_addr = ((unsigned long **) hst->h_addr_list)[0][0];
      else
      {
         printf("Unknown host: %d\n", WSAGetLastError());
         closesocket(my_sock);
         WSACleanup();
         return -1;
      }
   }


   char msg[256] = "AT*CONFIG=1,\"leds:leds_anim\",\"3,1073741824,2\"\r";
   int er = strlen(msg);
   // Передача сообщений на сервер
      sendto(my_sock, msg, er, 0, \
      (sockaddr *)&dest_addr, sizeof(dest_addr));


   // Шаг последний - выход
   closesocket(my_sock);
   WSACleanup();


   return 0;
}

UStart(DroneLed);


В проекте используем установки для компоновщика

Для этого нужно зайти в Проект -> Свойства -> Компоновщик -> Ввод -> Дополнительные зависимости и ввести в поле значение - ws2_32.lib

После построения проекта нужно подключить созданную библиотеку DroneLed.dll к серверу URBI

Я делаю это так копирую файл библиотеки в папку bin по адресу C:\Program Files\Gostai Engine Runtime\2.7.5\bin

Запускаю сервер URBI с помощью bat файла urbi.bat. Файл находится в папке C:\Program Files\Gostai Engine Runtime\2.7.5

Для запуска консольных программ использую программу Far. Вхожу в папку C:\Program Files\Gostai Engine Runtime\2.7.5\bin

Затем в командую строку ввожу команду : urbi-launch.exe -r DroneLed.dll

Подключаем компьютер к сети drone. На панели задач Windows находим значок сети и щелкаем левой кнопкой мыши, в меню выбираем "подключение или отключение" , затем в списке выбираем сеть drone и подключаемся к ней

Теперь нужно запустить программу UrbiConsole.exe. Она находится в папке C:\Program Files\Gostai urbiConsole 2.1\bin. Подключаем консоль URBI к серверу и в поле команд вводим две инструкции

var DroneLed = DroneLed.new;

DroneLed.led;

В результате drone моргает своими лампочками несколько секунд
erbol
 
Сообщения: 19
Зарегистрирован: 10 мар 2013, 07:06
programming: java

Re: Создание драйвера для робота в системе URBI

Сообщение erbol » 28 мар 2013, 11:53

Переменная v и метод add в данном случае не используются, остались от исходного класса MyAdder. Их можно удалить из кода дрйвера
erbol
 
Сообщения: 19
Зарегистрирован: 10 мар 2013, 07:06
programming: java

Re: Создание драйвера для робота в системе URBI

Сообщение erbol » 30 мар 2013, 06:00

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

URBI - событийный язык. Похож по моим ощущениям на аналоговое программирование, но без ненужных абстракций. Только конструкции процессов и способы их взаимодействия. Очень положительные ощущения от способов описания ситуации c инструментами URBI
erbol
 
Сообщения: 19
Зарегистрирован: 10 мар 2013, 07:06
programming: java

Re: Создание драйвера для робота в системе URBI

Сообщение erbol » 06 апр 2013, 08:37

Получение навигационных данных с AR.Drone

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

Порт 5554 используется для инициации периодической отправки данных с сенсоров. Данные поступают на порт 5554 компьютера клиента

5556 порт робота используется для получения команд от клиента

Определенна процедура которая сообщает роботу о желании клиента получать данные сенсоров (навигационных данных). Клиент отправляет кодовое слово на порт 5556 drone.

Клиент определяет количество передаваемых данных. С помощью команды AT*CONFIG=seq, general:navdata_demo,TRUE. Команда передается на порт 5554 drone

seq это порядковый номер команды, он должен быть уникален. Существует два режима отправки данных - полный режим и демо-режим.

После инициализации отправки данных с робота клиент постоянно отсылает роботу команду AT*COMWDG=seq. Эта команда не дает роботу "заснуть". Ее надо посылать не реже 5 раз в секунду
erbol
 
Сообщения: 19
Зарегистрирован: 10 мар 2013, 07:06
programming: java

Re: Создание драйвера для робота в системе URBI

Сообщение erbol » 06 апр 2013, 14:10

Текст консольной программы для получения навигационных данных от drone

В свойствах проекта Компоновщик -> Ввод -> Дополнительные зависимости пишем имя библиотеки сокетов для Windows - ws2_32.lib

Выводит периодически заряд батареи робота в процентах

Код: Выделить всё
#include <stdio.h>
#include <string.h>
#include <winsock2.h>
#include <windows.h>

#define NAVDATA_PORT         5554
#define AT_PORT             5556
#define NAVDATA_BUFFER_SIZE     2048
#define WIFI_MYKONOS_IP     "192.168.1.1"

int seq=0;
char msg[NAVDATA_BUFFER_SIZE];

int     at_socket = -1,         //sendto
    navdata_socket = -1;   //recvfrom

struct sockaddr_in
    pc_addr,   //INADDR_ANY
    drone_at,  //send at addr
    drone_nav, //send nav addr
    from;


int main()
{

    int l,size;
char trig[] = { 0x01, 0x00, 0x00, 0x00 };
char command[256];

//Инициализируем библиотеку WinSock с промощью вызова функции WSAStartup

char buff[10 * 1014];
    //printf("UDP DEMO Client\nType quit to quit\n");

    // Шаг 1 - иницилизация библиотеки Winsocks
    if (WSAStartup(0x202, (WSADATA *)&buff[0]))
    {
        printf("WSAStartup error: %d\n", WSAGetLastError());
        return -1;
    }

if((at_socket = socket (AF_INET, SOCK_DGRAM, 0)) < 0){
       printf ("at_socket error: %s\n", strerror(errno));
       goto fail;
    };

    if((navdata_socket = socket (AF_INET, SOCK_DGRAM, 0)) < 0){
       printf ("navdata_socket: %s\n", strerror(errno));
       goto fail;
    };

    //for recvfrom
    pc_addr.sin_family = AF_INET;
    pc_addr.sin_addr.s_addr = htonl(INADDR_ANY);
    pc_addr.sin_port = htons(NAVDATA_PORT);

    //for sendto AT
    drone_at.sin_family = AF_INET;
    drone_at.sin_addr.s_addr = inet_addr(WIFI_MYKONOS_IP);
    drone_at.sin_port = htons(AT_PORT);

    //for sendto navadata init
    drone_nav.sin_family = AF_INET;
    drone_nav.sin_addr.s_addr = inet_addr(WIFI_MYKONOS_IP);
    drone_nav.sin_port = htons(NAVDATA_PORT);

    if(bind( navdata_socket, (struct sockaddr *)&pc_addr, sizeof(pc_addr)) < 0){
       printf ("bind: %s\n", strerror(errno));
       goto fail;
    };

    //set unicast mode on
    sendto(navdata_socket, trig, 4, 0, (struct sockaddr *)&drone_nav, sizeof(drone_nav));
int n=0;
while ( 1 ) {
        if(seq<2)
sprintf(command,"AT*CONFIG=%d,\"general:navdata_demo\",\"TRUE\"\r",seq);
else
sprintf(command,"AT*COMWDG=%d\r",seq);

sendto(at_socket, command, strlen(command), 0, (struct sockaddr*)&drone_at, sizeof(drone_at) );
seq++;
size=0;
//sockaddr_in client_addr;
int l = sizeof(pc_addr);

size = recvfrom ( navdata_socket, &msg[0], NAVDATA_BUFFER_SIZE, 0, (struct sockaddr *)&from, &l);
        //printf("\33[2J\nread %d data ",size);

        //for(l=24;l<28;l++)
n++;
if (n>5) {
printf("vbat_flying_percentage %i \n",0xff & msg[24]);
n=0;
}
//sprintf( "Digits 10 equal:\n   Hex: %i  Octal: %i  Decimal: %i\n", msg[24], msg[24], msg[24] );
Sleep(200);

    }

fail:
    return 0;
}
erbol
 
Сообщения: 19
Зарегистрирован: 10 мар 2013, 07:06
programming: java

Re: Создание драйвера для робота в системе URBI

Сообщение erbol » 08 апр 2013, 09:10

Код драйвера для URBI. Получаем навигационные данные от drone.

Для того чтобы получить заряд батареи в процентах создаем объект и вызываем функцию opennav()


var DroneNav1 = DroneNav1.new;
DroneNav1.opennav();

Код: Выделить всё


#include <urbi/uobject.hh>
#include <stdio.h>
#include <string.h>
#include <winsock2.h>
#include <windows.h>

#define NAVDATA_PORT         5554
#define AT_PORT             5556
#define NAVDATA_BUFFER_SIZE     2048
#define WIFI_MYKONOS_IP     "192.168.1.1"

#define NAVDATA_STATE      4
#define NAVDATA_BATTERY   24
#define NAVDATA_PITCH    28
#define NAVDATA_ROLL     32
#define NAVDATA_YAW      36
#define NAVDATA_ALTITUDE  40
#define NAVDATA_VX  44
#define NAVDATA_VY  48
#define NAVDATA_VZ  52


class DroneNav1: public urbi::UObject
{
  public:
// The class must have a single constructor taking a string.
    DroneNav1(const std::string& str);
    int init();

   // Our variable.
   urbi::UVar v;

   int opennav() ;


};


// the constructor defines what is available from Urbi
DroneNav1::DroneNav1(const std::string& s)
: urbi::UObject(s)  // required
{
  // Bind the variable.
  UBindVar (DroneLed, v);

  // Bind the function.

  UBindFunction (DroneNav1, opennav);



  UBindFunction(DroneNav1, init);
};

int DroneNav1::init()
{
  return 0;
};



int
DroneNav1::opennav() {

int seq=0;
char msg[NAVDATA_BUFFER_SIZE];

int     at_socket = -1,         //sendto
    navdata_socket = -1;   //recvfrom

struct sockaddr_in
    pc_addr,   //INADDR_ANY
    drone_at,  //send at addr
    drone_nav, //send nav addr
    from;

    int l,size;
char trig[] = { 0x01, 0x00, 0x00, 0x00 };
char command[256];

//Инициализируем библиотеку WinSock с промощью вызова функции WSAStartup

char buff[10 * 1014];
    //printf("UDP DEMO Client\nType quit to quit\n");

    // Шаг 1 - иницилизация библиотеки Winsocks
    if (WSAStartup(0x202, (WSADATA *)&buff[0]))
    {
        printf("WSAStartup error: %d\n", WSAGetLastError());
        return -1;
    }

if((at_socket = socket (AF_INET, SOCK_DGRAM, 0)) < 0){
       printf ("at_socket error: %s\n", strerror(errno));
       goto fail;
    };

    if((navdata_socket = socket (AF_INET, SOCK_DGRAM, 0)) < 0){
       printf ("navdata_socket: %s\n", strerror(errno));
       goto fail;
    };

    //for recvfrom
    pc_addr.sin_family = AF_INET;
    pc_addr.sin_addr.s_addr = htonl(INADDR_ANY);
    pc_addr.sin_port = htons(NAVDATA_PORT);

    //for sendto AT
    drone_at.sin_family = AF_INET;
    drone_at.sin_addr.s_addr = inet_addr(WIFI_MYKONOS_IP);
    drone_at.sin_port = htons(AT_PORT);

    //for sendto navadata init
    drone_nav.sin_family = AF_INET;
    drone_nav.sin_addr.s_addr = inet_addr(WIFI_MYKONOS_IP);
    drone_nav.sin_port = htons(NAVDATA_PORT);

    if(bind( navdata_socket, (struct sockaddr *)&pc_addr, sizeof(pc_addr)) < 0){
       printf ("bind: %s\n", strerror(errno));
       goto fail;
    };

    //set unicast mode on
    sendto(navdata_socket, trig, 4, 0, (struct sockaddr *)&drone_nav, sizeof(drone_nav));
int v_bat;
for(l=0;l<10;l++) {
        if(seq<2)
sprintf(command,"AT*CONFIG=%d,\"general:navdata_demo\",\"TRUE\"\r",seq);
else
sprintf(command,"AT*COMWDG=%d\r",seq);

sendto(at_socket, command, strlen(command), 0, (struct sockaddr*)&drone_at, sizeof(drone_at) );
seq++;
size=0;

int l = sizeof(pc_addr);

size = recvfrom ( navdata_socket, &msg[0], NAVDATA_BUFFER_SIZE, 0, (struct sockaddr *)&from, &l);

if (size==500){
v_bat = msg[24];
}
    }
closesocket(at_socket);
closesocket(navdata_socket);

    WSACleanup();
return v_bat;

fail:
// Шаг последний - выход
    closesocket(at_socket);
closesocket(navdata_socket);

    WSACleanup();
    return 1000;
}

UStart(DroneNav1);

erbol
 
Сообщения: 19
Зарегистрирован: 10 мар 2013, 07:06
programming: java

Re: Создание драйвера для робота в системе URBI

Сообщение erbol » 11 апр 2013, 07:51

Новый драйвер для измерения навигационных данных. Добавил прием данных углов Эйлера и высоты drone над поверхностью

Код: Выделить всё
#include <urbi/uobject.hh>
#include <stdio.h>
#include <string.h>
#include <winsock2.h>
#include <windows.h>

#define NAVDATA_PORT         5554
#define AT_PORT             5556
#define NAVDATA_BUFFER_SIZE     2048
#define WIFI_MYKONOS_IP     "192.168.1.1"

#define NAVDATA_STATE      4
#define NAVDATA_BATTERY   24
#define NAVDATA_PITCH    28
#define NAVDATA_ROLL     32
#define NAVDATA_YAW      36
#define NAVDATA_ALTITUDE  40
#define NAVDATA_VX  44
#define NAVDATA_VY  48
#define NAVDATA_VZ  52

int32_t get_int(int offset);
char msg[NAVDATA_BUFFER_SIZE];



class DroneNav1: public urbi::UObject
{
  public:
// The class must have a single constructor taking a string.
    DroneNav1(const std::string& str);
    int init();

   // Our variable.
   urbi::UVar drone_battery;
   urbi::UVar drone_altitude;
   urbi::UVar drone_pitch;
   urbi::UVar drone_roll;
   urbi::UVar drone_yaw;

   int opennav() ;


};


// the constructor defines what is available from Urbi
DroneNav1::DroneNav1(const std::string& s)
 : urbi::UObject(s)  // required
{
  // Bind the variable.
  UBindVar (DroneNav1, drone_battery);
  UBindVar (DroneNav1, drone_altitude);
  UBindVar (DroneNav1, drone_pitch);
  UBindVar (DroneNav1, drone_roll);
  UBindVar (DroneNav1, drone_yaw);


  // Bind the function.

  UBindFunction (DroneNav1, opennav);

  UBindFunction(DroneNav1, init);
};

int DroneNav1::init()
{
  return 0;
};



int
DroneNav1::opennav() {

int seq=0;
int32_t dr_battery, dr_altitude, dr_pitch, dr_yaw, dr_roll, dr_vx, dr_vy, dr_vz ;


int     at_socket = -1,         //sendto
    navdata_socket = -1;   //recvfrom

struct sockaddr_in
    pc_addr,   //INADDR_ANY
    drone_at,  //send at addr
    drone_nav, //send nav addr
    from;

    int l,size;
char trig[] = { 0x01, 0x00, 0x00, 0x00 };
char command[256];

//Инициализируем библиотеку WinSock с промощью вызова функции WSAStartup

char buff[10 * 1014];
    //printf("UDP DEMO Client\nType quit to quit\n");

    // Шаг 1 - иницилизация библиотеки Winsocks
    if (WSAStartup(0x202, (WSADATA *)&buff[0]))
    {
        printf("WSAStartup error: %d\n", WSAGetLastError());
        return -1;
    }

if((at_socket = socket (AF_INET, SOCK_DGRAM, 0)) < 0){
       printf ("at_socket error: %s\n", strerror(errno));
       goto fail;
    };

    if((navdata_socket = socket (AF_INET, SOCK_DGRAM, 0)) < 0){
       printf ("navdata_socket: %s\n", strerror(errno));
       goto fail;
    };

    //for recvfrom
    pc_addr.sin_family = AF_INET;
    pc_addr.sin_addr.s_addr = htonl(INADDR_ANY);
    pc_addr.sin_port = htons(NAVDATA_PORT);

    //for sendto AT
    drone_at.sin_family = AF_INET;
    drone_at.sin_addr.s_addr = inet_addr(WIFI_MYKONOS_IP);
    drone_at.sin_port = htons(AT_PORT);

    //for sendto navadata init
    drone_nav.sin_family = AF_INET;
    drone_nav.sin_addr.s_addr = inet_addr(WIFI_MYKONOS_IP);
    drone_nav.sin_port = htons(NAVDATA_PORT);

    if(bind( navdata_socket, (struct sockaddr *)&pc_addr, sizeof(pc_addr)) < 0){
       printf ("bind: %s\n", strerror(errno));
       goto fail;
    };

    //set unicast mode on
    sendto(navdata_socket, trig, 4, 0, (struct sockaddr *)&drone_nav, sizeof(drone_nav));
int v_bat;
while(1) {
        if(seq<2)
sprintf(command,"AT*CONFIG=%d,\"general:navdata_demo\",\"TRUE\"\r",seq);
else
sprintf(command,"AT*COMWDG=%d\r",seq);
   
sendto(at_socket, command, strlen(command), 0, (struct sockaddr*)&drone_at, sizeof(drone_at) );
seq++;
size=0;

int l = sizeof(pc_addr);

size = recvfrom ( navdata_socket, &msg[0], NAVDATA_BUFFER_SIZE, 0, (struct sockaddr *)&from, &l);

if (size==500){
//v_bat = msg[24];
drone_battery = get_int(NAVDATA_BATTERY);

dr_altitude = get_int(NAVDATA_ALTITUDE);
drone_altitude = *(float *)&dr_altitude/1000;

dr_pitch = get_int(NAVDATA_PITCH);
drone_pitch = *(float *)&dr_pitch/1000;

dr_roll = get_int(NAVDATA_ROLL);
drone_roll = *(float *)&dr_roll/1000;

dr_yaw = get_int(NAVDATA_YAW);
drone_yaw = *(float *)&dr_yaw/1000;


//printf("dr_battery %i \n",drone_battery);
//printf("dr_altitude %i \n",drone_altitude);

//printf("dr_pitch %f \n",drone_pitch);
//printf("dr_roll %f \n",drone_roll);
//printf("dr_yaw %f \n",drone_yaw);



break;
}
    }
closesocket(at_socket);
closesocket(navdata_socket);

    WSACleanup();
return 0;

fail:
// Шаг последний - выход
    closesocket(at_socket);
closesocket(navdata_socket);

    WSACleanup();
    return 1000;
}

    int32_t get_int(int offset) {
        int32_t tmp = 0, n = 0;
        for (int i=3; i>=0; i--) {   
            n <<= 8;
            tmp = msg[offset + i] & 0xFF;   
            n |= tmp;   
        }
        return n;   
    }

UStart(DroneNav1);


 
erbol
 
Сообщения: 19
Зарегистрирован: 10 мар 2013, 07:06
programming: java

Re: Создание драйвера для робота в системе URBI

Сообщение erbol » 12 апр 2013, 11:04

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

Решил вернуться к идее метода Хафа и обсудить ее

Вот математический образ, который на мой взгляд помогает понять суть этого метода

Пусть имеется n элементов, каждый из них обладает m свойствами. Каждое свойство характеризует принадлежность элемента определенному множеству. То есть каждый элемент входит в m разных множеств.

В ситуации когда для каждого n элемента существует только одно общее с остальными элементами множество, можно сказать, что число элементов в этом множестве будет равно n, а число элементов в остальных множествах будет равно единице.

Если существует возможность пользуясь свойствами элементов определить в какие множества они входят, то мы получим список множеств

A(1), B(1), C(1), .... K(n), L(1), ...

и так далее. В скобках указанно количество элементов в множестве.

Все множества кроме K будут содержать по одному элементу, множество K будет содержать n элементов.

Суть метода Хафа состоит в данном случае в определении всех множеств в которые входят элементы и подсчете количества элементов из которых они состоят.

И далее находим множество у которого число элементов равно n (в общем случае - множество имеющее максимальное число элементов)

Массив таких множеств называется аккумуляторной матрицей. Размерность массива зависит от количества величин описывающих свойство вхождения элемента n в то или иное множество.

В случае если мы ищем прямые линии на рисунке то свойство вхождения в множество описывается двумя целыми значениями. То есть массив можно представить как двухмерную матрицу, элементами которой являются множества, а значение элемента этой двухмерной матрицы равно количеству элементов n в этом множестве

Эту матрицу можно представить в виде рисунка размером a*b, где а - ширина рисунка в пикселах (число столбцов в аккумуляторной матрице), b - высота рисунка в пикселах (число строк в матрице)

Для того чтобы отобразить матрицу в рисунок с "градациями серого цвета" надо посмотреть как кодируется информация в графическом файле. Содержимым файла является интенсивность свечения пикселов, которая задается целым числом от 0 до 255.

0 - это черный цвет пиксела
255 - белый цвет пиксела

В общем случае значение элемента аккумуляторной матрицы может быть больше чем 255. Можно нормировать значения элементов матрицы для того чтобы значение максимального элемента не превышало число 255

Находим отношение M_max/255= prop

M_max - элемент матрицы имеющий наибольшее значение

И делим значения всех элементов матрицы на число prop. Дробная часть у частного отбрасывается

После того как значения элементов матрицы таким образом откалиброваны надо подумать о том как будет отображаться информация о значениях на рисунке.

Интересует ситуация когда элемент с наибольшим значением имеет более черный цвет - "максимальная видимость этого элемента на рисунке"

Поэтому выполняем инверсию значений элементов матрицы

M = 255 - M

М - элемент матрицы
erbol
 
Сообщения: 19
Зарегистрирован: 10 мар 2013, 07:06
programming: java

Re: Создание драйвера для робота в системе URBI

Сообщение erbol » 15 апр 2013, 14:24

Для того чтобы URBI мог анализировать информацию с видеокамеры drone, надо научиться получать видеопоток, декодировать его и обрабатывать кадры потока

В решении этой задачи сильно помог этот проект

https://github.com/puku0x/cvdrone

Автор проекта puku0x

Обсуждение

https://projects.ardrone.org/boards/1/topics/show/4957

Установка

https://github.com/puku0x/cvdrone/wiki/How-to-build

Определился состав ПО. Это библиотеки

ffmpeg - работа с видео
opencv - обработка изображений
pthread - работа с потоками

Так же из проекта puku0x, можно узнать функции этих библиотек которые используются для решения задачи
erbol
 
Сообщения: 19
Зарегистрирован: 10 мар 2013, 07:06
programming: java

Re: Создание драйвера для робота в системе URBI

Сообщение erbol » 16 апр 2013, 12:55



Вот , парень показывает drone красный шарик и перемещает его в разные стороны . Drone следит за шариком и движется вслед за ним.

Потом на экране видим текст программы на urbiscript

Попробуем выполнить то же самое , пока без URBI

Первое , надо научится находить шарик на экране. У меня это будет теннисный шарик. Спасибо, тенниис !
erbol
 
Сообщения: 19
Зарегистрирован: 10 мар 2013, 07:06
programming: java

Re: Создание драйвера для робота в системе URBI

Сообщение erbol » 16 апр 2013, 12:56

Обнаружение шарика на картинке с помощью пакета Opencv

Чем точнее определяем критерии поиска , тем меньше посторонних объектов обнаруживаем в результате поиска

Функция cvHoughCircles, которую используем для поиска шарика

CVAPI(CvSeq*) cvHoughCircles( CvArr* image, void* circle_storage,
int method, double dp, double min_dist,
double param1 CV_DEFAULT(100),
double param2 CV_DEFAULT(100),
int min_radius CV_DEFAULT(0),
int max_radius CV_DEFAULT(0));

​имеет девять параметров

dp — разрешение сумматора, используемое для детектирования центров кругов.

Этот параметр задает точность определения радиуса окружности.

Допустим задан центр окружности, точка x0, y0. Задан радиус окружности , пусть он равен величине a.

Если dp равен например 5, то все контрастные точки, находящиеся от точки x0, y0 на расстоянии a, плюс/минус 2,5 пиксела, подходят для определения этой окружности.

Для поиска круга нужно составить представление о смысле параметров param1, param2

Значения интенсивности пиксела в картинке с градациями серого цвета хранится в виде числа. Максимальное значение интенсивности - 255. На границе предметов интенсивность имеет разрыв, то есть резко отличается для двух соседних пикселов. Чем больше разрыв значений , тем отчетливее видна граница предмета на рисунке

Чтобы различать малозаметные контуры предметов, например светло-зеленое на темно-зеленом фоне надо учитывать, что в данном случае разрыв значений интенсивности у соседних пикселов в области границы предмета будет маленьким

Отсюда надо исходить устанавливая значение для величин param1, param2
erbol
 
Сообщения: 19
Зарегистрирован: 10 мар 2013, 07:06
programming: java

Re: Создание драйвера для робота в системе URBI

Сообщение erbol » 21 апр 2013, 16:12

Получаем видео с drone, на нем виден красный теннисный шар.

Задача: Обнаружить шарик на видеокартинке, найти координаты центра и его радиус. Обвести шарик по окружности зеленой линией и результат показать в окне с видео

Решение: Ищем шарик по цвету и по форме контура. Для поиска по цвету видеокартинка разбивается на три матрицы H, S, V.

Для каждой из них подбираем фильтр из двух значений - верхнего и нижнего. Далее путем суммирования логического получаем величину HSV

Для поиска по форме используем функцию cvHoughCircles

Получился такой код

Код: Выделить всё

#include "ardrone/ardrone.h"

#define KEY_DOWN(key) (GetAsyncKeyState(key) & 0x8000)
#define KEY_PUSH(key) (GetAsyncKeyState(key) & 0x0001)

// --------------------------------------------------------------------------
// main(Number of arguments, Argument values)
// Description  : This is the entry point of the program.
// Return value : SUCCESS:0  ERROR:-1
// --------------------------------------------------------------------------

// для хранения каналов HSV
IplImage* hsv = 0;
IplImage* h_plane = 0;
IplImage* s_plane = 0;
IplImage* v_plane = 0;
// для хранения каналов HSV после преобразования
IplImage* h_range = 0;
IplImage* s_range = 0;
IplImage* v_range = 0;
// для хранения суммарной картинки
IplImage* hsv_and = 0;

int Hmin = 0;
int Hmax = 256;

int Smin = 0;
int Smax = 256;

int Vmin = 0;
int Vmax = 256;

int HSVmax = 256;

//
// функции-обработчики ползунков
//
void myTrackbarHmin(int pos) {
        Hmin = pos;
        cvInRangeS(h_plane, cvScalar(Hmin), cvScalar(Hmax), h_range);
}

void myTrackbarHmax(int pos) {
        Hmax = pos;
        cvInRangeS(h_plane, cvScalar(Hmin), cvScalar(Hmax), h_range);
}

void myTrackbarSmin(int pos) {
        Smin = pos;
        cvInRangeS(s_plane, cvScalar(Smin), cvScalar(Smax), s_range);
}

void myTrackbarSmax(int pos) {
        Smax = pos;
        cvInRangeS(s_plane, cvScalar(Smin), cvScalar(Smax), s_range);
}

void myTrackbarVmin(int pos) {
        Vmin = pos;
        cvInRangeS(v_plane, cvScalar(Vmin), cvScalar(Vmax), v_range);
}

void myTrackbarVmax(int pos) {
        Vmax = pos;
        cvInRangeS(v_plane, cvScalar(Vmin), cvScalar(Vmax), v_range);
}



int main(int argc, char **argv)
{
    // AR.Drone class
    ARDrone ardrone;

    // Initialize
    if (!ardrone.open()) {
        printf("Failed to initialize.\n");
        return -1;
    }

    // Get a image
    IplImage* image = ardrone.getImage();

        // создаём картинки
        hsv = cvCreateImage( cvGetSize(image), IPL_DEPTH_8U, 3 );
        h_plane = cvCreateImage( cvGetSize(image), IPL_DEPTH_8U, 1 );
        s_plane = cvCreateImage( cvGetSize(image), IPL_DEPTH_8U, 1 );
        v_plane = cvCreateImage( cvGetSize(image), IPL_DEPTH_8U, 1 );
        h_range = cvCreateImage( cvGetSize(image), IPL_DEPTH_8U, 1 );
        s_range = cvCreateImage( cvGetSize(image), IPL_DEPTH_8U, 1 );
        v_range = cvCreateImage( cvGetSize(image), IPL_DEPTH_8U, 1 );
        hsv_and = cvCreateImage( cvGetSize(image), IPL_DEPTH_8U, 1 );



        // окна для отображения картинки
        cvNamedWindow("H range",CV_WINDOW_AUTOSIZE);
        cvNamedWindow("S range",CV_WINDOW_AUTOSIZE);
        cvNamedWindow("V range",CV_WINDOW_AUTOSIZE);
        cvNamedWindow("hsv and",CV_WINDOW_AUTOSIZE);

        // Создаем ползунки
cvCreateTrackbar("Hmin", "H range", &Hmin, HSVmax, myTrackbarHmin);
        cvCreateTrackbar("Hmax", "H range", &Hmax, HSVmax, myTrackbarHmax);
        cvCreateTrackbar("Smin", "S range", &Smin, HSVmax, myTrackbarSmin);
        cvCreateTrackbar("Smax", "S range", &Smax, HSVmax, myTrackbarSmax);
        cvCreateTrackbar("Vmin", "V range", &Vmin, HSVmax, myTrackbarVmin);
        cvCreateTrackbar("Vmax", "V range", &Vmax, HSVmax, myTrackbarVmax);

    // Images
    IplImage *gray   = cvCreateImage(cvGetSize(image), image->depth, 1);
    IplImage *smooth = cvCreateImage(cvGetSize(image), image->depth, 1);
    IplImage *canny  = cvCreateImage(cvGetSize(image), image->depth, 1);

    // Canny thresholds
    int th1 = 50, th2 = 100;
    cvNamedWindow("canny");
    cvCreateTrackbar("th1", "canny", &th1, 255);
    cvCreateTrackbar("th2", "canny", &th2, 255);



    // Main loop
    while (!GetAsyncKeyState(VK_ESCAPE)) {
        // Update
        if (!ardrone.update()) break;

        // Get an image
        image = ardrone.getImage();



// Выделение шарика по цвету
//  конвертируем в HSV
cvCvtColor( image, hsv, CV_BGR2HSV );
// разбиваем на отдельные каналы
        cvSplit( hsv, h_plane, s_plane, v_plane, 0 );

cvInRangeS(v_plane, cvScalar(Vmin), cvScalar(Vmax), v_range);
cvInRangeS(s_plane, cvScalar(Smin), cvScalar(Smax), s_range);
cvInRangeS(h_plane, cvScalar(Hmin), cvScalar(Hmax), h_range);


        cvShowImage( "H range", h_range );
        cvShowImage( "S range", s_range );
        cvShowImage( "V range", v_range );

        // складываем
        cvAnd(h_range, s_range, hsv_and);
        cvAnd(hsv_and, v_range, hsv_and);

        cvShowImage( "hsv and", hsv_and );

        // Detect edges
        cvCanny(hsv_and, canny, th1, th2, 3);


        // Detect circles
        CvMemStorage *storage = cvCreateMemStorage(0);
        CvSeq *circles = cvHoughCircles(
canny,
storage,
CV_HOUGH_GRADIENT,
                5,
                image->width,
125, 50, 10, 30
                );

        // Draw circles
        for (int i = 0; i < circles->total; i++) {
            float *p = (float*) cvGetSeqElem(circles, i);
            cvCircle(image, cvPoint(cvRound(p[0]), cvRound(p[1])), cvRound(p[2]), CV_RGB(0,255,0), 3, 8, 0);
        }

        // Release memory
        cvReleaseMemStorage(&storage);

        // Display the image
        cvShowImage("camera", image);
        cvShowImage("canny", canny);

        cvWaitKey(1);
    }

    // Release memories
    cvReleaseImage(&gray);
    cvReleaseImage(&image);
    cvReleaseImage(&canny);
    cvReleaseImage(&smooth);
   
    cvReleaseImage(&hsv);
    cvReleaseImage(&h_plane);
    cvReleaseImage(&s_plane);
    cvReleaseImage(&v_plane);
    cvReleaseImage(&h_range);
    cvReleaseImage(&s_range);
    cvReleaseImage(&v_range);
    cvReleaseImage(&hsv_and);
    // удаляем окна
    cvDestroyAllWindows();

    // See you
    ardrone.close();

    return 0;
}


В проекте https://github.com/puku0x/cvdrone заменяем содержимое файла main.cpp на наш код

Для красного шарика у меня получились такие значения для фильтров

H - 150, 185

V - 150, 256

S - 70, 160
erbol
 
Сообщения: 19
Зарегистрирован: 10 мар 2013, 07:06
programming: java

След.

Вернуться в Программирование

Кто сейчас на конференции

Сейчас этот форум просматривают: нет зарегистрированных пользователей и гости: 1

cron
© 2009-2019 |  О проекте  |  Политика Конфиденциальности  |