advancedlanechangeplanner.h

Go to the documentation of this file.

/********************************************************************************
 * Copyright (C) 2017-2020 German Aerospace Center (DLR). 
 * Eclipse ADORe, Automated Driving Open Research https://eclipse.org/adore
 *
 * This program and the accompanying materials are made available under the 
 * terms of the Eclipse Public License 2.0 which is available at
 * http://www.eclipse.org/legal/epl-2.0.
 *
 * SPDX-License-Identifier: EPL-2.0 
 *
 * Contributors: 
 *   Matthias Nichting - initial API and implementation
 ********************************************************************************/
 
 
#pragma once
#include "decoupled_lflc_planner.h"
#include "basicconstraintsandreferences.h"
#include "lanechangeconstraintsandreferences.h"
#include <adore/view/alane.h>
#include <adore/params/ap_longitudinal_planner.h>
#include <adore/params/ap_lateral_planner.h>
#include <adore/params/ap_tactical_planner.h>
 
 
namespace adore
{
  namespace fun
  {
    template<int K,int P>
    class AdvancedLaneChangePlanner:public DecoupledLFLCPlanner<K,P>
    {
      private:
        LaneChangePhaseEstimator phaseEstimator_;      
        NominalReferenceSpeed nominalReferenceSpeed_;
        BreakAtHorizon breakAtHorizon_;
        CurvatureSpeedLimitPredict curvatureSpeedLimit_;
        LFVSpeedLimit lfvSpeedLimit_onTarget_;
        LFVSpeedLimit lfvSpeedLimit_onSource_;
        DontDriveBackwards dontDriveBackwards_;
        LongitudinalAccelerationConstraint longitudinalAccelerationConstraintUB_;
        LongitudinalAccelerationConstraint longitudinalAccelerationConstraintLB_;
        LaneWidthSpeedLimitLFV laneWidthSpeedLimitLFV_;
        StopAtBottleneckLFV stopAtBottleneckLFV_;
        StopAtNextGoalPoint stopAtNextGoalPoint_;
        AdhereToNextLimitLine stopAtRedLightOnSource_;
        AdhereToNextLimitLine stopAtRedLightOnTarget_;
        AdhereToNextLimitLine stopAtCheckPointOnSource_;
        AdhereToNextLimitLine stopAtCheckPointOnTarget_;
        FollowPrecedingVehicleOnLaneToBeLeft_BrakingCurve followPrecedingVehicleOnSource_;
        FollowPrecedingVehicle_BreakingCurve followPrecedingVehicleOnTarget_;
        LowerBoundSGapToPrecedingVehicle lowerBoundSGapToPrecedingVehicleOnTarget_;
        LateralAccelerationReference lateralAccelerationReference_;
        LateralJerkReference lateralJerkReference_;
        HeadingConstraint headingConstraintUB_;
        HeadingConstraint headingConstraintLB_;
        LateralAccelerationConstraint lateralAccelerationConstraintUB_;
        LateralAccelerationConstraint lateralAccelerationConstraintLB_;
        AdvancedLateralOffsetConstraintLC lateralOffsetConstraint_left_;
        AdvancedLateralOffsetConstraintLC lateralOffsetConstraint_right_;
        LaneChangeIntoGapReference laneChangeIntoGapReference_;
      public:
 
