Добавить Ego Body

Определите эго-тело, указав идентификатор, геометрию и состояние вместе в структуре. Геометрия капсулы имеет длину 3 м и радиус 1 м. Укажите состояние как линейный путь от x = 0m до x = 100m.

egoID1 = 1;
geom = struct("Length",3,"Radius",1,"FixedTransform",eye(3));
states = linspace(0,1,obsList.MaxNumSteps)'.*[100 0 0];

egoCapsule1 = struct('ID',egoID1,'States',states,'Geometry',geom);
addEgo(obsList,egoCapsule1);

show(obsList,"TimeStep",[1:numSteps]);
ylim([-20 20])

Добавить препятствия

Укажите состояния для двух препятствий, которые отделены от тела эго на 5 м в противоположных направлениях по оси y. Предположим, что препятствия имеют одинаковую геометрию geom как тело эго.

obsState1 = states + [0 5 0];
obsState2 = states + [0 -5 0];

obsCapsule1 = struct('ID',1,'States',obsState1,'Geometry',geom);
obsCapsule2 = struct('ID',2,'States',obsState2,'Geometry',geom);

addObstacle(obsList,obsCapsule1);
addObstacle(obsList,obsCapsule2);

show(obsList,"TimeStep",[1:numSteps]);
ylim([-20 20])

Обновить препятствия

Изменение местоположения препятствий и размеров геометрии с течением времени. Используйте ранее созданную структуру, измените поля и обновите препятствия, используя updateObstacleGeometry и updateObstaclePose функции объекта. Уменьшает радиус первого препятствия до 0,5 м и изменяет траекторию, чтобы переместить его в сторону тела эго.

obsCapsule1.Geometry.Radius = 0.5;

obsCapsule1.States = ...
    [linspace(0,100,numSteps)' ... % x
     linspace(5,-4,numSteps)' ... % y 
     zeros(numSteps,1)]; % theta

updateObstacleGeometry(obsList,1,obsCapsule1);
updateObstaclePose(obsList,1,obsCapsule1);

Проверка наличия коллизий

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

show(obsList,"TimeStep",[1:numSteps],"ShowCollisions",1);
ylim([-20 20])
xlabel("X (m)")
ylabel("Y (m)")

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

collisions = checkCollision(obsList)'

collisions = 1x31 logical array

   0   0   0   0   0   0   0   0   0   0   0   0   1   1   1   1   1   1   1   1   1   1   0   0   0   0   0   0   0   0   0

Чтобы проверить пути с большим количеством шагов, используйте any функция на векторе значений коллизий.

if any(collisions)
    disp("Collision detected.")
end

Collision detected.

Обновить путь Ego

Укажите новый путь для тела эго. Снова визуализируйте контуры, отображая конфликты.

egoCapsule1.States = ...
    [linspace(0,100,numSteps)' ... % x
    3*sin(linspace(0,2*pi,numSteps))' ... % y
    zeros(numSteps,1)]; % theta

updateEgoPose(obsList,1,egoCapsule1);

show(obsList,"TimeStep",[1:numSteps],"ShowCollisions",1);
ylim([-20 20])