В этом примере показано, как можно распределить приложение Automated Parking Valet (Automated Driving Toolbox) между различными узлами в сети ROS 2.
Этот пример является расширением примера автоматизированного парковочного шкафа (Automated Driving Toolbox) в Automated Driving Toolbox™. Типичное автономное приложение имеет следующие компоненты.
Для простоты в этом примере основное внимание уделяется планированию, управлению и упрощенной модели транспортного средства. В примере для замены информации о локализации используются предварительно записанные данные.
Это приложение демонстрирует типичное разделение различных функций на узлы ROS. На следующем рисунке показано разделение приведенного выше примера на различные узлы. Каждый узел: планирование, управление и транспортное средство - это узел АФК, реализующий функции, показанные ниже. Взаимосвязи между узлами показывают темы, используемые в каждом соединении узлов.
Сначала загрузите план маршрута и заданную карту затрат, используемую планировщиком поведения и анализатором пути. Планировщик поведения, планировщик пути, анализатор пути, боковой и логарифмический контроллеры реализуются классами-помощниками, которые настраиваются с помощью этого примера вызова функции-помощника.
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.
Узел планирования вычисляет каждый сегмент пути на основе текущей позиции транспортного средства. Этот узел отвечает за формирование плавного пути и публикует его в сети.
Этот узел публикует следующие разделы:
/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);
Узел управления отвечает за продольные и боковые контроллеры. Этот узел публикует следующие разделы:
/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);
Узел транспортного средства отвечает за моделирование модели транспортного средства. Этот узел публикует следующие разделы:
/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');