Code Generation for Path Planning and Vehicle Control

This example uses:

This example shows how to modify a Simulink® model of a path planning and vehicle control algorithm, generate C++ code, and verify the generated code using software-in-the-loop (SIL) simulation.

Introduction

Developing a path planning and vehicle control algorithm often involves designing and simulating an algorithm model in Simulink, implementing the algorithm in C++ code, and integrating the algorithm code into an external software environment for deployment into a vehicle. Automatically generating and verifying code from the algorithm model ensures functional equivalence between the simulation and implementation.

The Automated Parking Valet in Simulink example showed how to design a path planning and vehicle control algorithm. This example shows how to modify the design for implementation in C++. This steps in this workflow are:

Partition the design into algorithm and test bench models.
Modify the algorithm model to support code generation.
Generate C++ code from the algorithm model.
Verify the behavior of the generated code using SIL simulation.

You can then integrate the generated code into an external software project for further testing in a vehicle.

Partition the Algorithm and Test Bench

The original model from the Automated Parking Valet in Simulink example has already been partitioned into separate algorithm and test bench models.

Algorithm Model: AutomatedParkingValetAlgorithm specifies the path planning and vehicle control functionality to be implemented in C++.
Test Bench Model: AutomatedParkingValetTestBench specifies the stimulus and environment to test the algorithm model.

Simulate Test Bench Model

The AutomatedParkingValetTestBench model specifies the stimulus and environment to test the AutomatedParkingValetAlgorithm model. The main components of the AutomatedParkingValetTestBench include:

Algorithm Model Reference: The AutomatedParkingValetAlgorithm model block is referenced by a Model block. The Model block and supports simulating the referenced model in different modes of simulation including normal and SIL modes. To learn more about the Model block, refer to Reference Existing Models (Simulink).
Costmap: The Costmap Creator block creates the costmap of the environment and outputs it as a bus signal.
Behavior Planner: The Behavior Planner block triggers a sequence of navigation tasks based on the global route plan by providing an intermediate goal and configuration.
Vehicle Model: To demonstrate the performance of the algorithm, the parking valet controller is applied to the Vehicle Model block, which contains a Vehicle Body 3DOF block.

The AutomatedParkingValetTestBench model is also configured to log the pose(CurrPose) and longitudinal velocity (CurrVelocity) of the vehicle and the status of whether the goal from the behavioral planner was reached (GoalReached). These signals are logged to the workspace variable logsout.

Simulate the test bench model with the algorithm in normal mode.

open_system('AutomatedParkingValetTestBench')
snapnow
set_param('AutomatedParkingValetTestBench/AutomatedParkingValetAlgorithm','SimulationMode','Normal');
sim('AutomatedParkingValetTestBench')
helperPlotSimulationSignals(logsout)
snapnow

The first figure displays the path that the vehicle traversed from the parking lot input to the final parking space. The second figure plots the velocity and goal-reached signals. Notice that the vehicle velocity is smooth and continuous when transitioning between goals.

Modify Algorithm Model to Support Code Generation

The AutomatedParkingValetAlgorithm model specifies the functionality to be implemented in C++. The main components of the AutomatedParkingValetAlgorithm model are:

Path Planner: Plans a feasible path through the environment map using a pathPlannerRRT object.
Trajectory Generator: Smooths the reference path by fitting splines and converts the smoothed path into a trajectory by generating a speed profile.
Vehicle Controller: Controls the steering and velocity of the vehicle to follow the generated path and speed profile.

Open and update the algorithm model.

open_system('AutomatedParkingValetAlgorithm')
set_param('AutomatedParkingValetAlgorithm','SimulationCommand','Update');

The AutomatedValetParking model includes several modifications from the Automated Parking Valet in Simulink example to support code generation. The most significant modifications are specifying fixed-size component interfaces and explicit rate transitions.

Variable-size component interfaces have been replaced with fixed-size interface to enable generating and verifying C++ code with SIL simulation.

The variable-size Poses signal has been split into a fixed-size outport (RefPosesArray) with an additional outport specifying the size (RefPosesSize).
The costmapBus bus associated with the Costmap input port contains only fixed-size elements, since the costmap does not change size in this example.

The AutomatedValetParking model contains multiple rates. The color of the blocks represents different sample times. Path planning and trajectory generation is performed at a 0.1s sample time and is colored green. Vehicle control is performed at a 0.05s sample time and is colored red. To learn more about displaying sample time colors, refer to View Sample Time Information (Simulink).

Explicit rate transition blocks have been inserted into the model to treat each rate as a separate task.

