op_behavior_selector.cpp和op_behavior_selector_core.cpp

1 op_behavior_selector.cpp

#include <ros/ros.h>
#include <iostream>
#include "op_behavior_selector_core.h"

using namespace std;

int main(int argc, char **argv)
{
  ros::init(argc, argv, "op_behavior_selector");
  BehaviorGeneratorNS::BehaviorGen beh_gen;
  beh_gen.MainLoop();
  return 0;
}

主要是class BehaviorGen和MainLoop，先说BehaviorGen

具体代码，我就不贴了，实在是太长，而且主要就是一些sub和publish，其中要注意的有一句:

  ros::NodeHandle _nh;
  UpdatePlanningParams(_nh);

更新规划参数这个函数在下文有定义，具体更新那些参数先不管了，主要是其中有一个

 m_BehaviorGenerator.Init(controlParams, m_PlanningParams, m_CarInfo);
  m_BehaviorGenerator.m_pCurrentBehaviorState->m_Behavior = PlannerHNS::INITIAL_STATE;

里面init函数查看定义：

void DecisionMaker::Init(const ControllerParams& ctrlParams, const PlannerHNS::PlanningParams& params,const CAR_BASIC_INFO& carInfo)
   {
     m_CarInfo = carInfo;
     m_ControlParams = ctrlParams;
     m_params = params;

     m_pidVelocity.Init(0.01, 0.004, 0.01);
    m_pidVelocity.Setlimit(m_params.maxSpeed, 0);

    m_pidStopping.Init(0.05, 0.05, 0.1);
    m_pidStopping.Setlimit(m_params.horizonDistance, 0);

    m_pidFollowing.Init(0.05, 0.05, 0.01);
    m_pidFollowing.Setlimit(m_params.minFollowingDistance, 0);

    InitBehaviorStates();

    if(m_pCurrentBehaviorState)
      m_pCurrentBehaviorState->SetBehaviorsParams(&m_params);
   }

init函数里面有一个InitBehaviorStates()，初始化行为状态。

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);

  m_pGoToGoalState->InsertNextState(m_pAvoidObstacleState);//fangfa指向关联关系
  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;
}

定义个状态之间指向关系，爲什麽這麽指向我也不明白，然後初始化了。 m_pCurrentBehaviorState = m_pInitState;

而InitStateII*         m_pInitState;。