informationsetpostprocessing.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 "anominalplannerinformation.h"
#include <adore/mad/llinearpiecewisefunction.h>
 
namespace adore
{
  namespace fun
  {
      template<int N,int D>      
      class InformationSetPostProcessing
      {
        public:
            typedef NominalPlannerInformationSet<N+1,2> TInformationSet;
        private:
            TInformationSet info_;
        public://
            TInformationSet& getInformationSet()
            {
             return info_;
            }
            template<int K>
            bool isLongitudinalPlanValid(double t0_offset, double s0_offset,
                         adore::mad::LLinearPiecewiseFunctionM<double,K>* longitudinal_plan)
            {
                double trel,t,s,ds;
                auto& data = longitudinal_plan->getData();
                for(int i=0;i<data.nc();i++)
                {
                    trel = data(0,i);
                    t = trel + t0_offset;
                    s = data(1,i) + s0_offset;
                    ds = data(2,i);
                    auto value = longitudinal_plan->f(trel);
                    value(0) = value(0) + s0_offset;
                    
                    for(int der = 0; der<K && der<N; der++)//loop over derivative order
                    {
                        if(!(info_.getLB(0,der,t,s,ds)<=value(der) && value(der)<=info_.getUB(0,der,t,s,ds)))return false;
                    }
                }
                return true;
            }
            template<int K1,int K2>
            bool isLateralPlanValid(double t0_offset, double s0_offset,
                         adore::mad::LLinearPiecewiseFunctionM<double,K1>* longitudinal_plan,
                         adore::mad::LLinearPiecewiseFunctionM<double,K2>* lateral_plan)
            {
                double trel,t,s,ds;
                auto& data = longitudinal_plan->getData();
                for(int i=0;i<data.nc();i++)
                {
                    trel = data(0,i);
                    t = trel + t0_offset;
                    s = data(1,i) + s0_offset;
                    ds = data(2,i);
                    auto value = lateral_plan->f(trel);
 
                    for(int der = 0; der<K2 && der<N; der++)//loop over derivative order
                    {
                        if(!(info_.getLB(1,der,t,s,ds)<=value(der) && value(der)<=info_.getUB(1,der,t,s,ds)))return false;
                    }
                }
                return true;
            }
      };
  }
}