Автоматизированная парковка с ROS 2 в MATLAB

В этом примере показано, как можно распределить приложение Automated Parking Valet (Automated Driving Toolbox) между различными узлами в сети ROS 2.

Обзор

Этот пример является расширением примера Automated Parking Valet (Automated Driving Toolbox) в Automated Driving Toolbox™. Типичное автоматическое приложение имеет следующие компоненты.

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

Это приложение демонстрирует типовое разделение различных функций на узлы ROS. На следующем рисунке показано, как вышеописанный пример разделен на различные узлы. Каждый узел: Planning, Control and Vehicle является узлом ROS, реализующим функции, показанные ниже. Взаимосвязи между узлами показывают темы, используемые для каждого соединения узлов.

Setup

Во-первых, загрузите план маршрута и заданную косметику, используемую планировщиком поведения и анализатором пути. Behavior Planner, Путь Planner, Пути Analyzer, Lateral и Lognitudinal Контроллеров реализованы классами helper, которые настройка с этим примером вызова вспомогательной функции.

exampleHelperROSValetSetupGlobals;

% The initialized globals are organized as fields in the global structure
% |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]

Используйте узлы, чтобы разделить функции в приложении. Этот пример использует три узла: planningNode, controlNode, и vehicleNode.

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

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

Этот узел публикует следующие темы:

  • /smoothpath

  • /velprofile

  • /directions

  • /speed

  • /nextgoal

Узел подписан на следующие темы:

  • /currentvel

  • /currentpose

  • /desiredvel

  • /reachgoal

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

% Create planning node.
planningNode = ros2node('planning');

% Create publishers for planning node. Specify the message types the first
% time you create a publisher or subscriber for a topic.
planning.PathPub = ros2publisher(planningNode, '/smoothpath', 'std_msgs/Float64MultiArray');
planning.VelPub = ros2publisher(planningNode, '/velprofile', 'std_msgs/Float64MultiArray');
planning.DirPub = ros2publisher(planningNode, '/directions', 'std_msgs/Float64MultiArray');
planning.SpeedPub = ros2publisher(planningNode,'/speed','std_msgs/Float64MultiArray');
planning.NxtPub = ros2publisher(planningNode, '/nextgoal', 'geometry_msgs/Pose2D');

% Create subscribers for planning node.
planning.CurVelSub = ros2subscriber(planningNode, '/currentvel', 'std_msgs/Float64');
planning.CurPoseSub = ros2subscriber(planningNode, '/currentpose', 'geometry_msgs/Pose2D');
planning.DesrVelSub = ros2subscriber(planningNode, '/desiredvel', 'std_msgs/Float64');

% Create GoalReachSub part of planning node and provide the callback.
GoalReachSub = ros2subscriber(planningNode, '/reachgoal', 'std_msgs/Bool');
GoalReachSub.NewMessageFcn = @(msg)exampleHelperROS2ValetPlannerCallback(msg, planning, valet);

Контроль

Узел Control отвечает за продольные и боковые контроллеры. Этот узел публикует следующие темы:

  • /steeringangle

  • /accelcmd

  • /decelcmd

  • /vehdir

  • /reachgoal

Узел подписан на следующие темы:

  • /smoothpath

  • /directions

  • /speed

  • /currentpose

  • /currentvel

  • /nextgoal

  • /velprofile

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

% Create control node
controlNode = ros2node('control');

% Create publishers for control node
control.SteeringPub = ros2publisher(controlNode, '/steeringangle', 'std_msgs/Float64');
control.AccelPub = ros2publisher(controlNode, '/accelcmd', 'std_msgs/Float64');
control.DecelPub = ros2publisher(controlNode, '/decelcmd', 'std_msgs/Float64');
control.VehDirPub = ros2publisher(controlNode, '/vehdir', 'std_msgs/Float64');
control.VehGoalReachPub = ros2publisher(controlNode, '/reachgoal');

% Create subscribers for control node. Since all the message types for the
% topics are determined above, we can use the shorter version to create
% subscribers
control.PathSub = ros2subscriber(controlNode, '/smoothpath');
control.DirSub = ros2subscriber(controlNode, '/directions');
control.SpeedSub = ros2subscriber(controlNode, '/speed');
control.CurPoseSub = ros2subscriber(controlNode, '/currentpose');
control.CurVelSub = ros2subscriber(controlNode, '/currentvel');
control.NextGoalSub = ros2subscriber(controlNode, '/nextgoal');

% Create VelProfSub for control node and provide the callback
VelProfSub = ros2subscriber(controlNode, '/velprofile');
VelProfSub.NewMessageFcn = @(msg)exampleHelperROS2ValetControlCallback(msg, control, valet);

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

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

  • /currentvel

  • /currentpose

Узел подписан на следующие темы:

  • /accelcmd

  • /decelcmd

  • /vehdir

  • /steeringangle

% On receiving a |/steeringangle| message, the vehicle simulator is run in
% the |exampleHelperROS2ValetVehicleCallback| callback.
%
% <<../exampleHelperROSValetVehicleNode.PNG>>
%

% Create vehicle node.
vehicleNode = ros2node('vehicle');

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

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

% Create SteeringSub which runs the vehicle simulator as part of callback.
SteeringSub = ros2subscriber(vehicleNode, '/steeringangle', ...
    @(msg)exampleHelperROS2ValetVehicleCallback(msg, vehicle, valet));

Инициализация симуляции

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

curVelMsg = ros2message(vehicle.CurVelPub);
curVelMsg.data = valet.vehicleSim.getVehicleVelocity;
send(vehicle.CurVelPub, curVelMsg);

curPoseMsg = ros2message(vehicle.CurPosePub);
curPoseMsg.x = valet.currentPose(1);
curPoseMsg.y = valet.currentPose(2);
curPoseMsg.theta = valet.currentPose(3);
send(vehicle.CurPosePub, curPoseMsg);

reachMsg = ros2message(control.VehGoalReachPub);
reachMsg.data = true;
send(control.VehGoalReachPub, reachMsg);

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

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

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

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

Маневр Парка

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

VelProfSub.NewMessageFcn = @(msg)exampleHelperROS2ValetParkControlCallback(msg, control, valet);
GoalReachSub.NewMessageFcn = @(msg)exampleHelperROS2ValetParkManeuver(msg, planning, valet);

reachMsg = ros2message(control.VehGoalReachPub);
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');
Для просмотра документации необходимо авторизоваться на сайте