Commit b55761cb authored by konstantin blenz's avatar konstantin blenz
Browse files

merge of modular-driver v.0.7-update to hlrs branch

parent 1eb61651
......@@ -35,6 +35,8 @@
//#include <map>
#include <iostream>
#include "AlgorithmMentalModel.h"
#include "../../Sensor_Modular_Driver/src/Container/agent_representation.cpp"
#include "../../Sensor_Modular_Driver/src/Container/MentalModelLane.cpp"
MentalModel::MentalModel(StochasticsInterface *stochastics):
_stochastic(stochastics)
......
......@@ -27,8 +27,8 @@
#pragma once
//#include <vector>
#include "Container/ContainerStructures.h"
#include "Interfaces/stochasticsInterface.h"
#include "../../Sensor_Modular_Driver/src/Container/ContainerStructures.h"
#include "include/stochasticsInterface.h"
//! \brief This class conains the mental representation and extrapolation of the environment
//!
......
......@@ -302,21 +302,23 @@ void SituationAssessment::SetNearVehicle(RelationType RelationType,
void SituationAssessment::LogSetValues(int time)
{
observation->Insert(time,
SA_Input->MM_O->Ego->GetState()->id,
LoggingGroup::Driver,
"VWish",
std::to_string(vWish));
observation->Insert(time,
SA_Input->MM_O->Ego->GetState()->id,
LoggingGroup::Driver,
"ThwWish",
std::to_string(tGapWish));
observation->Insert(time,
SA_Input->MM_O->Ego->GetState()->id,
LoggingGroup::Driver,
"CommonSpeedlimitViolation",
std::to_string(commonSpeedLimit_Violation));
//!!!
// observation->Insert(time,
// SA_Input->MM_O->Ego->GetState()->id,
// LoggingGroup::Driver,
// "VWish",
// std::to_string(vWish));
// observation->Insert(time,
// SA_Input->MM_O->Ego->GetState()->id,
// LoggingGroup::Driver,
// "ThwWish",
// std::to_string(tGapWish));
// observation->Insert(time,
// SA_Input->MM_O->Ego->GetState()->id,
// LoggingGroup::Driver,
// "CommonSpeedlimitViolation",
// std::to_string(commonSpeedLimit_Violation));
//!!!
Logged = true;
}
......
......@@ -19,11 +19,11 @@
#pragma once
//#include <vector>
#include "Interfaces/stochasticsInterface.h"
#include "Interfaces/observationInterface.h"
#include "Container/ContainerStructures.h"
#include "../Algorithm_ModularDriver/SituationAssessmentMethods/SituationCalculation.h"
#include "../Algorithm_ModularDriver/SituationAssessmentMethods/SituationLogging.h"
#include "include/stochasticsInterface.h"
#include "include/observationInterface.h"
#include "../../Sensor_Modular_Driver/src/Container/ContainerStructures.h"
#include "SituationAssessmentMethods/SituationCalculation.h"
#include "SituationAssessmentMethods/SituationLogging.h"
//! \brief This class contains the assessment of the current situation, regarding the surrounding agents
//! and select the leader, follower, neighleader and neighfollower and checking the criticality of the situation
......
......@@ -34,15 +34,16 @@
#include <QtGlobal>
#include "Algorithm_ModularDriver_implementation.h"
#include "../Common/lateralSignal.h"
#include <Common/secondaryDriverTasksSignal.h>
#include "../Sensor_Modular_Driver/Signals/complexsignals.cpp"
#include <Common/steeringSignal.h>
#include <Common/vectorSignals.h>
#include "../Common/longitudinalSignal.h"
#include "../Common/accelerationSignal.h"
#include "../Common/parametersVehicleSignal.h"
#include "common/lateralSignal.h"
#include "common/secondaryDriverTasksSignal.h"
#include "../../Sensor_Modular_Driver/src/Signals/complexsignals.cpp"
#include "common/steeringSignal.h"
#include "common/vectorSignals.h"
#include "common/longitudinalSignal.h"
#include "common/accelerationSignal.h"
#include "common/parametersVehicleSignal.h"
#include <list>
#include "include/parameterInterface.h"
#include <cassert>
#include <memory>
......@@ -59,42 +60,44 @@ AlgorithmModularDriverImplementation::AlgorithmModularDriverImplementation(
StochasticsInterface *stochastics,
WorldInterface *world,
const ParameterInterface *parameters,
const std::map<int, ObservationInterface*> *observations,
PublisherInterface *const publisher,
const CallbackInterface *callbacks,
AgentInterface *agent) :
SensorInterface(
componentName,
isInit,
priority,
offsetTime,
responseTime,
cycleTime,
stochastics,
world,
parameters,
observations,
callbacks,
agent),
informationacquisition(stochastics, GetObservations()->at(0)),
componentName,
isInit,
priority,
offsetTime,
responseTime,
cycleTime,
stochastics,
world,
parameters,
publisher,
callbacks,
agent),
informationacquisition(stochastics, /*GetObservations()->at(0)*/nullptr),
mentalmodel(stochastics),
situationassessment(stochastics, cycleTime, GetObservations()->at(0)),
actiondeduction(cycleTime, stochastics, GetObservations()->at(0)),
situationassessment(stochastics, cycleTime, /*GetObservations()->at(0)*/nullptr),
actiondeduction(cycleTime, stochastics, /*GetObservations()->at(0)*/nullptr),
actionexecution(stochastics)
{
try
{
observer = GetObservations()->at(0);
if (observer == nullptr)
{
throw std::runtime_error("");
}
}
catch (...)
{
const std::string msg = COMPONENTNAME + " invalid observation module setup";
LOG(CbkLogLevel::Error, msg);
throw std::runtime_error(msg);
}
//!!!
// try
// {
// observer = GetObservations()->at(0);
// if (observer == nullptr)
// {
// throw std::runtime_error("");
// }
// }
// catch (...)
// {
// const std::string msg = COMPONENTNAME + " invalid observation module setup";
// LOG(CbkLogLevel::Error, msg);
// throw std::runtime_error(msg);
// }
//!!!
/**
* @ingroup al_mod_driver
......@@ -171,6 +174,7 @@ AlgorithmModularDriverImplementation::AlgorithmModularDriverImplementation(
if (parameters->GetParametersDouble().count("MinComfortDeceleration") > 0 || parameters->GetParametersDouble().count("5") > 0)
{
double MinComfortDeceleration = (parameters->GetParametersDouble().count("MinComfortDeceleration") > 0 ? parameters->GetParametersDouble().at("MinComfortDeceleration") : parameters->GetParametersDouble().at("5"));
MinComfortDeceleration = MinComfortDeceleration <= 0 ? MinComfortDeceleration : MinComfortDeceleration * -1.; // TODO Adjust to deceleration as absolute value
actiondeduction.SetMinComfortDecel(MinComfortDeceleration);
}
if (parameters->GetParametersDouble().count("ComfortAccelerationDeviation") > 0 || parameters->GetParametersDouble().count("6") > 0)
......@@ -380,6 +384,7 @@ void AlgorithmModularDriverImplementation::UpdateInput(int localLinkId, const st
throw std::runtime_error(msg);
}
vehicleParameters = signal->vehicleParameters;
vehicleParameters.maxDeceleration *= -1.; // TODO Adjust to deceleration as absolute value
situationassessment.SetVehicleParameters(&vehicleParameters);
actiondeduction.SetVehicleParameters(&vehicleParameters);
actionexecution.SetVehicleParameters(&vehicleParameters);
......@@ -674,7 +679,7 @@ void AlgorithmModularDriverImplementation::Action_Deduction (ActionDeduction_Inp
actiondeduction.CalcAccelerationWish(*AD_Output_BU);
if (!actiondeduction.InitialValuesLogged())
actiondeduction.LogSetValues(time);
actiondeduction.LogSetValues(time);
actiondeduction.CheckLaneChange(&time, AD_Output_BU);
actiondeduction.prepareLanechange(AD_Output_BU);
......
......@@ -85,8 +85,8 @@
#include <string>
#include <iostream>
//#include <map>
#include "modelInterface.h"
#include "Common/primitiveSignals.h"
#include "include/modelInterface.h"
#include "common/primitiveSignals.h"
#include "AlgorithmInformationAcquisition.h"
#include "AlgorithmMentalModel.h"
#include "AlgorithmSituationAssessment.h"
......
......@@ -26,7 +26,7 @@
#include "SituationCalculation.h"
#include "Container/ContainerStructures.h"
#include "../../../Sensor_Modular_Driver/src/Container/ContainerStructures.h"
SituationCalculation::SituationCalculation(int cycleTime, StochasticsInterface *stochastics):
cycleTime(cycleTime),
......
......@@ -21,7 +21,7 @@
#include "Common/globalDefinitions.h"
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include "Common/boostGeometryCommon.h"
#include "osi3/osi_sensordata.pb.h"
#include "thirdParty/osi/include/osi3/osi_sensordata.pb.h"
#include "boundingBoxCalculation.h"
//! \ingroup Algorithm_Situation_Assessment
......
......@@ -25,7 +25,7 @@
*/
#include "SituationLogging.h"
#include "Container/ContainerStructures.h"
#include "../../../Sensor_Modular_Driver/src/Container/ContainerStructures.h"
#include "SituationCalculation.h"
#include <memory>
......@@ -73,33 +73,37 @@ void SituationLogging::CheckForLoggingStatesAndLog(int time, std::string pov)
void SituationLogging::LoggDoubleValue(std::string keylog, double value, int time)
{
observer->Insert(time,
situationCalculation->GetEgoId(),
LoggingGroup::Driver,
keylog,
std::to_string(value));
//!!!
// observer->Insert(time,
// situationCalculation->GetEgoId(),
// LoggingGroup::Driver,
// keylog,
// std::to_string(value));
//!!!
}
void SituationLogging::LoggRelationValues(const std::vector<int> *agents, std::vector<double> values, std::string key, int time)
{
if (agents->size()>0)
{
int egoid = situationCalculation->GetEgoId();
uint32_t i = 0;
for (std::vector<int>::const_iterator it = agents->begin() ; it != agents->end(); ++it)
{
int agid = *it;
std::string keylog = key + (agid < 10 ? "0" : "") + (agid < 100 && agid >= 10 ? "0" : "") + std::to_string(agid);
const double value = values[i];
const std::string valuestring = std::to_string(value);
observer->Insert(time,
egoid,
LoggingGroup::Driver,
keylog,
valuestring);
i++;
}
}
//!!!
// if (agents->size()>0)
// {
// int egoid = situationCalculation->GetEgoId();
// uint32_t i = 0;
// for (std::vector<int>::const_iterator it = agents->begin() ; it != agents->end(); ++it)
// {
// int agid = *it;
// std::string keylog = key + (agid < 10 ? "0" : "") + (agid < 100 && agid >= 10 ? "0" : "") + std::to_string(agid);
// const double value = values[i];
// const std::string valuestring = std::to_string(value);
// observer->Insert(time,
// egoid,
// LoggingGroup::Driver,
// keylog,
// valuestring);
// i++;
// }
// }
//!!!
}
bool SituationLogging::Initialize(std::vector<std::string> loggingGroups, ObservationInterface *observation)
......
......@@ -21,10 +21,10 @@
#include "Common/globalDefinitions.h"
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include "Common/boostGeometryCommon.h"
#include "osi3/osi_sensordata.pb.h"
#include "thirdParty/osi/include/osi3/osi_sensordata.pb.h"
#include "SituationCalculation.h"
#include "Interfaces/observationInterface.h"
#include "Interfaces/modelInterface.h"
#include "include/observationInterface.h"
#include "include/modelInterface.h"
//! \ingroup Algorithm_Situation_Assessment
......
......@@ -25,7 +25,7 @@
*/
#include "boundingBoxCalculation.h"
#include "Container/ContainerStructures.h"
#include "../../../Sensor_Modular_Driver/src/Container/ContainerStructures.h"
SA_BoundingBoxCalculation::SA_BoundingBoxCalculation()
{
......
......@@ -21,8 +21,8 @@
#pragma once
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include "Common/boostGeometryCommon.h"
#include "Container/ContainerStructures.h"
#include "osi3/osi_sensordata.pb.h"
#include "../../../Sensor_Modular_Driver/src/Container/ContainerStructures.h"
#include "thirdParty/osi/include/osi3/osi_sensordata.pb.h"
//!
//! \brief The BoundingBox class
......
......@@ -16,22 +16,25 @@ add_openpass_target(
HEADERS
sensor_Modular_Driver.h
sensor_Modular_Driver_implementation.h
sensor_Modular_Driver_global.h
Signals/complexsignals.h
Signals/egodata.h
Signals/staticenvironmentdata.h
Signals/surroundingmovingobjectsdata.h
Container/MentalModelLane.h
Container/ContainerStructures.h
Container/agent_representation.h
src/sensor_Modular_Driver_global.h
src/sensor_Modular_Driver_implementation.h
src/Container/agent_representation.h
src/Container/ContainerStructures.h
src/Container/MentalModelLane.h
src/Signals/complexsignals.h
src/Signals/egodata.h
src/Signals/staticenvironmentdata.h
src/Signals/surroundingmovingobjectsdata.h
SOURCES
sensor_Modular_Driver.cpp
sensor_Modular_Driver_implementation.cpp
Signals/complexsignals.cpp
Container/MentalModelLane.cpp
Container/agent_representation.cpp
src/sensor_Modular_Driver_implementation.cpp
src/Container/agent_representation.cpp
src/Container/MentalModelLane.cpp
src/Signals/complexsignals.cpp
INCDIRS
../../core/slave/modules/World_OSI
LIBRARIES
Qt5::Core
......
......@@ -23,16 +23,16 @@ DEFINES += WORLD_CPP
include(../../../global.pri)
SUBDIRS += . \
Container \
Signals \
src/Container \
src/Signals \
../../Interfaces \
../../Interfaces/roadInterface \
../../Common \
INCLUDEPATH += $$SUBDIRS \
../../CoreModules/World_OSI \
../../CoreModules/World_OSI/OWL \
../../CoreModules/World_OSI/Localization \
../../core/slave/modules/World_OSI \
../../core/slave/modules/World_OSI/OWL \
../../core/slave/modules/World_OSI/Localization \
../../Common \
..
......
......@@ -14,7 +14,7 @@
//-----------------------------------------------------------------------------
#include "sensor_Modular_Driver.h"
#include "sensor_Modular_Driver_implementation.h"
#include "src/sensor_Modular_Driver_implementation.h"
const std::string Version = "0.0.1";
static const CallbackInterface *Callbacks = nullptr;
......@@ -33,7 +33,7 @@ extern "C" SENSOR_MODULAR_DRIVER_SHARED_EXPORT ModelInterface *OpenPASS_CreateIn
StochasticsInterface *stochastics,
WorldInterface *world,
const ParameterInterface *parameters,
const std::map<int, ObservationInterface*> *observations,
PublisherInterface *const publisher,
AgentInterface *agent,
const CallbackInterface *callbacks)
{
......@@ -50,7 +50,7 @@ extern "C" SENSOR_MODULAR_DRIVER_SHARED_EXPORT ModelInterface *OpenPASS_CreateIn
stochastics,
world,
parameters,
observations,
publisher,
callbacks,
agent));
}
......
......@@ -20,7 +20,7 @@
#ifndef SENSOR_MODULAR_DRIVER_H
#define SENSOR_MODULAR_DRIVER_H
#include "sensor_Modular_Driver_global.h"
#include "modelInterface.h"
#include "src/sensor_Modular_Driver_global.h"
#include "include/modelInterface.h"
#endif // SENSOR_MODULAR_DRIVER_H
/******************************************************************************
* 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
//!
#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() {
return length;
}
double MentalModelLane::GetWidth() const {
return width;
}
std::vector<std::tuple<double, double, double, double>> MentalModelLane::GetPoints() const {
return points;
}
std::tuple<double, double, double, double> MentalModelLane::GetLastPoint() const {
if (points.size() == 0) return std::tuple<double, double, double, double>();
std::vector<std::tuple<double, double, double, double>>::const_iterator it = points.begin();
std::advance(it, points.size() - 1);
return *it;
}
std::tuple<double, double, double, double> MentalModelLane::GetFirstPoint() const {
if (points.size() == 0) return std::tuple<double, double, double, double>();
std::vector<std::tuple<double, double, double, double>>::const_iterator it = points.begin();
return *it;
}
void MentalModelLane::AddPoint(double x, double y, double hdg, double so) {
points.push_back(std::tuple<double, double, double, double>(x, y, hdg, so));
}
void MentalModelLane::AddPredecessor(uint64_t id) {
bool found = (std::find(predecessorIds.begin(), predecessorIds.end(), id) != predecessorIds.end());
if (!found) predecessorIds.push_back(id);
}
std::list<uint64_t> MentalModelLane::GetPredecessors() const{
return predecessorIds;
}
bool MentalModelLane::HasPredecessors() {
return predecessorIds.size() > 0;
}
bool MentalModelLane::ContainsPredecessor(uint64_t otherLane) {
return (std::find(predecessorIds.begin(), predecessorIds.end(), otherLane) != predecessorIds.end());
}
void MentalModelLane::AddSuccessor(uint64_t id) {
bool found = (std::find(successorIds.begin(), successorIds.end(), id) != successorIds.end());
if (!found) successorIds.push_back(id);
}
std::list<uint64_t> MentalModelLane::GetSuccessors() const {
return successorIds;
}
bool MentalModelLane::HasSuccessors() {
return successorIds.size() > 0;
}
bool MentalModelLane::ContainsSuccessor(uint64_t otherLane) {
return (std::find(successorIds.begin(), successorIds.end(), otherLane) != successorIds.end());
}
/******************************************************************************
* 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