这个例子展示了如何分配自动停车员(自动驾驶工具箱)应用程序在ROS网络中各个节点之间。取决于您的系统,这个例子提供了活性氧ROS 2网络使用MATLAB®或仿真软件®。金宝app这里显示的示例使用ROS和MATLAB。其他的例子,看:
这个例子是一个扩展的自动停车员(自动驾驶工具箱)在自动驾驶的工具箱™。一个典型的autonmous应用程序有以下组件。
为简单起见,这个例子集中于规划、控制和一个简化的车辆模型。示例使用预先录制的数据代替定位信息。
这个应用程序演示了一个典型的各种功能分割成ROS节点。下面的图片显示了如何分成各个节点上面的例子。每个节点:规划、控制和车辆是ROS节点实现功能如下所示。节点之间的联系中显示的每个互连节点上使用的主题。
首先,加载一个给定costmap路线规划和规划和路径分析仪使用的行为。行为规划、路径规划、路径分析,横向和纵向的控制器是由助手类实现的,与这个例子设置辅助函数调用。
exampleHelperROSValetSetupGlobals;
初始化全局变量是组织为全球结构领域,代客
。
disp(管家)
mapLayers:(1×1结构)costmap: [1×1 vehicleCostmap] vehicleDims: [1×1 vehicleDimensions] maxSteeringAngle: 35数据:[1×1 struct] routePlan:[4×3表]currentPose: [4 12 0] vehicleSim: [1×1 ExampleHelperROSValetVehicleSimulator] behavioralPlanner: [1×1 ExampleHelperROSValetBehavioralPlanner] motionPlanner: [1×1 pathPlannerRRT] goalPose: [56 11 0] refPath:[1×1驾驶。路径]transitionPoses(14×3双):使用方法:[522×1双]currentVel: 0 approxSeparation: 0.1000 numSmoothPoses: 522 maxSpeed: 5 startSpeed: 0 endSpeed: 0 refPoses:[522×3双]cumLengths:[522×1双]曲率:[522×1双]refVelocities:[522×1双]sampleTime: 0.1000 lonController: [1×1 ExampleHelperROSValetLongitudinalController] controlRate: [1×1 ExampleHelperROSValetFixedRate] pathAnalyzer: [1×1 ExampleHelperROSValetPathAnalyzer] parkPose: [36 44 90]
初始化网络ROS。
rosinit;
启动ROS ..................................................................................核心…在5.3625秒内完成。
初始化ROS主在http://192.168.0.10:60903上。初始化全局节点/ matlab_global_node_49911 NodeURI http://sbd508773glnxa64:42335
masterHost =“localhost”;
应用程序中的函数分布在ROS节点。这个示例使用三个ROS节点:planningNode
,controlNode
,vehicleNode
。
计划节点计算每条路径段根据当前车辆位置。这个节点负责生成平滑路径和发布网络的路径。
这个节点发布这些主题:
/ smoothpath
/ velprofile
/方向
/速度
/ nextgoal
节点订阅这些主题:
/ currentvel
/ currentpose
/ desiredvel
/ reachgoal
收到一个/ reachgoal
消息,节点运行exampleHelperROS2ValetPlannerCallback
回调,计划下一段。
创建计划节点
planningNode = ros.Node (“规划”,masterHost);
创建计划节点的出版商。指定出版商的消息类型或用户话题ROS网络上不存在。
规划。PathPub = ros.Publisher (planningNode,' / smoothpath ',“std_msgs / Float64MultiArray”);规划。VelPub = ros.Publisher (planningNode,' / velprofile ',“std_msgs / Float64MultiArray”);规划。DirPub = ros.Publisher (planningNode,' /方向',“std_msgs / Float64MultiArray”);规划。SpeedPub = ros.Publisher (planningNode,' /速度',“std_msgs / Float64MultiArray”);规划。NxtPub = ros.Publisher (planningNode,' / nextgoal ',“geometry_msgs / Pose2D”);
创建计划的用户,planningNode
。
规划。CurVelSub = ros.Subscriber (planningNode,' / currentvel ',“std_msgs / Float64”);规划。CurPoseSub = ros.Subscriber (planningNode,' / currentpose ',“geometry_msgs / Pose2D”);规划。DesrVelSub = ros.Subscriber (planningNode,' / desiredvel ',“std_msgs / Float64”);
创建用户,GoalReachSub
,听/ reachgoal
主题的规划节点和指定回调的行动。
GoalReachSub = ros.Subscriber (planningNode,' / reachgoal ',“std_msgs / Bool”);GoalReachSub。味精NewMessageFcn = @ (~) exampleHelperROSValetPlannerCallback(味精、规划、代客);
控制节点负责纵向和横向控制器。这个节点发布这些主题:
/ steeringangle
/ accelcmd
/ decelcmd
/ vehdir
/ reachgoal
节点订阅这些主题:
/ smoothpath
/方向
/速度
/ currentpose
/ currentvel
/ nextgoal
/ velprofile
收到一个/ velprofile
消息,节点运行exampleHelperROS2ValetControlCallback
向车辆发送控制消息的回调
创建一个控制器,controlNode
,设置节点的发布者和订阅者。
controlNode = ros.Node (“控制”,masterHost);%的出版商controlNode控制。SteeringPub = ros.Publisher (controlNode,' / steeringangle ',“std_msgs / Float64”);控制。AccelPub = ros.Publisher (controlNode,' / accelcmd ',“std_msgs / Float64”);控制。DecelPub = ros.Publisher (controlNode,' / decelcmd ',“std_msgs / Float64”);控制。VehDirPub = ros.Publisher (controlNode,' / vehdir ',“std_msgs / Float64”);控制。VehGoalReachPub = ros.Publisher (controlNode,' / reachgoal ');%的用户为controlNode控制。PathSub = ros.Subscriber (controlNode,' / smoothpath ');控制。DirSub = ros.Subscriber (controlNode,' /方向');控制。SpeedSub = ros.Subscriber (controlNode,' /速度');控制。CurPoseSub = ros.Subscriber (controlNode,' / currentpose ');控制。CurVelSub = ros.Subscriber (controlNode,' / currentvel ');控制。NextGoalSub = ros.Subscriber (controlNode,' / nextgoal ');%为/ velprofile控制节点创建用户,并提供回调函数。VelProfSub = ros.Subscriber (controlNode,' / velprofile ');VelProfSub。味精NewMessageFcn = @ (~) exampleHelperROSValetControlCallback(味精、控制、代客);
车辆节点负责模拟车辆模型。这个节点发布这些主题:
/ currentvel
/ currentpose
节点订阅这些主题:
/ accelcmd
/ decelcmd
/ vehdir
/ steeringangle
收到一个/ steeringangle
信息,车辆运行模拟器在回调函数中,exampleHelperROSValetVehicleCallback
。
%创建车辆节点。vehicleNode = ros.Node (“汽车”,masterHost);%建立车辆节点的出版商。车辆。CurVelPub = ros.Publisher (vehicleNode,' / currentvel ');车辆。CurPosePub = ros.Publisher (vehicleNode,' / currentpose ');%为车辆节点创建用户。车辆。AccelSub = ros.Subscriber (vehicleNode,' / accelcmd ');车辆。DecelSub = ros.Subscriber (vehicleNode,' / decelcmd ');车辆。DirSub = ros.Subscriber (vehicleNode,' / vehdir ');% |创建用户/ steeringangle |,车辆运行模拟器%的回调。SteeringSub = ros.Subscriber (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
车辆到达prepark位置。
而~ reachedDestination (valet.behavioralPlanner)暂停(1);结束%显示车辆仿真图。showFigure (valet.vehicleSim);
停车演习从正常驾驶manueuver回调略有不同。取代的回调/ velprofile
和/ reachgoal
订阅者。
VelProfSub。味精NewMessageFcn = @ (~) exampleHelperROSValetParkControlCallback(味精、控制、代客);GoalReachSub。味精NewMessageFcn = @ (~) exampleHelperROSValetParkManeuver(味精、规划、代客);暂停(1);reachMsg = getROSMessage (control.VehGoalReachPub.MessageType);reachMsg。数据= false;发送(控制。VehGoalReachPub reachMsg);%接收一条消息从| / reachgoal使用subcriber |主题。这%将等待收到新消息。显示图。车辆%完成全自动化的代客机动。收到(GoalReachSub);exampleHelperROSValetCloseFigures;snapnow;
删除模拟器和关闭所有节点通过清除出版商、用户和节点处理。
删除(valet.vehicleSim);%清楚上面创建的变量。清楚(“管家”);GoalReachSub。NewMessageFcn = [];VelProfSub。NewMessageFcn = [];清楚(“规划”,“planningNode”,“GoalReachSub”);清楚(“控制”,“controlNode”,“VelProfSub”);清楚(“汽车”,“vehicleNode”,“SteeringSub”);清楚(“curPoseMsg”,“curVelMsg”,“reachMsg”);清楚(“masterHost”);%关闭ROS网络。rosshutdown;
关闭全球节点/ matlab_global_node_49911 NodeURI http://sbd508773glnxa64:42335关闭..... ROS大师在http://192.168.0.10:60903上