Commit 56b64caf authored by Uwe Woessner's avatar Uwe Woessner
Browse files

integrate the rest of Sensor_Modular_Driver


Signed-off-by: Uwe Woessner's avatarhpcwoess <woessner@hlrs.de>
parent fac110b3
/******************************************************************************
* Copyright (c) 2019 TU Dresden
* 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 ContainerStructures.h
//! @author Konstantin Blenz
//! @date Tue, 03.12.2019
//! @brief provide Container Structures for the driver's cognition
//!
//-----------------------------------------------------------------------------
//! \addtogroup Algorithm_ModularDriver
//-----------------------------------------------------------------------------
#pragma once
#include <string>
#include "Common/globalDefinitions.h"
#include "../Signals/complexsignals.h"
#include <list>
#include <memory>
#include "../Signals/surroundingmovingobjectsdata.h"
#include "../Signals/staticenvironmentdata.h"
#include "../Signals/egodata.h"
#include "agent_representation.h"
class AgentRepresentation;
class speedLimit
{
public:
double TransformSignToSpeed (CommonTrafficSign::Entity *sign)//match type of the signal (e.g. code according to StVO) to value
{
switch (sign->type)
{
case (274): //MaximumSpeedLimit
return sign->value / 3.6;
case (278): //EndOfMaximumSpeedLimit
return INFINITY;
case (310): //TownBegin
return 50 / 3.6;
case (311): //TownEnd
return 100 / 3.6;
case (2741): //Zone30Begin
return 30 / 3.6;
case (2742): //Zone30End
return 50 / 3.6;
case (3251): //TrafficCalmedDistrictBegin
return 4 / 3.6;
case (3252): //TrafficCalmedDistrictEnd
{
std::list<double>::iterator sl = SpeedLimits.begin();
std::advance(sl, 1);
if (*sl == 30 / 3.6)
return 30 / 3.6;
else
return 50 / 3.6;
}
default:
return -1;
}
}
void UpdateSpeedLimitAndDistance(double SpeedLimit, double Distance)
{
if (SpeedLimits.size()>0 && SpeedLimit != SpeedLimits.front())
{
SpeedLimits.push_front(SpeedLimit);
DistanceToSpeedLimits.push_front(Distance);
}
else if(DistanceToSpeedLimits.size()>0)
{
DistanceToSpeedLimits.front()=Distance;
}
}
double GetCurrentSpeedLimit()
{
if(SpeedLimits.size()==0)
{
SpeedLimits.push_front(INFINITY);
}
return SpeedLimits.front();
}
double GetCurrentDistanceToSign()
{
if(DistanceToSpeedLimits.size()==0)
{
DistanceToSpeedLimits.push_front(INFINITY);
}
return DistanceToSpeedLimits.front();
}
private:
std::list<double> SpeedLimits;
std::list<double> DistanceToSpeedLimits;
};
struct stopSign
{
bool exists = false;
CommonTrafficSign::Entity *Sign;
};
struct assessedEnvironment
{
StaticEnvironmentData *StaticEnvironment;
speedLimit SpeedLimit;
stopSign StopSign;
};
struct InformationAcquisition_Input_BU
{
StaticEnvironmentData StaticEnvironment;
std::list<SurroundingMovingObjectsData> SurroundingMovingObjects;
egoData EgoData;
};
struct InformationAcquisition_Output
{
egoData* Ego;
std::list<SurroundingMovingObjectsData> *SurroundingMovingObjects;
StaticEnvironmentData *EnvironmentInfo;
};
struct MentalModel_Input_BU
{
InformationAcquisition_Output *IA_O;
};
struct MentalModel_Output
{
egoData* Ego;
std::list <std::unique_ptr<AgentRepresentation>> *SurroundingMovingObjects={};
StaticEnvironmentData *EnvironmentInfo;
};
struct SitationAssessment_Input
{
MentalModel_Output *MM_O;
};
struct SituationAssessment_Output_TD //Top Down
{
};
struct SituationAssessment_Output_BU //Bottom Up
{
egoData* Ego;
std::map<RelationType, SurroundingMovingObjectsData> NearTraffic;
assessedEnvironment AssessedEnvironment;
};
struct ActionDeduction_Input
{
SituationAssessment_Output_BU *SA_O_BU;
MentalModel_Output *MM_O;
};
struct ActionDeduction_Output_TD //Top Down
{
};
struct ActionDeduction_Output_BU //Bottom Up
{
//! This flag states whether a collision has freshly occurred in the current time step.
bool notifyCollision;
//! the current gear of the current vehicle.
int *gear;
//! The velocity wish for the next time-step of the current vehicle in m/s.
double velocity_long_wish;
//! The acceleration wish for the next time-step of the current vehicle in m/s.
double acceleration_long_wish;
//! The lateral velocity of the current vehicle [m/s].
double out_lateral_speed = 0;
//! The relative lateral position of the vehicle [m].
double out_lateral_displacement = 0;
//! The gain for lateral displacement error controller [-].
double out_lateral_gain_displacement = 20.0;
//! The damping constant of the lateral oscillation of the vehicle [].
double out_lateral_damping = 0;
//! The lateral oscillation frequency of the vehicle [1/s].
double out_lateral_frequency = 0;
//! The heading angle error of the vehicle [rad].
double out_lateral_heading_error = 0;
//! The gain for heading error controller [-].
double out_lateral_gain_heading_error = 7.5;
double GainHeadingError;
double out_curvature;
double *steeringWheelAngle;
LaneChangeState LCState;
};
struct ActionExecution_Input
{
ActionDeduction_Output_BU *AD_O_BU;
MentalModel_Output *MM_O;
};
struct ActionExecution_Output_TD
{
};
struct ActionExecution_Output_BU
{
int out_gear = {0};
//! The state of the turning indicator [-].
int out_indicatorState = static_cast<int>(IndicatorState::IndicatorState_Off);
double out_accPP;
double out_brkPP;
double out_desiredSteeringWheelAngle;
double out_velocityX;
double out_YawAngle;
double out_YawVelocity;
};
struct MentalModel_Input_TD
{
SituationAssessment_Output_TD *SA_O_TD;
ActionDeduction_Output_TD *AD_O_TD;
ActionExecution_Output_TD *AE_O_TD;
};
struct InformationAcquisition_Input_TD
{
SituationAssessment_Output_TD *SA_O_TD;
ActionDeduction_Output_TD *AD_O_TD;
ActionExecution_Output_TD *AE_O_TD;
};
/******************************************************************************
* Copyright (c) 2019 TU Dresden
* 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 ContainerStructures.h
//! @author Konstantin Blenz
//! @date Tue, 03.12.2019
//! @brief provide Container Structures for the driver's cognition
//!
//-----------------------------------------------------------------------------
#pragma once
#include <string>
#include "Common/globalDefinitions.h"
#include "../Signals/complexsignals.h"
#include <list>
#include <memory>
#include "../Signals/surroundingmovingobjectsdata.h"
#include "../Signals/staticenvironmentdata.h"
#include "../Signals/egodata.h"
#include "agent_representation.h"
class AgentRepresentation;
//!
//! \brief The speedLimit class
//! Description and memory of the the speedlimit depending on the traffic sign
//!
class speedLimit
{
public:
//!
//! \brief TransformSignToSpeed
//! match type of the signal (e.g. code according to StVO) to value
//!
//! \param sign CommonTrafficSign::Entity
//! \return current speed limit depending on the type of the sign
//!
double TransformSignToSpeed (CommonTrafficSign::Entity *sign)
{
switch (sign->type)
{
case (274): //!MaximumSpeedLimit
return sign->value;
case (278): //!EndOfMaximumSpeedLimit
return INFINITY;
case (310): //!TownBegin
return 50 / 3.6;
case (311): //!TownEnd
return 100 / 3.6;
case (2741): //!Zone30Begin
return 30 / 3.6;
case (2742): //!Zone30End
return 50 / 3.6;
case (3251): //!TrafficCalmedDistrictBegin
return 4 / 3.6;
case (3252): //!TrafficCalmedDistrictEnd
{
std::list<double>::iterator sl = SpeedLimits.begin();
std::advance(sl, 1);
if (*sl == 30 / 3.6)
return 30 / 3.6;
else
return 50 / 3.6;
}
default:
return -1;
}
}
//!
//! \brief UpdateSpeedLimitAndDistance
//! Updates the current speedlimit depending on the distance to the agent
//!
//! \param SpeedLimit
//! \param Distance
//!
void UpdateSpeedLimitAndDistance(double SpeedLimit, double Distance)
{
if (SpeedLimits.size()>0 && SpeedLimit != SpeedLimits.front())
{
SpeedLimits.push_front(SpeedLimit);
DistanceToSpeedLimits.push_front(Distance);
}
else if(DistanceToSpeedLimits.size()>0)
{
DistanceToSpeedLimits.front()=Distance;
}
}
//!
//! \brief GetCurrentSpeedLimit
//! \return the current speed limit
//!
double GetCurrentSpeedLimit()
{
if(SpeedLimits.size()==0)
{
SpeedLimits.push_front(INFINITY);
}
return SpeedLimits.front();
}
//!
//! \brief GetCurrentDistanceToSign
//! \return the current distance to the sign
//!
double GetCurrentDistanceToSign()
{
if(DistanceToSpeedLimits.size()==0)
{
DistanceToSpeedLimits.push_front(INFINITY);
}
return DistanceToSpeedLimits.front();
}
private:
std::list<double> SpeedLimits; //! list of known speed-limits
std::list<double> DistanceToSpeedLimits; //! list of the distances to the known speed-limits
};
//!
//! \brief The stopSign struct
//! Stop-sign description with existance check
//!
struct stopSign
{
bool exists = false; /**< */
CommonTrafficSign::Entity *Sign; /**< */
};
//! Container with the information about the assessed environment
struct assessedEnvironment
{
StaticEnvironmentData *StaticEnvironment; /**< Object with information about the static environment */
speedLimit SpeedLimit; /**< current speedlimit */
stopSign StopSign; /**< currently next available stop-sign */
};
//! Container for the input of bottom-up information for the module "InformationAcquisition"
struct InformationAcquisition_Input_BU
{
StaticEnvironmentData StaticEnvironment; /**< Object with the information about the static environment */
std::list<SurroundingMovingObjectsData> SurroundingMovingObjects; /**< Container with the information about the surrounding moving objects */
egoData EgoData; /**< Object with the information about the current agent */
};
//! Container for the output of the module "InformationAcquisition"
struct InformationAcquisition_Output
{
egoData* Ego; /**< Object with the information about the current agent */
std::list<SurroundingMovingObjectsData> *SurroundingMovingObjects; /**< Container with the information about the surrounding moving objects */
StaticEnvironmentData *EnvironmentInfo; /**< Object with the information about the static environment */
};
//! Container for the input of bottom-up information for the module "MentalModel"
struct MentalModel_Input_BU
{
InformationAcquisition_Output *IA_O; /**< Object of the output of the module "InformationAcquisition" */
};
//! Container for the output of the module "MentalModel"
struct MentalModel_Output
{
egoData* Ego; /**< Object with the information about the current agent */
std::list <std::unique_ptr<AgentRepresentation>> *SurroundingMovingObjects={}; /**< Container with the agent representations of the surrounding moving objects */
StaticEnvironmentData *EnvironmentInfo; /**< Object with the information about the static environment */
};
//! Container for the input of the module "SituationAssessment"
struct SitationAssessment_Input
{
MentalModel_Output *MM_O; /**< Object of the output of the module "MentalModel" */
};
//! Container for the top-down output of the module "SituationAssessment" (currently empty)
struct SituationAssessment_Output_TD
{
};
//! Container for the bottom-up output of the module "SituationAssessment"
struct SituationAssessment_Output_BU
{
egoData* Ego; /**< Object with the information about the current agent */
std::map<RelationType, SurroundingMovingObjectsData> NearTraffic; /**< Map with the assessed near traffic with their relation-type to the current agent */
assessedEnvironment AssessedEnvironment; /**< Container with the information about the assessed environment */
};
//! Container for the input of the module "ActionDeduction"
struct ActionDeduction_Input
{
SituationAssessment_Output_BU *SA_O_BU; /**< Object of the bottom-up output of the module "SituationAssessment" */
MentalModel_Output *MM_O; /**< Object of the output of the module "MentalModel" */
};
//! Container for the top-down output of the module "ActionDeduction" (currently empty)
struct ActionDeduction_Output_TD
{
};
//! Container for the bottom-up output of the module "ActionDeduction"
struct ActionDeduction_Output_BU
{
//! This flag states whether a collision has freshly occurred in the current time step.
bool notifyCollision;
//! the current gear of the current vehicle.
int *gear;
//! The velocity wish for the next time-step of the current vehicle in m/s.
double velocity_long_wish;
//! The acceleration wish for the next time-step of the current vehicle in m/s.
double acceleration_long_wish;
//! The lateral velocity of the current vehicle [m/s].
double out_lateral_speed = 0;
//! The relative lateral position of the vehicle [m].
double out_lateral_displacement = 0;
//! The gain for lateral displacement error controller [-].
double out_lateral_gain_displacement = 20.0;
//! The damping constant of the lateral oscillation of the vehicle [].
double out_lateral_damping = 0;
//! The lateral oscillation frequency of the vehicle [1/s].
double out_lateral_frequency = 0;
//! The heading angle error of the vehicle [rad].
double out_lateral_heading_error = 0;
//! The gain for heading error controller [-].
double out_lateral_gain_heading_error = 7.5;
double GainHeadingError;
double out_curvature;
double *steeringWheelAngle;
//! lane-change status decision
LaneChangeState LCState;
};
//! Container for the input of the module "ActionExecution"
struct ActionExecution_Input
{
ActionDeduction_Output_BU *AD_O_BU; /**< Object of the bottom-up output of the module "ActionDeduction" */
MentalModel_Output *MM_O; /**< Object of the output of the module "MentalModel" */
};
//! Container for the top-down output of the module "ActionExecution" (currently empty)
struct ActionExecution_Output_TD
{
};
//! Container for the bottom-up output of the module "ActionExecution"
struct ActionExecution_Output_BU
{
int out_gear = {0}; /**< decision for the selected gear */
int out_indicatorState = static_cast<int>(IndicatorState::IndicatorState_Off); /**< The state of the turning indicator [-]. */
double out_accPP; /**< decision for the acceleration-pedal position */
double out_brkPP; /**< decision for the brake-pedal position */
double out_desiredSteeringWheelAngle; /**< decision for the steering wheel angle */
double out_velocityX; /**< current longitudinal velocity [m/s] */
double out_YawAngle; /**< current yaw angle */
double out_YawVelocity; /**< current yaw-velocity */
};
//! Container for the top-down input of the module "MentalModel"
struct MentalModel_Input_TD
{
SituationAssessment_Output_TD *SA_O_TD; /**< Object of the top-down output of the module "SituationAssessment" */
ActionDeduction_Output_TD *AD_O_TD; /**< Object of the top-down output of the module "ActionDeduction" */
ActionExecution_Output_TD *AE_O_TD; /**< Object of the top-down output of the module "ActionExecution" */
};
//! Container for the top-down input of the module "InformationAcquisition"
struct InformationAcquisition_Input_TD
{
SituationAssessment_Output_TD *SA_O_TD; /**< Object of the top-down output of the module "SituationAssessment" */
ActionDeduction_Output_TD *AD_O_TD; /**< Object of the top-down output of the module "ActionDeduction" */
ActionExecution_Output_TD *AE_O_TD; /**< Object of the top-down output of the module "ActionExecution" */
};
/******************************************************************************
* Copyright (c) 2019 TU Dresden
* 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 MentalModelLane.cpp
//! @author Christian Siebke
//! @author Konstantin Blenz
//! @date Tue, 03.12.2019
//! @brief class to provide geometrical information of the lanes
//!
//-----------------------------------------------------------------------------
//! \addtogroup Algorithm_ModularDriver
//-----------------------------------------------------------------------------
#include "MentalModelLane.h"
#include "algorithm"
MentalModelLane::MentalModelLane(uint64_t id, int64_t odId, double l, double w) :
laneId(id), odId(odId), length(l), width(w) {}
uint64_t MentalModelLane::GetId() const {
return laneId;
}
int64_t MentalModelLane::GetOdId() {
return odId;
}
double MentalModelLane::GetLength() {