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
/******************************************************************************
* 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 agent_representation.h
//! @author Christian Siebke
//! @author Konstantin Blenz
//! @author Christian Gärber
//! @author Vincent Adam
//! @date Tue, 03.12.2019
//! @brief provide container structure of agents seen by the driver
//!
//-----------------------------------------------------------------------------
#include <algorithm>
#include <tuple>
#include "agent_representation.h"
#include "MentalModelLane.h"
void AgentRepresentation::Extrapolate (const StaticEnvironmentData &_roadPerceptionData)
{
int laneid = internal_Data.GetState()->laneid;
std::map<int, const std::vector<MentalModelLane>*> Lanes = {};
PositionAgentOnLanes(laneid, &Lanes, _roadPerceptionData);
if((Lanes.find(0)!=Lanes.end()) && (internal_Data.GetState()->indicatorstate == IndicatorState::IndicatorState_Off))
{
ExtrapolateAlongLane(Lanes.at(0));
}
else
{
ExtrapolateKinematic();
}
}
void AgentRepresentation::PositionAgentOnLanes(int laneid, std::map<int, const std::vector<MentalModelLane>*>*Lanes, const StaticEnvironmentData &_roadPerceptionData)
{
const LaneInformation *Ego = &_roadPerceptionData.roadGeometry.laneEgo;
const LaneInformation *Left = &_roadPerceptionData.roadGeometry.laneLeft;
const LaneInformation *Right = &_roadPerceptionData.roadGeometry.laneRight;;
if (laneid == Ego->laneid)
{
Lanes->emplace(std::make_pair(0,&Ego->mentalModelLanes));
if (Left->exists)
Lanes->emplace(std::make_pair(1,&Left->mentalModelLanes));
if (Right->exists)
Lanes->emplace(std::make_pair(-1,&Right->mentalModelLanes));
}
if (laneid == _roadPerceptionData.roadGeometry.laneLeft.laneid)
{
if (Left->exists)
Lanes->emplace(std::make_pair(0,&Left->mentalModelLanes));
Lanes->emplace(std::make_pair(-1,&Ego->mentalModelLanes));
}
if (laneid == _roadPerceptionData.roadGeometry.laneRight.laneid)
{
if (Right->exists)
Lanes->emplace(std::make_pair(0,&Right->mentalModelLanes));
Lanes->emplace(std::make_pair(1,&Ego->mentalModelLanes));
}
}
void AgentRepresentation::ExtrapolateKinematic()
{
double ds = (internal_Data.GetState()->acceleration_long / 2) * std::pow((cycletime / 1000),2) + internal_Data.GetState()->velocity_long * cycletime / 1000;
internal_Data.GetState()->pos.xPos += (internal_Data.GetState()->acceleration_long * std::cos(CommonHelper::ConvertDegreeToRadian(internal_Data.GetState()->pos.yawAngle)))/2 * std::pow((cycletime / 1000),2)+ internal_Data.GetState()->velocity_x * cycletime / 1000;
internal_Data.GetState()->pos.yPos += (internal_Data.GetState()->acceleration_long * std::sin(CommonHelper::ConvertDegreeToRadian(internal_Data.GetState()->pos.yawAngle)))/2 * std::pow((cycletime / 1000),2)+ internal_Data.GetState()->velocity_y * cycletime / 1000;
internal_Data.GetState()->velocity_long += internal_Data.GetState()->acceleration_long * cycletime / 1000 ;
internal_Data.GetState()->velocity_x += internal_Data.GetState()->acceleration_long * std::cos(CommonHelper::ConvertDegreeToRadian(internal_Data.GetState()->pos.yawAngle)) * cycletime / 1000 ;
internal_Data.GetState()->velocity_y += internal_Data.GetState()->acceleration_long * std::sin(CommonHelper::ConvertDegreeToRadian(internal_Data.GetState()->pos.yawAngle)) * cycletime / 1000 ;
internal_Data.GetState()->roadPos.s += ds;
internal_Data.GetState()->secondarycoveredlanes.clear();
//CHECK
//if (internal_Data.GetState()->id==12)
//{
// std::cout << "MM_kin: s=" << internal_Data.GetState()->roadPos.s << ", vlong=" << internal_Data.GetState()->velocity_long << ", along=" << internal_Data.GetState()->acceleration_long << ", x=" << internal_Data.GetState()->pos.xPos << ", y=" << internal_Data.GetState()->pos.yPos << std::endl;
//}
}
void AgentRepresentation::ExtrapolateAlongLane(const std::vector<MentalModelLane> *lane)
{
double ds = (internal_Data.GetState()->acceleration_long / 2) * std::pow((cycletime / 1000),2) + internal_Data.GetState()->velocity_long * cycletime / 1000;
double s = internal_Data.GetState()->roadPos.s + ds;
double s_near = internal_Data.GetState()->roadPos.s + ds;
double dsmin = {std::numeric_limits<double>::max()};
double dslast = {std::numeric_limits<double>::max()};
double x_near = internal_Data.GetState()->pos.xPos;
double y_near = internal_Data.GetState()->pos.yPos;
double yaw_near = internal_Data.GetState()->pos.yawAngle;
std::tuple<double,double,double,double> previoustuple;
std::tuple<double,double,double,double> nexttuple;
std::tuple<double,double,double,double> neighbortuple;
for (std::vector<MentalModelLane>::const_iterator lanesection = lane->begin(); lanesection != lane->end(); lanesection++)
{
const std::vector<std::tuple<double,double,double,double>> Points = lanesection->GetPoints();
for(std::vector<std::tuple<double,double,double,double>>::const_iterator tuple = Points.begin(); tuple != Points.end(); tuple++)
{
double deltas = std::abs(s-std::get<3>(*tuple));
if (dslast < deltas)
break;
if (deltas<dsmin)
{
dsmin = deltas;
s_near = std::get<3>(*tuple);
x_near = std::get<0>(*tuple);
y_near = std::get<1>(*tuple);
yaw_near = std::get<2>(*tuple);
nexttuple = *(tuple++);
tuple--;
if (tuple == Points.begin())
previoustuple = *(tuple);
else
previoustuple = *(tuple--);
tuple++;
}
dslast = deltas;
}
}
neighbortuple = s_near < s ? nexttuple : previoustuple;
internal_Data.GetState()->pos.yawAngle = LinearInterpolation(yaw_near, s_near, std::get<2>(neighbortuple), std::get<3>(neighbortuple), s);
double x = std::get<0>(neighbortuple);
double y = std::get<1>(neighbortuple);
internal_Data.GetState()->pos.xPos = LinearInterpolation(x_near, s_near, std::get<0>(neighbortuple), std::get<3>(neighbortuple), s);
internal_Data.GetState()->pos.yPos = LinearInterpolation(y_near, s_near, std::get<1>(neighbortuple), std::get<3>(neighbortuple), s);
internal_Data.GetState()->velocity_long += internal_Data.GetState()->acceleration_long * cycletime / 1000 ;
internal_Data.GetState()->velocity_x = internal_Data.GetState()->velocity_long * std::cos(CommonHelper::ConvertDegreeToRadian(internal_Data.GetState()->pos.yawAngle));
internal_Data.GetState()->velocity_y = internal_Data.GetState()->velocity_long * std::sin(CommonHelper::ConvertDegreeToRadian(internal_Data.GetState()->pos.yawAngle));
internal_Data.GetState()->roadPos.s = s;
internal_Data.GetState()->secondarycoveredlanes.clear();
//CHECK
//if (internal_Data.GetState()->id==12)
//{
// std::cout << "MM: s=" << s << ", vlong=" << internal_Data.GetState()->velocity_long << ", along=" << internal_Data.GetState()->acceleration_long << ", x=" << internal_Data.GetState()->pos.xPos << ", y=" << internal_Data.GetState()->pos.yPos << std::endl;
//}
}
void AgentRepresentation::LinkPosToRoadPos(int laneid, std::map<int, const std::vector<MentalModelLane*>> lanes)
{
double dmin = {std::numeric_limits<double>::max()};
double x = internal_Data.GetState()->pos.xPos;
double y = internal_Data.GetState()->pos.yPos;
double xnew = x;
double ynew = y;
double yaw = internal_Data.GetState()->pos.yawAngle;
double s = internal_Data.GetState()->roadPos.s;
int lane = laneid;
for (auto it = lanes.begin(); it!=lanes.end(); it++)
{
for (auto lit : it->second)
{
for(auto& pit : lit->GetPoints())
{
double dcurr = sqrt(pow(x-std::get<0>(pit),2)+pow(y-std::get<1>(pit),2));
if (dcurr<dmin)
{
dmin = dcurr;
s = std::get<3>(pit);
xnew = std::get<0>(pit);
ynew = std::get<1>(pit);
yaw = std::get<2>(pit);
lane = laneid + it->first;
}
}
}
}
internal_Data.GetState()->laneid = lane;
internal_Data.GetState()->velocity_long = (s-internal_Data.GetState()->roadPos.s)/cycletime + internal_Data.GetState()->acceleration_long * cycletime / 1000 ;
internal_Data.GetState()->pos.yawAngle = yaw;
internal_Data.GetState()->velocity_x = (xnew-x)/cycletime + internal_Data.GetState()->acceleration_long * std::cos(internal_Data.GetState()->pos.yawAngle) * cycletime / 1000 ;
internal_Data.GetState()->velocity_y = (ynew-y)/cycletime + internal_Data.GetState()->acceleration_long * std::sin(internal_Data.GetState()->pos.yawAngle) * cycletime / 1000 ;
internal_Data.GetState()->roadPos.s = s;
}
/******************************************************************************
* 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 agent_representation.h
//! @author Christian Siebke
//! @author Konstantin Blenz
//! @author Christian Gärber
//! @author Vincent Adam
//! @date Tue, 03.12.2019
//! @brief provide container structure of agents seen by the driver
//!
//-----------------------------------------------------------------------------
#include <algorithm>
#include <tuple>
#include "agent_representation.h"
#include "MentalModelLane.h"
void AgentRepresentation::Extrapolate (const StaticEnvironmentData &_roadPerceptionData)
{
int laneid = internal_Data.GetState()->laneid;
std::map<int, const std::vector<MentalModelLane>*> Lanes = {};
PositionAgentOnLanes(laneid, &Lanes, _roadPerceptionData);
if((Lanes.find(0)!=Lanes.end()) && (internal_Data.GetState()->indicatorstate == IndicatorState::IndicatorState_Off))
{
ExtrapolateAlongLane(Lanes.at(0));
}
else
{
ExtrapolateKinematic();
}
}
void AgentRepresentation::PositionAgentOnLanes(int laneid, std::map<int, const std::vector<MentalModelLane>*>*Lanes, const StaticEnvironmentData &_roadPerceptionData)
{
const LaneInformation *Ego = &_roadPerceptionData.roadGeometry.laneEgo;
const LaneInformation *Left = &_roadPerceptionData.roadGeometry.laneLeft;
const LaneInformation *Right = &_roadPerceptionData.roadGeometry.laneRight;;
if (laneid == Ego->laneid)
{
Lanes->emplace(std::make_pair(0,&Ego->mentalModelLanes));
if (Left->exists)
Lanes->emplace(std::make_pair(1,&Left->mentalModelLanes));
if (Right->exists)
Lanes->emplace(std::make_pair(-1,&Right->mentalModelLanes));
}
if (laneid == _roadPerceptionData.roadGeometry.laneLeft.laneid)
{
if (Left->exists)
Lanes->emplace(std::make_pair(0,&Left->mentalModelLanes));
Lanes->emplace(std::make_pair(-1,&Ego->mentalModelLanes));
}
if (laneid == _roadPerceptionData.roadGeometry.laneRight.laneid)
{
if (Right->exists)
Lanes->emplace(std::make_pair(0,&Right->mentalModelLanes));
Lanes->emplace(std::make_pair(1,&Ego->mentalModelLanes));
}
}
void AgentRepresentation::ExtrapolateKinematic()
{
double ds = (internal_Data.GetState()->acceleration_long / 2) * std::pow((cycletime / 1000),2) + internal_Data.GetState()->velocity_long * cycletime / 1000;
internal_Data.GetState()->pos.xPos += (internal_Data.GetState()->acceleration_long * std::cos(CommonHelper::ConvertDegreeToRadian(internal_Data.GetState()->pos.yawAngle)))/2 * std::pow((cycletime / 1000),2)+ internal_Data.GetState()->velocity_x * cycletime / 1000;
internal_Data.GetState()->pos.yPos += (internal_Data.GetState()->acceleration_long * std::sin(CommonHelper::ConvertDegreeToRadian(internal_Data.GetState()->pos.yawAngle)))/2 * std::pow((cycletime / 1000),2)+ internal_Data.GetState()->velocity_y * cycletime / 1000;
internal_Data.GetState()->velocity_long += internal_Data.GetState()->acceleration_long * cycletime / 1000 ;
internal_Data.GetState()->velocity_x += internal_Data.GetState()->acceleration_long * std::cos(CommonHelper::ConvertDegreeToRadian(internal_Data.GetState()->pos.yawAngle)) * cycletime / 1000 ;
internal_Data.GetState()->velocity_y += internal_Data.GetState()->acceleration_long * std::sin(CommonHelper::ConvertDegreeToRadian(internal_Data.GetState()->pos.yawAngle)) * cycletime / 1000 ;
internal_Data.GetState()->roadPos.s += ds;
internal_Data.GetState()->secondarycoveredlanes.clear();
//CHECK
//if (internal_Data.GetState()->id==12)
//{
// std::cout << "MM_kin: s=" << internal_Data.GetState()->roadPos.s << ", vlong=" << internal_Data.GetState()->velocity_long << ", along=" << internal_Data.GetState()->acceleration_long << ", x=" << internal_Data.GetState()->pos.xPos << ", y=" << internal_Data.GetState()->pos.yPos << std::endl;
//}
}
void AgentRepresentation::ExtrapolateAlongLane(const std::vector<MentalModelLane> *lane)
{
double ds = (internal_Data.GetState()->acceleration_long / 2) * std::pow((cycletime / 1000),2) + internal_Data.GetState()->velocity_long * cycletime / 1000;
double s = internal_Data.GetState()->roadPos.s + ds;
double s_near = internal_Data.GetState()->roadPos.s + ds;
double dsmin = {std::numeric_limits<double>::max()};
double dslast = {std::numeric_limits<double>::max()};
double x_near = internal_Data.GetState()->pos.xPos;
double y_near = internal_Data.GetState()->pos.yPos;
double yaw_near = internal_Data.GetState()->pos.yawAngle;
std::tuple<double,double,double,double> previoustuple;
std::tuple<double,double,double,double> nexttuple;
std::tuple<double,double,double,double> neighbortuple;
for (std::vector<MentalModelLane>::const_iterator lanesection = lane->begin(); lanesection != lane->end(); lanesection++)
{
std::vector<std::tuple<double,double,double,double>> Points = lanesection->GetPoints();
for(std::vector<std::tuple<double,double,double,double>>::iterator tuple = Points.begin(); tuple < Points.end(); tuple++)
{
double deltas = std::abs(s-std::get<3>(*tuple));
if (dslast < deltas)
break;
if (deltas<dsmin)
{
dsmin = deltas;
s_near = std::get<3>(*tuple);
x_near = std::get<0>(*tuple);
y_near = std::get<1>(*tuple);
yaw_near = std::get<2>(*tuple);
nexttuple = *(tuple++);
tuple--;
if (tuple == Points.begin())
previoustuple = *(tuple);
else
previoustuple = *(tuple--);
tuple++;
}
dslast = deltas;
}
}
neighbortuple = s_near < s ? nexttuple : previoustuple;
internal_Data.GetState()->pos.yawAngle = LinearInterpolation(yaw_near, s_near, std::get<2>(neighbortuple), std::get<3>(neighbortuple), s);
double x = std::get<0>(neighbortuple);
double y = std::get<1>(neighbortuple);
internal_Data.GetState()->pos.xPos = LinearInterpolation(x_near, s_near, std::get<0>(neighbortuple), std::get<3>(neighbortuple), s);
internal_Data.GetState()->pos.yPos = LinearInterpolation(y_near, s_near, std::get<1>(neighbortuple), std::get<3>(neighbortuple), s);
internal_Data.GetState()->velocity_long += internal_Data.GetState()->acceleration_long * cycletime / 1000 ;
internal_Data.GetState()->velocity_x = internal_Data.GetState()->velocity_long * std::cos(CommonHelper::ConvertDegreeToRadian(internal_Data.GetState()->pos.yawAngle));
internal_Data.GetState()->velocity_y = internal_Data.GetState()->velocity_long * std::sin(CommonHelper::ConvertDegreeToRadian(internal_Data.GetState()->pos.yawAngle));
internal_Data.GetState()->roadPos.s = s;
internal_Data.GetState()->secondarycoveredlanes.clear();
//CHECK
//if (internal_Data.GetState()->id==12)
//{
// std::cout << "MM: s=" << s << ", vlong=" << internal_Data.GetState()->velocity_long << ", along=" << internal_Data.GetState()->acceleration_long << ", x=" << internal_Data.GetState()->pos.xPos << ", y=" << internal_Data.GetState()->pos.yPos << std::endl;
//}
}
void AgentRepresentation::LinkPosToRoadPos(int laneid, std::map<int, const std::vector<MentalModelLane*>> lanes)
{
double dmin = {std::numeric_limits<double>::max()};
double x = internal_Data.GetState()->pos.xPos;
double y = internal_Data.GetState()->pos.yPos;
double xnew = x;
double ynew = y;
double yaw = internal_Data.GetState()->pos.yawAngle;
double s = internal_Data.GetState()->roadPos.s;
int lane = laneid;
for (auto it = lanes.begin(); it!=lanes.end(); it++)
{
for (auto lit : it->second)
{
for(auto& pit : lit->GetPoints())
{
double dcurr = sqrt(pow(x-std::get<0>(pit),2)+pow(y-std::get<1>(pit),2));
if (dcurr<dmin)
{
dmin = dcurr;
s = std::get<3>(pit);
xnew = std::get<0>(pit);
ynew = std::get<1>(pit);
yaw = std::get<2>(pit);
lane = laneid + it->first;
}
}
}
}
internal_Data.GetState()->laneid = lane;
internal_Data.GetState()->velocity_long = (s-internal_Data.GetState()->roadPos.s)/cycletime + internal_Data.GetState()->acceleration_long * cycletime / 1000 ;
internal_Data.GetState()->pos.yawAngle = yaw;
internal_Data.GetState()->velocity_x = (xnew-x)/cycletime + internal_Data.GetState()->acceleration_long * std::cos(internal_Data.GetState()->pos.yawAngle) * cycletime / 1000 ;
internal_Data.GetState()->velocity_y = (ynew-y)/cycletime + internal_Data.GetState()->acceleration_long * std::sin(internal_Data.GetState()->pos.yawAngle) * cycletime / 1000 ;
internal_Data.GetState()->roadPos.s = s;
}
/******************************************************************************
* 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 agent_representation.h
//! @author Christian Siebke
//! @author Konstantin Blenz
//! @date Tue, 03.12.2019
//! @brief provide container structure of agents seen by the driver
//!
# pragma once
#include "ContainerStructures.h"
class AgentRepresentation
{
public:
AgentRepresentation(SurroundingMovingObjectsData in_perceptionData, int in_cycletime) :
internal_Data(*in_perceptionData.GetVehicleType(),
*in_perceptionData.GetState(),
*in_perceptionData.GetProperties()),
cycletime{in_cycletime}
{
lifetime = 0 ;
}
AgentRepresentation(const AgentRepresentation&) = delete;
AgentRepresentation(AgentRepresentation&&) = delete;
AgentRepresentation& operator=(const AgentRepresentation&) = delete;
AgentRepresentation& operator=(AgentRepresentation&&) = delete;
~AgentRepresentation() = default;
//!
//! \brief Extrapolate
//! Extrapolate the internal_Data of an agent if he is not fixated
//!
//! \param _roadPerceptionData
//!
void Extrapolate(const StaticEnvironmentData & _roadPerceptionData);
//!
//! \brief PositionAgentOnLanes
//! Sets the current agent on the mental representation of the lanes while creating a container of mental-model-lanes (one id with each several sections)
//! \param laneid
//! \param Lanes map with laneid and the mental-model-lane
//! \param _roadPerceptionData
//!
void PositionAgentOnLanes(int laneid, std::map<int, const std::vector<MentalModelLane>*>*Lanes, const StaticEnvironmentData &_roadPerceptionData);
//!
//! \brief ExtrapolateKinematic
//! Extrapolates the in the last timestep detected agent only kinematic with their last kinematic parameters
//!
void ExtrapolateKinematic();
//!
//! \brief ExtrapolateAlongLane
//! Extrapolates the in the last timestep detected agent along his lane
//!
//! \param lane
//!
void ExtrapolateAlongLane(const std::vector<MentalModelLane> *lane);
//!
//! \brief LinkPosToRoadPos
//! links the current intertial position to the road position (s, t)
//!
//! \param laneid
//! \param lanes
//!
void LinkPosToRoadPos(int laneid, std::map<int, const std::vector<MentalModelLane*>> lanes);
//!
//! \brief LinearInterpolation
//!
//! linear interpolation between two values along two s-values at special value s
//!
//! \param a value a
//! \param sa s-position at value a
//! \param b value b
//! \param sb s-position at value b
//! \param s s-position where the output variable is searched
//! \return value between a and b at position of s
//!
double LinearInterpolation(double a, double sa, double b, double sb, double s)
{
double div = (b-a)/(sb-sa);
double c = div*s + (-(div)*sa + a);
return c;
}
//!
//! \brief MisjudgePosition
//!
//! Misjudge the position of the agent by a given value.
//!
//! \param ds
//!
void MisjudgePosition(double ds)
{
if(lifetime == 0)
{
internal_Data.GetState()->pos.xPos += sin(internal_Data.GetState()->pos.yawAngle) * ds;
internal_Data.GetState()->pos.yPos += cos(internal_Data.GetState()->pos.yawAngle) * ds;
internal_Data.GetState()->roadPos.s = internal_Data.GetState()->roadPos.s + ds;
}
}
//!
//! \brief MisjudgeVelocity
//!
//! Misjudge the velocity of the agent by a given value.
//! If the difference of the velocity in the last time-step isn't more than 10% it isn't changed
//!
//! \param dv
//!
void MisjudgeVelocity(double dv)
{
if(lifetime == 0)
{
if(abs(last_v-internal_Data.GetState()->velocity_long)/internal_Data.GetState()->velocity_long >= 0.1)
{
last_v = internal_Data.GetState()->velocity_long + dv;
}
internal_Data.GetState()->velocity_long = last_v;
internal_Data.GetState()->velocity_abs = last_v;
internal_Data.GetState()->velocity_x = (last_v) * sin(internal_Data.GetState()->pos.yawAngle);
internal_Data.GetState()->velocity_y = (last_v) * cos(internal_Data.GetState()->pos.yawAngle);
}