        AdvancedLaneChangePlanner(   adore::view::ALaneChangeView* lcv,
                                  adore::view::ANavigationGoalView* goalview,
                                  adore::view::ALimitLineEnRoute* controlledConnectionOnSource,
                                  adore::view::ALimitLineEnRoute* controlledConnectionOnTarget,
                                  adore::view::ALimitLineEnRoute* checkpoint_on_source,
                                  adore::view::ALimitLineEnRoute* checkpoint_on_target,
                                  adore::params::APLongitudinalPlanner* plon,
                                  adore::params::APLateralPlanner* plat,
                                  adore::params::APTacticalPlanner* ptac,
                                  adore::params::APVehicle* pveh,
                                  adore::params::APTrajectoryGeneration* ptrajectory,
                                  double lateral_i_grid = 0.0)
              : DecoupledLFLCPlanner<K,P>(lcv->getTargetLane(),plon,plat,pveh,ptrajectory),
                phaseEstimator_(lcv,pveh,plat,ptrajectory,ptac),
                nominalReferenceSpeed_(lcv->getTargetLane(),plon,ptac),
                breakAtHorizon_(lcv->getTargetLane(),pveh,ptrajectory),
                curvatureSpeedLimit_(lcv->getTargetLane(),plon),
                lfvSpeedLimit_onTarget_(lcv->getTargetLane(),ptac),
                lfvSpeedLimit_onSource_(lcv->getSourceLane(),ptac),
                dontDriveBackwards_(),
                longitudinalAccelerationConstraintUB_(plon,ANominalConstraint::UB),
                longitudinalAccelerationConstraintLB_(plon,ANominalConstraint::LB),
                lateralAccelerationReference_(lcv->getTargetLane()),
                lateralJerkReference_(lcv->getTargetLane()),
                followPrecedingVehicleOnSource_(lcv->getSourceLane(),&phaseEstimator_,pveh,ptac,ptrajectory),
                followPrecedingVehicleOnTarget_(lcv->getTargetLane(),pveh,ptac,ptrajectory),
                lowerBoundSGapToPrecedingVehicleOnTarget_(lcv->getTargetLane(),pveh,ptac,ptrajectory),
                laneWidthSpeedLimitLFV_(lcv->getTargetLane(),plon),
                stopAtBottleneckLFV_(lcv->getTargetLane(),plon,pveh,ptrajectory,ptac),
                stopAtNextGoalPoint_(goalview),
                stopAtRedLightOnSource_(controlledConnectionOnSource),
                stopAtRedLightOnTarget_(controlledConnectionOnTarget),
                stopAtCheckPointOnSource_(checkpoint_on_source),
                stopAtCheckPointOnTarget_(checkpoint_on_target),
                headingConstraintUB_(plat,ANominalConstraint::UB),
                headingConstraintLB_(plat,ANominalConstraint::LB),
                lateralAccelerationConstraintUB_(lcv->getTargetLane(),plat,ANominalConstraint::UB),
                lateralAccelerationConstraintLB_(lcv->getTargetLane(),plat,ANominalConstraint::LB),
                lateralOffsetConstraint_left_(lcv,&phaseEstimator_,pveh,plat,ANominalConstraint::UB),
                lateralOffsetConstraint_right_(lcv,&phaseEstimator_,pveh,plat,ANominalConstraint::LB),
                laneChangeIntoGapReference_(lcv,pveh,plat,lateral_i_grid)
        {
          this->info_.add(&nominalReferenceSpeed_);
          this->info_.add(&breakAtHorizon_);
          this->info_.add(&stopAtCheckPointOnSource_);
          this->info_.add(&stopAtCheckPointOnTarget_);
          this->info_.add(&stopAtRedLightOnSource_);
          this->info_.add(&stopAtRedLightOnTarget_);          
          this->info_.add(&stopAtNextGoalPoint_);
          this->info_.add(&curvatureSpeedLimit_);
          this->info_.add(&lfvSpeedLimit_onTarget_);
          this->info_.add(&lfvSpeedLimit_onSource_);
          this->info_.add(&dontDriveBackwards_);
          this->info_.add(&longitudinalAccelerationConstraintUB_);
          this->info_.add(&longitudinalAccelerationConstraintLB_);
          // this->info_.add(&laneWidthSpeedLimitLFV_);
          this->info_.add(&followPrecedingVehicleOnSource_);
          this->info_.add(&followPrecedingVehicleOnTarget_);
          this->info_.add(&lowerBoundSGapToPrecedingVehicleOnTarget_);
 
          this->info_.add(&laneChangeIntoGapReference_);
          this->info_.add(&lateralAccelerationReference_);
          this->info_.add(&lateralJerkReference_);
 
          this->info_.add(&lateralOffsetConstraint_left_);
          this->info_.add(&lateralOffsetConstraint_right_);
          this->info_.add(&headingConstraintUB_);
          this->info_.add(&headingConstraintLB_);
          this->info_.add(&lateralAccelerationConstraintUB_);
          this->info_.add(&lateralAccelerationConstraintLB_);
          this->info_.add(&phaseEstimator_);
        }
        void setGap(adore::view::AGap* gap)
        {
          lateralOffsetConstraint_left_.setGap(gap);
          lateralOffsetConstraint_right_.setGap(gap);
          laneChangeIntoGapReference_.setGap(gap);
          phaseEstimator_.setGap(gap);
        }
        void setSpeedScale(double value)
        {
          nominalReferenceSpeed_.setSpeedScale(value);
        }
    };
  }
}