Automated Parking Valet with ROS in Simulink

Distribute an automated parking valet application among various nodes in a ROS network in Simulink®. This example extends the Automated Parking Valet (Automated Driving Toolbox) example in the Automated Driving Toolbox™. Using the Simulink model in the Automated Parking Valet in Simulink example, tune the planner, controller and vehicle dynamics parameters before partitioning the model into ROS nodes.

Prerequisite: Automated Parking Valet (Automated Driving Toolbox), Generate a Standalone ROS Node from Simulink

Introduction

A typical autonomous vehicle application has the following workflow.

This example concentrates on simulating the Planning, Control and the Vehicle components. For Localization, this example uses pre-recorded map localization data. The Planning component are further divided into Behavior planner and Path Planner components. This results in a ROS network comprised of four ROS nodes: Behavioral Planner, Path Planner, Controller and Vehicle Sim. The following figure shows the relationships between each ROS node in the network and the topics used in each.

Explore Simulink ROS Nodes and Connectivity

Observe the division of the components into four separate Simulink models. Each Simulink model represents a ROS node.

Vehicle Sim Node

1. Open the vehicle model.

open_system('ROSValetVehicleExample');

2. To simulate the vehicle controller effects and output the vehicle information, the Vehicle model subsystem uses a Bicycle Model (Automated Driving Toolbox) block and a Vehicle Body 3DOF block.

Behavioral Planner Node

1. Open the behavioral planner model.

open_system('ROSValetBehavioralPlannerExample');

2. This model reads the current vehicle information, sends the next goal and checks if the vehicle has reached the goal pose of the segment using exampleHelperROSValetGoalChecker.

3. The Behavior Planner and Goal Checker model runs when a new message is available on either /currentpose or /currentvel.

4. The model sends the status if the vehicle has reached the parking goal using the /reachgoal topic, which uses a std_msgs/Bool message. All the models stop simulation when this message is true.

Path Planner Node

1. Open the path planner model.

open_system('ROSValetPathPlannerExample');

2. This model plans a feasible path through the environment map using a pathPlannerRRT (Automated Driving Toolbox) object, which implements the optimal rapidly exploring random tree (RRT*) algorithm and sends the plan to the controller over the ROS network.

3. The Path Planner subsystem runs when a new message is available on /plannerConfig or /nextgoal topics.

Controller Node

1. Open the vehicle controller model.

open_system('ROSValetControllerExample');

2. This model calculates and sends the steering and velocity commands.

3. The Controller subsystem runs when a new message is available on the /velprofile topic.

Simulate ROS Nodes to Verify Partitioning

Model references in this example use Variant Subsystem (Simulink) to achieve easy switch between simulation and code generation. During simulation, messages pass from one node to other directly without ROS blocks to showcase the data flow and execution order among these four ROS nodes. During code generation, variant subsystem under each node will use publish and subscribe blocks to replace input and output ports. In this way, after we lock down the control algorithm during simulation, each node can be deployed as a standalone ROS node without any changes to the model.

Verify that the behavior of the model remains the same after partitioning the system into four Simuilnk models.

1. Run rosinit in MATLAB® Command Window to initialize the global node and ROS master

rosinit

Launching ROS Core...
.....Done in 5.8712 seconds.
Initializing ROS master on http://172.21.17.215:60882.
Initializing global node /matlab_global_node_77781 with NodeURI http://ah-joshchen:62108/ and MasterURI http://localhost:60882.

2. Load the pre-recorded localization map data in MATLAB® base workspace using the exampleHelperROSValetLoadLocalizationData helper function.

exampleHelperROSValetLoadLocalizationData;

3. Open the simulation model.

open_system('ROSValetSimulationExample.slx');

In the left parking selection area, you can also select a spot. The default parking spot is the sixth spot at the top row.

4. In the Simulation tab, click Run from SIMULATE section or run sim('ROSValetSimulationExample.slx') in MATLAB Command Window. A figure opens and shows how the vehicle tracks the reference path. The blue line represents the reference path while the red line is the actual path traveled by the vehicle. Simulation for all the models stop when the vehicle reaches the final parking spot.

sim('ROSValetSimulationExample.slx');

Simulation Results

The Visualization subsystem in the vehicle model generates the results for this example.

open_system('ROSValetVehicleExample/Vehicle model/Visualization');

The visualizePath block is responsible for creating and updating the plot of the vehicle paths shown previously. The vehicle speed and steering commands are displayed in a scope.

open_system("ROSValetVehicleExample/Vehicle model/Visualization/Commands")

Deploy ROS Nodes

Generate ROS applications for Behavioral planner, Path planner, Controller nodes, and simulate the Vehicle node in MATLAB and compare the results with simulation. For more informaiton on generating ROS notes, see Generate a Standalone ROS Node from Simulink.

1. Set VEHICLE_MODE to switch variant subsystem in Vehicle node for deployment.

VEHICLE_MODE=1;

2. Deploy the Behavioral planner, Path planner and Controller ROS nodes.

3. Open the vehicle model and set

open_system('ROSValetVehicleExample');

4. From the Simulation tab, click Run to start the simulation.

5. Observe the vehicle movement on the plot and compare the results from simulation run.

6. Shut down the ROS network using rosshutdown.

rosshutdown

Shutting down global node /matlab_global_node_77781 with NodeURI http://ah-joshchen:62108/ and MasterURI http://localhost:60882.
Shutting down ROS master on http://172.21.17.215:60882.