basiclanefollowingplanner.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: 
 *   Daniel Heß - initial API and implementation
 ********************************************************************************/
 
#pragma once
#include "decoupled_lflc_planner.h"
#include "basicconstraintsandreferences.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 BasicLaneFollowingPlanner:public DecoupledLFLCPlanner<K,P>
    {
      private:
        NominalReferenceSpeed nominalReferenceSpeed_;
        BreakAtHorizon breakAtHorizon_;
        DistanceToLaneEndConstraint keepDistanceToLaneEnd_;
        CurvatureSpeedLimitPredict curvatureSpeedLimit_;
        LFVSpeedLimit lfvSpeedLimit_;
        DontDriveBackwards dontDriveBackwards_;
        LongitudinalAccelerationConstraint longitudinalAccelerationConstraintUB_;
        LongitudinalAccelerationConstraint longitudinalAccelerationConstraintLB_;
        // FollowPrecedingVehicle followPrecedingVehicle_; /**< uses time gap*/
        FollowPrecedingVehicle_BreakingCurve followPrecedingVehicle_;
        LowerBoundSGapToPrecedingVehicle lowerBoundSGapToPrecedingVehicle_;
        LaneWidthSpeedLimitLFV laneWidthSpeedLimitLFV_;
        StopAtBottleneckLFV stopAtBottleneckLFV_;
        StopAtNextGoalPoint stopAtNextGoalPoint_;
        AdhereToNextLimitLine stopAtRedLight_;
        AdhereToNextLimitLine stopAtCheckPoint_;
        StopAtNextConflictPoint stopAtNextConflictPoint_;
        FollowCenterlineReference followCenterlineReference_;
        LateralAccelerationReference lateralAccelerationReference_;
        LateralJerkReference lateralJerkReference_;
        LateralOffsetConstraintLF lateralOffsetConstraintLFUB_;
        LateralOffsetConstraintLF lateralOffsetConstraintLFLB_;
        HeadingConstraint headingConstraintUB_;
        HeadingConstraint headingConstraintLB_;
        LateralAccelerationConstraint lateralAccelerationConstraintUB_;
        LateralAccelerationConstraint lateralAccelerationConstraintLB_;
      public:
        BasicLaneFollowingPlanner(adore::view::ALane* lfv,
                                  adore::view::ANavigationGoalView* goalview,
                                  adore::view::ALimitLineEnRoute* controlledConnection,
                                  adore::view::ALimitLineEnRoute* checkpoint,
                                  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>(lfv,plon,plat,pveh,ptrajectory),
                nominalReferenceSpeed_(lfv,plon,ptac),
                breakAtHorizon_(lfv,pveh,ptrajectory),
                keepDistanceToLaneEnd_(lfv,pveh,ptrajectory,ptac),
                curvatureSpeedLimit_(lfv,plon),
                lfvSpeedLimit_(lfv,ptac),
                dontDriveBackwards_(),
                longitudinalAccelerationConstraintUB_(plon,ANominalConstraint::UB),
                longitudinalAccelerationConstraintLB_(plon,ANominalConstraint::LB),
                followPrecedingVehicle_(lfv,pveh,ptac,ptrajectory),
                lowerBoundSGapToPrecedingVehicle_(lfv,pveh,ptac,ptrajectory),
                laneWidthSpeedLimitLFV_(lfv,plon),
                stopAtBottleneckLFV_(lfv,plon,pveh,ptrajectory,ptac),
                stopAtNextGoalPoint_(goalview),
                stopAtNextConflictPoint_(nullptr),
                stopAtRedLight_(controlledConnection),
                stopAtCheckPoint_(checkpoint),
                followCenterlineReference_(lfv,lateral_i_grid),
                lateralAccelerationReference_(lfv),
                lateralJerkReference_(lfv),
                lateralOffsetConstraintLFUB_(lfv,pveh,plat,ANominalConstraint::UB),
                lateralOffsetConstraintLFLB_(lfv,pveh,plat,ANominalConstraint::LB),
                headingConstraintUB_(plat,ANominalConstraint::UB),
                headingConstraintLB_(plat,ANominalConstraint::LB),
                lateralAccelerationConstraintUB_(lfv,plat,ANominalConstraint::UB),
                lateralAccelerationConstraintLB_(lfv,plat,ANominalConstraint::LB)
        {
          this->info_.add(&nominalReferenceSpeed_);
          this->info_.add(&breakAtHorizon_);
          this->info_.add(&keepDistanceToLaneEnd_);
          this->info_.add(&curvatureSpeedLimit_);
          this->info_.add(&lfvSpeedLimit_);
          this->info_.add(&dontDriveBackwards_);
          this->info_.add(&longitudinalAccelerationConstraintUB_);
          this->info_.add(&longitudinalAccelerationConstraintLB_);
          this->info_.add(&followPrecedingVehicle_);
          this->info_.add(&lowerBoundSGapToPrecedingVehicle_);
          this->info_.add(&stopAtNextGoalPoint_);
          this->info_.add(&stopAtRedLight_);
          this->info_.add(&stopAtCheckPoint_);          
          this->info_.add(&followCenterlineReference_);
          this->info_.add(&lateralAccelerationReference_);
          this->info_.add(&lateralJerkReference_);
          this->info_.add(&lateralOffsetConstraintLFUB_);
          this->info_.add(&lateralOffsetConstraintLFLB_);
          this->info_.add(&headingConstraintUB_);
          this->info_.add(&headingConstraintLB_);
          this->info_.add(&lateralAccelerationConstraintUB_);
          this->info_.add(&lateralAccelerationConstraintLB_);
          this->info_.add(&stopAtNextConflictPoint_);
        }
 
        void addConstraint(ANominalConstraint* constraint)
        {
          this->info_.add(constraint);
        }
 
        void addReference(ANominalReference* reference)
        {
          this->info_.add(reference);
        }
        void setSpeedScale(double value)
        {
          nominalReferenceSpeed_.setSpeedScale(value);
        }
        void setConflictSet(adore::view::AConflictPointSet* conflicts)
        {
          stopAtNextConflictPoint_.setView(conflicts);
        }
    };
  }
}