Пакет аппаратной поддержки для TurtleBot

Этот пример дает обзор работы с TurtleBot с помощью его нативного интерфейса ROS. Пакет поддержки ROS Toolbox™ для основанных на TurtleBot Роботов обеспечивает более оптимизированный интерфейс к TurtleBot. Это позволяет вам:

Получите данные о датчике и отправьте команды управления, явным образом не вызывая команды ROS

Свяжитесь прозрачно с симулированным роботом в Gazebo или с физическим TurtleBot

Чтобы установить пакет поддержки, откройтесь, Дополнения> Получают Пакеты Аппаратной поддержки на вкладке Home MATLAB® и выбирают основанных на ROS Toolbox™ Support Package for TurtleBot Роботов. В качестве альтернативы используйте roboticsAddons (Robotics System Toolbox) команда.

Соединитесь с TurtleBot

Убедитесь, что у вас есть TurtleBot, запускающийся или в симуляции через Gazebo® или на действительном оборудовании. Обратитесь к Начало работы с Gazebo и Симулированным TurtleBot или Начало работы с Действительным TurtleBot для процедуры запуска. Если вы используете оборудование, найдите, что синий мяч использует для отслеживания. Если вы используете Gazebo®, синий мяч должен быть в мире перед роботом (убедитесь, что вы используете мир Office Gazebo).

Инициализируйте ROS. Соединитесь с TurtleBot, заменив ipaddress с IP-адресом TurtleBot

ipaddress = "192.168.178.133";
rosinit(ipaddress,11311)

Initializing global node /matlab_global_node_76155 with NodeURI http://192.168.178.1:49316/

Убедитесь, что вы запустили камеру Kinect, если вы работаете с действительным оборудованием TurtleBot. Команда, чтобы запустить камеру:

roslaunch turtlebot_bringup 3dsensor.launch. 

Необходимо ввести это в терминал на TurtleBot.

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

handles.colorImgSub = exampleHelperTurtleBotEnableColorCamera;

Successfully Enabled Camera (raw image)

useHardware = exampleHelperTurtleBotIsPhysicalRobot;
if useHardware
    handles.cliffSub = rossubscriber("/mobile_base/events/cliff",...
        "BufferSize",5,"DataFormat","struct");
    handles.bumpSub = rossubscriber("/mobile_base/sensors/bumper_pointcloud",...
        "BufferSize",5,"DataFormat","struct");
    handles.soundPub = rospublisher("/mobile_base/commands/sound", "kobuki_msgs/Sound",...
        "DataFormat","struct");
    handles.velPub = rospublisher("/mobile_base/commands/velocity",...
        "DataFormat","struct");
else
    % Cliff sensor, bumper sensor and sound emitter are only present in
    % real TurtleBot hardware
    handles.cliffSub = [];
    handles.bumpSub = [];
    handles.soundPub = [];
    handles.velPub = rospublisher("/cmd_vel","DataFormat","struct");
end

Настройте синее обнаружение мяча

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

blueBallParams.blueMax = 120;  % Maximum permissible deviation from pure blue
blueBallParams.darkMin = 30;   % Minimum acceptable darkness value

Попытайтесь визуализировать мяч, чтобы убедиться, что нахождение параметров мяча может определить местоположение его. Запустите exampleHelperTurtleBotFindBlueBall функция, чтобы видеть, найден ли круг. Если так, c и m присвоенные значения. ball бинарное изображение, созданное путем применения фильтров синевы и темноты на изображение. Просмотрите ball чтобы видеть, был ли синий мяч правильно изолирован:

latestImg = rosReadImage(handles.colorImgSub.LatestMessage);
[c,~,ball] = exampleHelperTurtleBotFindBlueBall(latestImg,blueBallParams,useHardware);

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

exampleHelperTurtleBotPlotObject(latestImg,ball,c);

Если мяч не найден, попытайтесь увеличить или уменьшить blueBallParams.blueMax и blueBallParams.darkMin. Просмотрите график снова, пока мяч не будет найден. Этот метод является хорошим способом точно настроить находящий мяч алгоритм перед использованием контроллера.

В Gazebo используемые параметры не могут найти мяч, потому что пороговые значения слишком щедры. Изображение Gazebo (оставленный фигуры) включает части стены и других объектов в пробеле. Действительное изображение (правильные фигуры) выглядит очень влажным с белым. Попытайтесь изменить параметры так, чтобы они были более строгими:

blueBallParams.blueMax = 200; % Maximum permissible deviation from pure blue
blueBallParams.darkMin = 220; % Minimum acceptable darkness value
latestImg = rosReadImage(handles.colorImgSub.LatestMessage);
[c,~,ball] = exampleHelperTurtleBotFindBlueBall(latestImg,blueBallParams,useHardware);

Используйте этого помощника в качестве примера, чтобы отобразить фигуры.

exampleHelperTurtleBotPlotObject(latestImg,ball,c);

Теперь параметры слишком строги. Часть мяча даже не обнаруживается в изображении Gazebo, и вы ничего не видите в действительном изображении. Если вы настраиваете параметры далее, можно найти второй план. В Gazebo следующие параметры должны работать хорошо. С оборудованием окружающая подсветка может потребовать, чтобы вы провели больше времени, точно настраивая параметры.

  blueBallParams.blueMax = 30; % Maximum permissible deviation from pure blue
  blueBallParams.darkMin = 90; % Minimum acceptable darkness value
  latestImg = rosReadImage(handles.colorImgSub.LatestMessage);
  [c,~,ball] = exampleHelperTurtleBotFindBlueBall(latestImg,blueBallParams, useHardware);

Используйте этого помощника в качестве примера, чтобы отобразить фигуры.

 exampleHelperTurtleBotPlotObject(latestImg,ball,c);

Настройка цветовых порогов сложна когда по сравнению с настройкой их в симулированной среде как Gazebo.

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

  handles.params = blueBallParams;

Протестируйте контроллер Фиксированного Расстояния

Установите усиления контроллера для TurtleBot. TurtleBot использует ПИД-регулятор, чтобы остаться в постоянном расстоянии от мяча.

Первый набор усилений контроллера хорош для TurtleBot в Gazebo. Второй набор хорош для TurtleBot в действительном оборудовании. Настройте усиления, как вы считаете целесообразным.

Вот компактный способ присвоить значения struct.

Эффективные усиления для симуляции Gazebo:

  gains.lin = struct("pgain",1/100,"dgain",1/100,"igain",0,"maxwindup",0,"setpoint",0.65);
  gains.ang = struct("pgain",1/400,"dgain",1/500,"igain",0,"maxwindup",0,"setpoint",0.5);
 

Эффективные усиления для оборудования TurtleBot:

gains.lin = struct("pgain",1/100,"dgain",1/1000,"igain",0,"maxwindup",0,"setpoint",0.75);
gains.ang = struct("pgain",1/100,"dgain",1/3000,"igain",0,"maxwindup",0,"setpoint",0.5);

Убедитесь, что добавили gains struct к handles переменная.

handles.gains = gains;

Задайте таймер, чтобы выполнить поведение отслеживания мяча через коллбэк. Задайте функцию остановки, чтобы закрыть ROS. Включайте указатели в функцию обратного вызова для таймера:

timer2 = timer("TimerFcn",{@exampleHelperTurtleBotTrackingTimer,handles,useHardware},"Period",0.1,"ExecutionMode","fixedSpacing");
timer2.StopFcn = {@exampleHelperTurtleBotStopCallback};

Запустите таймер используя следующую команду. Вы видите, что TurtleBot начинает перемещаться во всем мире, ища мяч. Когда это найдет его в изображении Kinect, робот будет использовать контроллер, чтобы остаться в фиксированном расстоянии.

start(timer2);
pause(1);

Датчик удара не активируется в симуляции, таким образом, TurtleBot не может восстановиться, когда это врезается в стену.

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

g = ExampleHelperGazeboCommunicator();
    ballhandle = ExampleHelperGazeboSpawnedModel("unit_sphere_1",g)
    duration = 2;
    forceVector = [0 4 0];
    applyForce(ballhandle,"link",duration,forceVector)

Если вы хотите далее исследовать управление Gazebo симуляции, относятся, чтобы Добавить, Создать, и Удалить Объекты в Gazebo.

Остановите движение робота

Чтобы остановить таймер и автономное поведение, используйте следующую команду:

stop(timer2);

Если таймер очищен от рабочей области, прежде чем это будет остановлено, необходимо удалить его иначе. Чтобы остановить все таймеры (даже таймеры в фоновом режиме) выполняют следующую команду:

delete(timerfindall)

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

clear

Больше информации

ПРИМЕЧАНИЕ: Код в этом разделе не для выполнения командной строки MATLAB

