Commit 83eea9ae authored by Uwe Woessner's avatar Uwe Woessner
Browse files

add Modular Driver


Signed-off-by: Uwe Woessner's avatarhpcwoess <woessner@hlrs.de>
parent 07791bbf
......@@ -24,6 +24,7 @@ add_openpass_target(
src/AlgorithmInformationAcquisition.h
src/AlgorithmMentalModel.h
src/AlgorithmSituationAssessment.h
src/agentParameters.h
src/ActionDeductionMethods/AbstractLaneChangeModel.h
src/ActionDeductionMethods/CFModel.h
src/ActionDeductionMethods/CFModel_Daniel1.h
......@@ -41,6 +42,7 @@ add_openpass_target(
src/AlgorithmInformationAcquisition.cpp
src/AlgorithmMentalModel.cpp
src/AlgorithmSituationAssessment.cpp
src/agentParameters.cpp
src/ActionDeductionMethods/AbstractLaneChangeModel.cpp
src/ActionDeductionMethods/CFModel.cpp
src/ActionDeductionMethods/CFModel_Daniel1.cpp
......
......@@ -29,14 +29,15 @@
#pragma once
//#include <vector>
#include <Interfaces/stochasticsInterface.h>
#include <Interfaces/observationInterface.h>
#include <include/stochasticsInterface.h>
#include <include/observationInterface.h>
#include "../../Sensor_Modular_Driver/src/Container/ContainerStructures.h"
#include "ActionDeductionMethods/CFModel.h"
#include "ActionDeductionMethods/CFModel_Daniel1.h"
#include "ActionDeductionMethods/AbstractLaneChangeModel.h"
#include "ActionDeductionMethods/LCM_LC2013.h"
#include "ActionDeductionMethods/TargetBraking.h"
#include "agentParameters.h"
//! \brief This class contains the data calculations of possible actions
//!
......@@ -306,7 +307,7 @@ public:
//! \brief SetVehicleParameters
//! \param vehicleParameters
//!
void SetVehicleParameters (const VehicleModelParameters *vehicleParameters)
void SetVehicleParameters (const agentParameters *vehicleParameters)
{
this->vehicleParameters = vehicleParameters;
}
......@@ -444,7 +445,7 @@ private:
//! parameters for the lateral acceleration wish [m/s]
const LateralDynamicConstants lateralDynamicConstants;
//!
const VehicleModelParameters *vehicleParameters;
const agentParameters *vehicleParameters;
};
......
......@@ -25,8 +25,8 @@
//#include <vector>
#include <iostream>
#include "components/Sensor_Modular_Driver/src/Container/ContainerStructures.h"
#include <Interfaces/stochasticsInterface.h>
#include <Interfaces/observationInterface.h>
#include <include/stochasticsInterface.h>
#include <include/observationInterface.h>
//! \brief This class contains the acquisition of informations like visual and auditive.
//! Singleton such that data calculations is generated only once and not seperately
......
......@@ -28,7 +28,7 @@
//#include <vector>
#include "../../Sensor_Modular_Driver/src/Container/ContainerStructures.h"
#include <Interfaces/stochasticsInterface.h>
#include <include/stochasticsInterface.h>
//! \brief This class conains the mental representation and extrapolation of the environment
//!
......
......@@ -19,8 +19,9 @@
#pragma once
//#include <vector>
#include <Interfaces/stochasticsInterface.h>
#include <Interfaces/observationInterface.h>
#include <include/stochasticsInterface.h>
#include <include/observationInterface.h>
#include "agentParameters.h"
#include "../../Sensor_Modular_Driver/src/Container/ContainerStructures.h"
#include "SituationAssessmentMethods/SituationCalculation.h"
#include "SituationAssessmentMethods/SituationLogging.h"
......@@ -364,7 +365,7 @@ public:
//! \brief SetVehicleParameters
//! \param vehicleParameters
//!
void SetVehicleParameters(const VehicleModelParameters *vehicleParameters)
void SetVehicleParameters(const agentParameters *vehicleParameters)
{
this->vehicleParameters = vehicleParameters;
}
......
......@@ -85,13 +85,14 @@
#include <string>
#include <iostream>
//#include <map>
#include <Interfaces/modelInterface.h>
#include <include/modelInterface.h>
#include "common/primitiveSignals.h"
#include "AlgorithmInformationAcquisition.h"
#include "AlgorithmMentalModel.h"
#include "AlgorithmSituationAssessment.h"
#include "AlgorithmActionDeduction.h"
#include "AlgorithmActionExecution.h"
#include "agentParameters.h"
/**
......@@ -127,7 +128,7 @@ public:
StochasticsInterface *stochastics,
WorldInterface *world,
const ParameterInterface *parameters,
const std::map<int, ObservationInterface*> *observations,
PublisherInterface *const publisher,
const CallbackInterface *callbacks,
AgentInterface *agent);
......@@ -214,7 +215,7 @@ public:
* \param vehicleParameters Parameters of the ego-vehicle
* \param time Current scheduling time
*/
void Situation_Assessment (SitationAssessment_Input *SA_I, SituationAssessment_Output_BU *SA_O_BU, SituationAssessment_Output_TD *&SA_O_TD, const VehicleModelParameters *vehicleParameters, int time);
void Situation_Assessment (SitationAssessment_Input *SA_I, SituationAssessment_Output_BU *SA_O_BU, SituationAssessment_Output_TD *&SA_O_TD, const agentParameters *vehicleParameters, int time);
/*!
* \brief Action_Deduction
......@@ -228,7 +229,7 @@ public:
* \param vehicleParameters Parameters of the ego-vehicle
* \param time Current scheduling time
*/
void Action_Deduction (ActionDeduction_Input *AD_I, ActionDeduction_Output_BU *AD_O_BU, ActionDeduction_Output_TD *AD_O_TD, const VehicleModelParameters *vehicleParameters, int time);
void Action_Deduction(ActionDeduction_Input *AD_I, ActionDeduction_Output_BU *AD_O_BU, ActionDeduction_Output_TD *AD_O_TD, const agentParameters *vehicleParameters, int time);
/*!
* \brief Action_Execution
......@@ -242,7 +243,7 @@ public:
* \param vehicleParameters Parameters of the ego-vehicle
* \param time Current scheduling time
*/
void Action_Execution (ActionExecution_Input *AE_I, ActionExecution_Output_BU *AE_O_BU, ActionExecution_Output_TD *&AE_O_TD, const VehicleModelParameters *vehicleParameters, int time);
void Action_Execution(ActionExecution_Input *AE_I, ActionExecution_Output_BU *AE_O_BU, ActionExecution_Output_TD *&AE_O_TD, const agentParameters *vehicleParameters, int time);
//----------------------------------------------------------------
......@@ -312,7 +313,7 @@ private:
//! Flag that indicates the need to initialize the ActionExcecution - e.g parsing input informations
bool initialisationAE = false;
VehicleModelParameters vehicleParameters;
agentParameters vehicleParameters;
StochasticsInterface *stochastic;
......
......@@ -18,6 +18,7 @@ add_subdirectory(AgentUpdater)
add_subdirectory(AlgorithmAFDM)
add_subdirectory(Algorithm_AEB)
#add_subdirectory(Algorithm_CruiseControlByDistance)
add_subdirectory(Algorithm_ModularDriver)
add_subdirectory(Algorithm_FmuWrapper)
add_subdirectory(Algorithm_Lateral)
add_subdirectory(Algorithm_Longitudinal)
......@@ -33,6 +34,7 @@ add_subdirectory(OpenScenarioActions)
add_subdirectory(Parameters_Vehicle)
add_subdirectory(SensorAggregation_OSI)
add_subdirectory(SensorFusionErrorless_OSI)
add_subdirectory(Sensor_Modular_Driver)
#add_subdirectory(Sensor_Distance)
add_subdirectory(Sensor_Driver)
add_subdirectory(Sensor_OSI)
......
......@@ -29,9 +29,9 @@
#include <vector>
#include <string>
#include "common/globalDefinitions.h"
#include <Interfaces/signalInterface.h>
#include <include/signalInterface.h>
#include "egodata.h"
#include "../../../Sensor_Driver/Signals/sensor_driverDefinitions.h"
#include "../../../Sensor_Driver/src/Signals/sensor_driverDefinitions.h"
#include "../Container/MentalModelLane.h"
class MentalModelLane;
......
......@@ -336,7 +336,7 @@ void Sensor_Modular_Driver_Implementation::SetEgoData()
/** @endcode */
}
void Sensor_Modular_Driver_Implementation::SetMovingObjects(AgentInterface *agent, const std::map<int, AgentInterface*> *Agents)
void Sensor_Modular_Driver_Implementation::SetMovingObjects(const std::map<int, AgentInterface*> *Agents)
{
/** @addtogroup surr_sensing
* - Clear the Output Container
......@@ -348,18 +348,30 @@ void Sensor_Modular_Driver_Implementation::SetMovingObjects(AgentInterface *agen
* - Sort agents by their distance to the egos road-position
* - Select the 10 nearest agents (memory)
*/
double sightdistance = GetWorld()->GetVisibilityDistance();
std::list<std::pair<int,int>> AgentAndDist;
double roadPos_s = agent->GetRoadPosition().s;
for (std::map<int, AgentInterface*>::const_iterator it=Agents->begin(); it!=Agents->end(); it++)
//!!!!!!!!!!!!!!!!!!!!TODO check if this there is no easier and better way to do this
const auto &roadIds = GetAgent()->GetRoads(MeasurementPoint::Front);
if (roadIds.empty())
{
double absdist2ego = abs(roadPos_s-it->second->GetRoadPosition().s);
AgentAndDist.push_back(std::make_pair(absdist2ego, it->first));
return;
}
double roadPos_s = GetAgent()->GetObjectPosition().mainLocatePoint.at(roadIds.front()).roadPosition.s;
for (auto it=Agents->begin(); it!=Agents->end(); it++)
{
const auto &roadIds = GetAgent()->GetRoads(MeasurementPoint::Front);
if (!roadIds.empty())
{
double absdist2ego = abs(roadPos_s - it->second->GetObjectPosition().mainLocatePoint.at(roadIds.front()).roadPosition.s);
AgentAndDist.push_back(std::make_pair(absdist2ego, it->first));
}
}
AgentAndDist.sort();
std::list<std::pair<int,int>>::iterator it1, it2;
it1 = AgentAndDist.begin();
it2 = AgentAndDist.end();
auto it1 = AgentAndDist.begin();
auto it2 = AgentAndDist.end();
//size_t disttoend = AgentAndDist.size();
//size_t maxsize = disttoend - 10;
//if (maxsize > 0 )
......@@ -485,7 +497,7 @@ LaneInformationTrafficRules Sensor_Modular_Driver_Implementation::GetTrafficRule
LaneInformationTrafficRules laneInformation;
const double visibilityDistance = GetWorld()->GetVisibilityDistance();
laneInformation.trafficSigns = GetAgent()->GetTrafficSignsInRange(visibilityDistance);
laneInformation.trafficSigns = GetAgent()->GetEgoAgent().GetTrafficSignsInRange(visibilityDistance);
return laneInformation;
}
......@@ -496,7 +508,7 @@ LaneInformationTrafficRules Sensor_Modular_Driver_Implementation::GetTrafficRule
const int relativeLaneId = 1;
const double visibilityDistance = GetWorld()->GetVisibilityDistance();
laneInformation.trafficSigns = GetAgent()->GetTrafficSignsInRange(visibilityDistance, relativeLaneId);
laneInformation.trafficSigns = GetAgent()->GetEgoAgent().GetTrafficSignsInRange(visibilityDistance, relativeLaneId);
return laneInformation;
}
......@@ -507,7 +519,7 @@ LaneInformationTrafficRules Sensor_Modular_Driver_Implementation::GetTrafficRule
const int relativeLaneId = -1;
const double visibilityDistance = GetWorld()->GetVisibilityDistance();
laneInformation.trafficSigns = GetAgent()->GetTrafficSignsInRange(visibilityDistance, relativeLaneId);
laneInformation.trafficSigns = GetAgent()->GetEgoAgent().GetTrafficSignsInRange(visibilityDistance, relativeLaneId);
return laneInformation;
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment