Автоматизированный камердинер парковки с ROS в MATLAB

Этот пример использует:

Скрипт Open Live Script

В этом примере показано, как распределить Автоматизированного Камердинера Парковки (Automated Driving Toolbox) приложение среди различных узлов в сети ROS. В зависимости от вашей системы этот пример обеспечивается для ROS и ROS 2 сети с помощью или MATLAB® или Simulink®. Пример, показанный здесь, использует ROS и MATLAB. Для других примеров см.:

Обзор

Этим примером является расширение Автоматизированного Камердинера Парковки (Automated Driving Toolbox) пример в Automated Driving Toolbox™. Типичное autonmous приложение имеет следующие компоненты.

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

Это приложение демонстрирует типичное разделение различных функций в узлы ROS. Следующее изображение показывает, как вышеупомянутый пример разделен в различные узлы. Каждый узел: Планирование, Управление и Транспортное средство являются узлом ROS, реализующим функциональности, показанные как ниже. Соединения между узлами показывают темы, используемые на каждом соединении узлов.

Настройка

Во-первых, загрузите план маршрута и данный costmap, используемый планировщиком поведения и путем анализатор. Планировщик поведения, Планировщик Пути, Путь, Анализатор, Ответвление и Контроллеры Lognitudinal реализованы классами помощника, которые являются настройкой с этим вызовом функции помощника в качестве примера.

exampleHelperROSValetSetupGlobals;

Инициализированные глобальные переменные организованы как поля в глобальной структуре, valet.

disp(valet)
            mapLayers: [1×1 struct]
              costmap: [1×1 vehicleCostmap]
          vehicleDims: [1×1 vehicleDimensions]
     maxSteeringAngle: 35
                 data: [1×1 struct]
            routePlan: [4×3 table]
          currentPose: [4 12 0]
           vehicleSim: [1×1 ExampleHelperROSValetVehicleSimulator]
    behavioralPlanner: [1×1 ExampleHelperROSValetBehavioralPlanner]
        motionPlanner: [1×1 pathPlannerRRT]
             goalPose: [56 11 0]
              refPath: [1×1 driving.Path]
      transitionPoses: [14×3 double]
           directions: [522×1 double]
           currentVel: 0
     approxSeparation: 0.1000
       numSmoothPoses: 522
             maxSpeed: 5
           startSpeed: 0
             endSpeed: 0
             refPoses: [522×3 double]
           cumLengths: [522×1 double]
           curvatures: [522×1 double]
        refVelocities: [522×1 double]
           sampleTime: 0.1000
        lonController: [1×1 ExampleHelperROSValetLongitudinalController]
          controlRate: [1×1 ExampleHelperROSValetFixedRate]
         pathAnalyzer: [1×1 ExampleHelperROSValetPathAnalyzer]
             parkPose: [36 44 90]

Инициализируйте сеть ROS.

rosinit;
Launching ROS Core...
..................................................................................Done in 5.3625 seconds.


Initializing ROS master on http://192.168.0.10:60903.
Initializing global node /matlab_global_node_49911 with NodeURI http://sbd508773glnxa64:42335/

masterHost = 'localhost';

Функции в приложении распределяются среди узлов ROS. Этот пример использует три узла ROS: planningNode, controlNode, и vehicleNode.

Планирование

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

Этот узел публикует эти темы:

  • /smoothpath

  • /velprofile

  • /directions

  • /speed

  • /nextgoal

Узел подписывается на эти темы:

  • /currentvel

  • /currentpose

  • /desiredvel

  • /reachgoal

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

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

planningNode = ros.Node('planning', masterHost);

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

planning.PathPub = ros.Publisher(planningNode, '/smoothpath', 'std_msgs/Float64MultiArray');
planning.VelPub = ros.Publisher(planningNode, '/velprofile', 'std_msgs/Float64MultiArray');
planning.DirPub = ros.Publisher(planningNode, '/directions', 'std_msgs/Float64MultiArray');
planning.SpeedPub = ros.Publisher(planningNode,'/speed','std_msgs/Float64MultiArray');
planning.NxtPub = ros.Publisher(planningNode, '/nextgoal', 'geometry_msgs/Pose2D');

Создайте подписчиков для планировщика, planningNode.

planning.CurVelSub = ros.Subscriber(planningNode, '/currentvel', 'std_msgs/Float64');
planning.CurPoseSub = ros.Subscriber(planningNode, '/currentpose', 'geometry_msgs/Pose2D');
planning.DesrVelSub = ros.Subscriber(planningNode, '/desiredvel', 'std_msgs/Float64');

Создайте подписчика, GoalReachSub, слушать /reachgoal тема планирования узла и задает действие коллбэка.

GoalReachSub = ros.Subscriber(planningNode, '/reachgoal', 'std_msgs/Bool');
GoalReachSub.NewMessageFcn = @(~,msg)exampleHelperROSValetPlannerCallback(msg, planning, valet);

Управление

Узел Управления ответственен за продольные и боковые контроллеры. Этот узел публикует эти темы:

  • /steeringangle

  • /accelcmd

  • /decelcmd

  • /vehdir

  • /reachgoal

Узел подписывается на эти темы:

  • /smoothpath

  • /directions

  • /speed

  • /currentpose

  • /currentvel

  • /nextgoal

  • /velprofile

При получении /velprofile обменивайтесь сообщениями, узел запускает exampleHelperROS2ValetControlCallback коллбэк, который отправляет управляющие сообщения в транспортное средство

Создайте контроллер, controlNode, и установите издателей и подписчиков в узле.

controlNode = ros.Node('control', masterHost);

% Publishers for controlNode
control.SteeringPub = ros.Publisher(controlNode, '/steeringangle', 'std_msgs/Float64');
control.AccelPub = ros.Publisher(controlNode, '/accelcmd', 'std_msgs/Float64');
control.DecelPub = ros.Publisher(controlNode, '/decelcmd', 'std_msgs/Float64');
control.VehDirPub = ros.Publisher(controlNode, '/vehdir', 'std_msgs/Float64');
control.VehGoalReachPub = ros.Publisher(controlNode, '/reachgoal');

% Subscribers for controlNode
control.PathSub = ros.Subscriber(controlNode, '/smoothpath');
control.DirSub = ros.Subscriber(controlNode, '/directions');
control.SpeedSub = ros.Subscriber(controlNode, '/speed');
control.CurPoseSub = ros.Subscriber(controlNode, '/currentpose');
control.CurVelSub = ros.Subscriber(controlNode, '/currentvel');
control.NextGoalSub = ros.Subscriber(controlNode, '/nextgoal');

% Create subscriber for /velprofile for control node and provide the callback function.
VelProfSub = ros.Subscriber(controlNode, '/velprofile');
VelProfSub.NewMessageFcn = @(~,msg)exampleHelperROSValetControlCallback(msg, control, valet);

Транспортное средство

Узел Транспортного средства ответственен за симуляцию модели транспортного средства. Этот узел публикует эти темы:

  • /currentvel

  • /currentpose

Узел подписывается на эти темы:

  • /accelcmd

  • /decelcmd

  • /vehdir

  • /steeringangle

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

% Create vehicle node.
vehicleNode = ros.Node('vehicle', masterHost);

% Create publishers for vehicle node.
vehicle.CurVelPub = ros.Publisher(vehicleNode, '/currentvel');
vehicle.CurPosePub = ros.Publisher(vehicleNode, '/currentpose');

% Create subscribers for vehicle node.
vehicle.AccelSub = ros.Subscriber(vehicleNode, '/accelcmd');
vehicle.DecelSub = ros.Subscriber(vehicleNode, '/decelcmd');
vehicle.DirSub = ros.Subscriber(vehicleNode, '/vehdir');

% Create subscriber for |/steeringangle|, which runs the vehicle simulator 
% callback.
SteeringSub = ros.Subscriber(vehicleNode, '/steeringangle', ...
    @(~,msg)exampleHelperROSValetVehicleCallback(msg, vehicle, valet));

Инициализируйте симуляцию

Чтобы инициализировать симуляцию, отправьте первое скоростное сообщение и текущее сообщение положения. Это сообщение заставляет планировщика запускать цикл планирования.

curVelMsg = getROSMessage(vehicle.CurVelPub.MessageType);
curVelMsg.Data = valet.vehicleSim.getVehicleVelocity;
send(vehicle.CurVelPub, curVelMsg);

curPoseMsg = getROSMessage(vehicle.CurPosePub.MessageType);
curPoseMsg.X = valet.currentPose(1);
curPoseMsg.Y = valet.currentPose(2);
curPoseMsg.Theta = valet.currentPose(3);
send(vehicle.CurPosePub, curPoseMsg);

reachMsg = getROSMessage(control.VehGoalReachPub.MessageType);
reachMsg.Data = true;
send(control.VehGoalReachPub, reachMsg);

Основной цикл

Основной цикл ожидает behavioralPlanner сказать транспортное средство достигло положения перед парком. 

while ~reachedDestination(valet.behavioralPlanner)
    pause(1);
end

% Show the vehicle simulation figure.
showFigure(valet.vehicleSim);

Припаркуйте маневр

Коллбэки маневра парковки немного отличаются от нормального управления manueuver. Замените коллбэки для /velprofile и /reachgoal подписчики.

VelProfSub.NewMessageFcn = @(~,msg)exampleHelperROSValetParkControlCallback(msg, control, valet);
GoalReachSub.NewMessageFcn = @(~,msg)exampleHelperROSValetParkManeuver(msg, planning, valet);

pause(1);
reachMsg = getROSMessage(control.VehGoalReachPub.MessageType);
reachMsg.Data = false;
send(control.VehGoalReachPub, reachMsg);

% Receive a message from the |/reachgoal| topic using the subcriber. This 
% waits until a new message is received. Display the figure. The vehicle
% has completed the full automated valet manuever.
receive(GoalReachSub);

exampleHelperROSValetCloseFigures;
snapnow;

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

delete(valet.vehicleSim);

% Clear variables that were created above.
clear('valet');

GoalReachSub.NewMessageFcn = [];
VelProfSub.NewMessageFcn = [];

clear('planning', 'planningNode', 'GoalReachSub');
clear('control', 'controlNode', 'VelProfSub');
clear('vehicle', 'vehicleNode', 'SteeringSub');
clear('curPoseMsg', 'curVelMsg', 'reachMsg');
clear('masterHost');

% Shutdown the ROS network.
rosshutdown;
Shutting down global node /matlab_global_node_49911 with NodeURI http://sbd508773glnxa64:42335/
Shutting down ROS master on http://192.168.0.10:60903.
.....

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

Поддержка

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