Commit 255e3331 authored by Uwe Woessner's avatar Uwe Woessner
Browse files

added missing files


Signed-off-by: Uwe Woessner's avatarhpcwoess <woessner@hlrs.de>
parent 3c4e6451
/******************************************************************************
* Copyright (c) 2018 in-tech GmbH
*
* This program and the accompanying materials are made available under the
* terms of the Eclipse Public License 2.0 which is available at
* https://www.eclipse.org/legal/epl-2.0/
*
* SPDX-License-Identifier: EPL-2.0
******************************************************************************/
#include "Algorithm_ModularDriver.h"
#include "src/Algorithm_ModularDriver_implementation.h"
const std::string Version = "0.0.1";
static const CallbackInterface *Callbacks = nullptr;
extern "C" ALGORITHM_MODULARDRIVER_SHARED_EXPORT const std::string &OpenPASS_GetVersion()
{
return Version;
}
extern "C" ALGORITHM_MODULARDRIVER_SHARED_EXPORT ModelInterface *OpenPASS_CreateInstance(
std::string componentName,
bool isInit,
int priority,
int offsetTime,
int responseTime,
int cycleTime,
StochasticsInterface *stochastics,
WorldInterface *world,
const ParameterInterface *parameters,
PublisherInterface *const publisher,
AgentInterface *agent,
const CallbackInterface *callbacks)
{
Callbacks = callbacks;
try
{
return (ModelInterface*)(new (std::nothrow) AlgorithmModularDriverImplementation(
componentName,
isInit,
priority,
offsetTime,
responseTime,
cycleTime,
stochastics,
world,
parameters,
publisher,
callbacks,
agent));
}
catch(const std::runtime_error &ex)
{
if(Callbacks != nullptr)
{
Callbacks->Log(CbkLogLevel::Error, __FILE__, __LINE__, ex.what());
}
return nullptr;
}
catch(...)
{
if(Callbacks != nullptr)
{
Callbacks->Log(CbkLogLevel::Error, __FILE__, __LINE__, "unexpected exception");
}
return nullptr;
}
}
extern "C" ALGORITHM_MODULARDRIVER_SHARED_EXPORT void OpenPASS_DestroyInstance(ModelInterface *implementation)
{
delete (AlgorithmModularDriverImplementation*)implementation;
}
extern "C" ALGORITHM_MODULARDRIVER_SHARED_EXPORT bool OpenPASS_UpdateInput(
ModelInterface *implementation,
int localLinkId,
const std::shared_ptr<SignalInterface const> &data,
int time)
{
try
{
implementation->UpdateInput(localLinkId, data, time);
}
catch(const std::runtime_error &ex)
{
if(Callbacks != nullptr)
{
Callbacks->Log(CbkLogLevel::Error, __FILE__, __LINE__, ex.what());
}
return false;
}
catch(...)
{
if(Callbacks != nullptr)
{
Callbacks->Log(CbkLogLevel::Error, __FILE__, __LINE__, "unexpected exception");
}
return false;
}
return true;
}
extern "C" ALGORITHM_MODULARDRIVER_SHARED_EXPORT bool OpenPASS_UpdateOutput(
ModelInterface *implementation,
int localLinkId,
std::shared_ptr<SignalInterface const> &data,
int time)
{
try
{
implementation->UpdateOutput(localLinkId, data, time);
}
catch(const std::runtime_error &ex)
{
if(Callbacks != nullptr)
{
Callbacks->Log(CbkLogLevel::Error, __FILE__, __LINE__, ex.what());
}
return false;
}
catch(...)
{
if(Callbacks != nullptr)
{
Callbacks->Log(CbkLogLevel::Error, __FILE__, __LINE__, "unexpected exception");
}
return false;
}
return true;
}
extern "C" ALGORITHM_MODULARDRIVER_SHARED_EXPORT bool OpenPASS_Trigger(
ModelInterface *implementation,
int time)
{
try
{
implementation->Trigger(time);
}
catch(const std::runtime_error &ex)
{
if(Callbacks != nullptr)
{
Callbacks->Log(CbkLogLevel::Error, __FILE__, __LINE__, ex.what());
}
return false;
}
catch(const std::exception& exstd)
{
if(Callbacks != nullptr)
{
Callbacks->Log(CbkLogLevel::Error, __FILE__, __LINE__, exstd.what());
}
return false;
}
catch(...)
{
if(Callbacks != nullptr)
{
#ifndef _MSC_VER
std::exception_ptr p = std::current_exception();
const std::string exType = p ? p.__cxa_exception_type()->name() : "null";
#else
const std::string exType = "null";
#endif
Callbacks->Log(CbkLogLevel::Error, __FILE__, __LINE__, "unexpected exception - type: " + exType);
}
return false;
}
return true;
}
/******************************************************************************
* Copyright (c) 2019 AMFD GmbH
*
* This program and the accompanying materials are made available under the
* terms of the Eclipse Public License 2.0 which is available at
* https://www.eclipse.org/legal/epl-2.0/
*
* SPDX-License-Identifier: EPL-2.0
*****************************************************************************/
//! @file AlgorithmModularDriver.h
/// @author Konstantin Blenz
/// @date Tue, 03.12.2019
//! @brief This file contains the implementation header file
#pragma once
#include "src/Algorithm_ModularDriver_global.h"
#include "include/modelInterface.h"
#-----------------------------------------------------------------------------
# /file AlgorithmModularDriver.pro
# /brief This file contains the information for the QtCreator-project of the
# module AlgorithmModularDriver
#
# Copyright (c) 2018 in-tech GmbH
#
#-----------------------------------------------------------------------------/
DEFINES += ALGORITHM_MODULARDRIVER_LIBRARY
CONFIG += OPENPASS_LIBRARY
include(../../../global.pri)
SUBDIRS += . \
../Sensor_Modular_Driver/src/Signals \
../Sensor_Modular_Driver/src/Container \
src/ActionDeductionMethods \
src/SituationAssessmentMethods \
../../Common \
../../Interfaces \
../Sensor_Driver/src/Signals \
..
INCLUDEPATH += $$SUBDIRS \
../Algorithm_Longitudinal \
..
SOURCES += \
$$getFiles(SUBDIRS, cpp) \
$$getFiles(SUBDIRS, cc) \
$$getFiles(SUBDIRS, c) \
../Algorithm_Longitudinal/src/algorithm_longitudinalCalculations.cpp \
HEADERS += \
$$getFiles(SUBDIRS, hpp) \
$$getFiles(SUBDIRS, h) \
../Algorithm_Longitudinal/src/algorithm_longitudinalCalculations.h \
/****************************************************************************/
// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
// Copyright (C) 2001-2018 German Aerospace Center (DLR) and others.
// This program and the accompanying materials
// are made available under the terms of the Eclipse Public License v2.0
// which accompanies this distribution, and is available at
// http://www.eclipse.org/legal/epl-v20.html
// SPDX-License-Identifier: EPL-2.0
/****************************************************************************/
/// @file AbstractLaneChangeModel.cpp
/// @author Daniel Krajzewicz
/// @author Friedemann Wesner
/// @author Sascha Krieg
/// @author Michael Behrisch
/// @author Jakob Erdmann
/// @author Leonhard Luecken
/// @date Fri, 29.04.2005
/// @version $Id$
///
// Interface for lane-change models
/****************************************************************************/
// ===========================================================================
// DEBUG
// ===========================================================================
//#define DEBUG_TARGET_LANE
//#define DEBUG_SHADOWLANE
// ===========================================================================
// included modules
// ===========================================================================
#include "AbstractLaneChangeModel.h"
#include "LCM_LC2013.h"
#include "../agentParameters.h"
/* -------------------------------------------------------------------------
* static members
* ----------------------------------------------------------------------- */
bool AbstractLaneChangeModel::myAllowOvertakingRight(false);
bool AbstractLaneChangeModel::myLCOutput(false);
bool AbstractLaneChangeModel::myLCStartedOutput(false);
bool AbstractLaneChangeModel::myLCEndedOutput(false);
const double AbstractLaneChangeModel::NO_NEIGHBOR(std::numeric_limits<double>::max());
/* -------------------------------------------------------------------------
* AbstractLaneChangeModel-methods
* ----------------------------------------------------------------------- */
AbstractLaneChangeModel* AbstractLaneChangeModel::build(int lcmId, double Cycletime, double SpeedGain, double KeepRight, double Cooperative, CFModel *CarFollowingModel, const agentParameters vehicleParameters)
{
switch (lcmId)
{
case (0):
return new LCM_LC2013(Cycletime, SpeedGain, KeepRight, Cooperative, CarFollowingModel, vehicleParameters);
default:
return new LCM_LC2013(Cycletime, SpeedGain, KeepRight, Cooperative, CarFollowingModel, vehicleParameters);
}
}
AbstractLaneChangeModel::AbstractLaneChangeModel(double Cycletime, const LaneChangeModel model, const CFModel *CarFollowingModel, agentParameters vehicleParameters) :
myOwnState(0),
myPreviousState(0),
myPreviousState2(0),
mySpeedLat(0),
myCommittedSpeed(0),
myLaneChangeCompletion(1.0),
myLaneChangeDirection(0),
myManeuverDist(0.),
myAlreadyChanged(false),
myShadowLane(nullptr),
myLastLateralGapLeft(0.),
myLastLateralGapRight(0.),
myLastLeaderGap(0.),
myLastFollowerGap(0.),
myLastLeaderSecureGap(0.),
myLastFollowerSecureGap(0.),
myLastOrigLeaderGap(0.),
myLastOrigLeaderSecureGap(0.),
myCarFollowModel(CarFollowingModel),
Cycletime(Cycletime),
vehicleParameters(vehicleParameters),
myModel(model),
myAmOpposite(false)
{
myAllowOvertakingRight=false;
}
AbstractLaneChangeModel::~AbstractLaneChangeModel() {
}
void AbstractLaneChangeModel::setOwnState(const int state) {
myPreviousState2 = myPreviousState;
myOwnState = state;
myPreviousState = state; // myOwnState is modified in prepareStep so we make a backup
}
void
AbstractLaneChangeModel::updateSafeLatDist(const double travelledLatDist) {
}
void
AbstractLaneChangeModel::setManeuverDist(const double dist) {
myManeuverDist = dist;
}
double
AbstractLaneChangeModel::getManeuverDist() const {
return myManeuverDist;
}
bool AbstractLaneChangeModel::congested(SurroundingMovingObjectsData* neighLeader, egoData* agent) {
if (neighLeader == 0) {
return false;
}
// Congested situation are relevant only on highways (maxSpeed > 70km/h)
// and congested on German Highways means that the vehicles have speeds
// below 60km/h. Overtaking on the right is allowed then.
if ((vehicleParameters.maxVelocity <= 70.0 / 3.6)) {
return false;
}
return false;
}
bool AbstractLaneChangeModel::predInteraction(bool lexists, SurroundingMovingObjectsData* leader, egoData* agent) {
if (!lexists) {
return false;
}
// let's check it on highways only
if (leader->GetState()->velocity_long < (80.0 / 3.6)) {
return false;
}
return *leader->GetDistanceToEgo() < myCarFollowModel->interactionGap(&agent->GetState_Ego()->velocity_long, &vehicleParameters.maxVelocity, &vehicleParameters.maxAcceleration, &leader->GetState()->velocity_long);
}
double
AbstractLaneChangeModel::computeSpeedLat(egoData* agent, double& maneuverDist) {
return agent->GetState()->velocity_y;
}
double
AbstractLaneChangeModel::getAssumedDecelForLaneChangeDuration() const {
return 0.0;
}
bool
AbstractLaneChangeModel::cancelRequest(int state, int laneOffset) {
// store request before canceling
myCanceledStates[laneOffset] |= state;
int ret = state;
return ret != state;
}
int AbstractLaneChangeModel::getWaitingSeconds(double velocity_x)
{
if (velocity_x<0.1)
{
tWaiting = tWaiting + 100;
}
else
{
tWaiting = 0;
}
return tWaiting;
}
double
AbstractLaneChangeModel::estimateLCDuration(const double speed, const double remainingManeuverDist, const double decel, const driverInformation* agent) const {
if(remainingManeuverDist==0)
{
return 0;
}
return remainingManeuverDist/ *agent->v_y_Max;
}
void
AbstractLaneChangeModel::setFollowerGaps(bool fexists, SurroundingMovingObjectsData* follower, double secGap, egoData* agent) {
if (fexists) {
myLastFollowerGap = agent->GetState()->roadPos.s + vehicleParameters.distanceReferencePointToLeadingEdge - vehicleParameters.length - follower->GetState()->roadPos.s - *agent->GetDriverInformation()->MinGap - follower->GetProperties()->distanceReftoLeadingEdge;
myLastFollowerSecureGap = secGap;
}
}
void
AbstractLaneChangeModel::setLeaderGaps(bool lexists, SurroundingMovingObjectsData* leader, double secGap, egoData* agent) {
if (lexists) {
myLastLeaderGap = leader->GetState()->roadPos.s + leader->GetProperties()->distanceReftoLeadingEdge - leader->GetProperties()->lx - agent->GetState()->roadPos.s - *agent->GetDriverInformation()->MinGap - vehicleParameters.distanceReferencePointToLeadingEdge;
myLastLeaderSecureGap = secGap;
}
}
void
AbstractLaneChangeModel::setOrigLeaderGaps(bool lexists, SurroundingMovingObjectsData* leader, double secGap, egoData* agent) {
if (lexists) {
myLastOrigLeaderGap = leader->GetState()->roadPos.s + leader->GetProperties()->distanceReftoLeadingEdge - leader->GetProperties()->lx - agent->GetState()->roadPos.s - *agent->GetDriverInformation()->MinGap - vehicleParameters.distanceReferencePointToLeadingEdge;
myLastOrigLeaderSecureGap = secGap;
}
}
/****************************************************************************/
// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
// Copyright (C) 2012-2018 German Aerospace Center (DLR) and others.
// This program and the accompanying materials
// are made available under the terms of the Eclipse Public License v2.0
// which accompanies this distribution, and is available at
// http://www.eclipse.org/legal/epl-v20.html
// SPDX-License-Identifier: EPL-2.0
/****************************************************************************/
/// @file CFModel_Daniel1.cpp
/// @author Daniel Krajzewicz
/// @author Jakob Erdmann
/// @author Michael Behrisch
/// @date Tue, 05 Jun 2012
/// @version $Id$
///
// The original Krauss (1998) car-following model and parameter
/****************************************************************************/
/**
* @ingroup alg_ac_ded
* @defgroup cf_model Car-follwing Model: Daniel1
* * The original Krauss (1998) car-following model and parameter - SUMO
*/
// ===========================================================================
// included modules
// ===========================================================================
#include "CFModel_Daniel1.h"
#include "AbstractLaneChangeModel.h"
// ===========================================================================
// method definitions
// ===========================================================================
CFModel_Daniel1::CFModel_Daniel1(double Accel, double Decel, double maxAccel, double maxDecel, double Headwaytime, double CycleTime) :
CFModel(Accel, Decel, maxAccel, maxDecel, Headwaytime, CycleTime),
myDawdle(0),
myTauDecel(-myDecel * myHeadwayTime)
{ }
CFModel_Daniel1::~CFModel_Daniel1() {}
double CFModel_Daniel1::finalizeSpeed(const double *v_x, const double *v_x_max, const double *a_x_max, double *vPos, const double *MinGap, const AbstractLaneChangeModel *LCModel) const {
const double oldV = *v_x; // save old v for optional acceleration computation
const double vSafe = *vPos; // process stops
// we need the acceleration for emission computation;
// in this case, we neglect dawdling, nonetheless, using
// vSafe does not incorporate speed reduction due to interaction
// on lane changing
const double vMin = getSpeedAfterMaxDecel(oldV);
const double vMax = std::min(std::min(*v_x_max, maxNextSpeed(*v_x, *v_x_max, *a_x_max)), vSafe);
return LCModel->patchSpeed(&vMin, std::max(vMin, vMax), &vMax, v_x, v_x_max, a_x_max, this, MinGap); // agent->GetLaneChangeModel()->patchSpeed(vMin, std::max(vMin, vMax), vMax, *this);
}
double CFModel_Daniel1::followSpeed(const double &speed, const double &gap, const double &predSpeed, const double &v_x_max) const {
return std::min(_vsafe(gap, predSpeed),maxNextSpeed(speed, v_x_max, myAccel));
}
double CFModel_Daniel1::stopSpeed(const double *speed, const double *gap, const double *v_x_max) const {
double v = 0;
return std::min(_vsafe(*gap, v), maxNextSpeed(*speed, *v_x_max, myAccel));
}
double CFModel_Daniel1::dawdle(double speed) const {
return std::max(0., speed + CycleTime*(0 * -myDecel * 1));
}
/** Returns the SK-vsafe. */
double CFModel_Daniel1::_vsafe(const double &gap, const double &predSpeed) const {
if (predSpeed <= 0.5 && gap < 2) {
return 0;
}
double vsafe = (double)(-1. * myTauDecel + sqrt((myTauDecel * myTauDecel) + (predSpeed * predSpeed) + (2. * -myDecel * gap)));
return vsafe;
}
CFModel* CFModel_Daniel1::duplicate(double Accel, double Decel, double maxAccel, double maxDecel, double Headwaytime, double CycleTime) const {
return new CFModel_Daniel1(Accel, Decel, maxAccel, maxDecel, Headwaytime, CycleTime);
}