Catalogue of series articles
Tip: you can add the directories of all articles in the series here. You need to add the directories manually
TODO: finishing after writing
preface
Limited cognition, I hope you will forgive me. If you have any problems, I hope to communicate with you and grow together!This paper first makes a brief introduction to the simple finite state machine design method. The specific content will be more later. Other modules can refer to my other articles
Tip: the following is the main content of this article
This paper takes fast as an example_ Planning is abstracted from the open source project as an example
1, Step 1: think clearly about all States and transition conditions of the system and draw a state transition diagram
Example (fast_planning)
Drawing a state transition diagram can clarify all States and transition conditions and check the rationality. For example, if there is no state, it will not come out, resulting in the system jamming [anti-theft mark - box king hzj]
The most important thing to draw a state transition diagram is to know more clearly whether the state machine matches the business and check the rationality [anti-theft mark - box hzj]
2, Design of enumerating States and variables of Finite State Machine FSM
The general definition of the variable state is in the class corresponding to. h, and the logic part of the state machine implementation is written in. cpp
[anti theft mark – box hzj]
class KinoReplanFSM {
private:
/* ---------- FSM flag ---------- */
enum FSM_EXEC_STATE { INIT, WAIT_TARGET, GEN_NEW_TRAJ, REPLAN_TRAJ, EXEC_TRAJ, REPLAN_NEW };
FSM_EXEC_STATE exec_state_;
/*--- fsm functions ---*/
void execFSMCallback(const ros::TimerEvent& e);
void changeFSMExecState(FSM_EXEC_STATE new_state, string pos_call);
void printFSMExecState();
/*--- other functions ---*/
//Implementation logic function in a state of state machine
/* ---other flag --- */
//Flag bits of other modules
/* ---other parameters--- */
//Parameters of other modules
/* ---other data--- */
//Data from other modules
/* ---other modular API--- */
//API or proxy of other modules
public:
KinoReplanFSM(/* args */) { //FSM constructor
}
~KinoReplanFSM() { //FSM destructor
}
void init(ros::NodeHandle& nh);//FSM initialization function
}
.
.
3, Design initialization function of state machine
This function generally sets a series of parameters, instantiates a series of class singletons of module algorithms, initializes a series of system components, and so on. The specific steps are implemented in the call of these components and the callback function / timer function. [anti theft mark – box hzj]
void KinoReplanFSM::init(ros::NodeHandle& nh) {
/* Some flag States of other module states of the system */
/* initialize main modules */
/* fsm param */
/* callback of topic, service and timer */
}
.
.
4, Design state transition function
The function of the state transition function is responsible for the state transition of the finite state, i.e. FSM, when the conditions are met. It is best to print out the log [anti theft mark - box hzj] every time the state transition
//State transition function
void FSM::changeFSMExecState(FSM_EXEC_STATE new_state, string pos_call) {
string state_str[5] = { "INIT", "WAIT_TARGET", "GEN_NEW_TRAJ", "REPLAN_TRAJ", "EXEC_TRAJ" };
int pre_s = int(exec_state_);
exec_state_ = new_state;
cout << "[" + pos_call + "]: from " + state_str[pre_s] + " to " + state_str[int(new_state)] << endl;
}
.
.
5, Design a function to check the state being executed by the State Machine FSM (regular call for debug ging)
void FSM::printFSMExecState() {
string state_str[5] = { "INIT", "WAIT_TARGET", "GEN_NEW_TRAJ", "REPLAN_TRAJ", "EXEC_TRAJ" };
cout << "[FSM]: state: " + state_str[int(exec_state_)] << endl;
}
.
.
6, [important] design the timer callback function to execute the State Machine FSM
Note: this function must be called in a timed loop. It is best to open a separate thread to it without interruption to improve stability. However, it is also possible to implement it in a timed loop or main's dead loop [anti theft flag - hzj]
void FSM::execFSMCallback(const ros::TimerEvent& e) {
//(1) Set the running frequency of the state machine, check the status of other modules called by FSM, and print the current execution status regularly
static int fsm_num = 0;
fsm_num++;
if (fsm_num == 100) {
printFSMExecState();
if (!have_odom_) cout << "no odom." << endl;
if (!trigger_) cout << "wait for goal." << endl;
fsm_num = 0;
}
//(2) According to the execution state variable exec_state_ Enter the switch loop for state transition
switch (exec_state_) {
case INIT: {
/*(1)Run the algorithm logic function related to this state*/
//Note that the running time of algorithm logic function function shall not exceed the running cycle of FSM,
//Even the algorithm logic function cannot have dead loops
//Otherwise, if the current state is not finished, start the next operation, and there will be occasional bug s in the system
/*(2)Judge the state transition conditions of the Finite State Machine FSM, and conduct state transition if they meet the requirements*/
if(XXX1){
changeFSMExecState(XXX1, "FSM");
}
else if(XXX2){
changeFSMExecState(XXX2, "FSM");
}
else{
changeFSMExecState(XXX3, "FSM");
}
/*(3)Jump out of this state*/
break;
}
case WAIT_TARGET: {
/*(1)Run the algorithm logic function related to this state*/
//Note that the running time of algorithm logic function function shall not exceed the running cycle of FSM,
//Even the algorithm logic function cannot have dead loops
//Otherwise, if the current state is not finished, start the next operation, and there will be occasional bug s in the system
/*(2)Judge the state transition conditions of the Finite State Machine FSM, and conduct state transition if they meet the requirements*/
if(XXX1){
changeFSMExecState(XXX1, "FSM");
}
else if(XXX2){
changeFSMExecState(XXX2, "FSM");
}
else{
changeFSMExecState(XXX3, "FSM");
}
/*(3)Jump out of this state*/
break;
}
case GEN_NEW_TRAJ: {
/*(1)Run the algorithm logic function related to this state*/
//Note that the running time of algorithm logic function function shall not exceed the running cycle of FSM,
//Even the algorithm logic function cannot have dead loops
//Otherwise, if the current state is not finished, start the next operation, and there will be occasional bug s in the system
/*(2)Judge the state transition conditions of the Finite State Machine FSM, and conduct state transition if they meet the requirements*/
if(XXX1){
changeFSMExecState(XXX1, "FSM");
}
else if(XXX2){
changeFSMExecState(XXX2, "FSM");
}
else{
changeFSMExecState(XXX3, "FSM");
}
/*(3)Jump out of this state*/
break;
}
case EXEC_TRAJ: {
/*(1)Run the algorithm logic function related to this state*/
//Note that the running time of algorithm logic function function shall not exceed the running cycle of FSM,
//Even the algorithm logic function cannot have dead loops
//Otherwise, if the current state is not finished, start the next operation, and there will be occasional bug s in the system
/*(2)Judge the state transition conditions of the Finite State Machine FSM, and conduct state transition if they meet the requirements*/
if(XXX1){
changeFSMExecState(XXX1, "FSM");
}
else if(XXX2){
changeFSMExecState(XXX2, "FSM");
}
else{
changeFSMExecState(XXX3, "FSM");
}
/*(3)Jump out of this state*/
break;
}
case REPLAN_TRAJ: {
/*(1)Run the algorithm logic function related to this state*/
//Note that the running time of algorithm logic function function shall not exceed the running cycle of FSM,
//Even the algorithm logic function cannot have dead loops
//Otherwise, if the current state is not finished, start the next operation, and there will be occasional bug s in the system
/*(2)Judge the state transition conditions of the Finite State Machine FSM, and conduct state transition if they meet the requirements*/
if(XXX1){
changeFSMExecState(XXX1, "FSM");
}
else if(XXX2){
changeFSMExecState(XXX2, "FSM");
}
else{
changeFSMExecState(XXX3, "FSM");
}
/*(3)Jump out of this state*/
break;
}
}
}
summary
This is a relatively simple basic implementation framework of finite state machine. Specifically, it designs the state and state implementation according to its own business, or expands the state diagram