continuelcmrmplanner.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 "mrmplanner.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>
#include "lanechangeconstraintsandreferences.h"
 
 
namespace adore
{
  namespace fun
  {
 
    template<int K,int P>
    class ContinueLCMRMPlanner:public MRMPlanner<K,P>
    {
      private:
        ContinueLCReference lateralReference_;
        LateralAccelerationReference lateralAccelerationReference_;
        LateralJerkReference lateralJerkReference_;
        LateralOffsetConstraintLC lateralOffsetConstraint_left_;
        LateralOffsetConstraintLC lateralOffsetConstraint_right_;
        HeadingConstraint headingConstraintUB_;
        HeadingConstraint headingConstraintLB_;
        LateralAccelerationConstraint lateralAccelerationConstraintUB_;
        LateralAccelerationConstraint lateralAccelerationConstraintLB_;
        LaneChangeIntoGapReference LaneChangeIntoGapReference_;
        FollowPrecedingVehicle followVehicleConstraint_;
      public:
        ContinueLCMRMPlanner(adore::view::ALaneChangeView* lcv,
                                  adore::params::APLateralPlanner* plat,
                                  adore::params::APTacticalPlanner* ptac,
                                  adore::params::APVehicle* pveh,
                                  adore::params::APTrajectoryGeneration* ptrajectory,
                                  double lateral_i_grid = 0.0)
              : MRMPlanner<K,P>(lcv->getTargetLane(),plat,pveh,ptrajectory),            
                followVehicleConstraint_(lcv->getSourceLane(),pveh,ptac,ptrajectory),
                lateralReference_(lcv),
                LaneChangeIntoGapReference_(lcv,pveh,plat,lateral_i_grid),                
                lateralAccelerationReference_(lcv->getTargetLane()),
                lateralJerkReference_(lcv->getTargetLane()),
                lateralOffsetConstraint_left_(lcv,pveh,plat,ANominalConstraint::UB),
                lateralOffsetConstraint_right_(lcv,pveh,plat,ANominalConstraint::LB),
                headingConstraintUB_(plat,ANominalConstraint::UB),
                headingConstraintLB_(plat,ANominalConstraint::LB),
                lateralAccelerationConstraintUB_(lcv->getTargetLane(),plat,ANominalConstraint::UB),
                lateralAccelerationConstraintLB_(lcv->getTargetLane(),plat,ANominalConstraint::LB)
        {
          // this->getInformationSet().add(&followVehicleConstraint_);
          // this->getOffsetSolver().getInformationSet().add(&lateralReference_);
          this->getOffsetSolver().getInformationSet().add(&LaneChangeIntoGapReference_);
          this->getOffsetSolver().getInformationSet().add(&lateralAccelerationReference_);
          this->getOffsetSolver().getInformationSet().add(&lateralJerkReference_);
          this->getOffsetSolver().getInformationSet().add(&lateralOffsetConstraint_left_);
          this->getOffsetSolver().getInformationSet().add(&lateralOffsetConstraint_right_);
          this->getOffsetSolver().getInformationSet().add(&headingConstraintUB_);
          this->getOffsetSolver().getInformationSet().add(&headingConstraintLB_);
          this->getOffsetSolver().getInformationSet().add(&lateralAccelerationConstraintUB_);
          this->getOffsetSolver().getInformationSet().add(&lateralAccelerationConstraintLB_);
        }
 
        void setGap(adore::view::AGap* gap)
        {
          lateralOffsetConstraint_left_.setGap(gap);
          lateralOffsetConstraint_right_.setGap(gap);
          LaneChangeIntoGapReference_.setGap(gap);
        }
 
        void addConstraint(ANominalConstraint* constraint)
        {
          this->info_.add(constraint);
        }
 
        void addReference(ANominalReference* reference)
        {
          this->info_.add(reference);
        }
    };
  }
}