A Rate Transition block has been inserted to the fixed-size CurrPose signal.
A helper Varsize Rows Rate Transition block (named RT) has been inserted to variable-size signals that connect blocks of different rates.

Treating each rate as a specific task enables generating a C++ class with separate method entry points for each rate. Generating separate methods for each rate simplifies integration into multi-tasking software schedulers or operating systems in the vehicle. To learn more about treating rates as separate tasks, refer to Modeling for Multitasking Execution (Simulink).

Configure and Generate Code from Algorithm Model

Configuring the AutomatedParkingValetAlgorithm model to generate code includes setting parameters to:

Generate C++ code with entry points for each rate.
Apply common optimizations.
Generate a report to facilitate exploring the generated code.

Set and view model parameters to enable C++ code generation.

helperSetModelParametersForCodeGeneration('AutomatedParkingValetAlgorithm')

 
Set AutomatedParkingValetAlgorithm configuration parameters:
 
               Parameter                    Value                                                              Description                                                      
    _______________________________    _______________    ______________________________________________________________________________________________________________________

    {'SystemTargetFile'           }    {'ert.tlc'    }    {'Code Generation>System target file'                                                                                }
    {'TargetLang'                 }    {'C'          }    {'Code Generation>Language'                                                                                          }
    {'SolverType'                 }    {'Fixed-step' }    {'Solver>Type'                                                                                                       }
    {'FixedStep'                  }    {'auto'       }    {'Solver>Fixed-step size (fundamental sample time)'                                                                  }
    {'EnableMultiTasking'         }    {'on'         }    {'Solver>Treat each discrete rate as a separate task'                                                                }
    {'ProdLongLongMode'           }    {'on'         }    {'Hardware Implementation>Support long long'                                                                         }
    {'BlockReduction'             }    {'on'         }    {'Simulation Target>Block reduction'                                                                                 }
    {'MATLABDynamicMemAlloc'      }    {'on'         }    {'Simulation Target>Simulation Target>Dynamic memory allocation in MATLAB functions'                                 }
    {'OptimizeBlockIOStorage'     }    {'on'         }    {'Simulation Target>Signal storage reuse'                                                                            }
    {'InlineInvariantSignals'     }    {'on'         }    {'Simulation Target>Inline invariant signals'                                                                        }
    {'BuildConfiguration'         }    {'Faster Runs'}    {'Code Generation>Build configuration'                                                                               }
    {'RTWVerbose'                 }    {'of'         }    {'Code Generation>Verbose build'                                                                                     }
    {'CombineSignalStateStructs'  }    {'on'         }    {'Code Generation>Interface>Combine signal/state structures'                                                         }
    {'SupportVariableSizeSignals' }    {'on'         }    {'Code Generation>Interface>Support variable-size signals'                                                           }
    {'EfficientFloat2IntCast'     }    {'on'         }    {'Code Generation>Optimization>Remove code from floating-point to integer conversions that wraps out-of-range values'}
    {'ZeroExternalMemoryAtStartup'}    {'off'        }    {'Code Generation>Optimization>Remove root level I/O zero initialization (inverse logic)'                            }
    {'CustomSymbolStrGlobalVar'   }    {'$N$M'       }    {'Code Generation>Symbols>Global variables'                                                                          }
    {'CustomSymbolStrType'        }    {'$N$M_T'     }    {'Code Generation>Symbols>Global types'                                                                              }
    {'CustomSymbolStrField'       }    {'$N$M'       }    {'Code Generation>Symbols>Field name of global types'                                                                }
    {'CustomSymbolStrFcn'         }    {'APV_$N$M$F' }    {'Code Generation>Symbols>Subsystem methods'                                                                         }
    {'CustomSymbolStrTmpVar'      }    {'$N$M'       }    {'Code Generation>Symbols>Local temporary variables'                                                                 }
    {'CustomSymbolStrMacro'       }    {'$N$M'       }    {'Code Generation>Symbols>Constant macros'                                                                           }

Generate code and the code generation report from the algorithm model.

slbuild('AutomatedParkingValetAlgorithm');

### Starting build procedure for: AutomatedParkingValetAlgorithm
### Successful completion of build procedure for: AutomatedParkingValetAlgorithm

Build Summary

Top model targets:

Model                           Build Reason                                         Status                        Build Duration
=================================================================================================================================
AutomatedParkingValetAlgorithm  Information cache folder or artifacts were missing.  Code generated and compiled.  0h 2m 48.548s 

1 of 1 models built (0 models already up to date)
Build duration: 0h 2m 55.111s

Use the Code Generation Report to explore the generated code. To learn more about the Code Generation Report, refer to Reports for Code Generation (Simulink Coder). Use the Code Interface Report link in the Code Generation Report to explore these generated methods:

