Design and create environment for testing and tuning closed-loop behavior
Description
The work on #296 (closed) and #208 (closed) and the associated MRs !175 (closed) and !252 (merged) has shown that it is at the current stage rather tricky to identify what the problems are with operating our control system in closed loop.
In my opinion, this has to do with there being too many uncertain and (at the current stage) unwieldy components in the loop required to run a test: LGSVL, the ROS2 bridge, ROS2 itself and finally the actual components doing control (currently only really the MPC, but this will become more complicated once we no longer just replay a set trajectory). This leads to the following complicating aspects:
- There is model mismatch between the controller and the actual vehicle.
- There is computation delay (may be negligible for MPC and slow-moving vehicles) and potentially also a variable delay due to the bridge or the simulator not running smoothly yet
- There is presumably noise on the state measurement
- There are potentially other issues we don't know about yet: Unit conversions, sign conventions, offsets.
The control architecture has to be tuned and designed to take all of that into account, but doing everything at once is difficult. Additionally, it seems that right now it's difficult to impossible to automate reproducible experiments. A methodical approach to building such a complex control loop would be to start from a perfect situation - a simulation with no model mismatch, no delays and no noise. Once the control architecture is shown to work properly there, complexity is increased one step at a time, for example by adding delay, then noise, then model mismatch and fixing issues by tuning or adapting the controller as they arise.
While this approach may sound over-engineered for the problem at hand, it will also be useful at a later stage for tuning and debugging more complicated control loops.
Expected behavior
This issue is to discuss the format of, then create, facilities to allow for the methodical approach described above to be taken. The idea is to be maximally helpful with a minimal set of tools, if that is possible - or decide that taking this approach is unnecessary for one reason or the other and propose a better solution instead.
What should be made possible by the developed environment is the following:
- Simulations with perfect situation (no noise, no delay, no model mismatch)
- Automatic verification that the closed loop behaves as expected
- Reproducible, easy-to-conduct experiments
In the spirit of reinventing the weel as little as possible, we could for example implement all this as a ROS2 node that publishes any and all relevant data on topics so RViz can be used and rosbags can be recorded. The ROS2 node would then implement a bare-bones simulation loop that interfaces with two configurable interfaces of the kind controlInput = computeControlInput(currentState)
and nextState = integrateDynamics(currentState)
in some way. I assume the ROS way of doing this would be to have separate nodes for these things - what would be important is that they could be operated in a blocking way to enforce determinism though.
Definition of Done
-
Discuss and agree on a format and set of features for the environment -
Implement that environment -
General simulation environment with interfaces for controller and dynamics -
Document design concept in a design/
folder -
Add tests to motion_model_testing_simulator
andcontroller_testing
-
Integration for MPC controller and kinematic bicycle model via ROS -
Turn the code into a proper ROS2 package -
Add a package.xml file -
Make the simulator into a node (with the kinematic bicycle model already built in) -
Add a ControlSimulationInterface
node that talks to the MPC controller -
Test and debug the simulator with MPC in the loop (see #408 (comment 335742026) for more specific notes)This is part of #208 (closed) and out-of-scope for this issue
-
-
Make the functionality accessible to developers -
Create a launch file for this simulation -
Create some example code for running a simple MPC-based simulation
-
-
-