Построение карты при известном местоположении

Этот пример показывает, как создать карту среды с помощью показаний датчика области значений, если положение робота известно во время чтения датчика. Этот пример также показывает, как использовать функции преобразования (такие как quat2eul) от Robotics System Toolbox™.

Этот пример создает карту из показаний датчика области значений и известных положений робота. В целях этого примера вы будете использовать основанное на MATLAB® средство моделирования, чтобы управлять роботом, наблюдать показания датчика области значений и положения робота. Можно заменить средство моделирования или на действительного робота или на моделируемого робота в средстве моделирования Gazebo, в этом случае вам будут нужны некоторые средние значения, чтобы получить истинное положение робота во время чтения датчика.

Предпосылки: Следование траектории для Робота с дифференциальным приводом, доступ к tf Дереву Преобразования в ROS, обменивается Данными с Издателями ROS и Подписчиками

Инициализируйте средство моделирования робота

Запустите ведущее устройство ROS в MATLAB.

rosinit

Initializing ROS master on http://bat6230glnxa64:33643/.
Initializing global node /matlab_global_node_62984 with NodeURI http://bat6230glnxa64:44831/

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

sim = ExampleHelperRobotSimulator('simpleMap');

setRobotPose(sim,[2 3 -pi/2]);

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

enableROSInterface(sim,true);

Увеличьте лазерное разрешение датчика в средстве моделирования, чтобы упростить создание карты.

sim.LaserSensor.NumReadings = 50;

Setup интерфейс ROS

Создайте издателей ROS и подписчиков, чтобы связаться со средством моделирования. Создайте rossubscriber для получения лазерных данных о датчике из средства моделирования.

scanSub = rossubscriber('scan');

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

[velPub, velMsg] = rospublisher('/mobile_base/commands/velocity');

Средство моделирования MATLAB публикует положение робота относительно источника карты как преобразование по теме /tf, который используется в качестве источника для наземного положения робота истины в этом примере. Средство моделирования использует map и robot_base как имена кадра в этом преобразовании.

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

tftree = rostf;

Сделайте паузу в течение секунды для объекта дерева преобразования закончить инициализацию.

pause(1)

Интерфейс ROS связывается со средством моделирования робота, и состояние моделируемого робота отображено в фигуре.

Создайте контроллер пути

Робот должен будет управлять вокруг целой карты, чтобы собрать лазерные данные о датчике и создать полную карту. Присвойте путь с waypoints, которые покрывают целую карту.

path = [2 3; 3.25 6.25; 2 11; 6 7; 11 11; 8 6; 10 5; 7 3; 11 1.5];

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

plot(path(:,1),path(:,2),'k--d');

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

controller = robotics.PurePursuit('Waypoints',path);
controller.DesiredLinearVelocity = 0.4;

Установите уровень контроллера достигать 10 Гц.

controlRate = robotics.Rate(10);

Задайте целевую точку и целевой радиус, чтобы остановить робота в конце пути.

goalRadius = 0.1;
robotCurrentLocation = path(1,:);
robotGoal = path(end,:);
distanceToGoal = norm(robotCurrentLocation - robotGoal);

Задайте пустую карту

Задайте карту с высоким разрешением с помощью robotics.OccupancyGrid, чтобы получить показания датчика. Это создает карту с 14 м X 13 м размером и разрешением 20 ячеек на метр.

map = robotics.OccupancyGrid(14,13,20);

Визуализируйте карту в окне рисунка.

figureHandle = figure('Name', 'Map');
axesHandle = axes('Parent', figureHandle);
mapHandle = show(map, 'Parent', axesHandle);
title(axesHandle, 'OccupancyGrid: Update 0');

Следующий цикл с условием продолжения создаст карту среды при управлении роботом. Следующие шаги выполняются:

Во-первых, вы получаете лазерные данные сканирования с помощью подписчика scanSub. Используйте функцию getTransform с меткой времени на сообщении scan, чтобы получить преобразование между map и кадрами robot_base во время чтения датчика.
Получите положение робота и ориентацию от преобразования. Ориентация робота является вращением Отклонения от курса вокруг оси Z робота. Можно получить вращение Отклонения от курса путем преобразования кватерниона в углы Эйлера с помощью quat2eul.
Предварительно обработайте лазерные данные сканирования. Средство моделирования возвращает области значений NaN для лазерных лучей, которые не поражают препятствия в максимальной области значений. Замените области значений NaN максимальным значением области значений.
Вставьте лазерное наблюдение сканирования с помощью insertRay метода на сетке заполнения map.
Вычислите линейные и угловые скоростные команды с помощью объекта controller управлять роботом.
(Необязательно), Чтобы визуализировать карту после каждых 50 обновлений, не прокомментируйте раздел в цикле с условием продолжения.

updateCounter = 1;
while( distanceToGoal > goalRadius )
    % Receive a new laser sensor reading.
    scanMsg = receive(scanSub);
    
    % Get robot pose at the time of sensor reading.
    pose = getTransform(tftree, 'map', 'robot_base', scanMsg.Header.Stamp, 'Timeout', 2);
    
    % Convert robot pose to 1x3 vector [x y yaw].
    position = [pose.Transform.Translation.X, pose.Transform.Translation.Y];
    orientation =  quat2eul([pose.Transform.Rotation.W, pose.Transform.Rotation.X, ...
        pose.Transform.Rotation.Y, pose.Transform.Rotation.Z], 'ZYX');
    robotPose = [position, orientation(1)];
    
    % Extract the laser scan.
    scan = lidarScan(scanMsg);
    ranges = scan.Ranges;
    ranges(isnan(ranges)) = sim.LaserSensor.MaxRange;
    modScan = lidarScan(ranges, scan.Angles);
    
    % Insert the laser range observation in the map.
    insertRay(map, robotPose, modScan, sim.LaserSensor.MaxRange);
    
    % Compute the linear and angular velocity of the robot and publish it
    % to drive the robot.
    [v, w] = controller(robotPose);
    velMsg.Linear.X = v;
    velMsg.Angular.Z = w;
    send(velPub, velMsg);
    
    % Visualize the map after every 50th update.
    %     if ~mod(updateCounter,50)
    %         mapHandle.CData = occupancyMatrix(map);
    %         title(axesHandle, ['OccupancyGrid: Update ' num2str(updateCounter)]);
    %     end
    
    % Update the counter and distance to goal.
    updateCounter = updateCounter+1;
    distanceToGoal = norm(robotPose(1:2) - robotGoal);
    
    % Wait for control rate to ensure 10 Hz rate.
    waitfor(controlRate);
end

Отобразите итоговую карту, которая включила все показания датчика.

show(map, 'Parent', axesHandle);
title(axesHandle, 'OccupancyGrid: Final Map');

Завершите работу сети ROS

Закройте ведущее устройство ROS и удалите глобальный узел.

rosshutdown

Shutting down global node /matlab_global_node_62984 with NodeURI http://bat6230glnxa64:44831/
Shutting down ROS master on http://bat6230glnxa64:33643/.

Документация