В этом примере показано, как распределить Автоматизированного Камердинера Парковки (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. .....