autoware 1 op_behavior_selector行为选择器状态机代码分析
/这里是整个状态机运行时的结构：/
//停止状态:[#停止状态]
//任务完成状态:[#任务完成状态]
//转向状态:[*前进状态,#转向状态]
//停止信号停止状态:[*停止信号等待状态,#停止信号停止状态]
//前进状态 :[*目标状态, #前进状态, 转向状态, 停止信号停止状态, 交通灯停止状态, 跟随状态, ]
//目标状态 :[*任务完成状态,#目标状态, 前进状态]
//停止信号等待状态:[*前进状态,#停止信号等待状态,停止信号停止状态, 目标状态]
//交通灯停止状态:[*前进状态, #交通灯停止状态, 交通灯等待状态]
//交通灯等待状态:[*前进状态, #交通灯等待状态, 交通灯停止状态, 目标状态]
//跟随状态:[*前进状态, #跟随状态, 转向状态, 停止信号停止状态, 交通灯停止状态, 目标状态]
//初始状态:[*前进状态, #初始状态]
/*
有几个问题
1、等待状态 没有用到的地方
2、停止状态 除了初始化作用之外，其他没有用到的地方
3、任务完成状态 只有 目标状态能切换到位，但是，一旦切入就切换不出去了。这块儿，会不会导致状态机停滞？？？？？待查
待深入
*/
上码（做了部分注释）
//-------------------------------------------------------------------------------------------------
/*
/r2ware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector
op_behavior_selector.cpp
*/
/*
如果 新位置 有全局路径
如果新灯状态
如果新信号灯
检测灯设为当前灯
更新 检测灯状态
*/
//当前行为 设为=刷新行为（间隔时间，当前位置，车辆状态，1， 检测灯，轨迹最佳代价，0，m_loop)
m_CurrentBehavior = m_BehaviorGenerator.DoOneStep(dt, m_CurrentPos, m_VehicleStatus, 1, m_PrevTrafficLight, m_TrajectoryBestCost, 0 , m_Loop);
//void BehaviorGen::MainLoop()中循环判断执行：
if(bNewCurrentPos && m_GlobalPaths.size()>0)
{
if(bNewLightStatus)
{
if(bNewLightSignal)
{
m_PrevTrafficLight = m_CurrTrafficLight;
bNewLightSignal = false;
}
bNewLightStatus = false;
// std::cout << "m_PrevTrafficLight.size(): " << m_PrevTrafficLight.size() << std::endl;
for(unsigned int itls = 0 ; itls < m_PrevTrafficLight.size() ; itls++){
m_PrevTrafficLight.at(itls).lightState = m_CurrLightStatus;
// std::cout << "itls: " << itls << " lightState: " << m_CurrLightStatus << std::endl;
}
}
m_CurrentBehavior = m_BehaviorGenerator.DoOneStep(dt, m_CurrentPos, m_VehicleStatus, 1, m_PrevTrafficLight, m_TrajectoryBestCost, 0 , m_Loop);
SendLocalPlanningTopics();
VisualizeLocalPlanner();
LogLocalPlanningInfo(dt);
}
else
sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &BehaviorGen::callbackGetGlobalPlannerPath, this);
//m_CurrentBehavior
//m_BehaviorGenerator
//定义
//-------------------------------------------------------------------------------------------------
/*~/r2ware.ai/src/autoware/core_planning/op_local_planner/include
op_behavior_selector_core.h
*/
//85行
PlannerHNS::DecisionMaker m_BehaviorGenerator;
PlannerHNS::BehaviorState m_CurrentBehavior;
/*可见执行
m_CurrentBehavior返回的是 PlannerHNS::BehaviorState beh;
*/
//PlannerHNS::BehaviorState DecisionMaker::DoOneStep()定义
//-------------------------------------------------------------------------------------------------
/*~/r2ware.ai/src/autoware/common/op_planner/src
DecisionMaker.cpp
*/
PlannerHNS::BehaviorState DecisionMaker::DoOneStep 中有
PlannerHNS::BehaviorState beh;
//。。。
if(m_TotalPath.size()==0) return beh;
UpdateCurrentLane(m_MaxLaneSearchDistance);
CalculateImportantParameterForDecisionMaking(vehicleState, goalID, bEmergencyStop, trafficLight, tc, loop);
beh = GenerateBehaviorState(vehicleState);
beh.bNewPlan = SelectSafeTrajectory();
beh.maxVelocity = UpdateVelocityDirectlyToTrajectory(beh, vehicleState, dt);
return beh;
PlannerHNS::BehaviorState DecisionMaker::GenerateBehaviorState(const PlannerHNS::VehicleState& vehicleState)
m_pCurrentBehaviorState = m_pCurrentBehaviorState->GetNextState();//===================跳转状态
if(m_pCurrentBehaviorState==0)
m_pCurrentBehaviorState = m_pInitState;
//m_pCurrentBehaviorState定义
//BehaviorStateMachine* 行为状态机 指针
//-------------------------------------------------------------------------------------------------
/*~/r2ware.ai/src/autoware/common/op_planner/include/op_planner
DecisionMaker.h
*/
namespace PlannerHNS
{
class DecisionMaker
BehaviorStateMachine* m_pCurrentBehaviorState;//------------------基类指针
//以下为BehaviorStateMachine子类实例指针 12个
StopState* m_pStopState;//
WaitState* m_pWaitState;//------- 没有实例
SwerveStateII* m_pAvoidObstacleState;
TrafficLightStopStateII* m_pTrafficLightStopState;
TrafficLightWaitStateII* m_pTrafficLightWaitState;
ForwardStateII * m_pGoToGoalState;;
InitStateII* m_pInitState;
MissionAccomplishedStateII* m_pMissionCompleteState;
GoalStateII* m_pGoalState;
FollowStateII* m_pFollowState;
StopSignStopStateII* m_pStopSignStopState;
StopSignWaitStateII* m_pStopSignWaitState;
void DecisionMaker::InitBehaviorStates()
{
//这里初始化，开始组织出整个状态机的结构
m_pStopState = new StopState(&m_params, 0, 0);
m_pMissionCompleteState = new MissionAccomplishedStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), 0);
m_pGoalState = new GoalStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pMissionCompleteState);
m_pGoToGoalState = new ForwardStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoalState);
m_pInitState = new InitStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
m_pFollowState = new FollowStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
m_pAvoidObstacleState = new SwerveStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
m_pStopSignWaitState = new StopSignWaitStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
m_pStopSignStopState = new StopSignStopStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pStopSignWaitState);
m_pTrafficLightWaitState= new TrafficLightWaitStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
m_pTrafficLightStopState= new TrafficLightStopStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
//停止状态:[#停止状态]
//任务完成状态:[#任务完成状态]
//转向状态:[*前进状态,#转向状态]
//停止信号停止状态:[*停止信号等待状态,#停止信号停止状态]
/*
1.前进状态
2.任务完成状态
3.跟随状态
4.转向状态
5.停止状态
6.交通灯停止状态
7.交通灯等待状态
8.停止信号停止状态
9.停止信号等待状态
10.等待状态
11.初始状态
12.目标状态
*/
//前进状态 :[*目标状态, #前进状态, 转向状态, 停止信号停止状态, 交通灯停止状态, 跟随状态, ]
m_pGoToGoalState->InsertNextState(m_pAvoidObstacleState);
m_pGoToGoalState->InsertNextState(m_pStopSignStopState);
m_pGoToGoalState->InsertNextState(m_pTrafficLightStopState);
m_pGoToGoalState->InsertNextState(m_pFollowState);
m_pGoToGoalState->decisionMakingCount = 0;//m_params.nReliableCount;
//目标状态 :[*任务完成状态,#目标状态, 前进状态]
m_pGoalState->InsertNextState(m_pGoToGoalState);
//停止信号等待状态:[*前进状态,#停止信号等待状态,停止信号停止状态, 目标状态]
m_pStopSignWaitState->decisionMakingTime = m_params.stopSignStopTime;
m_pStopSignWaitState->InsertNextState(m_pStopSignStopState);
m_pStopSignWaitState->InsertNextState(m_pGoalState);
//交通灯停止状态:[*前进状态, #交通灯停止状态, 交通灯等待状态]
m_pTrafficLightStopState->InsertNextState(m_pTrafficLightWaitState);
//交通灯等待状态:[*前进状态, #交通灯等待状态, 交通灯停止状态, 目标状态]
m_pTrafficLightWaitState->InsertNextState(m_pTrafficLightStopState);
m_pTrafficLightWaitState->InsertNextState(m_pGoalState);
//跟随状态:[*前进状态, #跟随状态, 转向状态, 停止信号停止状态, 交通灯停止状态, 目标状态]
m_pFollowState->InsertNextState(m_pAvoidObstacleState);
m_pFollowState->InsertNextState(m_pStopSignStopState);
m_pFollowState->InsertNextState(m_pTrafficLightStopState);
m_pFollowState->InsertNextState(m_pGoalState);
m_pFollowState->decisionMakingCount = 0;//m_params.nReliableCount;
//初始状态:[*前进状态, #初始状态]
m_pInitState->decisionMakingCount = 0;//m_params.nReliableCount;
m_pCurrentBehaviorState = m_pInitState;
}
/*整个状态机运行时的结构：*/
//停止状态:[#停止状态]
//任务完成状态:[#任务完成状态]
//转向状态:[*前进状态,#转向状态]
//停止信号停止状态:[*停止信号等待状态,#停止信号停止状态]
//前进状态 :[*目标状态, #前进状态, 转向状态, 停止信号停止状态, 交通灯停止状态, 跟随状态, ]
//目标状态 :[*任务完成状态,#目标状态, 前进状态]
//停止信号等待状态:[*前进状态,#停止信号等待状态,停止信号停止状态, 目标状态]
//交通灯停止状态:[*前进状态, #交通灯停止状态, 交通灯等待状态]
//交通灯等待状态:[*前进状态, #交通灯等待状态, 交通灯停止状态, 目标状态]
//跟随状态:[*前进状态, #跟随状态, 转向状态, 停止信号停止状态, 交通灯停止状态, 目标状态]
//初始状态:[*前进状态, #初始状态]
//-------------------------------------------------------------------------------------------------
/*~/r2ware.ai/src/autoware/common/op_planner/src
BehaviorStateMachine.cpp
*/
//-------------------------------------------------------------------------------------------------
/*/r2ware.ai/src/autoware/common/op_planner/include/op_planner
RoadNetwork.h
*/
//路网工作头文件中枚举定义
namespace PlannerHNS
{
enum DIRECTION_TYPE { FORWARD_DIR, FORWARD_LEFT_DIR, FORWARD_RIGHT_DIR,
BACKWARD_DIR, BACKWARD_LEFT_DIR, BACKWARD_RIGHT_DIR, STANDSTILL_DIR};
enum OBSTACLE_TYPE {SIDEWALK, TREE, CAR, TRUCK, HOUSE, PEDESTRIAN, CYCLIST, GENERAL_OBSTACLE};
enum DRIVABLE_TYPE {DIRT, TARMAC, PARKINGAREA, INDOOR, GENERAL_AREA};
enum GLOBAL_STATE_TYPE {G_WAITING_STATE, G_PLANING_STATE, G_FORWARD_STATE, G_BRANCHING_STATE, G_FINISH_STATE};
//状态类型枚举
enum STATE_TYPE {INITIAL_STATE, WAITING_STATE, FORWARD_STATE, STOPPING_STATE, EMERGENCY_STATE,
TRAFFIC_LIGHT_STOP_STATE,TRAFFIC_LIGHT_WAIT_STATE, STOP_SIGN_STOP_STATE, STOP_SIGN_WAIT_STATE, FOLLOW_STATE, LANE_CHANGE_STATE, OBSTACLE_AVOIDANCE_STATE, GOAL_STATE, FINISH_STATE, YIELDING_STATE, BRANCH_LEFT_STATE, BRANCH_RIGHT_STATE};
enum LIGHT_INDICATOR {INDICATOR_LEFT, INDICATOR_RIGHT, INDICATOR_BOTH , INDICATOR_NONE};
enum SHIFT_POS {SHIFT_POS_PP = 0x60, SHIFT_POS_RR = 0x40, SHIFT_POS_NN = 0x20,
SHIFT_POS_DD = 0x10, SHIFT_POS_BB = 0xA0, SHIFT_POS_SS = 0x0f, SHIFT_POS_UU = 0xff };
//动作类型枚举
enum ACTION_TYPE {FORWARD_ACTION, BACKWARD_ACTION, STOP_ACTION, LEFT_TURN_ACTION,
RIGHT_TURN_ACTION, U_TURN_ACTION, SWERVE_ACTION, OVERTACK_ACTION, START_ACTION, SLOWDOWN_ACTION, CHANGE_DESTINATION, WAITING_ACTION, DESTINATION_REACHED, UNKOWN_ACTION};
//行为状态类型枚举
enum BEH_STATE_TYPE {BEH_FORWARD_STATE=0,BEH_STOPPING_STATE=1, BEH_BRANCH_LEFT_STATE=2, BEH_BRANCH_RIGHT_STATE=3, BEH_YIELDING_STATE=4, BEH_ACCELERATING_STATE=5, BEH_SLOWDOWN_STATE=6};
enum SEGMENT_TYPE {NORMAL_ROAD_SEG, INTERSECTION_ROAD_SEG, UTURN_ROAD_SEG, EXIT_ROAD_SEG, MERGE_ROAD_SEG, HIGHWAY_ROAD_SEG};
enum RoadSegmentType {NORMAL_ROAD, INTERSECTION_ROAD, UTURN_ROAD, EXIT_ROAD, MERGE_ROAD, HIGHWAY_ROAD};
enum MARKING_TYPE {UNKNOWN_MARK, TEXT_MARK, AF_MARK, AL_MARK, AR_MARK, AFL_MARK, AFR_MARK, ALR_MARK, UTURN_MARK, NOUTURN_MARK};
enum TrafficSignTypes {UNKNOWN_SIGN, STOP_SIGN, MAX_SPEED_SIGN, MIN_SPEED_SIGN};
//-------------------------------------------------------------------------------------------------
/*/r2ware.ai/src/autoware/common/op_planner/include/op_planner
BehaviorStateMachine.h
*/
//行为状态机
//定义基类
namespace PlannerHNS
{
class BehaviorStateMachine
virtual BehaviorStateMachine* GetNextState() = 0;//子类实现功能，切换下一个状态
virtual void Init();
virtual void ResetTimer();
virtual void InsertNextState(BehaviorStateMachine* nextState);//插入下一个状态
BehaviorStateMachine(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* nextState);
virtual ~BehaviorStateMachine() ;
STATE_TYPE m_Behavior;//状态类型--------------------------------------
。。。
。。。
std::vector<BehaviorStateMachine*> pNextStates;//下一个状态列表
BehaviorStateMachine* FindBehaviorState(const STATE_TYPE& behavior);//查找状态
//遍历 pNextStates 每个元素为 pState
// 若pState && behavior == pState->m_Behavior
// if(pState == 0) return this; -------------容错？？？
// m_BehaviorsLog.clear();
// pState->ResetTimer();
// return pState;
// pNextStates为空返回空指针
void UpdateLogCount(BehaviorStateMachine* pState);//-------------------------注释了？？？！！！
//从 m_BehaviorsLog查找 pState相关 行为，找到提升对中的数字
//找不到则插入 默认组对数字为1
BehaviorStateMachine* FindBestState(int nMinCount);//查找最好状态
//从 m_BehaviorsLog查找 对数字大于nMinCount 的行为
//找不到返回空指针
std::vector<std::pair<BehaviorStateMachine*, int> > m_BehaviorsLog;//
//行为实例 与数字 构成一对，存入列表
/*
1.ForwardState
ForwardStateII
2.MissionAccomplishedState
MissionAccomplishedStateII
3.FollowState
FollowStateII
4.SwerveState
SwerveStateII
5.StopState
6.TrafficLightStopState
TrafficLightStopStateII
7.TrafficLightWaitState
TrafficLightWaitStateII
8.StopSignStopState
StopSignStopStateII
9.StopSignWaitState
StopSignWaitStateII
10.WaitState
11.InitState
InitStateII
12.GoalState
GoalStateII
1.前进状态
2.任务完成状态
3.跟随状态
4.转向状态
5.停止状态
6.交通灯停止状态
7.交通灯等待状态
8.停止信号停止状态
9.停止信号等待状态
10.等待状态
11.初始状态
12.目标状态
*/
//------------------------------------------------------------------
/*
以下是各个状态类的实现扼要
有关切换条件做了简要注释
*/
class ForwardState : public BehaviorStateMachine
m_Behavior = FORWARD_STATE;
class ForwardStateII : public BehaviorStateMachine
m_Behavior = FORWARD_STATE;
class MissionAccomplishedState : public BehaviorStateMachine
m_Behavior = FINISH_STATE;
class MissionAccomplishedStateII : public BehaviorStateMachine
m_Behavior = FINISH_STATE;
class FollowState : public BehaviorStateMachine
m_Behavior = FOLLOW_STATE;
class FollowStateII : public BehaviorStateMachine
m_Behavior = FOLLOW_STATE;
class SwerveState : public BehaviorStateMachine
m_Behavior = OBSTACLE_AVOIDANCE_STATE;
class SwerveStateII : public BehaviorStateMachine
m_Behavior = OBSTACLE_AVOIDANCE_STATE;
class StopState : public BehaviorStateMachine
m_Behavior = STOPPING_STATE;
class TrafficLightStopState : public BehaviorStateMachine
m_Behavior = TRAFFIC_LIGHT_STOP_STATE;
class TrafficLightWaitState : public BehaviorStateMachine
m_Behavior = TRAFFIC_LIGHT_WAIT_STATE;
class StopSignStopState : public BehaviorStateMachine
m_Behavior = STOP_SIGN_STOP_STATE;
class StopSignStopStateII : public BehaviorStateMachine
m_Behavior = STOP_SIGN_STOP_STATE;
class StopSignWaitState : public BehaviorStateMachine
m_Behavior = STOP_SIGN_WAIT_STATE;
class StopSignWaitStateII : public BehaviorStateMachine
m_Behavior = STOP_SIGN_WAIT_STATE;
class WaitState : public BehaviorStateMachine
m_Behavior = WAITING_STATE;
class InitState : public BehaviorStateMachine
m_Behavior = INITIAL_STATE;
class InitStateII : public BehaviorStateMachine
m_Behavior = INITIAL_STATE;
class GoalState : public BehaviorStateMachine
m_Behavior = GOAL_STATE;
class GoalStateII : public BehaviorStateMachine
m_Behavior = GOAL_STATE;
class TrafficLightStopStateII : public BehaviorStateMachine
m_Behavior = TRAFFIC_LIGHT_STOP_STATE;
class TrafficLightWaitStateII : public BehaviorStateMachine
m_Behavior = TRAFFIC_LIGHT_WAIT_STATE;
BehaviorStateMachine* ForwardState::GetNextState()
{
if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
return this; //return this behavior only , without reset
PreCalculatedConditions* pCParams = GetCalcParams();
if(pCParams->currentGoalID != pCParams->prevGoalID)
return FindBehaviorState(GOAL_STATE);
else if(m_pParams->enableSwerving
&& pCParams->distanceToNext <= m_pParams->minDistanceToAvoid
&& !pCParams->bFullyBlock
&& (pCParams->iCurrSafeTrajectory != pCParams->iPrevSafeTrajectory || pCParams->iCurrSafeLane != pCParams->iPrevSafeLane)
)
return FindBehaviorState(OBSTACLE_AVOIDANCE_STATE);
else if(m_pParams->enableTrafficLightBehavior
&& pCParams->currentTrafficLightID > 0
&& pCParams->bTrafficIsRed
&& pCParams->currentTrafficLightID != pCParams->prevTrafficLightID)
return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
else if(m_pParams->enableStopSignBehavior
&& pCParams->currentStopSignID > 0
&& pCParams->currentStopSignID != pCParams->prevStopSignID)
return FindBehaviorState(STOP_SIGN_STOP_STATE);
else if(m_pParams->enableFollowing
&& pCParams->bFullyBlock)
return FindBehaviorState(FOLLOW_STATE);
// else if(pCParams->distanceToNext <= m_pParams->maxDistanceToAvoid)
// return FindBehaviorState(STOPPING_STATE);
else
{
if(pCParams->iCurrSafeTrajectory == pCParams->iCentralTrajectory
&& pCParams->iPrevSafeTrajectory != pCParams->iCurrSafeTrajectory)
pCParams->bRePlan = true;
return FindBehaviorState(this->m_Behavior); // return and reset
}
}
BehaviorStateMachine* ForwardStateII::GetNextState()
{
PreCalculatedConditions* pCParams = GetCalcParams();
if(pCParams->currentGoalID != pCParams->prevGoalID)//当前目标 不是 上个目标
return FindBehaviorState(GOAL_STATE); //切换为 目标状态
else if(m_pParams->enableTrafficLightBehavior //使能红绿灯行为
&& pCParams->currentTrafficLightID > 0 //红绿灯ID >0
&& pCParams->bTrafficIsRed //红灯
&& pCParams->currentTrafficLightID != pCParams->prevTrafficLightID)//灯ID 不是上个灯ID
return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE); //切换到 红绿灯停车
else if(m_pParams->enableStopSignBehavior //使能停止信号行为
&& pCParams->currentStopSignID > 0 //停止信号ID >0
&& pCParams->currentStopSignID != pCParams->prevStopSignID) //停止信号ID不是上个信号ID
return FindBehaviorState(STOP_SIGN_STOP_STATE); //切换到 停止信号停车
else if(m_pParams->enableFollowing && pCParams->bFullyBlock) //使能跟随 并且 FullyBlock？？
return FindBehaviorState(FOLLOW_STATE); //切换到 跟随状态
else if(m_pParams->enableSwerving //使能急转弯行为
&& pCParams->distanceToNext <= m_pParams->minDistanceToAvoid //下个距离 小于 最小避障距离
&& !pCParams->bFullyBlock //
&& pCParams->iCurrSafeTrajectory != pCParams->iPrevSafeTrajectory) //安全轨迹 不是 前一个安全轨迹
return FindBehaviorState(OBSTACLE_AVOIDANCE_STATE); //切换到 障碍回避状态
else
return FindBehaviorState(this->m_Behavior); //继续当前状态
}
BehaviorStateMachine* MissionAccomplishedState::GetNextState()
{
return FindBehaviorState(this->m_Behavior); // return and reset
}
BehaviorStateMachine* MissionAccomplishedStateII::GetNextState()
{
return FindBehaviorState(this->m_Behavior); //继续当前状态
}
BehaviorStateMachine* StopState::GetNextState()
{
if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)//时间 小于 决策时间
return this; //继续当前状态
PreCalculatedConditions* pCParams = GetCalcParams();
if(pCParams->distanceToNext > m_pParams->maxDistanceToAvoid) //下个距离 大于最大避障距离
return FindBehaviorState(FORWARD_STATE); //切换到 前进状态
else
return FindBehaviorState(this->m_Behavior); // return and reset //继续当前状态
}
BehaviorStateMachine* TrafficLightStopState::GetNextState()
{
if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
return this;
PreCalculatedConditions* pCParams = GetCalcParams();
if(!pCParams->bTrafficIsRed)
{
pCParams->prevTrafficLightID = pCParams->currentTrafficLightID;
return FindBehaviorState(FORWARD_STATE);
}
else if(pCParams->bTrafficIsRed && pCParams->currentVelocity <= m_zero_velocity)
return FindBehaviorState(TRAFFIC_LIGHT_WAIT_STATE);
else
return FindBehaviorState(this->m_Behavior); // return and reset
}
BehaviorStateMachine* TrafficLightStopStateII::GetNextState()
{
PreCalculatedConditions* pCParams = GetCalcParams();
//std::cout << "Stopping for trafficLight " << std::endl;
if(!pCParams->bTrafficIsRed) //不是红灯
{
//std::cout << "Color Changed Stopping for trafficLight " << std::endl;
pCParams->prevTrafficLightID = pCParams->currentTrafficLightID;
return FindBehaviorState(FORWARD_STATE); //切换到 前进状态
}
else if(pCParams->bTrafficIsRed && pCParams->currentVelocity <= m_zero_velocity) //红灯 并且 速度 小于m_zero_velocity
{
//std::cout << "Velocity Changed Stopping for trafficLight (" <<pCParams->currentVelocity << ", " << m_zero_velocity << ")" << std::endl;
return FindBehaviorState(TRAFFIC_LIGHT_WAIT_STATE); //切换到 红绿灯待状态
}
else
{
return FindBehaviorState(this->m_Behavior); // return and reset //继续当前状态
}
}
BehaviorStateMachine* TrafficLightWaitState::GetNextState()
{
if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
return this;
PreCalculatedConditions* pCParams = GetCalcParams();
if(!pCParams->bTrafficIsRed) //不是红灯
{
pCParams->prevTrafficLightID = pCParams->currentTrafficLightID;
return FindBehaviorState(FORWARD_STATE); //切换到 前进状态
}
else if(pCParams->currentVelocity > m_zero_velocity) //红灯 速度 大于m_zero_velocity
return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE); //切换到 红绿灯停止状态
else
return FindBehaviorState(this->m_Behavior); // return and reset
}
BehaviorStateMachine* TrafficLightWaitStateII::GetNextState()
{
PreCalculatedConditions* pCParams = GetCalcParams();
//std::cout << "Wait for trafficLight " << std::endl;
if(!pCParams->bTrafficIsRed) //不是红灯
{
pCParams->prevTrafficLightID = pCParams->currentTrafficLightID;
return FindBehaviorState(FORWARD_STATE); //切换到 前进状态
}
// else if(pCParams->currentVelocity > m_zero_velocity)
// return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
else
return FindBehaviorState(this->m_Behavior); // return and reset
}
BehaviorStateMachine* StopSignStopState::GetNextState()
{
if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
return this;
PreCalculatedConditions* pCParams = GetCalcParams();
//std::cout << "From Stop Beh D: " << pCParams->distanceToStop() << ", Prev LineID: " << pCParams->prevStopSignID << ", Curr SignID: " << pCParams->currentStopSignID << std::endl;
if(pCParams->currentVelocity < m_zero_velocity)
return FindBehaviorState(STOP_SIGN_WAIT_STATE);
else
return FindBehaviorState(this->m_Behavior); // return and reset
}
BehaviorStateMachine* StopSignStopStateII::GetNextState()
{
PreCalculatedConditions* pCParams = GetCalcParams();
if(pCParams->currentGoalID != pCParams->prevGoalID) //当前目标ID不是上个目标ID
return FindBehaviorState(GOAL_STATE); //切换 目标状态
else if(pCParams->currentVelocity < m_zero_velocity) //目标没变 速度 小于m_zero_velocity
return FindBehaviorState(STOP_SIGN_WAIT_STATE); //切换 停止信号等待状态
else
return FindBehaviorState(this->m_Behavior); // return and reset
}
BehaviorStateMachine* StopSignWaitState::GetNextState()
{
if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
return this;
PreCalculatedConditions* pCParams = GetCalcParams();
//std::cout << "From Wait Beh D: " << pCParams->distanceToStop() << ", Prev LineID: " << pCParams->prevStopSignID << ", Curr SignID: " << pCParams->currentStopSignID << std::endl;
pCParams->prevStopSignID = pCParams->currentStopSignID;
return FindBehaviorState(FORWARD_STATE);
}
BehaviorStateMachine* StopSignWaitStateII::GetNextState()
{
if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
return this;
PreCalculatedConditions* pCParams = GetCalcParams();
pCParams->prevStopSignID = pCParams->currentStopSignID;
return FindBehaviorState(FORWARD_STATE); //切换 前进状态
}
BehaviorStateMachine* WaitState::GetNextState()
{
if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
return this;
//PreCalculatedConditions* pCParams = GetCalcParams();
return FindBehaviorState(FORWARD_STATE); //切换 前进状态
}
BehaviorStateMachine* InitState::GetNextState()
{
if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
return this;
PreCalculatedConditions* pCParams = GetCalcParams();
if(pCParams->bOutsideControl == 1)
{
pCParams->prevGoalID = pCParams->currentGoalID;
return FindBehaviorState(FORWARD_STATE);
}
else
return FindBehaviorState(this->m_Behavior); // return and reset
}
BehaviorStateMachine* InitStateII::GetNextState()
{
PreCalculatedConditions* pCParams = GetCalcParams();
if(pCParams->currentGoalID > 0)
return FindBehaviorState(FORWARD_STATE); //切换 前进状态
else
return FindBehaviorState(this->m_Behavior);
}
BehaviorStateMachine* FollowState::GetNextState()
{
if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
return this;
PreCalculatedConditions* pCParams = GetCalcParams();
//std::cout << "Following State >> followDistance: " << pCParams->distanceToNext << ", followSpeed: " << pCParams->velocityOfNext << std::endl;
if(m_pParams->enableTrafficLightBehavior
&& pCParams->currentTrafficLightID > 0
&& pCParams->bTrafficIsRed
&& pCParams->currentTrafficLightID != pCParams->prevTrafficLightID)
return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
else if(m_pParams->enableStopSignBehavior
&& pCParams->currentStopSignID > 0
&& pCParams->currentStopSignID != pCParams->prevStopSignID)
return FindBehaviorState(STOP_SIGN_STOP_STATE);
else if(pCParams->currentGoalID != pCParams->prevGoalID
|| !pCParams->bFullyBlock)
return FindBehaviorState(FORWARD_STATE);
else
return FindBehaviorState(this->m_Behavior); // return and reset
}
BehaviorStateMachine* FollowStateII::GetNextState()
{
PreCalculatedConditions* pCParams = GetCalcParams();
if(pCParams->currentGoalID != pCParams->prevGoalID) //当前目标ID不是上个目标ID
return FindBehaviorState(GOAL_STATE); //切换 目标状态
else if(m_pParams->enableTrafficLightBehavior //使能红绿灯行为
&& pCParams->currentTrafficLightID > 0 //红绿灯ID >0
&& pCParams->bTrafficIsRed //红灯
&& pCParams->currentTrafficLightID != pCParams->prevTrafficLightID)//灯ID 不是上个灯ID
return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE); //切换到 红绿灯停车
else if(m_pParams->enableStopSignBehavior //使能停止信号行为
&& pCParams->currentStopSignID > 0 //停止信号ID >0
&& pCParams->currentStopSignID != pCParams->prevStopSignID) //停止信号ID不是上个信号ID
return FindBehaviorState(STOP_SIGN_STOP_STATE); //切换到 停止信号停车
else if(m_pParams->enableSwerving //使能急转弯行为
&& pCParams->distanceToNext <= m_pParams->minDistanceToAvoid //下个距离 小于 最小避障距离
&& !pCParams->bFullyBlock //
&& pCParams->iCurrSafeTrajectory != pCParams->iPrevSafeTrajectory) //安全轨迹 不是 前一个安全轨迹
return FindBehaviorState(OBSTACLE_AVOIDANCE_STATE); //切换到 障碍回避状态
else if(!pCParams->bFullyBlock) //使能跟随 并且 FullyBlock？？
return FindBehaviorState(FORWARD_STATE); //切换到 前进状态
else
return FindBehaviorState(this->m_Behavior); // return and reset
}
BehaviorStateMachine* SwerveState::GetNextState()
{
if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
return this;
PreCalculatedConditions* pCParams = GetCalcParams();
if(pCParams->distanceToNext > 0 //下一个距离 大于0
&& pCParams->distanceToNext < m_pParams->minDistanceToAvoid //下个距离 小于 最小避障距离
&& !pCParams->bFullyBlock //
&& pCParams->iCurrSafeTrajectory != pCParams->iPrevSafeTrajectory)//安全轨迹 不是 前一个安全轨迹
return FindBehaviorState(this->m_Behavior); //
else
return FindBehaviorState(FORWARD_STATE); //切换到 前进状态
}
BehaviorStateMachine* SwerveStateII::GetNextState()
{
PreCalculatedConditions* pCParams = GetCalcParams();
pCParams->iPrevSafeTrajectory = pCParams->iCurrSafeTrajectory;
pCParams->bRePlan = true;
return FindBehaviorState(FORWARD_STATE); //切换到 前进状态
}
BehaviorStateMachine* GoalState::GetNextState()
{
if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
return this;
PreCalculatedConditions* pCParams = GetCalcParams();
if(pCParams->currentGoalID == -1)
return FindBehaviorState(FINISH_STATE);
else if(pCParams->currentGoalID != pCParams->prevGoalID)
{
pCParams->prevGoalID = pCParams->currentGoalID;
return FindBehaviorState(FORWARD_STATE);
}
else
return FindBehaviorState(this->m_Behavior); // return and reset
}
BehaviorStateMachine* GoalStateII::GetNextState()
{
PreCalculatedConditions* pCParams = GetCalcParams();
if(pCParams->currentGoalID == -1) //无目标
return FindBehaviorState(FINISH_STATE); //切换到 完成状态
else
{
pCParams->prevGoalID = pCParams->currentGoalID;//有目标
return FindBehaviorState(FORWARD_STATE); //切换到 前进状态
}
}
