Skip to content
Snippets Groups Projects
Commit c7657607 authored by Reinhard Biegel's avatar Reinhard Biegel
Browse files

OSI World: First implementation

Change-Id: Ib7ef125dd06acbed62c67fdd1de85e1c96b09eb9
parent 97f22ddc
No related branches found
No related tags found
No related merge requests found
Showing
with 1129 additions and 34 deletions
Thumbs.db
/.project
*.autosave
*.orig
*.rej
*.user
DIR_DEBUG = "C:\OpenPASS\BinDebug"
DIR_RELEASE = "C:\OpenPASS\BinRelease"
win32 {
DIR_DEBUG = "C:\OpenPASS\BinDebug"
DIR_RELEASE = "C:\OpenPASS\BinRelease"
}
unix {
DIR_DEBUG = "/OpenPASS/BinDebug"
DIR_RELEASE = "/OpenPASS/BinRelease"
}
Debug:DESTDIR=$$DIR_DEBUG
Release:DESTDIR=$$DIR_RELEASE
#debug/release predicates are buggy on linux qmake
unix:DESTDIR=$$DIR_DEBUG
#unix:DESTDIR=$$DIR_RELEASE
# adjust path to your needs
EXTRA_LIBS=$$(HOME)/repos/ThirdParty/lib
#CONFIG += USEOPENPASSSLAVEASLIBRARY
#CONFIG += USEOPENPASSMASTERASLIBRARY
# accumulate list of files for given directories (first parameter)
# according to file name ending (second parameter)
defineReplace(getFiles) {
variable = $$1
files = $$eval($$variable)
result =
for(file, files) {
result += $$files($$file/*.$$2)
}
return($$result)
}
# prefer bundled libs in OpenPassSlave directory over system libraries
QMAKE_LFLAGS += -Wl,-rpath=\'\$\$ORIGIN\'
##################################################################
# Configuration specific for open pass libraries #
# Usage: #
# set "CONFIG += OPENPASS_LIBRARY" before including this file #
##################################################################
OPENPASS_LIBRARY {
message("[$$TARGET] Set library configuration")
TEMPLATE = lib
CONFIG += shared
CONFIG -= lib_bundle
DESTDIR = $$system_path($$DESTDIR/$$LIB_DIR)
message("[$$TARGET] Build path set to $$DESTDIR")
}
TEMPLATE = subdirs
SUBDIRS = \
openPASS/OpenPass_OSI.pro
/******************************************************************************
* Copyright (c) 2018 AMFD GmbH
* 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
******************************************************************************/
//-----------------------------------------------------------------------------
//! @file ALongitudinalDriverToDLongitudinalSignal.h
//! @brief This file contains all functions for class
//! ALongitudinalDriverToDLongitudinalSignal
//!
//! This class contains all functionality of the signal.
//-----------------------------------------------------------------------------
#pragma once
#include "modelInterface.h"
#include "../Algorithm_LongitudinalDriver_OSI//AlgorithmLongitudinalDriverDataprovider.h"
class ALongitudinalDriverToDLongitudinalSignal: public SignalInterface
{
public:
const std::string COMPONENTNAME = "ALongitudinalDriverToDLongitudinalSignal";
//-----------------------------------------------------------------------------
//! Constructor
//-----------------------------------------------------------------------------
ALongitudinalDriverToDLongitudinalSignal(
double accPedalPos,
double brakePedalPos,
std::vector<int> gear,
ALongitudinalDataProvider::CarParameters carParameters
):
accPedalPos(accPedalPos),
brakePedalPos(brakePedalPos),
gear(gear),
carParameters(carParameters)
{
}
ALongitudinalDriverToDLongitudinalSignal(const ALongitudinalDriverToDLongitudinalSignal&) = delete;
ALongitudinalDriverToDLongitudinalSignal(ALongitudinalDriverToDLongitudinalSignal&&) = delete;
ALongitudinalDriverToDLongitudinalSignal& operator=(const ALongitudinalDriverToDLongitudinalSignal&) = delete;
ALongitudinalDriverToDLongitudinalSignal& operator=(ALongitudinalDriverToDLongitudinalSignal&&) = delete;
virtual ~ALongitudinalDriverToDLongitudinalSignal()
{}
//-----------------------------------------------------------------------------
//! Returns the content/payload of the signal as an std::string
//!
//! @return Content/payload of the signal as an std::string
//-----------------------------------------------------------------------------
virtual operator std::string() const
{
std::ostringstream stream;
stream << COMPONENTNAME;
return stream.str();
}
double accPedalPos;
double brakePedalPos;
std::vector<int> gear;
ALongitudinalDataProvider::CarParameters carParameters;
};
/**********************************************
* ITK Engineering AG (www.itk-engineering.de) *
* *
***********************************************/
//-----------------------------------------------------------------------------
//! @file DLateralToPRfsSignal.h
//! @brief This file contains all functions for class DLateralToPRfsSignal
//!
//! This class contains all functionality of the module.
//-----------------------------------------------------------------------------
#pragma once
#include "signalInterface.h"
//-----------------------------------------------------------------------------
//! Signal class
//-----------------------------------------------------------------------------
class DLateralToPRfsSignal : public SignalInterface
{
public:
const std::string COMPONENTNAME = "DLateralToPRfsSignal";
//-----------------------------------------------------------------------------
//! Constructor
//-----------------------------------------------------------------------------
DLateralToPRfsSignal(
double steeringAngleDegrees,
double steeringCurvature) :
steeringAngleDegrees(steeringAngleDegrees),
steeringCurvature(steeringCurvature)
{}
DLateralToPRfsSignal(const DLateralToPRfsSignal&) = delete;
DLateralToPRfsSignal(DLateralToPRfsSignal&&) = delete;
DLateralToPRfsSignal& operator=(const DLateralToPRfsSignal&) = delete;
DLateralToPRfsSignal& operator=(DLateralToPRfsSignal&&) = delete;
virtual ~DLateralToPRfsSignal()
{}
//-----------------------------------------------------------------------------
//! Returns the content/payload of the signal as an std::string
//!
//! @return Content/payload of the signal as an std::string
//-----------------------------------------------------------------------------
virtual operator std::string() const
{
std::ostringstream stream;
stream << COMPONENTNAME << ": steeringAngleDegrees = " << steeringAngleDegrees << ", steeringCurvature = " << steeringCurvature;
return stream.str();
}
double steeringAngleDegrees;
double steeringCurvature;
};
#pragma once
#include <boost/geometry.hpp>
#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
typedef boost::geometry::model::d2::point_xy<double> point_t;
typedef boost::geometry::model::polygon<point_t> polygon_t;
typedef boost::geometry::model::box<point_t> box_t;
namespace bg = boost::geometry;
namespace bt = boost::geometry::strategy::transform;
constexpr double PI = boost::math::constants::pi<double>();
///*!
///* \brief Convert degrees to radians
///* \param[in] angle in degree
///* \return angle in radian
///*/
inline double Deg2Rad(double degree){
return degree * PI/180;
}
///*!
///* \brief Convert radian to degree
///* \param[in] angle in radian
///* \return angle in degree
///*/
inline double Rad2Deg(double rad){
return rad * 180.0/PI;
}
/******************************************************************************
* Copyright (c) 2018 AMFD GmbH
* 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
******************************************************************************/
//-----------------------------------------------------------------------------
//! @file brakingSignal.h
//! @brief This file contains all functions for class
//! BrakingSignal
//!
//! This class contains all functionality of the module.
//-----------------------------------------------------------------------------
#pragma once
#include "modelInterface.h"
//-----------------------------------------------------------------------------
//! Signal class
//-----------------------------------------------------------------------------
class BrakingSignal : public ComponentStateSignalInterface
{
public:
const std::string COMPONENTNAME = "BrakingSignal";
//-----------------------------------------------------------------------------
//! Constructor
//-----------------------------------------------------------------------------
BrakingSignal(int senderID)
{
componentState = ComponentState::Disabled;
this->senderID = senderID;
}
//-----------------------------------------------------------------------------
//! Constructor
//-----------------------------------------------------------------------------
BrakingSignal(BrakingSignal &other) :
BrakingSignal(other.senderID,
other.componentState,
other.acceleration)
{
}
//-----------------------------------------------------------------------------
//! Constructor
//-----------------------------------------------------------------------------
BrakingSignal(int senderId,
ComponentState componentState,
double acceleration) :
acceleration(acceleration)
{
this->senderID = senderId;
this->componentState = componentState;
}
BrakingSignal(const BrakingSignal&) = delete;
BrakingSignal(BrakingSignal&&) = delete;
BrakingSignal& operator=(const BrakingSignal&) = delete;
BrakingSignal& operator=(BrakingSignal&&) = delete;
virtual ~BrakingSignal()
{}
//-----------------------------------------------------------------------------
//! Returns the content/payload of the signal as an std::string
//!
//! @return Content/payload of the signal as an std::string
//-----------------------------------------------------------------------------
virtual operator std::string() const
{
std::ostringstream stream;
stream << COMPONENTNAME;
return stream.str();
}
double acceleration;
};
/******************************************************************************
* Copyright (c) 2017 ITK Engineering GmbH.
* All rights reserved. This program and the accompanying materials
* are made available under the terms of the Eclipse Public License v1.0
* which accompanies this distribution, and is available at
* http://www.eclipse.org/legal/epl-v10.html
* 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
******************************************************************************/
//-----------------------------------------------------------------------------
......@@ -15,6 +18,7 @@
#define GLOBALDEFINITIONS
#include <string>
#include <tuple>
//-----------------------------------------------------------------------------
//! @brief Containing the three possible states regarding lane change
......@@ -41,6 +45,17 @@ enum class Weekday
Sunday
};
//-----------------------------------------------------------------------------
//! Agent category classification
//-----------------------------------------------------------------------------
enum class AgentCategory
{
Ego = 0,
Scenario,
Common,
Any
};
//-----------------------------------------------------------------------------
//! Agent type classification
//-----------------------------------------------------------------------------
......@@ -79,6 +94,45 @@ struct Position
double curvature;
};
struct RoadPosition
{
double s;
double t;
double hdg;
};
enum class MeasurementPoint
{
Front,
Reference,
Rear
};
//TODO: decide where this class should really be
struct Remainder
{
std::tuple<double, bool> left{0, false};
std::tuple<double, bool> right{0, false};
void setLeft(double value)
{
std::get<0>(left) = value;
std::get<1>(left) = true;
}
void setRight(double value)
{
std::get<0>(right) = value;
std::get<1>(right) = true;
}
void setBoth(double value)
{
setLeft(value);
setRight(value);
}
};
//! @brief Areas of interest for surrounding data.
//! \details "Downstream" is a gaze state, but not an area of interest for surrounding data.
//! The downstream gaze state is used to obtain data for anticipation.
......@@ -127,6 +181,12 @@ enum class AgentViewDirection
NumberOfCarViewDirections
};
enum class Side
{
Left,
Right
};
class WorldParameter
{
public:
......
/******************************************************************************
* Copyright (c) 2018 AMFD GmbH
* 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
******************************************************************************/
//-----------------------------------------------------------------------------
//! @file LateralDriverSignal.h
//! @brief This file contains all functions for class LateralDriverSignal
//!
//! This class contains all functionality of the signal.
//-----------------------------------------------------------------------------
#pragma once
#include "modelInterface.h"
class LateralDriverSignal : public ComponentStateSignalInterface
{
public:
const std::string COMPONENTNAME = "LateralDriverSignal";
//-----------------------------------------------------------------------------
//! Constructor
//-----------------------------------------------------------------------------
LateralDriverSignal(int senderId)
{
componentState = ComponentState::Disabled;
this->senderID = senderId;
}
//-----------------------------------------------------------------------------
//! Constructor
//-----------------------------------------------------------------------------
LateralDriverSignal(LateralDriverSignal &other) :
LateralDriverSignal(
other.currCurvature,
other.laneWidth,
other.vLatEgo,
other.vLongEgo,
other.notifyCollision,
other.freqLateralOscillation,
other.zeta,
other.LatDisplacement,
other.DeltaPhi,
other.GainDeltaPhi,
other.isLaneChangeAborted,
other.componentState,
other.senderID)
{
}
//-----------------------------------------------------------------------------
//! Constructor
//-----------------------------------------------------------------------------
LateralDriverSignal(
double currCurvature,
double laneWidth,
double vLatEgo,
double vLongEgo,
bool notifyCollision,
double freqLateralOscillation,
double zeta,
double LatDisplacement,
double DeltaPhi,
double GainDeltaPhi,
bool isLaneChangeAborted, //LaneChangeState laneChangeStateEgo,
ComponentState componentStateP,
int senderId
):
currCurvature(currCurvature),
laneWidth(laneWidth),
vLatEgo(vLatEgo),
vLongEgo(vLongEgo),
notifyCollision(notifyCollision),
freqLateralOscillation(freqLateralOscillation),
zeta(zeta),
LatDisplacement(LatDisplacement),
DeltaPhi(DeltaPhi),
GainDeltaPhi(GainDeltaPhi),
isLaneChangeAborted(isLaneChangeAborted)
{
componentState = componentStateP;
senderID = senderId;
}
virtual ~LateralDriverSignal()
{}
//-----------------------------------------------------------------------------
//! Returns the content/payload of the signal as an std::string
//!
//! @return Content/payload of the signal as an std::string
//-----------------------------------------------------------------------------
virtual operator std::string() const
{
std::ostringstream stream;
stream << COMPONENTNAME;
return stream.str();
}
double currCurvature {0};
double laneWidth {0};
double vLatEgo {0};
double vLongEgo {0};
bool notifyCollision {false};
double freqLateralOscillation {0};
double zeta {0};
double LatDisplacement {0};
double DeltaPhi {0};
double GainDeltaPhi {0};
bool isLaneChangeAborted {false};
};
/******************************************************************************
* Copyright (c) 2018 AMFD GmbH
* 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
******************************************************************************/
//-----------------------------------------------------------------------------
//! @file secondaryDriverTasksSignal.h
//! @brief This file contains all functions for class
//! SecondaryDriverTasksSignal
//!
//! This class contains all functionality of the signal.
//-----------------------------------------------------------------------------
#pragma once
#include "modelInterface.h"
class SecondaryDriverTasksSignal : public ComponentStateSignalInterface
{
public:
const std::string COMPONENTNAME = "SecondaryDriverTasksSignal";
//-----------------------------------------------------------------------------
//! Constructor
//-----------------------------------------------------------------------------
SecondaryDriverTasksSignal(int senderID)
{
componentState = ComponentState::Disabled;
this->senderID = senderID;
}
//-----------------------------------------------------------------------------
//! Constructor
//-----------------------------------------------------------------------------
SecondaryDriverTasksSignal(
int indicatorState,
bool hornSwitch,
bool headLightSwitch,
bool highBeamLightSwitch,
bool flasher,
int senderId,
ComponentState componentState
):
indicatorState(indicatorState),
hornSwitch(hornSwitch),
headLightSwitch(headLightSwitch),
highBeamLightSwitch(highBeamLightSwitch),
flasherSwitch(flasher)
{
this->senderID = senderId;
this->componentState = componentState;
}
//-----------------------------------------------------------------------------
//! Constructor
//-----------------------------------------------------------------------------
SecondaryDriverTasksSignal(SecondaryDriverTasksSignal &other) :
SecondaryDriverTasksSignal(other.indicatorState,
other.hornSwitch,
other.headLightSwitch,
other.highBeamLightSwitch,
other.flasherSwitch,
other.senderID,
other.componentState)
{
}
SecondaryDriverTasksSignal(const SecondaryDriverTasksSignal&) = delete;
SecondaryDriverTasksSignal(SecondaryDriverTasksSignal&&) = delete;
SecondaryDriverTasksSignal& operator=(const SecondaryDriverTasksSignal&) = delete;
SecondaryDriverTasksSignal& operator=(SecondaryDriverTasksSignal&&) = delete;
virtual ~SecondaryDriverTasksSignal()
{}
//-----------------------------------------------------------------------------
//! Returns the content/payload of the signal as an std::string
//!
//! @return Content/payload of the signal as an std::string
//-----------------------------------------------------------------------------
virtual operator std::string() const
{
std::ostringstream stream;
stream << COMPONENTNAME;
return stream.str();
}
int indicatorState;
bool hornSwitch;
bool headLightSwitch;
bool highBeamLightSwitch;
bool flasherSwitch;
};
/******************************************************************************
* Copyright (c) 2018 AMFD GmbH
* 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
******************************************************************************/
//-----------------------------------------------------------------------------
//! @file SteeringSignal.h
//! @brief This file contains all functions for class
//! SteeringSignal
//!
//! This class contains all functionality of the module.
//-----------------------------------------------------------------------------
#pragma once
#include "modelInterface.h"
//-----------------------------------------------------------------------------
//! Signal class
//-----------------------------------------------------------------------------
class SteeringSignal : public ComponentStateSignalInterface
{
public:
const std::string COMPONENTNAME = "SteeringSignal";
//-----------------------------------------------------------------------------
//! Constructor
//-----------------------------------------------------------------------------
SteeringSignal(int senderId,
ComponentState componentState,
double steeringWheelAngle) :
steeringWheelAngle(steeringWheelAngle)
{
this->senderID = senderId;
this->componentState = componentState;
}
SteeringSignal(const SteeringSignal&) = delete;
SteeringSignal(SteeringSignal&&) = delete;
SteeringSignal& operator=(const SteeringSignal&) = delete;
SteeringSignal& operator=(SteeringSignal&&) = delete;
virtual ~SteeringSignal()
{}
//-----------------------------------------------------------------------------
//! Returns the content/payload of the signal as an std::string
//!
//! @return Content/payload of the signal as an std::string
//-----------------------------------------------------------------------------
virtual operator std::string() const
{
std::ostringstream stream;
stream << COMPONENTNAME;
return stream.str();
}
double steeringWheelAngle;
};
/******************************************************************************
* Copyright (c) 2018 in-tech on behalf of BMW AG
* All rights reserved. This program and the accompanying materials
* are made available under the terms of the Eclipse Public License v1.0
* which accompanies this distribution, and is available at
* http://www.eclipse.org/legal/epl-v10.html
* 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
******************************************************************************/
//! \brief This file implements a generic 2D vector.
......
/******************************************************************************
* Copyright (c) 2017 ITK Engineering GmbH.
* All rights reserved. This program and the accompanying materials
* are made available under the terms of the Eclipse Public License v1.0
* which accompanies this distribution, and is available at
* http://www.eclipse.org/legal/epl-v10.html
* 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 <cmath>
#include "vector2d.h"
#define EPSILON 1e-9
namespace Common {
Vector2d::Vector2d(double x, double y) : x(x), y(y)
......@@ -53,12 +58,12 @@ void Vector2d::Sub(const Vector2d &in)
y -= in.y;
}
double Vector2d::Dot(const Vector2d &in)
double Vector2d::Dot(const Vector2d &in) const
{
return x * in.x + y * in.y;
}
double Vector2d::Cross(const Vector2d &in)
double Vector2d::Cross(const Vector2d &in) const
{
return x * in.y - y * in.x;
}
......@@ -86,24 +91,25 @@ double Vector2d::Angle() const
return atan2(y, x);
}
Vector2d Vector2d::operator-(const Vector2d &in)
Vector2d Vector2d::operator-(const Vector2d &in) const
{
return Vector2d(x - in.x, y - in.y);
}
Vector2d Vector2d::operator+(const Vector2d &in)
Vector2d Vector2d::operator+(const Vector2d &in) const
{
return Vector2d(x + in.x, y + in.y);
}
Vector2d Vector2d::operator*(double in)
Vector2d Vector2d::operator*(double in) const
{
return Vector2d(x * in, y * in);
}
bool Vector2d::operator==(const Vector2d &in) const
{
return x == in.x && y == in.y;
return (std::abs(x - in.x) < EPSILON) &&
(std::abs(y - in.y) < EPSILON);
}
} // namespace Common
/******************************************************************************
* Copyright (c) 2017 ITK Engineering GmbH.
* All rights reserved. This program and the accompanying materials
* are made available under the terms of the Eclipse Public License v1.0
* which accompanies this distribution, and is available at
* http://www.eclipse.org/legal/epl-v10.html
* 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
******************************************************************************/
//! \brief This file implements a generic 2D vector.
......@@ -81,7 +84,7 @@ public:
* \param[in] in 2d vector
* \return returns dot product of the 2 vectors
*/
double Dot(const Vector2d &in);
double Dot(const Vector2d &in) const;
/*!
* cross product with Z=0
......@@ -89,7 +92,7 @@ public:
* \param[in] in 2d vector
* \return returns z-component of the cross product
*/
double Cross(const Vector2d &in);
double Cross(const Vector2d &in) const;
/*!
* Normalizes the 2d vector
......@@ -116,9 +119,9 @@ public:
*/
double Angle() const;
Vector2d operator-(const Vector2d &in);
Vector2d operator+(const Vector2d &in);
Vector2d operator*(double in);
Vector2d operator-(const Vector2d &in) const;
Vector2d operator+(const Vector2d &in) const;
Vector2d operator*(double in) const;
bool operator==(const Vector2d &in) const;
double x;
......
/******************************************************************************
* Copyright (c) 2016 ITK Engineering AG.
*
* 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 <cmath>
#include "vector3d.h"
namespace Common
{
Vector3d::Vector3d(double x, double y, double z) : x(x), y(y), z(z)
{}
void Vector3d::Translate(double x, double y, double z)
{
this->x += x;
this->y += y;
this->z += z;
}
void Vector3d::Scale(double scale)
{
x *= scale;
y *= scale;
z *= scale;
}
void Vector3d::Add(const Vector3d &in)
{
x += in.x;
y += in.y;
z += in.z;
}
void Vector3d::Sub(const Vector3d &in)
{
x -= in.x;
y -= in.y;
z -= in.z;
}
bool Vector3d::Norm()
{
double length = std::sqrt(x*x + y*y + z*z);
if(0 == length)
{
return false;
}
x /= length;
y /= length;
z /= length;
return true;
}
double Vector3d::Length() const
{
return std::sqrt(x*x + y*y + z*z);
}
Vector3d Vector3d::operator-(const Vector3d &in)
{
return Vector3d(x - in.x, y - in.y, z - in.z);
}
Vector3d Vector3d::operator+(const Vector3d &in)
{
return Vector3d(x + in.x, y + in.y, z + in.z);
}
Vector3d Vector3d::operator*(double in)
{
return Vector3d(x * in, y * in, z * in);
}
bool Vector3d::operator==(const Vector3d &in) const
{
return x==in.x && y==in.y && z==in.z;
}
} // namespace Common
/******************************************************************************
* Copyright (c) 2016 ITK Engineering AG.
*
* 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
******************************************************************************/
//! \brief This file implements a generic 3D vector.
#pragma once
namespace Common
{
/*!
* class for 3d vectors in cartesian coordinate system
*/
class Vector3d
{
public:
/*!
* create 2d vector from pair (x, y, z)
*
* \param[in] x x-value
* \param[in] y y-value
* \param[in] z z-value
*/
Vector3d(double x = 0, double y = 0, double z = 0);
Vector3d(const Vector3d&) = default;
Vector3d(Vector3d&&) = default;
Vector3d& operator=(const Vector3d&) = default;
Vector3d& operator=(Vector3d&&) = default;
virtual ~Vector3d() = default;
/*!
* translation of vector
*
* \param[in] x x-value of displacement vector
* \param[in] y y-value of displacement vector
* \param[in] z z-value of displacement vector
*/
void Translate(double x, double y, double z);
/*!
* scales vector by a factor
*
* \param[in] scale scaling factor
*/
void Scale(double scale);
/*!
* adds a vector
*
* \param[in] in added 3d vector
*/
void Add(const Vector3d &in);
/*!
* subtracts a vector
*
* \param[in] in subtracted 3d vector
*/
void Sub(const Vector3d &in);
/*!
* Normalizes the 3d vector
*
* Each component of the vector is devided by the length of the vector.
*
* In case of a vector with length 0, the vector cannot be normalized and false is returned.
*
* \return returns true if vector could be normalized, false otherwise
*/
bool Norm();
/*!
* returns length of the vector
*
* \return length of the vector
*/
double Length() const;
Vector3d operator-(const Vector3d &in);
Vector3d operator+(const Vector3d &in);
Vector3d operator*(double in);
bool operator==(const Vector3d &in) const;
double x;
double y;
double z;
};
} // namespace Common
/******************************************************************************
* Copyright (c) 2016 ITK Engineering AG.
* All rights reserved. This program and the accompanying materials
* are made available under the terms of the Eclipse Public License v1.0
* which accompanies this distribution, and is available at
* http://www.eclipse.org/legal/epl-v10.html
* 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 <memory>
#include <qglobal.h>
#include "action_brakelight_basic_implementation.h"
#include "brakingSignal.h"
void Action_BrakeLight_Basic_Implementation::UpdateInput(int localLinkId, const std::shared_ptr<SignalInterface const> &data, int time)
{
......@@ -42,6 +46,18 @@ void Action_BrakeLight_Basic_Implementation::UpdateInput(int localLinkId, const
in_aCoasting = signal->value;
}
else if (localLinkId == 2)
{
const std::shared_ptr<BrakingSignal const> signal = std::dynamic_pointer_cast<BrakingSignal const>(data);
if(!signal)
{
const std::string msg = COMPONENTNAME + " invalid signaltype";
LOG(CbkLogLevel::Debug, msg);
throw std::runtime_error(msg);
}
in_aVehicle = signal->acceleration;
}
else
{
const std::string msg = COMPONENTNAME + " invalid link";
......
#-----------------------------------------------------------------------------
# /file AlgorithmAgentFollowingDriverModel.pro
# /brief This file contains the information for the QtCreator-project of the
# module AlgorithmAgentFollowingDriverModel
#
# Copyright (c) 2018 in-tech GmbH
#
#-----------------------------------------------------------------------------/
DEFINES += ALGORITHM_AGENTFOLLOWINGDRIVERMODEL_LIBRARY
CONFIG += OPENPASS_LIBRARY
include(../../../Global.pri)
SUBDIRS += .
INCLUDEPATH += $$SUBDIRS \
../../Common \
../../Common_BMW/Signals \
../../Interfaces \
..
SOURCES += \
$$getFiles(SUBDIRS, cpp) \
$$getFiles(SUBDIRS, cc) \
$$getFiles(SUBDIRS, c)
HEADERS += \
$$getFiles(SUBDIRS, hpp) \
$$getFiles(SUBDIRS, h)
/******************************************************************************
* 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_agentFollowingDriverModel.h"
#include "Algorithm_agentFollowingDriverModel_implementation.h"
const std::string Version = "0.0.1";
static const CallbackInterface *Callbacks = nullptr;
extern "C" ALGORITHM_AGENTFOLLOWINGDRIVERMODEL_SHARED_EXPORT const std::string &OpenPASS_GetVersion()
{
return Version;
}
extern "C" ALGORITHM_AGENTFOLLOWINGDRIVERMODEL_SHARED_EXPORT ModelInterface *OpenPASS_CreateInstance(
int componentId,
bool isInit,
int priority,
int offsetTime,
int responseTime,
int cycleTime,
StochasticsInterface *stochastics,
WorldInterface *world,
const ParameterInterface *parameters,
const std::map<int, ObservationInterface*> *observations,
AgentInterface *agent,
const CallbackInterface *callbacks)
{
Callbacks = callbacks;
try
{
return (ModelInterface*)(new (std::nothrow) AlgorithmAgentFollowingDriverModelImplementation(
componentId,
isInit,
priority,
offsetTime,
responseTime,
cycleTime,
stochastics,
world,
parameters,
observations,
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_AGENTFOLLOWINGDRIVERMODEL_SHARED_EXPORT void OpenPASS_DestroyInstance(ModelInterface *implementation)
{
delete (AlgorithmAgentFollowingDriverModelImplementation*)implementation;
}
extern "C" ALGORITHM_AGENTFOLLOWINGDRIVERMODEL_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_AGENTFOLLOWINGDRIVERMODEL_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_AGENTFOLLOWINGDRIVERMODEL_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)
{
std::exception_ptr p = std::current_exception();
const std::string exType = p ? p.__cxa_exception_type()->name() : "null";
Callbacks->Log(CbkLogLevel::Error, __FILE__, __LINE__, "unexpected exception - type: " + exType);
}
return false;
}
return true;
}
/******************************************************************************
* 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
******************************************************************************/
//! @file AlgorithmAgentFollowingDriverModel.h
//! @brief This file contains the implementation header file
#pragma once
#include "Algorithm_agentFollowingDriverModel_global.h"
#include "modelInterface.h"
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment