Description
Path-following steering P-like controller with variable path prediction.
The algorithm is from: M.C. Best, "A simple realistic driver model," AVEC `12: The 11th International Symposium on Advanced Vehicle Control, 9th-12th September 2012, Seoul, Korea. The path to be followed is specified as a ChBezierCurve object and the the original definition points are extracted automatically. Open and closed course definitions can be handled. The ChBezier is still used for visualization.
#include <ChSteeringController.h>
Public Member Functions | |
ChPathSteeringControllerSR (std::shared_ptr< ChBezierCurve > path, bool isClosedPath=false, double max_wheel_turn_angle=0.0, double axle_space=2.5) | |
Construct a steering controller to track the specified path. More... | |
ChPathSteeringControllerSR (const std::string &filename, std::shared_ptr< ChBezierCurve > path, bool isClosedPath=false, double max_wheel_turn_angle=0.0, double axle_space=2.5) | |
Construct a steering controller to track the specified path. More... | |
void | SetGains (double Klat=0.1, double Kug=0.0) |
void | SetPreviewTime (double Tp=0.5) |
virtual double | Advance (const ChFrameMoving<> &ref_frame, double time, double step) override |
Advance the state of the SR controller. | |
virtual void | Reset (const ChFrameMoving<> &ref_frame) override |
Reset the PID controller. More... | |
virtual void | CalcTargetLocation () override |
Calculate the current target point location. More... | |
Public Member Functions inherited from chrono::vehicle::ChSteeringController | |
virtual | ~ChSteeringController () |
Destructor. | |
void | SetLookAheadDistance (double dist) |
Specify the look-ahead distance. More... | |
const ChVector3d & | GetSentinelLocation () const |
Return the current location of the sentinel point. More... | |
const ChVector3d & | GetTargetLocation () const |
Return the current value of the target point. More... | |
std::shared_ptr< ChBezierCurve > | GetPath () const |
Return a pointer to the Bezier curve. | |
void | StartDataCollection () |
Start/restart data collection. | |
void | StopDataCollection () |
Suspend/stop data collection. | |
bool | IsDataCollectionEnabled () const |
Return true if data is being collected. | |
bool | IsDataAvailable () const |
Return true if data is available for output. | |
void | WriteOutputFile (const std::string &filename) |
Output data collected so far to the specified file. | |
Additional Inherited Members | |
Protected Member Functions inherited from chrono::vehicle::ChSteeringController | |
ChSteeringController (std::shared_ptr< ChBezierCurve > path) | |
Construct a steering controller with default parameters. | |
Protected Attributes inherited from chrono::vehicle::ChSteeringController | |
std::shared_ptr< ChBezierCurve > | m_path |
tracked path (piecewise cubic Bezier curve) | |
double | m_dist |
look-ahead distance | |
ChVector3d | m_sentinel |
position of sentinel point in global frame | |
ChVector3d | m_target |
position of target point in global frame | |
double | m_err |
current error (signed distance to target point) | |
double | m_errd |
error derivative | |
double | m_erri |
integral of error | |
utils::ChWriterCSV * | m_csv |
ChWriterCSV object for data collection. | |
bool | m_collect |
flag indicating whether or not data is being collected | |
Constructor & Destructor Documentation
◆ ChPathSteeringControllerSR() [1/2]
chrono::vehicle::ChPathSteeringControllerSR::ChPathSteeringControllerSR | ( | std::shared_ptr< ChBezierCurve > | path, |
bool | isClosedPath = false , |
||
double | max_wheel_turn_angle = 0.0 , |
||
double | axle_space = 2.5 |
||
) |
Construct a steering controller to track the specified path.
This version uses default controller parameters (zero gains). The user is responsible for calling SetGains and SetLookAheadDistance.
◆ ChPathSteeringControllerSR() [2/2]
chrono::vehicle::ChPathSteeringControllerSR::ChPathSteeringControllerSR | ( | const std::string & | filename, |
std::shared_ptr< ChBezierCurve > | path, | ||
bool | isClosedPath = false , |
||
double | max_wheel_turn_angle = 0.0 , |
||
double | axle_space = 2.5 |
||
) |
Construct a steering controller to track the specified path.
This version reads controller gains and lookahead distance from the specified JSON file.
Member Function Documentation
◆ CalcTargetLocation()
|
inlineoverridevirtual |
Calculate the current target point location.
The target point is the point on the associated path that is closest to the current location of the sentinel point.
Implements chrono::vehicle::ChSteeringController.
◆ Reset()
|
overridevirtual |
Reset the PID controller.
This function resets the underlying path tracker using the current location of the sentinel point.
Reimplemented from chrono::vehicle::ChSteeringController.
The documentation for this class was generated from the following files:
- /builds/uwsbel/chrono/src/chrono_vehicle/utils/ChSteeringController.h
- /builds/uwsbel/chrono/src/chrono_vehicle/utils/ChSteeringController.cpp