Добавить тело Ego

Задайте тело ego путем определения идентификатора, геометрии и состояния вместе в структуре. Геометрия капсулы имеет длину 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

Задайте новый путь для тела 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])