addEgo

Добавьте тела эго в список капсул

Описание

пример

addEgo(capsuleListObj,egoStruct) добавляют одно или несколько тел эго к 2D динамическому списку капсул с заданным ID, состоянием и значениями геометрии, данными в egoStruct.

status = addEgo(capsuleListObj,egoStruct) дополнительно возвращает индикатор того, было ли каждое заданное тело эго добавлено, обновлено, или копия.

Примеры

свернуть все

Создайте путь к телу эго и обеспечьте состояния препятствия с помощью dynamicCapsuleList объект. Визуализируйте состояния всех объектов в среде в различных метках времени. Подтвердите путь тела эго путем проверки на столкновения с препятствиями в среде.

Создайте dynamicCapsuleList объект. Извлеките максимальное количество шагов, чтобы использовать в качестве количества меток времени для ваших контуров объектов.

obsList = dynamicCapsuleList;
numSteps = obsList.MaxNumSteps;

Добавьте тело эго

Задайте тело эго путем определения ID, геометрии, и утвердите вместе в структуре. Краткая геометрия имеет длину 3 м и радиус 1 м. Задайте состояние как линейный контур от x = 0m к x = 100 м.

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])

Figure contains an axes. The axes contains an object of type patch.

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

Задайте состояния для двух препятствий, которые разделяются от тела эго на 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])

Figure contains an axes. The axes contains 3 objects of type patch.

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

Изменяйте свои местоположения препятствия и размерности геометрии в зависимости от времени. Используйте ранее сгенерированную структуру, измените поля и обновите препятствия с помощью 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)")

Figure contains an axes. The axes contains 3 objects of type patch.

Программно проверяйте на столкновения при помощи 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.

Обновите путь к эго

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

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])

Figure contains an axes. The axes contains 3 objects of type patch.

Входные параметры

свернуть все

Динамический список капсул в виде dynamicCapsuleList объект.

Параметры тела эго в виде N - структура элемента или массив структур, где N является количеством добавленных тел эго. Поля каждой структуры задают ID, геометрию и состояния тела эго:

  • ID – Целое число, которое идентифицирует каждый объект. Сохраненный в EgoIDs свойство dynamicCapsuleList объект задан capsuleListObj аргумент.

  • States – Местоположение и ориентация объекта как M-by-3 матрица, где каждая строка имеет форму [x y theta], и M является количеством состояний для заданного тела эго в мировой системе координат. Список состояний принимает, что каждое состояние разделяется фиксированным временным интервалом. xy - положения исчисляются в метрах и theta исчисляется в радианах.

  • Geometry – Структура с полями Length, Radius, и FixedTransform. Эти поля задают размер основанного на капсуле объекта с помощью заданной длины для цилиндра и радиуса полукруга для заглушек. Чтобы переключить краткую геометрию от источника по умолчанию, задайте FixedTransform поле как фиксированное преобразование относительно локальной системы координат капсулы. Чтобы сохранить краткий источник по умолчанию, задайте преобразование как eye(3).

Capsule geometry image showing the position and orientation of the capsule dimensions. Positive X is the right direction in the world frame. Positive Y is up. Positive theta is a counter-clockwise rotation from the world frame. The capsule geometry has a radius for the circular ends and a length for the rectangular section in the middle.

Выходные аргументы

свернуть все

Результат добавляющих тел эго, возвращенных как N - вектор-столбец элемента из единиц, нулей и отрицательных единиц. N является количеством тел эго, заданных в egoStruct аргумент. Каждое значение указывает, добавляется ли связанное тело (1), обновленный (0), или копия (-1). При добавлении тел эго, если несколько структур с тем же ID тела найдены в массиве структур egoStruct, затем функция отмечает предыдущую запись как копия и игнорирует его.

Расширенные возможности

Генерация кода C/C++
Генерация кода C и C++ с помощью MATLAB® Coder™.

Введенный в R2020b
Для просмотра документации необходимо авторизоваться на сайте