说op_behavior_selector_core.cpp—第二部分

最新推荐文章于 2023-08-11 17:41:47 发布

荆棘妖怪

最新推荐文章于 2023-08-11 17:41:47 发布

阅读量288

点赞数 1

文章标签： c++

本文链接：https://blog.csdn.net/weixin_44680789/article/details/121828578

版权

上个文章写了如何初始化， m_pCurrentBehaviorState = m_pInitState;那么他是在哪里进行状态流更新得呢？

见op_behavior_selector_core.cpp

void BehaviorGen::MainLoop()

m_CurrentBehavior = m_BehaviorGenerator.DoOneStep(dt, m_CurrentPos, m_VehicleStatus, 1, m_CurrTrafficLight, m_TrajectoryBestCost, 0 );

转到DoOneStep:转到对对对

PlannerHNS::BehaviorState DecisionMaker::DoOneStep(
    const double& dt,
    const PlannerHNS::WayPoint currPose,
    const PlannerHNS::VehicleState& vehicleState,
    const int& goalID,
    const std::vector<TrafficLight>& trafficLight,
    const TrajectoryCost& tc,
    const bool& bEmergencyStop)
{
   PlannerHNS::BehaviorState beh;
   state = currPose;
   m_TotalPath.clear();
  for(unsigned int i = 0; i < m_TotalOriginalPath.size(); i++)
  {
    t_centerTrajectorySmoothed.clear();
    PlannerHNS::PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(m_TotalOriginalPath.at(i), state, m_params.horizonDistance ,  m_params.pathDensity , t_centerTrajectorySmoothed);
    m_TotalPath.push_back(t_centerTrajectorySmoothed);
  }

  if(m_TotalPath.size()==0) return beh;

  UpdateCurrentLane(m_MaxLaneSearchDistance);

  CalculateImportantParameterForDecisionMaking(vehicleState, goalID, bEmergencyStop, trafficLight, tc);

  beh = GenerateBehaviorState(vehicleState);

  beh.bNewPlan = SelectSafeTrajectory();

  beh.maxVelocity = UpdateVelocityDirectlyToTrajectory(beh, vehicleState, dt);

  //std::cout << "Eval_i: " << tc.index << ", Curr_i: " <<  m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory << ", Prev_i: " << m_pCurrentBehaviorState->GetCalcParams()->iPrevSafeTrajectory << std::endl;

  return beh;
 }

转到GenerateBehaviorState

PlannerHNS::BehaviorState DecisionMaker::GenerateBehaviorState(const PlannerHNS::VehicleState& vehicleState)
 {
  PlannerHNS::PreCalculatedConditions *preCalcPrams = m_pCurrentBehaviorState->GetCalcParams();

  m_pCurrentBehaviorState = m_pCurrentBehaviorState->GetNextState();//可能是跳转状态
  if(m_pCurrentBehaviorState==0)
    m_pCurrentBehaviorState = m_pInitState;

  PlannerHNS::BehaviorState currentBehavior;

  currentBehavior.state = m_pCurrentBehaviorState->m_Behavior;
  currentBehavior.followDistance = preCalcPrams->distanceToNext;

  currentBehavior.minVelocity    = 0;
  currentBehavior.stopDistance   = preCalcPrams->distanceToStop();
  currentBehavior.followVelocity   = preCalcPrams->velocityOfNext;
  if(preCalcPrams->iPrevSafeTrajectory<0 || preCalcPrams->iPrevSafeTrajectory >= m_RollOuts.size())
    currentBehavior.iTrajectory    = preCalcPrams->iCurrSafeTrajectory;
  else
    currentBehavior.iTrajectory    = preCalcPrams->iPrevSafeTrajectory;

  double average_braking_distance = -pow(vehicleState.speed, 2)/(m_CarInfo.max_deceleration) + m_params.additionalBrakingDistance;

  if(average_braking_distance  < m_params.minIndicationDistance)
    average_braking_distance = m_params.minIndicationDistance;

  currentBehavior.indicator = PlanningHelpers::GetIndicatorsFromPath(m_Path, state, average_braking_distance );

  return currentBehavior;
 }

m_pCurrentBehaviorState = m_pCurrentBehaviorState->GetNextState();//可能是跳转状态

他可能是一个周期一个更新吗？和simulink里面stateflow不一样啊？