В этом примере организация вспомогательных файлов позволяет вам большую гибкость в настройке и перенамерении кода. Можно изменить нахождение параметров мяча и усиления контроллера путем изменения значений в handles struct (). Этот пример включает a timer это управляет всеми аспектами алгоритма управления. Этим таймером является exampleHelperTurtleBotTrackingTimer. Этот таймер имеет Пары "имя-значение" Period и ExecutionMode это собирается определить, как часто коллбэк таймера называется. Кроме того, коллбэк остановки используется. Можно включить дополнительные функции обратного вызова, если вы хотите.

Указатели, переданные в таймер, включают params для нахождения мяча и gains для контроллера.

Структура exampleHelperTurtleBotTrackingTimer просто. Это - основной конечный автомат с некоторыми шагами инициализации. Функция инициализации определяет, который отслеживание алгоритма и который утверждает диспетчер использовать если не в утесе или восстановлении бампера. Функция:

function [objectTrack, imgControl] = initControl()
   % INITCONTROL - Initialization function to determine which control
   % and object detection algorithms to use
   objectTrack = @exampleHelperTurtleBotFindBlueBall;
   imgControl = @exampleHelperTurtleBotPointController;     

В этом примере функцией отслеживания является exampleHelperTurtleBotFindBlueBall и контроллером является exampleHelperTurtleBotPointController Можно заменить эту функцию и контроллер с любыми пользовательскими функциями, которые имеют ту же структуру аргумента ввода и вывода. Входные параметры для exampleHelperTurtleBotFindBlueBall цветное изображение и struct нахождения параметров мяча. Выходными аргументами является центр, величина и бинарное изображение разыскиваемого объекта. Входные параметры для exampleHelperTurtleBotPointController центр объекта, величина (хотя величина не используется в примере), размер изображения и усиления контроллера (struct). Выходные аргументы линейны и скорости вращения.

Основной конечный автомат используется в exampleHelperTurtleBotTrackingTimer :

switch state
        case ExampleHelperTurtleBotStates.Seek
            % Object-finding state
            [center, scale] = findObject(handles.Tbot.ImColor,handles.params);
            % Wander if no circle is found, target the circle if it exists
            if isempty(center)
                [linearV, angularV] = exampleHelperTurtleBotWanderController();
            else
                [linearV, angularV] = imageControl(center, scale, size(handles.Tbot.ImColor),handles.gains);
                setSound(handles.Tbot,2);
            end
            state = ExampleHelperTurtleBotStates.Seek;           
        case ExampleHelperTurtleBotStates.Bumper
            % Bumper contact state
        case ExampleHelperTurtleBotStates.Spin
            % Spin state            
        case ExampleHelperTurtleBotStates.Cliff
            % Cliff avoidance
end

Можно добавить или удалить случаи из конечного автомата. Если вы хотите поменять имена состояния, используйте ExampleHelperTurtleBotStates класс.

Находящий мяч алгоритм является модульным и изменяемым. Это использует два фильтра изображений (один на темноте и один на синеве) замаскированный вместе, чтобы изолировать синий мяч. Можно изменить маски, чтобы найти красный или зеленый мяч вместо этого. Если вы хотите исследовать другие формы отслеживания формы, основной рабочий процесс остается то же самое.

Синий канал изолируется (с некоторыми масштабными коэффициентами), и порог применяется, чтобы произвести маску бинарного изображения.

blueImg = img(:,:,1)/2 + img(:,:,2)/2 - img(:,:,3)/2;
blueThresh = blueImg < params.blueMax;

Эти команды изолируют инверсию синего (с различным масштабированием) и подчеркивают темноту. Порог применяется.

darkIso = -img(:,:,1)/2 - img(:,:,2)/2 + 3*img(:,:,3) - 2*rgb2gray(img);
darkThresh = darkIso > params.darkMin;

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

ball1 = blueThresh & darkThresh;

Константы и масштабные коэффициенты на изображении определяются пользователями изолировать определенный цвет. Можно экспериментировать с различными комбинациями.

Можно также найти непрерывные области в отфильтрованном изображении с помощью regionprops, который является частью Image Processing Toolbox.

s = regionprops(ball1, {"Centroid","Area","EquivDiameter"});

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

exampleHelperTurtleBotPointController функционируйте использует ExampleHelperPIDControl класс, чтобы сохранить заданную точку (в этом случае местоположение центра мяча) в точном местоположении в изображении.

Модульный принцип и гибкость примера кода позволяют вам экспериментировать со своими собственными алгоритмами и функциями.