自动泊车代客与ROS 2在MATLAB

打开脚本

这个例子展示了如何进行分发自动代泊车员(自动驾驶工具箱)ROS 2网络中各个节点之间的应用程序。

概述

的扩展自动代泊车员(自动驾驶工具箱)自动驾驶工具箱™中的示例。典型的自治应用程序具有以下组件。

为简单起见，本例主要讨论计划、控制和简化的车辆模型。该示例使用预先录制的数据来代替本地化信息。

这个应用程序演示了将各种功能典型地拆分为ROS节点。下图显示了如何将上面的示例划分为不同的节点。每个节点:规划、控制和车辆是实现如下所示功能的ROS节点。节点之间的互连显示了节点之间的每个互连所使用的主题。

设置

首先，加载一个路由计划和行为规划器和路径分析器使用的给定成本图。行为规划器，路径规划器，路径分析器，横向和纵向控制器是由辅助类实现的，这些类是通过这个示例辅助函数调用来设置的。

exampleHelperROSValetSetupGlobals;初始化的全局变量被组织为全局结构中的字段% |代客|。disp(管家)

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。路径]transitionpose: [14×3 double]方向:[522×1 double] currentVel: 0 approxSeparation: 0.1000 numsmoothpose: 522 maxSpeed: 5 startSpeed: 0 endSpeed: 0 refpose: [522×3 double] cumlength: [522×1 double]弯曲:[522×1 double] refvelocity: [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
/方向
/速度
/ nextgoal

节点订阅这些主题:

/ currentvel
/ currentpose
/ desiredvel
/ reachgoal

在收到/ reachgoal消息时，节点将运行exampleHelperROS2ValetPlannerCallbackCallback，它计划下一个段。

创建规划节点。planningNode = ros2node(“规划”）;为规划节点创建发布者。首先指定消息类型为主题创建发布者或订阅者的%时间。规划。PathPub = ros2publisher(planningNode，' / smoothpath '，“std_msgs / Float64MultiArray”）;规划。VelPub = ros2publisher(planningNode，' / velprofile '，“std_msgs / Float64MultiArray”）;规划。DirPub = ros2publisher(planningNode，' /方向'，“std_msgs / Float64MultiArray”）;规划。SpeedPub = ros2publisher(planningNode，' /速度'，“std_msgs / Float64MultiArray”）;规划。NxtPub = ros2publisher(planningNode，' / nextgoal '，“geometry_msgs / Pose2D”）;为规划节点创建订阅者。规划。CurVelSub = ros2subscriber(planningNode，' / currentvel '，“std_msgs / Float64”）;规划。CurPoseSub = ros2subscriber(planningNode，' / currentpose '，“geometry_msgs / Pose2D”）;规划。DesrVelSub = ros2subscriber(planningNode，' / desiredvel '，“std_msgs / Float64”）;创建规划节点的GoalReachSub部分并提供回调。GoalReachSub = ros2subscriber(planningNode，' / reachgoal '，“std_msgs / Bool”）;GoalReachSub。NewMessageFcn = @(msg)exampleHelperROS2ValetPlannerCallback(msg, planning, valet);

控制

控制节点负责纵向和横向控制器。该节点发布以下主题:

/ steeringangle
/ accelcmd
/ decelcmd
/ vehdir
/ reachgoal

节点订阅这些主题:

/ smoothpath
/方向
/速度
/ currentpose
/ currentvel
/ nextgoal
/ velprofile

在收到/ velprofile消息时，节点将运行exampleHelperROS2ValetControlCallback回调函数，它向车辆发送控制消息

创建控制节点controlNode = ros2node(“控制”）;为控制节点创建发布者控制。SteeringPub = ros2publisher(controlNode，' / steeringangle '，“std_msgs / Float64”）;控制。AccelPub = ros2publisher(controlNode，' / accelcmd '，“std_msgs / Float64”）;控制。pub = ros2publisher(controlNode，' / decelcmd '，“std_msgs / Float64”）;控制。VehDirPub = ros2publisher(controlNode，' / vehdir '，“std_msgs / Float64”）;控制。VehGoalReachPub = ros2publisher(controlNode，' / reachgoal '）;为控制节点创建订阅用户。的所有消息类型%的主题，我们可以使用较短的版本来创建%的用户控制。PathSub = ros2subscriber(controlNode，' / smoothpath '）;控制。DirSub = ros2subscriber(controlNode，' /方向'）;控制。SpeedSub = ros2subscriber(controlNode，' /速度'）;控制。CurPoseSub = ros2subscriber(controlNode，' / currentpose '）;控制。CurVelSub = ros2subscriber(controlNode，' / currentvel '）;控制。NextGoalSub = ros2subscriber(controlNode，' / nextgoal '）;为控制节点创建VelProfSub并提供回调VelProfSub = ros2subscriber(controlNode，' / velprofile '）;VelProfSub。例如helperros2valetcontrolcallback (msg, control, valet);

车辆

Vehicle节点负责模拟车辆模型。该节点发布以下主题:

/ currentvel
/ currentpose

节点订阅这些主题:

/ accelcmd
/ decelcmd
/ vehdir
/ steeringangle

%在收到|/转向|消息时，车辆模拟器开始磨合|exampleHelperROS2ValetVehicleCallback|回调。％% < < . . / exampleHelperROSValetVehicleNode.PNG > >％创建车辆节点。vehicleNode = ros2node(“汽车”）;为车辆节点创建发布者。车辆。CurVelPub = ros2publisher(vehicleNode，' / currentvel '）;车辆。CurPosePub = ros2publisher(vehicleNode，' / currentpose '）;为车辆节点创建订阅者。车辆。AccelSub = ros2subscriber(vehicleNode，' / accelcmd '）;车辆。sub = ros2subscriber(vehicleNode，' / decelcmd '）;车辆。DirSub = ros2subscriber(vehicleNode，' / vehdir '）;创建SteeringSub，它运行车辆模拟器作为回调的一部分。转向子= ros2subscriber(vehicleNode，' / steeringangle '，.．.@(msg)exampleHelperROS2ValetVehicleCallback(msg, vehicle, valet));

初始化模拟

为了初始化模拟，发送第一个速度消息和当前姿态消息。该消息导致计划器启动计划循环。

curVelMsg = ros2message(vehicle.CurVelPub);curVelMsg。data = valet.vehicleSim.getVehicleVelocity;发送(车辆。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说车辆到达了预停车位置。

而~ reachedDestination (valet.behavioralPlanner)暂停(1);结束显示车辆仿真图showFigure (valet.vehicleSim);

公园机动

停车机动回调与正常驾驶机动略有不同。的回调函数/ velprofile而且/ reachgoal订阅者。

VelProfSub。NewMessageFcn = @(msg)exampleHelperROS2ValetParkControlCallback(msg, control, valet);GoalReachSub。helperros2valetparkmaneuver (msg, planning, valet);reachMsg = ros2message(control.VehGoalReachPub);reachMsg。数据= false;发送(控制。VehGoalReachPub reachMsg);使用订阅者从|/reachgoal|主题接收消息。这%等待，直到收到新消息。显示图形。车辆%已完成全自动代客操作。收到(GoalReachSub);exampleHelperROSValetCloseFigures;snapnow;

删除模拟器并通过清除发布者、订阅者和节点句柄来关闭所有节点。

删除(valet.vehicleSim);清除上面创建的变量。清楚(“管家”）;GoalReachSub。NewMessageFcn = [];VelProfSub。NewMessageFcn = [];清楚(“规划”，“planningNode”，“GoalReachSub”）;清楚(“控制”，“controlNode”，“VelProfSub”）;清楚(“汽车”，“vehicleNode”，“SteeringSub”）;清楚(“curPoseMsg”，“curVelMsg”，“reachMsg”）;