Automated Parking Valet with ROS 2 in Simulink
This example shows how to distribute the Automated Parking Valet application among various nodes in a ROS 2 network in Simulink® and deploy them as standalone ROS 2 nodes. 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 dynamic parameters before partitioning the model into ROS 2 nodes.
Prerequisites: Automated Parking Valet (Automated Driving Toolbox), Generate a Standalone ROS 2 Node from Simulink®
Introduction
This autonomous vehicle application has the following components.
This example concentrates on simulating the Planning, Control and the Vehicle components. For Localization, this example uses pre-recorded localization map data. The Planning component is further divided into Behavior planner and Path Planner components. This results in a ROS 2 Network comprised of four ROS 2 nodes: Behavioral Planner, Path Planner, Controller and Vehicle. The following figure shows the relationships between each ROS 2 node in the network and the topics used in each.
Explore the Simulink ROS 2 nodes and connectivity
Observe the division of the components into four separate Simulink models. Each Simulink model represents a ROS 2 node.
Vehicle Node
1. Open the vehicle model.
open_system('ROS2ValetVehicleExample'); 2. The Subscribe subsystem contains the ROS 2 Subscribe blocks that read input data from the Controller node.
3. The Vehicle model subsystem contains a Bicycle Model block, Vehicle Body 3DOF, to simulate the vehicle controller effects and sends the vehicle information over ROS 2 network through ROS 2 Publish blocks in the Publish subsystem.
Behavioral Planner Node
1. Open the behavioral planner model.
open_system('ROS2ValetBehavioralPlannerExample'); 2. This model reads the current vehicle information from ROS 2 network, sends the next goal and checks if the vehicle has reached the final pose of the segment using rosAutomatedValetHelperGoalChecker.
3. The Behavioral Planner and Goal Checker subsystem runs when a new message is available on either /currentvel or /currentpose.
4. The model sends the status if the vehicle has reached the final parking goal using /reachgoal topic, which uses a std_msgs/Bool message type. All the models stop simulation when this message is true.
Path Planner Node
1. Open the path planner model.
open_system('ROS2ValetPathPlannerExample'); 2. This model plans a feasible path through the environment map using a pathPlannerRRT object, which implements the optimal rapidly exploring random tree (RRT*) algorithm and sends the plan to the controller over the ROS 2 network.
Controller Node
1. Open the vehicle controller model.
open_system('ROS2ValetControllerExample');2. This model calculates and sends the steering and velocity commands over ROS 2 network.
Simulate the ROS 2 nodes to verify partitioning
Verify that the behavior of the model remains the same after partitioning the system into four ROS 2 nodes.
1. Load the pre-recorded localization map data in MATLAB base workspace using exampleHelperROSValetLoadLocalizationData helper function.
exampleHelperROSValetLoadLocalizationData;
2. Open all the models and start simulation using exampleHelperROS2ValetStartSimulation helper function. 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.
exampleHelperROS2ValetStartSimulation
Simulation Results
The Visualization subsystem in vehicle model generates the results for this example.
open_system('ROS2ValetVehicleExample/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("ROS2ValetVehicleExample/Vehicle model/Visualization/Commands")
Deploy ROS 2 Nodes
Generate ROS 2 applications for Behavioral Planner, Path planner and Controller nodes. Simulate the Vehicle node in MATLAB and compare the results with simulation.
Generate and deploy Behavioral Planner, Path Planner and Controller node applications using exampleHelperROS2ValetDeployNodes helper function. The helper function calls slbuild command with the name of the Simulink model as input argument, for each model, to generate C++ code and deploy the application on the host computer.
exampleHelperROS2ValetDeployNodes(); % generate C++ code and deploy the application for ROS 2 nodes### Starting build procedure for model: ROS2ValetBehavioralPlannerExample ### Generating code and artifacts to 'Model specific' folder structure ### Generating code into build folder: H:\Documents\MATLAB\Examples\ros-ex88924338\ROS2ValetBehavioralPlannerExample_ert_rtw ### Invoking Target Language Compiler on ROS2ValetBehavioralPlannerExample.rtw ### Using System Target File: W:\21\agadkari.Bdoc20a.j1181109.Examples\matlab\rtw\c\ert\ert.tlc ### Loading TLC function libraries ........ ### Initial pass through model to cache user defined code ... ### Caching model source code ........................................................................... ### Writing header file ROS2ValetBehavioralPlannerExample_types.h ### Writing source file ROS2ValetBehavioralPlannerExample.cpp ### Writing header file ROS2ValetBehavioralPlannerExample_private.h ### Writing header file ROS2ValetBehavioralPlannerExample.h ### Writing header file rtwtypes.h . ### Writing header file rtGetNaN.h ### Writing source file rtGetNaN.cpp ### Writing header file rt_nonfinite.h ### Writing source file rt_nonfinite.cpp ### Writing header file rtGetInf.h . ### Writing source file rtGetInf.cpp ### Writing header file rtmodel.h ### Writing source file ROS2ValetBehavioralPlannerExample_data.cpp ### Writing source file ert_main.cpp ### TLC code generation complete. .### Evaluating PostCodeGenCommand specified in the model ### Using toolchain: Colcon Tools ### Building 'ROS2ValetBehavioralPlannerExample': all Running colcon build in folder 'H:/Documents/MATLAB/Examples/ros-ex88924338'.Done. Success ### Successfully generated all binary outputs. ### Successful completion of build procedure for model: ROS2ValetBehavioralPlannerExample ### Creating HTML report file ROS2ValetBehavioralPlannerExample_codegen_rpt.html ### Simulink cache artifacts for 'ROS2ValetBehavioralPlannerExample' were created in 'H:\Documents\MATLAB\Examples\ros-ex88924338\ROS2ValetBehavioralPlannerExample.slxc'. ### Starting build procedure for model: ROS2ValetPathPlannerExample ### Generating code and artifacts to 'Model specific' folder structure ### Generating code into build folder: H:\Documents\MATLAB\Examples\ros-ex88924338\ROS2ValetPathPlannerExample_ert_rtw ### Invoking Target Language Compiler on ROS2ValetPathPlannerExample.rtw ### Using System Target File: W:\21\agadkari.Bdoc20a.j1181109.Examples\matlab\rtw\c\ert\ert.tlc ### Loading TLC function libraries ........ ### Initial pass through model to cache user defined code ...... ### Caching model source code ............................................................................... ............................................................................... ............................................................................... ............................................................................... ............................................................................... ... ### Writing header file ROS2ValetPathPlannerExample_types.h ### Writing source file ROS2ValetPathPlannerExample.cpp . ### Writing header file ROS2ValetPathPlannerExample_private.h ### Writing header file ROS2ValetPathPlannerExample.h ### Writing header file rtwtypes.h ### Writing header file rtGetInf.h ### Writing source file rtGetInf.cpp . ### Writing header file rtGetNaN.h ### Writing source file rtGetNaN.cpp ### Writing header file rt_defines.h ### Writing header file rt_nonfinite.h ### Writing source file rt_nonfinite.cpp . ### Writing source file ROS2ValetPathPlannerExample_data.cpp ### Writing header file rtmodel.h ### Writing source file ert_main.cpp ### TLC code generation complete. .### Evaluating PostCodeGenCommand specified in the model ### Using toolchain: Colcon Tools ### Building 'ROS2ValetPathPlannerExample': all Running colcon build in folder 'H:/Documents/MATLAB/Examples/ros-ex88924338'.Done. Success ### Successfully generated all binary outputs. ### Successful completion of build procedure for model: ROS2ValetPathPlannerExample ### Creating HTML report file ROS2ValetPathPlannerExample_codegen_rpt.html ### Simulink cache artifacts for 'ROS2ValetPathPlannerExample' were created in 'H:\Documents\MATLAB\Examples\ros-ex88924338\ROS2ValetPathPlannerExample.slxc'. ### Starting build procedure for model: ROS2ValetControllerExample ### Generating code and artifacts to 'Model specific' folder structure ### Generating code into build folder: H:\Documents\MATLAB\Examples\ros-ex88924338\ROS2ValetControllerExample_ert_rtw ### Invoking Target Language Compiler on ROS2ValetControllerExample.rtw ### Using System Target File: W:\21\agadkari.Bdoc20a.j1181109.Examples\matlab\rtw\c\ert\ert.tlc ### Loading TLC function libraries ....... ### Initial pass through model to cache user defined code ..... ### Caching model source code ............................................................................... ..................................................................... ### Writing header file ROS2ValetControllerExample_types.h ### Writing source file ROS2ValetControllerExample.cpp ### Writing header file ROS2ValetControllerExample_private.h ### Writing header file ROS2ValetControllerExample.h . ### Writing header file rtwtypes.h ### Writing header file rtGetNaN.h ### Writing source file rtGetNaN.cpp ### Writing header file rt_nonfinite.h ### Writing source file rt_nonfinite.cpp ### Writing header file rt_assert.h . ### Writing header file rtGetInf.h ### Writing source file rtGetInf.cpp ### Writing header file rtmodel.h ### Writing source file ROS2ValetControllerExample_data.cpp ### Writing source file ert_main.cpp . ### TLC code generation complete. ....### Evaluating PostCodeGenCommand specified in the model ### Using toolchain: Colcon Tools ### Building 'ROS2ValetControllerExample': all Running colcon build in folder 'H:/Documents/MATLAB/Examples/ros-ex88924338'.Done. Success ### Successfully generated all binary outputs. ### Successful completion of build procedure for model: ROS2ValetControllerExample ### Creating HTML report file ROS2ValetControllerExample_codegen_rpt.html ### Simulink cache artifacts for 'ROS2ValetControllerExample' were created in 'H:\Documents\MATLAB\Examples\ros-ex88924338\ROS2ValetControllerExample.slxc'.
Open the vehicle model and start simulation.
open_system("ROS2ValetVehicleExample"); set_param("ROS2ValetVehicleExample","SimulationCommand","start");
Verify that the results from simulation match with the deployed ROS 2 nodes.