initialize: Call once on initialization.
step0: Call periodically every 0.05s to execute trajectory generation and vehicle control.
step1: Call periodically every 0.1s seconds to execute path planning.
terminate: Call once on termination.

Additional get and set methods for signal interface are declared in AutomatedParkingValetAlgorithm.h and defined in AutomatedParkingValetAlgorithm.c.

Verify Implementation with SIL Simulation

Software-in-the-loop (SIL) simulation provides early insight into the behavior of a deployed application. To learn more about SIL simulation, refer to SIL and PIL Simulations (Embedded Coder).

SIL simulation enables you to: * Verify that the compiled generated code on the host is functionally equivalent to the normal mode. * Log execution times of generated code on the host computer. These times can be an early indicator of performance of the generated code. For accurate execution time measurements, profile the generated code when it is integrated into the external environment or when using with processor-in-the-loop(PIL) simulation. To learn more about SIL profiling, refer to Create Execution-Time Profile for Generated Code (Embedded Coder).

Configure algorithm and test bench model parameters to support SIL simulation and log execution profiling information.

helperSetModelParametersForSIL('AutomatedParkingValetAlgorithm');
helperSetModelParametersForSIL('AutomatedParkingValetTestBench');

 
Set AutomatedParkingValetAlgorithm configuration parameters:
 
               Parameter                       Value                                    Description                         
    ________________________________    ____________________    ____________________________________________________________

    {'SystemTargetFile'            }    {'ert.tlc'         }    {'Code Generation>System target file'                      }
    {'TargetLang'                  }    {'C'               }    {'Code Generation>Language'                                }
    {'CodeExecutionProfiling'      }    {'on'              }    {'Code Generation>Verification>Measure task execution time'}
    {'CodeProfilingSaveOptions'    }    {'AllData'         }    {'Code Generation>Verification>Save options'               }
    {'CodeExecutionProfileVariable'}    {'executionProfile'}    {'Code Generation>Verification>Workspace variable'         }

 
Set AutomatedParkingValetTestBench configuration parameters:
 
               Parameter                       Value                                    Description                         
    ________________________________    ____________________    ____________________________________________________________

    {'SystemTargetFile'            }    {'ert.tlc'         }    {'Code Generation>System target file'                      }
    {'TargetLang'                  }    {'C'               }    {'Code Generation>Language'                                }
    {'CodeExecutionProfiling'      }    {'on'              }    {'Code Generation>Verification>Measure task execution time'}
    {'CodeProfilingSaveOptions'    }    {'AllData'         }    {'Code Generation>Verification>Save options'               }
    {'CodeExecutionProfileVariable'}    {'executionProfile'}    {'Code Generation>Verification>Workspace variable'         }

Simulate the test bench model with the algorithm in SIL mode and plot the results.

open_system('AutomatedParkingValetTestBench')
set_param('AutomatedParkingValetTestBench/AutomatedParkingValetAlgorithm','SimulationMode','Software-in-the-loop (SIL)');
save_system('AutomatedParkingValetAlgorithm');
sim('AutomatedParkingValetTestBench');

### Searching for referenced models in model 'AutomatedParkingValetTestBench'.
### Found 1 model reference targets to update.
### Starting build procedure for: AutomatedParkingValetAlgorithm
### Generated code for 'AutomatedParkingValetAlgorithm' is up to date because no structural, parameter or code replacement library changes were found.
### Successful completion of build procedure for: AutomatedParkingValetAlgorithm

Build Summary

0 of 1 models built (1 models already up to date)
Build duration: 0h 0m 4.6073s
### Preparing to start SIL simulation ...
Building with 'Microsoft Visual C++ 2019 (C)'.
MEX completed successfully.
### Updating code generation report with SIL files ...
### Starting SIL simulation for component: AutomatedParkingValetAlgorithm
### Application stopped
### Stopping SIL simulation for component: AutomatedParkingValetAlgorithm

helperPlotSimulationSignals(logsout, executionProfile)
snapnow

The execution time for the step0 and step1 methods are shown in the lower plot. The plots indicate that the maximum execution time is required at the lower rate (step1) after a goal pose is achieved. This lower rate is expected because it corresponds to the time when a new path is planned.

Conclusion

This example demonstrated a workflow to generate and verify C++ code for a path planner and vehicle control algorithm. Compiling and verifying the code with SIL simulation established confidence that the generated code is functionally correct before integrating into an external software environment. The workflow was demonstrated as an extension of the Automated Parking Valet in Simulink example and is generally applicable to designing and implementing path planning applications.