Automated Parking Valet with ROS in MATLAB

This example uses:

This example shows how to distribute the Automated 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 the Automated Parking Valet (Automated Driving Toolbox) example in Automated Driving Toolbox™. A typical autonmous application has the following components.

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)

% Initialize the ROS network.
rosinit;
masterHost = 'localhost';

            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]

Initializing ROS master on http://ah-sradford:65486/.
Initializing global node /matlab_global_node_34026 with NodeURI http://ah-sradford:65490/

Use nodes to split the functions in the application. This example uses three nodes: planningNode, controlNode, and vehicleNode.

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.

This node publishes these topics:

/smoothpath
/velprofile
/directions
/speed
/nextgoal

The node subscribes to these topics:

/currentvel
/currentpose
/desiredvel
/reachgoal

On receiving a /reachgoal message, the node runs the exampleHelperROS2ValetPlannerCallback callback, which plans the next segment.

% Create planning node
planningNode = ros.Node('planning', masterHost);

% Create publishers for planning node. Specify the message types the first
% time you create a publisher or subscriber for a topic.
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');

% Create subscribers for planning node
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 GoalReachSub part of planning node and provide the callback
GoalReachSub = ros.Subscriber(planningNode, '/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

The node subscribes to these topics:

/smoothpath
/directions
/speed
/currentpose
/currentvel
/nextgoal
/velprofile

On receiving a /velprofile message, the node runs the exampleHelperROS2ValetControlCallback callback, which sends control messages to the vehicle

% Create control node.
controlNode = ros.Node('control', masterHost);

% Create publishers for control node.
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');

% Create subscribers for control node.
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);

Vehicle

The Vehicle node is responsible for simulating the vehicle model. This node publishes these topics:

/currentvel
/currentpose

The node subscribes to these topics:

/accelcmd
/decelcmd
/vehdir
/steeringangle

% On receiving a |/steeringangle| message, the vehicle simulator is run in
% the |exampleHelperROS2ValetVehicleCallback| callback.
%
% <<../exampleHelperROSValetVehicleNode.PNG>>
%

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

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 = rosmessage(vehicle.CurVelPub);
curVelMsg.Data = valet.vehicleSim.getVehicleVelocity;
send(vehicle.CurVelPub, curVelMsg);

curPoseMsg = rosmessage(vehicle.CurPosePub);
curPoseMsg.X = valet.currentPose(1);
curPoseMsg.Y = valet.currentPose(2);
curPoseMsg.Theta = valet.currentPose(3);
send(vehicle.CurPosePub, curPoseMsg);

reachMsg = rosmessage(control.VehGoalReachPub);
reachMsg.Data = true;
send(control.VehGoalReachPub, reachMsg);

Main Loop

The main loop waits for the behavioralPlanner to say the vehicle reached the prepark position.

while ~reachedDestination(valet.behavioralPlanner)
    pause(1);
end

% 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 and /reachgoal subscribers.

VelProfSub.NewMessageFcn = @(~,msg)exampleHelperROSValetParkControlCallback(msg, control, valet);
GoalReachSub.NewMessageFcn = @(~,msg)exampleHelperROSValetParkManeuver(msg, planning, valet);

pause(1);
reachMsg = rosmessage(control.VehGoalReachPub);
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.

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_34026 with NodeURI http://ah-sradford:65490/
Shutting down ROS master on http://ah-sradford:65486/.

Documentation

Automated Parking Valet with ROS in MATLAB

Overview

Setup

Planning

Control

Vehicle

Initialize Simulation

Main Loop

Park Maneuver

ROS Toolbox Documentation

Support