High Level Controller Software In the Loop (SIL) Testing

High-level Controls SIL

The controls software in the loop (C-SIL) modules is designed for providing a testing environment for high-level control packages in real-time. Fundamentally, the C-SIL contains a discrete real-time model of the steering and vehicle kinematics on the backend while taking inputs from the high-level controller. The state of the vehicle will update accordingly with the control input. Thus, providing a virtual SIL platform for integration testing of the high-level controller.

Source Code

The source directory of the C-SIL package is under FusionAD/src/utility/control_sil.

Control SIL functionality

• Steering Servo Model (Simple Quadratic Lag)
• Kinematic Bicycle Model
• Path Publisher

Control SIL Inputs and Outputs

Input:

• /control/controlcmd
  • Message Type: interface::ControlCmd

Output:

• /localization/state
  • Message Type: interface::Chassis_state
• /planning/trajectory
  • Message Type: nav_msgs/Path

Control SIL Setting Parameters

• Simulation Time Steps
• Vehicle Wheelbase
• Vehicle initial positions and heading

How to run the C-SIL package with high-level control

1. Build FusionAD with catkin_make and source the executables by source devel/setup.bash
   (Both commands ran at the root directory of the FusionAD software stack)
2. Run roslaunch control pure_high_lvl_control.launch in one of your terminal window pane to launch the high-level control stack
3. Run this command in another terminal window pane:
   roslaunch control_sil control_sil_test.launch
4. The Rviz visualizer should pop up and you shall see two major visual cues in your visualizer's space, a red arrow, and a long green line.
   • The red arrow's position and orientation represents the car's position and heading. The green line represents the path.
   • See:
     
5. As usual, sending a true signal or message to the topic: /control/mode will initiate the controller to enter into autonomous mode and you shall see the vehicle (red arrow) move along the path, which shows the path tracker working.
   • Sending a true signal or message: rostopic pub /control/mode std_msgs/Bool true

Sidenote

• This SIL module involves a lot of number crunching in real-time, especially if the time step parameter is set to north of 1000 Hz. You might experience a slow down in the updating rate of the SIL node if you set the time step to be too small. I have experimentally tried to set the SIL system to run at 10000 Hz but due to limitation in computing power, the node only ran in between 1800 to 1900 Hz (unstable rate). For optimal and stable result, please keep the time step at 0.001 seconds (1000 Hz).
• This SIL module is intended to validate high-level controller algorithm. It does not serve the purpose of tuning or reflecting perfectly accurate vehicle kinematics and dynamics. Please do not use this module for controller tuning purpose.

Software Changelog

• Version 1.0
  • Introduction of the C-SIL module. Features include:
    • Discrete real-time Steering Model
    • Discrete real-time vehicle kinematics model
    • Hardcoded path publisher

Maintainer

Menson Li
Project Lead
Planner and Controls Lead
FusionAD
San Jose State University
Mechanical Engineering 2019

Common Configuration Settings

For atm_cv2_path.txt

<!-- Launch file for the control software in the loop testing node -->


<launch>
  <!-- type-> executable , pkg->package, name-> node name-->
  <node type="control_sil_sim" pkg="control_sil" name="control_sil_sim" output="screen" >
    <!-- Please note that 0.001 is the optimal value for simulation time step, smaller time step may cause system instability and slowness -->
    <!-- Time is in seconds, wheelbase and positions are in meters, heading is in radians-->
    <param name="simulation_time_step" value="0.001"/>
    <param name="vehicle_wheelbase" value="2.3622"/>
    <param name="initial_x_position" value="147"/>
    <param name="initial_y_position" value="247"/>
    <param name="initial_vehicle_heading" value="-1.0"/>
    <param name="use_sil_hardcoded_planner" value="false"/>
  </node>

  <node type="rviz" name="rviz" pkg="rviz" args="-d $(find control_sil)/rviz/control_sil_rviz.rviz" />
</launch>