Automated Parking Valet with ROS in MATLAB

This example uses:

打开实时脚本

This example shows how to distribute theAutomated Parking Valet(Automated Driving Toolbox)application among various nodes in a ROS network. Depending on your system, this example is provided for ROS and ROS 2 networks using either MATLAB® or Simulink® . The example shown here uses ROS and MATLAB. For the other examples, see:

Overview

This example is an extension of theAutomated Parking Valet(Automated Driving Toolbox)自动驾驶Toolbox™中的示例。典型的自动应用程序具有以下组件。

For simplicity, this example concentrates on Planning, Control, and a simplified Vehicle Model. The example uses prerecorded data to substitute localization information.

This application demonstrates a typical split of various functions into ROS nodes. The following picture shows how the above example is split into various nodes. Each node: Planning, Control and Vehicle is a ROS node implementing the functionalities shown as below. The interconnections between the nodes show the topics used on each interconnection of the nodes.

Setup

First, load a route plan and a given costmap used by the behavior planner and path analyzer. Behavior Planner, Path Planner, Path Analyzer, Lateral and Lognitudinal Controllers are implemented by helper classes, which are setup with this example helper function call.

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]

Initialize the ROS network.

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，和车辆.

Planning

The Planning node calculates each path segment based on the current vehicle position. This node is responsible for generating the smooth path and publishes the path to the network.

该节点发布了这些主题：

/smoothpath
/velprofile
/directions
/速度
/nextgoal

节点订阅了这些主题：

/curressvel
/currentpose
/desiredvel
/reachgoal

On receiving a/reachgoalmessage, the node runs theexampleHelperROS2ValetPlannerCallbackcallback, which plans the next segment.

Create the planning node

planningnode = ros.node（'planning', masterHost);

Create publishers for planning node. Specify the message types for the publisher or subscriber for a topic that is not present on ROS network.

planning.PathPub = ros.Publisher(planningNode,'/smoothpath','std_msgs/Float64MultiArray'）；planning.VelPub = ros.Publisher(planningNode,'/velprofile','std_msgs/Float64MultiArray'）；planning.DirPub = ros.Publisher(planningNode,'/指示','std_msgs/Float64MultiArray'）；planning.speedpub = ros.publisher（PlanningNode，'/speed','std_msgs/Float64MultiArray'）；planning.NxtPub = ros.Publisher(planningNode,'/nextgoal','geometry_msgs/Pose2D'）；

Create the subscribers for the planner,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'）；

Create subscriber,GoalReachSub, to listen to the/reachgoaltopic of planning node and specify the callback action.

goareachSub = ros.subscriber（PlanceNnode，'/reachgoal','std_msgs/Bool'）；GoalReachSub.NewMessageFcn = @(~,msg)exampleHelperROSValetPlannerCallback(msg, planning, valet);

Control

The Control node is responsible for longitudinal and lateral controllers. This node publishes these topics:

/steeringangle
/accelcmd
/decelcmd
/vehdir
/reachgoal

节点订阅了这些主题：

/smoothpath
/directions
/速度
/currentpose
/curressvel
/nextgoal
/velprofile

On receiving a/velprofilemessage, the node runs theexampleHelperROS2ValetControlCallback回调，将控制消息发送给车辆

创建控制器，controlNode，和setup the publishers and subscribers in the node.

controlNode = ros.Node('control', masterHost);% Publishers for controlNodecontrol.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 controlNodecontrol.PathSub = ros.Subscriber(controlNode,'/smoothpath'）；control.DirSub = ros.Subscriber(controlNode,'/指示'）；control.speedsub = ros.subscriber（控制节点，'/speed'）；control.CurPoseSub = ros.Subscriber(controlNode,'/currentpose'）；control.CurVelSub = ros.Subscriber(controlNode,'/currentvel'）；control.nextgoalsub = ros.subscriber（控制节点，'/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);

Vehicle

车辆节点负责模拟车辆模型。该节点发布了这些主题：

/curressvel
/currentpose

节点订阅了这些主题：

/accelcmd
/decelcmd
/vehdir
/steeringangle

On receiving a/steeringanglemessage, the vehicle simulator is run in the callback function,exampleHelperROSValetVehicleCallback.

% Create vehicle node.车辆= ros.Node('车辆', masterHost);% Create publishers for vehicle node.车辆。CurvelPub= ros.publisher（车辆列货，，'/currentvel'）；车辆。CurposePub= ros.publisher（车辆列货，，'/currentpose'）；% Create subscribers for vehicle node.车辆。accelsub = ros.subscriber（车辆列货，，'/accelcmd'）；vehicle.DecelSub = ros.Subscriber(vehicleNode,'/decelcmd'）；车辆。dirsub= ros.subscriber（车辆烯码，，'/vehdir'）；% Create subscriber for |/steeringangle|, which runs the vehicle simulator% callback.steeringsub = ros.subscriber（车辆烯码，，'/steeringangle',...@(~,msg)exampleHelperROSValetVehicleCallback(msg, vehicle, valet));

Initialize Simulation

To initialize the simulation, send the first velocity message and current pose message. This message causes the planner to start the planning loop.

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);

Main Loop

主要循环等待behavioralPlannerto say the vehicle reached the prepark position.

while~reachedDestination(valet.behavioralPlanner) pause(1);结束% Show the vehicle simulation figure.showfigure（Valet.Vehiclesim）;

Park Maneuver

The parking maneuver callbacks are slightly different from the normal driving manueuver. Replace the callbacks for the/velprofile和/reachgoalsubscribers.

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 the simulator and shutdown all the nodes by clearing publishers, subscribers and node handles.

删除（Vehet.Vehiclesim）;% Clear variables that were created above.清除（'valet'）；GoalReachSub.NewMessageFcn = []; VelProfSub.NewMessageFcn = []; clear('planning','planningNode',“守门员”）；清除（'control','controlNode','VelProfSub'）；清除（'车辆','vehicleNode','steeringsub'）；清除（'curPoseMsg',“ curvelmsg','reachMsg'）；清除（'masterHost'）；% Shutdown the ROS network.Rosshutdown;

关闭/ matlab_global_node_4991全球节点1 with NodeURI http://sbd508773glnxa64:42335/ Shutting down ROS master on http://192.168.0.10:60903. .....