自动泊车代客与ROS的MATLAB
该示例演示如何分发自动停车员(自动驾驶工具箱)ROS网络中各个节点之间的应用。根据您的系统,本示例提供了使用MATLAB®或Simulink®的ROS和ROS 2网络。这里显示的示例使用ROS和MATLAB。有关其他示例,请参见:
概述
的扩展自动停车员(自动驾驶工具箱)自动驾驶工具箱™中的例子。典型的自治应用程序具有以下组件。
为了简单起见,这个示例集中于规划、控制和简化的车辆模型。该示例使用预先录制的数据代替定位信息。
这个应用程序演示了将各种功能划分为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] directions: [522×1 double] currentVel: 0 approxSeparation: 0.1000 numsmoothpose: 522 maxSpeed: 5 startSpeed: 0 endSpeed: 0 refpose: [522×3 double] cumlength: [522×1 double] curvatures: [522×1 double] refvelocity: [522×1 double] sampleTime: 0.1000 lonController: [1×1 examplehelperrosvaletlongitude inalcontroller] controlRate: [1×1 ExampleHelperROSValetFixedRate] pathAnalyzer: [1×1 ExampleHelperROSValetPathAnalyzer] parkPose: [36 44 90]
初始化ROS网络
rosinit;
启动ROS核心 ... ..................................................................................5.3625秒完成。
在http://192.168.0.10:60903上初始化ROS master。使用NodeURI http://sbd508773glnxa64:42335/初始化全局节点/matlab_global_node_49911
masterHost =“localhost”;
应用程序中的功能分布在ROS节点之间。这个例子使用了三个ROS节点:planningNode
,controlNode
,vehicleNode
.
规划
Planning节点根据当前车辆位置计算每个路径段。该节点负责生成平滑路径,并将路径发布到网络。
该节点发布以下主题:
/ smoothpath
/ velprofile
/方向
/速度
/ nextgoal
节点订阅了以下主题:
/ currentvel
/ currentpose
/ desiredvel
/ reachgoal
收到一个/ reachgoal
消息时,节点运行exampleHelperROS2ValetPlannerCallback
Callback,它计划下一个片段。
创建规划节点
planningNode = ros。节点(“规划”, masterHost);
为规划节点创建发布者。为没有出现在ROS网络上的主题的发布者或订阅者指定消息类型。
规划。PathPub = ros。出版商(planningNode' / smoothpath ',“std_msgs / Float64MultiArray”);规划。VelPub = ros。出版商(planningNode' / velprofile ',“std_msgs / Float64MultiArray”);规划。DirPub = ros。出版商(planningNode' /方向',“std_msgs / Float64MultiArray”);规划。SpeedPub = ros。出版商(planningNode' /速度',“std_msgs / Float64MultiArray”);规划。NxtPub = ros。出版商(planningNode' / nextgoal ',“geometry_msgs / Pose2D”);
为计划器创建订阅者,planningNode
.
规划。CurVelSub = ros。订户(planningNode' / currentvel ',“std_msgs / Float64”);规划。CurPoseSub = ros。订户(planningNode' / currentpose ',“geometry_msgs / Pose2D”);规划。DesrVelSub = ros。订户(planningNode' / desiredvel ',“std_msgs / Float64”);
创建用户,GoalReachSub
,要听才行/ reachgoal
规划节点的主题,并指定回调动作。
GoalReachSub = ros。订户(planningNode' / reachgoal ',“std_msgs / Bool”);GoalReachSub。exampleHelperROSValetPlannerCallback(msg, planning, valet);
控制
控制节点负责纵向和横向控制器。该节点发布以下主题:
/ steeringangle
/ accelcmd
/ decelcmd
/ vehdir
/ reachgoal
节点订阅了以下主题:
/ smoothpath
/方向
/速度
/ currentpose
/ currentvel
/ nextgoal
/ velprofile
收到一个/ velprofile
消息时,节点运行exampleHelperROS2ValetControlCallback
Callback,它向车辆发送控制消息
创建一个控制器,controlNode
,并在节点中设置发布者和订阅者。
controlNode = ros。节点(“控制”, masterHost);controlNode的发布者%控制。SteeringPub = ros。出版商(controlNode' / steeringangle ',“std_msgs / Float64”);控制。AccelPub = ros。出版商(controlNode' / accelcmd ',“std_msgs / Float64”);控制。DecelPub = ros。出版商(controlNode' / decelcmd ',“std_msgs / Float64”);控制。VehDirPub = ros。出版商(controlNode' / vehdir ',“std_msgs / Float64”);控制。VehGoalReachPub = ros。出版商(controlNode' / reachgoal ');controlNode的订阅服务器百分比控制。PathSub = ros。订户(controlNode' / smoothpath ');控制。DirSub = ros。订户(controlNode' /方向');控制。SpeedSub = ros。订户(controlNode' /速度');控制。CurPoseSub = ros。订户(controlNode' / currentpose ');控制。CurVelSub = ros。订户(controlNode' / currentvel ');控制。NextGoalSub = ros。订户(controlNode' / nextgoal ');为控制节点/velprofile创建订阅者,并提供回调函数。VelProfSub = ros。订户(controlNode' / velprofile ');VelProfSub。exampleHelperROSValetControlCallback(msg, control, valet);
车辆
Vehicle节点负责模拟车辆模型。该节点发布以下主题:
/ currentvel
/ currentpose
节点订阅了以下主题:
/ accelcmd
/ decelcmd
/ vehdir
/ steeringangle
收到一个/ steeringangle
消息,车辆模拟器在回调函数中运行,exampleHelperROSValetVehicleCallback
.
%创建车辆节点。vehicleNode = ros。节点(“汽车”, masterHost);为车辆节点创建发布者。车辆。CurVelPub = ros。出版商(vehicleNode' / currentvel ');车辆。CurPosePub = ros。出版商(vehicleNode' / currentpose ');为车辆节点创建订阅用户。车辆。AccelSub = ros。订户(vehicleNode' / accelcmd ');车辆。DecelSub = ros。订户(vehicleNode' / decelcmd ');车辆。DirSub = ros。订户(vehicleNode' / vehdir ');为|/转向角|创建订阅者,它运行车辆模拟器%的回调。SteeringSub = ros。订户(vehicleNode' / steeringangle ',...@(~,味精)exampleHelperROSValetVehicleCallback(味精、车辆、代客));
初始化模拟
为了初始化模拟,发送第一个速度消息和当前姿势消息。此消息导致计划器启动计划循环。
curVelMsg = getROSMessage (vehicle.CurVelPub.MessageType);curVelMsg。数据= valet.vehicleSim.getVehicleVelocity;发送(车辆。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
就是说车辆到达了停车前的位置。
而~ reachedDestination (valet.behavioralPlanner)暂停(1);结束显示车辆仿真图。showFigure (valet.vehicleSim);
公园机动
停车机动回调与正常驾驶机动稍有不同。方法的回调函数/ velprofile
而且/ reachgoal
订阅者。
VelProfSub。exampleHelperROSValetParkControlCallback(msg, control, valet);GoalReachSub。exampleHelperROSValetParkManeuver(msg, planning, valet);暂停(1);reachMsg = getROSMessage (control.VehGoalReachPub.MessageType);reachMsg。数据= false;发送(控制。VehGoalReachPub reachMsg);使用订阅者从|/reachgoal|主题接收消息。这%等待直到接收到新消息。显示图。车辆已完成全自动化代客操作。。收到(GoalReachSub);exampleHelperROSValetCloseFigures;snapnow;
删除模拟器并通过清除发布者、订阅者和节点句柄来关闭所有节点。
删除(valet.vehicleSim);清除上面创建的变量。清楚(“管家”);GoalReachSub。NewMessageFcn = [];VelProfSub。NewMessageFcn = [];清楚(“规划”,“planningNode”,“GoalReachSub”);清楚(“控制”,“controlNode”,“VelProfSub”);清楚(“汽车”,“vehicleNode”,“SteeringSub”);清楚(“curPoseMsg”,“curVelMsg”,“reachMsg”);清楚(“masterHost”);关闭ROS网络。rosshutdown;
使用NodeURI关闭全局节点/matlab_global_node_49911 http://sbd508773glnxa64:42335/在http://192.168.0.10:60903. .....上关闭ROS master