Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • romahnp/opSimulation
  • eclipse/openpass/opSimulation
  • dweiwg6/simopenpass
  • rbiegel/simopenpass
  • fweissenbacher/simopenpass
  • adascri/simopenpass
  • mbauerm6f/simopenpass
  • rcaloudis2v6/simopenpass
  • mscharfenberg/simopenpass
  • kblenz/simopenpass
  • wangkun970101/simopenpass
  • fgurr/simopenpass
  • m121212/simopenpass
  • victorhexad/simopenpass
  • senigueve/opSimulation
  • fgurr/opSimulation
  • malowe/opSimulation
  • heuerfin/opSimulation
  • nmraghu/opSimulation
  • naidagoro/opSimulation
  • rbiegel/opSimulation
  • benni/opSimulation
  • emaschke/open-pass-simulation
  • szipfel/opSimulation
  • arnauvazquez/opSimulation
  • jdobberstein/opSimulation
  • gwendallucas/op-simulation-gl
27 results
Show changes
Commits on Source (5)
Showing
with 1768 additions and 25 deletions
......@@ -353,8 +353,11 @@ The following road markings are supported:
| RoadMarking | StVo Type | Subtype | Value and Units |
|-----------------------------------------------|-----------|-------------|-------------------|
| PedestrianCrossing | 293 | - | - |
| Stop line | 294 | - | - |
The pedestrian crossing can also be defined in OpenDRIVE as object with type "crosswalk".
\subsection dev_framework_modules_world_lanemarking Lane Markings
The world also supports lane markings (i.e. printed lines between two lanes) according to the OpenDRIVE standard.
......
/*******************************************************************************
* Copyright (c) 2020 in-tech GmbH
* Copyright (c) 2020, 2021 in-tech GmbH
*
* This program and the accompanying materials are made
* available under the terms of the Eclipse Public License 2.0
......@@ -172,6 +172,12 @@ public:
//! \return distance to other object or nullopt if the other object is not on the route of this agent
virtual LongitudinalDistance GetDistanceToObject(const WorldObjectInterface* otherObject) const = 0;
//! Returns the (longitudinal) distance to a specified point along the route
//!
//! \param roadId road on which the point lies
//! \param s s coordinate of the point
virtual double GetDistanceToPoint(const std::string& roadId, double s) const = 0;
//! Returns the (lateral) obstruction with another object along the route
//!
//! \param otherObject object to calculate obstruction
......@@ -179,9 +185,9 @@ public:
virtual Obstruction GetObstruction(const WorldObjectInterface* otherObject) const = 0;
//! Returns the yaw of the agent at the MainLaneLocater relative to the road respecting intended driving direction
//! Returns the yaw of the agent at the measurement point (only "Front" or "Reference" allowed) relative to the road respecting intended driving direction
//! (i.e. driving in the intended direction equals zero relative yaw)
virtual double GetRelativeYaw() const = 0;
virtual double GetRelativeYaw(MeasurementPoint measurementPoint = MeasurementPoint::Front) const = 0;
//! Returns the distance of the MainLaneLocator to the middle of the lane respecting intended driving direction
//! (i.e. positive values are to the left w.r.t. driving direction)
......
/*******************************************************************************
* Copyright (c) 2017, 2018, 2019 in-tech GmbH
* Copyright (c) 2017, 2018, 2019, 2020 in-tech GmbH
* 2016, 2017, 2018 ITK Engineering GmbH
*
* This program and the accompanying materials are made
......@@ -75,7 +75,7 @@ enum class RoadLinkSideType
//-----------------------------------------------------------------------------
//! Type of lane
//-----------------------------------------------------------------------------
enum class RoadLaneType // http://www.opendrive.org/docs/OpenDRIVEFormatSpecRev1.4H.pdf page 93
enum class RoadLaneType // https://www.asam.net/standards/detail/opendrive/
{
Undefined = 0,
None,
......@@ -104,7 +104,7 @@ enum class RoadLaneType // http://www.opendrive.org/docs/OpenDRIVEFormatSpecRev1
//-----------------------------------------------------------------------------
//! Type of lane line
//-----------------------------------------------------------------------------
enum class RoadLaneRoadMarkType // http://www.opendrive.org/docs/OpenDRIVEFormatSpecRev1.4H.pdf page 92
enum class RoadLaneRoadMarkType // https://www.asam.net/standards/detail/opendrive/
{
Undefined = 0,
None,
......@@ -123,7 +123,7 @@ enum class RoadLaneRoadMarkType // http://www.opendrive.org/docs/OpenDRIVEFormat
//-----------------------------------------------------------------------------
//! Lane description: left, right or center
//-----------------------------------------------------------------------------
enum class RoadLaneRoadDescriptionType // http://www.opendrive.org/docs/OpenDRIVEFormatSpecRev1.4H.pdf page 59
enum class RoadLaneRoadDescriptionType // https://www.asam.net/standards/detail/opendrive/
{
Left,
Right,
......@@ -133,7 +133,7 @@ enum class RoadLaneRoadDescriptionType // http://www.opendrive.org/docs/OpenDRIV
//-----------------------------------------------------------------------------
//! LaneChange of the lane line
//-----------------------------------------------------------------------------
enum class RoadLaneRoadMarkLaneChange // http://www.opendrive.org/docs/OpenDRIVEFormatSpecRev1.4H.pdf page 59
enum class RoadLaneRoadMarkLaneChange // https://www.asam.net/standards/detail/opendrive/
{
Undefined = 0,
None,
......@@ -145,7 +145,7 @@ enum class RoadLaneRoadMarkLaneChange // http://www.opendrive.org/docs/OpenDRIVE
//-----------------------------------------------------------------------------
//! Color of the road mark
//-----------------------------------------------------------------------------
enum class RoadLaneRoadMarkColor // http://www.opendrive.org/docs/OpenDRIVEFormatSpecRev1.4H.pdf page 92
enum class RoadLaneRoadMarkColor // https://www.asam.net/standards/detail/opendrive/
{
Undefined = 0,
Blue,
......@@ -157,7 +157,7 @@ enum class RoadLaneRoadMarkColor // http://www.opendrive.org/docs/OpenDRIVEForma
};
//! Weight of the road mark
enum class RoadLaneRoadMarkWeight // http://www.opendrive.org/docs/OpenDRIVEFormatSpecRev1.4H.pdf page 92
enum class RoadLaneRoadMarkWeight // https://www.asam.net/standards/detail/opendrive/
{
Undefined = 0,
Standard,
......@@ -174,7 +174,7 @@ enum class RoadElementOrientation
//-----------------------------------------------------------------------------
//! Units used by signals
//-----------------------------------------------------------------------------
enum class RoadSignalUnit // http://www.opendrive.org/docs/OpenDRIVEFormatSpecRev1.4H.pdf page 12
enum class RoadSignalUnit // https://www.asam.net/standards/detail/opendrive/
{
Undefined = 0,
Meter,
......@@ -235,25 +235,28 @@ enum class RoadObjectType
none = -1,
obstacle,
car,
truck,
pole,
tree,
vegetation,
barrier,
building,
parkingSpace,
patch,
railing,
trafficIsland,
crosswalk,
streetlamp,
gantry,
soundBarrier,
van,
bus,
trailer,
bike,
motorbike,
tram,
train,
pedestrian,
pole,
tree,
vegetation,
barrier,
building,
parkingSpace,
wind,
patch,
guardRail,
roadSideMarkerPost
roadMark
};
struct RoadObjectSpecification // http://www.opendrive.org/docs/OpenDRIVEFormatSpecRev1.4H.pdf page 65
......@@ -265,7 +268,7 @@ struct RoadObjectSpecification // http://www.opendrive.org/docs/OpenDRIVEFormatS
double t {0};
double zOffset {0};
double validLength {0};
RoadElementOrientation orientation;
RoadElementOrientation orientation{RoadElementOrientation::positive};
double width {0};
double length {0};
double radius {0};
......
/*******************************************************************************
* Copyright (c) 2017, 2018, 2019, 2020 in-tech GmbH
* Copyright (c) 2017, 2018, 2019, 2020, 2021 in-tech GmbH
* 2016, 2017, 2018 ITK Engineering GmbH
*
* This program and the accompanying materials are made
......@@ -601,7 +601,7 @@ public:
//! \return type and OpenDrive Id of next upstream element
virtual RoadNetworkElement GetRoadPredecessor(std::string roadId) const = 0;
virtual std::pair<RoadGraph, RoadGraphVertex> GetRoadGraph (const RouteElement& start, int maxDepth) const = 0;
virtual std::pair<RoadGraph, RoadGraphVertex> GetRoadGraph (const RouteElement& start, int maxDepth, bool allowReverse = false) const = 0;
virtual std::map<RoadGraphEdge, double> GetEdgeWeights (const RoadGraph& roadGraph) const = 0;
......
/*******************************************************************************
* Copyright (c) 2021 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
*******************************************************************************/
#pragma once
#include "common/events/basicEvent.h"
#include "common/openScenarioDefinitions.h"
namespace openpass::events {
//-----------------------------------------------------------------------------
/** This class implements all functionality of the RouteActionEvent.
*
* \ingroup Event */
//-----------------------------------------------------------------------------
class RouteActionEvent : public OpenScenarioEvent
{
public:
static constexpr char TOPIC[] {"openSCENARIO/RoutingAction/AssignRouteAction"};
RouteActionEvent(int time, const std::string eventName, const std::string source, int agentId, const openScenario::AssignRouteAction route) :
OpenScenarioEvent(time, std::move(eventName), std::move(source), {}, {{agentId}}),
route(std::move(route))
{
}
const openScenario::AssignRouteAction route;
};
} // namespace openpass::events
/*******************************************************************************
* Copyright (c) 2021 in-tech
*
* 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 speedActionSignal.h
//! @brief This file contains all functions for class RouteActionSignal
//!
//! This class contains all functionality of the module.
//-----------------------------------------------------------------------------
#pragma once
#include "include/modelInterface.h"
#include "common/openScenarioDefinitions.h"
//-----------------------------------------------------------------------------
//! Signal class
//-----------------------------------------------------------------------------
class RouteActionSignal : public ComponentStateSignalInterface
{
public:
static constexpr char COMPONENTNAME[] = "RouteActionSignal";
//-----------------------------------------------------------------------------
//! Constructor
//-----------------------------------------------------------------------------
RouteActionSignal()
{
componentState = ComponentState::Disabled;
}
//-----------------------------------------------------------------------------
//! Constructor
//-----------------------------------------------------------------------------
RouteActionSignal(RouteActionSignal &other) :
RouteActionSignal(other.componentState,
other.route)
{}
//-----------------------------------------------------------------------------
//! Constructor
//-----------------------------------------------------------------------------
RouteActionSignal(ComponentState componentState,
const openScenario::AssignRouteAction& route) :
ComponentStateSignalInterface(componentState),
route(route)
{}
virtual ~RouteActionSignal() override = default;
//-----------------------------------------------------------------------------
//! 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 << std::endl;
return stream.str();
}
openScenario::AssignRouteAction route {};
};
......@@ -318,6 +318,7 @@ enum Type
OvertakingBanEnd = 280,
OvertakingBanTrucksEnd = 281,
EndOffAllSpeedLimitsAndOvertakingRestrictions = 282,
PedestrianCrossing = 293,
RightOfWayNextIntersection = 301,
RightOfWayBegin = 306,
RightOfWayEnd = 307,
......
/*******************************************************************************
* Copyright (c) 2021 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 "include/callbackInterface.h"
#include "Algorithm_Bicyclist.h"
#include "src/bicyclist.h"
#include <exception>
const std::string Version = "0.0.1";
static const CallbackInterface *Callbacks = nullptr;
extern "C" ALGORITHM_BICYCLIST_SHARED_EXPORT const std::string &OpenPASS_GetVersion()
{
return Version;
}
extern "C" ALGORITHM_BICYCLIST_SHARED_EXPORT ModelInterface *OpenPASS_CreateInstance(
std::string componentName,
bool isInit,
int priority,
int offsetTime,
int responseTime,
int cycleTime,
StochasticsInterface *stochastics,
WorldInterface *world,
const ParameterInterface *parameters,
PublisherInterface * const publisher,
AgentInterface *agent,
const CallbackInterface *callbacks)
{
Callbacks = callbacks;
try
{
return (ModelInterface*)(new (std::nothrow) AlgorithmBicyclistImplementation(
componentName,
isInit,
priority,
offsetTime,
responseTime,
cycleTime,
stochastics,
world,
parameters,
publisher,
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_BICYCLIST_SHARED_EXPORT void OpenPASS_DestroyInstance(ModelInterface *implementation)
{
delete (AlgorithmBicyclistImplementation*)implementation;
}
extern "C" ALGORITHM_BICYCLIST_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_BICYCLIST_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_BICYCLIST_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();
#ifndef _MSC_VER
const std::string exType = p ? p.__cxa_exception_type()->name() : "null";
#else
const std::string exType = "null";
#endif
Callbacks->Log(CbkLogLevel::Error, __FILE__, __LINE__, "unexpected exception - type: " + exType);
}
return false;
}
return true;
}
/*******************************************************************************
* Copyright (c) 2021 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 Algorithm_Bicyclist.h
//! @brief This file contains the implementation header file
#pragma once
#include <QtCore/qglobal.h>
#if defined(ALGORITHM_BICYCLIST_LIBRARY)
# define ALGORITHM_BICYCLIST_SHARED_EXPORT Q_DECL_EXPORT
#else
# define ALGORITHM_BICYCLIST_SHARED_EXPORT Q_DECL_IMPORT
#endif
#include "include/modelInterface.h"
# /*********************************************************************
# * Copyright (c) 2021 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.pro
# \brief This file contains the information for the QtCreator-project of the
# module AlgorithmAgentFollowingDriverModel
#-----------------------------------------------------------------------------/
# shortened .pro file name due to MinGW path length problems
TARGET = AlgorithmAgentFollowingDriverModel
DEFINES += ALGORITHM_AGENTFOLLOWINGDRIVERMODEL_LIBRARY
CONFIG += OPENPASS_LIBRARY
include(../../../global.pri)
SUBDIRS += .\
src
INCLUDEPATH += \
$$SUBDIRS \
../../.. \
../..
SOURCES += \
AlgorithmAFDM.cpp \
src/followingDriverModel.cpp
HEADERS += \
AlgorithmAFDM.h \
src/followingDriverModel.h
set(COMPONENT_NAME Algorithm_Bicyclist)
add_compile_definitions(ALGORITHM_BICYCLIST_LIBRARY)
add_openpass_target(
NAME ${COMPONENT_NAME} TYPE library LINKAGE shared COMPONENT core
HEADERS
Algorithm_Bicyclist.h
src/bicyclist.h
SOURCES
Algorithm_Bicyclist.cpp
src/bicyclist.cpp
LIBRARIES
Qt5::Core
Common
)
/*******************************************************************************
* Copyright (c) 2021 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 <QtGlobal>
#include "bicyclist.h"
#include "common/steeringSignal.h"
#include "common/secondaryDriverTasksSignal.h"
#include "common/accelerationSignal.h"
#include "common/routeActionSignal.h"
#include "include/egoAgentInterface.h"
#include "include/worldInterface.h"
#include "include/stochasticsInterface.h"
//! Conversion factor radiant to degree
constexpr double RadiantToDegree = 180 / M_PI;
AlgorithmBicyclistImplementation::AlgorithmBicyclistImplementation(
std::string componentName,
bool isInit,
int priority,
int offsetTime,
int responseTime,
int cycleTime,
StochasticsInterface *stochastics,
WorldInterface *world,
const ParameterInterface *parameters,
PublisherInterface * const publisher,
const CallbackInterface *callbacks,
AgentInterface *agent) :
SensorInterface(
componentName,
isInit,
priority,
offsetTime,
responseTime,
cycleTime,
stochastics,
world,
parameters,
publisher,
callbacks,
agent)
{
if (parameters->GetParametersDouble().count("VelocityWish") > 0)
{
vWish = parameters->GetParametersDouble().at("VelocityWish");
}
if (parameters->GetParametersDouble().count("VelocityWishDismounted") > 0)
{
vWishDismounted = parameters->GetParametersDouble().at("VelocityWishDismounted");
}
if (parameters->GetParametersDouble().count("Delta") > 0)
{
delta = parameters->GetParametersDouble().at("Delta");
}
if (parameters->GetParametersDouble().count("TGapWish") > 0)
{
tGapWish = parameters->GetParametersDouble().at("TGapWish");
}
if (parameters->GetParametersDouble().count("MinDistance") > 0)
{
minDistance = parameters->GetParametersDouble().at("MinDistance");
}
if (parameters->GetParametersDouble().count("MaxAcceleration") > 0)
{
maxAcceleration = parameters->GetParametersDouble().at("MaxAcceleration");
}
if (parameters->GetParametersDouble().count("MaxDeceleration") > 0)
{
decelerationWish = parameters->GetParametersDouble().at("MaxDeceleration");
}
if (parameters->GetParametersDouble().count("PropabilityDismountAtCrosswalk") > 0)
{
p_dismountAtCrosswalk = parameters->GetParametersDouble().at("PropabilityDismountAtCrosswalk");
}
if (parameters->GetParametersDouble().count("PropabilityUseCrosswalk") > 0)
{
p_useCrosswalk = parameters->GetParametersDouble().at("PropabilityUseCrosswalk");
}
}
AlgorithmBicyclistImplementation::~AlgorithmBicyclistImplementation()
{}
void AlgorithmBicyclistImplementation::UpdateInput(int localLinkId, const std::shared_ptr<SignalInterface const> &data, [[maybe_unused]]int time)
{
if (localLinkId == 0)
{
// from SensorDriverBicyclist
const std::shared_ptr<SensorDriverBicyclistSignal const> signal = std::dynamic_pointer_cast<SensorDriverBicyclistSignal const>(data);
if(!signal)
{
const std::string msg = COMPONENTNAME + " invalid signaltype";
LOG(CbkLogLevel::Debug, msg);
throw std::runtime_error(msg);
}
ownVehicleInformation = signal->GetOwnVehicleInformation();
geometryInformation = signal->GetGeometryInformation();
surroundingObjects = signal->GetSurroundingObjects();
}
else if (localLinkId == 2)
{
// from OSC Actions
const std::shared_ptr<RouteActionSignal const> signal = std::dynamic_pointer_cast<RouteActionSignal const>(data);
if(!signal)
{
const std::string msg = COMPONENTNAME + " invalid signaltype";
LOG(CbkLogLevel::Debug, msg);
throw std::runtime_error(msg);
}
if (signal->componentState == ComponentState::Acting)
{
InitRoute(signal->route);
}
}
else
{
const std::string msg = COMPONENTNAME + " invalid link";
LOG(CbkLogLevel::Debug, msg);
throw std::runtime_error(msg);
}
}
void AlgorithmBicyclistImplementation::UpdateOutput(int localLinkId, std::shared_ptr<SignalInterface const> &data, [[maybe_unused]]int time)
{
if(localLinkId == 0)
{
try
{
data = std::make_shared<SteeringSignal const>(
componentState,
out_steeringwheelangle);
}
catch(const std::bad_alloc&)
{
const std::string msg = COMPONENTNAME + " could not instantiate signal";
LOG(CbkLogLevel::Debug, msg);
throw std::runtime_error(msg);
}
}
else if(localLinkId == 2)
{
try
{
data = std::make_shared<AccelerationSignal const>(componentState, out_longitudinal_acc);
}
catch(const std::bad_alloc&)
{
const std::string msg = COMPONENTNAME + " could not instantiate signal";
LOG(CbkLogLevel::Debug, msg);
throw std::runtime_error(msg);
}
}
else
{
const std::string msg = COMPONENTNAME + " invalid link";
LOG(CbkLogLevel::Debug, msg);
throw std::runtime_error(msg);
}
}
void AlgorithmBicyclistImplementation::Trigger([[maybe_unused]]int time)
{
const auto& mainLocatePosition = GetAgent()->GetEgoAgent().GetMainLocatePosition();
if (routeCurrent == route.cend())
{
NormalDriving(std::numeric_limits<double>::max(), false);
return;
}
if (std::holds_alternative<RoutePointDriving>(*routeCurrent))
{
const auto& routePoint = std::get<RoutePointDriving>(*routeCurrent);
if (GetAgent()->GetEgoAgent().GetDistanceToPoint(routePoint.roadId, routePoint.s) < 0)
{
++routeCurrent;
if (routePoint.stopAtEnd)
{
waitForCrossing = true;
}
}
}
else if (std::holds_alternative<RoutePointCrossing>(*routeCurrent))
{
const auto& routePoint = std::get<RoutePointCrossing>(*routeCurrent);
if (mainLocatePosition.laneId == routePoint.targetLane && (routePoint.direction ? mainLocatePosition.roadPosition.t < 0. : mainLocatePosition.roadPosition.t > 0.))
{
++routeCurrent;
const auto& newRoute = egoRoutes.front();
GetAgent()->GetEgoAgent().SetRoadGraph(std::move(newRoute.roadGraph), newRoute.current, newRoute.target);
egoRoutes.pop_front();
}
}
else if (std::holds_alternative<RoutePointFinishCrossing>(*routeCurrent))
{
if (std::abs(ownVehicleInformation.heading) < 0.1)
{
++routeCurrent;
}
}
if (waitForCrossing)
{
if (GetTGapForCrossing() <= desiredTGapForCrossing)
{
Stop();
return;
}
else
{
waitForCrossing = false;
}
}
if (std::holds_alternative<RoutePointDriving>(*routeCurrent))
{
const auto& routePoint = std::get<RoutePointDriving>(*routeCurrent);
NormalDriving(GetAgent()->GetEgoAgent().GetDistanceToPoint(routePoint.roadId, routePoint.s), routePoint.stopAtEnd);
}
else if (std::holds_alternative<RoutePointCrossing>(*routeCurrent))
{
const auto& routePoint = std::get<RoutePointCrossing>(*routeCurrent);
CrossingStreet(routePoint.dismount);
}
else if (std::holds_alternative<RoutePointFinishCrossing>(*routeCurrent))
{
const auto& routePoint = std::get<RoutePointFinishCrossing>(*routeCurrent);
FinishCrossing(routePoint.turnLeft, routePoint.stopAtEnd, routePoint.dismount);
}
const auto currentSteering = GetAgent()->GetSteeringWheelAngle();
if (std::abs(out_steeringwheelangle - currentSteering) > GetCycleTime() * MAX_STEERING_PER_SECOND /1000.0)
{
out_steeringwheelangle = currentSteering + std::copysign(GetCycleTime() * MAX_STEERING_PER_SECOND / 1000.0, out_steeringwheelangle - currentSteering);
}
}
std::vector<RouteElement> GetWayToTargetRoad(const RoadGraph& roadGraph, const RoadGraphVertex& current, const std::string& targetRoad)
{
auto currentRouteElement = get(RouteElement{}, roadGraph, current);
if (currentRouteElement.roadId == targetRoad)
{
return {currentRouteElement};
}
for (auto [neighbour, neighbourEnd] = adjacent_vertices(current, roadGraph); neighbour != neighbourEnd; neighbour++)
{
auto wayToTargetRoad = GetWayToTargetRoad(roadGraph, *neighbour, targetRoad);
if (!wayToTargetRoad.empty())
{
wayToTargetRoad.push_back(currentRouteElement);
return wayToTargetRoad;
}
}
return {};
}
void AlgorithmBicyclistImplementation::InitRoute(const openScenario::AssignRouteAction& routeAction)
{
const auto currentRoad = GetAgent()->GetRoads(MeasurementPoint::Front).front();
RouteElement previousElement{currentRoad, std::abs(GetAgent()->GetObjectPosition().mainLocatePoint.at(currentRoad).roadPosition.hdg) <= M_PI_2};
int previousLane = 0;
auto* egoRoute = &egoRoutes.emplace_back();
RoadGraph* roadGraph = &egoRoute->roadGraph;
RoadGraphVertex* start = &egoRoute->current;
RoadGraphVertex current{};
RoadGraphVertex* next = &egoRoute->target;
bool addStart = true;
for (const auto& position : routeAction)
{
if (position.roadId == previousElement.roadId)
{
int targetLane = GetTargetLane(position);
bool direction = previousElement.inOdDirection ? targetLane > previousLane : targetLane < previousLane;
previousLane = targetLane;
double s = position.s;
if (GetStochastics()->GetUniformDistributed(0,1) < p_useCrosswalk)
{
//use crosswalk
bool foundCrosswalk = false;
auto root = add_vertex(RouteElement{position.roadId, direction}, *roadGraph);
auto roadMarks = GetWorld()->GetRoadMarkingsInRange(*roadGraph, root, -1, 0, std::numeric_limits<double>::max()).at(root);
for (auto roadMark : roadMarks)
{
if (roadMark.type == CommonTrafficSign::Type::PedestrianCrossing)
{
foundCrosswalk = true;
s = roadMark.distanceToStartOfRoad;
break;
}
}
if (!foundCrosswalk)
{
//get random s to cross road
s = GetStochastics()->GetUniformDistributed(0, GetWorld()->GetRoadLength(position.roadId));
}
}
else {
//get random s to cross road
s = GetStochastics()->GetUniformDistributed(0, GetWorld()->GetRoadLength(position.roadId));
}
bool dismount = GetStochastics()->GetUniformDistributed(0, 1) < p_dismountAtCrosswalk;
if (!route.empty())
{
auto& previous = std::get<RoutePointDriving>(route.back());
previous.s = s;
previous.stopAtEnd = dismount;
}
route.push_back(RoutePointCrossing{targetLane, direction, dismount});
route.push_back(RoutePointFinishCrossing{true, targetLane > previousLane, false, dismount});
egoRoute = &egoRoutes.emplace_back();
roadGraph = &egoRoute->roadGraph;
start = &egoRoute->current;
next = &egoRoute->target;
addStart = true;
}
else
{
auto [roadGraphFromCurrentElement, root] = GetWorld()->GetRoadGraph(previousElement, 10, true);
auto wayToTargetRoad = GetWayToTargetRoad(roadGraphFromCurrentElement, root, position.roadId);
if (wayToTargetRoad.empty())
{
previousElement.inOdDirection = !previousElement.inOdDirection;
std::tie(roadGraphFromCurrentElement, root) = GetWorld()->GetRoadGraph(previousElement, 10, true);
wayToTargetRoad = GetWayToTargetRoad(roadGraphFromCurrentElement, root, position.roadId);
}
if (addStart)
{
*start = add_vertex(previousElement, *roadGraph);
*next = *start;
current = *start;
addStart = false;
}
if (!route.empty() && std::holds_alternative<RoutePointFinishCrossing>(route.back()))
{
auto& previous = std::get<RoutePointFinishCrossing>(route.back());
previous.turnLeft = !(previous.ascendingLanes xor previousElement.inOdDirection);
}
wayToTargetRoad.pop_back();
for (auto routeElement = wayToTargetRoad.rbegin(); routeElement != wayToTargetRoad.rend(); routeElement++)
{
*next = add_vertex(*routeElement, *roadGraph);
add_edge(current, *next, *roadGraph);
current = *next;
previousLane = 0;
if (!route.empty() && std::holds_alternative<RoutePointDriving>(route.back()))
{
route.pop_back();
}
route.push_back(RoutePointDriving{routeElement->roadId, routeElement->inOdDirection ? world->GetRoadLength(routeElement->roadId) : 0, routeElement->inOdDirection, false});
}
previousElement = wayToTargetRoad.front();
}
}
const auto& firstEgoRoute = egoRoutes.front();
GetAgent()->GetEgoAgent().SetRoadGraph(std::move(firstEgoRoute.roadGraph), firstEgoRoute.current, firstEgoRoute.target);
egoRoutes.pop_front();
routeCurrent = route.begin();
}
void AlgorithmBicyclistImplementation::NormalDriving(double distance, bool stopAtEnd)
{
double steeringCurvature = std::atan(wheelBase * geometryInformation.laneEgo.curvature) * RadiantToDegree;
double steeringHeadingError = -ownVehicleInformation.heading * RadiantToDegree;
out_steeringwheelangle = steeringCurvature + steeringHeadingError;
const auto& frontAgent = surroundingObjects.objectFront;
double decelerationCoeff = 0.0;
if (frontAgent.exist)
{
auto vDelta = std::abs(ownVehicleInformation.absoluteVelocity - frontAgent.absoluteVelocity);
auto effectiveMinimumGap = minDistance + ownVehicleInformation.absoluteVelocity*tGapWish + (ownVehicleInformation.absoluteVelocity*vDelta)/2 * std::sqrt(maxAcceleration * decelerationWish);
decelerationCoeff = std::pow(effectiveMinimumGap/frontAgent.relativeLongitudinalDistance, 2);
}
if (stopAtEnd)
{
decelerationCoeff = std::max(decelerationCoeff, std::pow(0.75 * ownVehicleInformation.absoluteVelocity * ownVehicleInformation.absoluteVelocity / distance / decelerationWish, 2));
}
auto freeRoadCoeff = 1.0 - std::pow(ownVehicleInformation.absoluteVelocity/vWish, delta);
auto intelligentDriverModelAcceleration = maxAcceleration * (freeRoadCoeff - decelerationCoeff);
if(intelligentDriverModelAcceleration >= 0)
{
out_longitudinal_acc = std::min(maxAcceleration, intelligentDriverModelAcceleration);
}
else
{
auto maxDeceleration = -decelerationWish;
out_longitudinal_acc = std::max(maxDeceleration, intelligentDriverModelAcceleration);
}
}
void AlgorithmBicyclistImplementation::CrossingStreet(bool dismount)
{
out_steeringwheelangle = (-ownVehicleInformation.heading + (std::get<RoutePointCrossing>(*routeCurrent).direction ? M_PI_2 : -M_PI_2)) * RadiantToDegree;
double targetVelocity = dismount ? vWishDismounted : vWish;
auto freeRoadCoeff = 1.0 - std::pow(ownVehicleInformation.absoluteVelocity/targetVelocity, delta);
auto intelligentDriverModelAcceleration = maxAcceleration * freeRoadCoeff;
if(intelligentDriverModelAcceleration >= 0)
{
out_longitudinal_acc = std::min(maxAcceleration, intelligentDriverModelAcceleration);
}
else
{
auto maxDeceleration = -decelerationWish;
out_longitudinal_acc = std::max(maxDeceleration, intelligentDriverModelAcceleration);
}
}
void AlgorithmBicyclistImplementation::FinishCrossing(bool turnLeft, bool stopAtEnd, bool dismount)
{
out_steeringwheelangle = (turnLeft ? M_PI_4 : -M_PI_4) * RadiantToDegree;
double targetVelocity = stopAtEnd ? 0 : (dismount ? vWishDismounted : vWish);
auto freeRoadCoeff = 1.0 - std::pow(ownVehicleInformation.absoluteVelocity/targetVelocity, delta);
auto intelligentDriverModelAcceleration = maxAcceleration * freeRoadCoeff;
if(intelligentDriverModelAcceleration >= 0)
{
out_longitudinal_acc = std::min(maxAcceleration, intelligentDriverModelAcceleration);
}
else
{
auto maxDeceleration = -decelerationWish;
out_longitudinal_acc = std::max(maxDeceleration, intelligentDriverModelAcceleration);
}
}
void AlgorithmBicyclistImplementation::Stop()
{
out_steeringwheelangle = (-ownVehicleInformation.heading /*+ (std::get<RoutePointCrossing>(*routeCurrent).direction ? M_PI_2 : -M_PI_2)*/) * RadiantToDegree;
out_longitudinal_acc = -decelerationWish;
}
int AlgorithmBicyclistImplementation::GetTargetLane(const openScenario::RoadPosition& position)
{
double widthSum = 0;
int laneId = 0;
if (position.t <= 0)
{
while (widthSum > position.t)
{
--laneId;
widthSum -= GetWorld()->GetLaneWidth(position.roadId, laneId, position.s);
}
}
else
{
while (widthSum < position.t)
{
++laneId;
widthSum += GetWorld()->GetLaneWidth(position.roadId, laneId, position.s);
}
}
return laneId;
}
double AlgorithmBicyclistImplementation::GetTGapForCrossing()
{
return surroundingObjects.ttcForCrossing;
}
/*******************************************************************************
* Copyright (c) 2021 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 AlgorithmAgentFollowingDriverModelImplementation.h
//! \brief This file contains the implementation header file.
#pragma once
#include "include/callbackInterface.h"
#include "include/modelInterface.h"
#include "include/parameterInterface.h"
#include "common/openScenarioDefinitions.h"
#include "common/primitiveSignals.h"
#include "components/Sensor_DriverBicyclist/src/Signals/sensorDriverBicyclistSignal.h"
class AgentInterface;
class StochasticsInterface;
class ParameterInterface;
class WorldInterface;
//! route point for driving along the road
struct RoutePointDriving
{
std::string roadId;
double s;
bool direction;
bool stopAtEnd;
};
//! route point for crossing the street
struct RoutePointCrossing
{
int targetLane;
bool direction; //! true for turning left, false for turning right
bool dismount;
};
//!route point for aligning along the road after crossing it
struct RoutePointFinishCrossing
{
bool turnLeft;
bool ascendingLanes;
bool stopAtEnd;
bool dismount;
};
//!Contents of the route for the EgoAgent
struct EgoRoute
{
RoadGraph roadGraph{};
RoadGraphVertex current{};
RoadGraphVertex target{};
};
using RoutePoint = std::variant<RoutePointDriving, RoutePointCrossing, RoutePointFinishCrossing>;
class AlgorithmBicyclistImplementation : public SensorInterface
{
public:
const std::string COMPONENTNAME = "Algorithm_Bicyclist";
//! \brief Constructor.
//!
//! \param [in] componentId Component ID
//! \param [in] isInit Component's init state
//! \param [in] priority Task priority level
//! \param [in] offsetTime Start time offset
//! \param [in] responseTime Update response time
//! \param [in] cycleTime Cycle time
//! \param [in] stochastics Stochastics instance
//! \param [in] world World interface
//! \param [in] parameters Paramaters
//! \param [in] pubslisher Publisher instance
//! \param [in] callbacks Callbacks
//! \param [in] agent Agent
AlgorithmBicyclistImplementation(
std::string componentName,
bool isInit,
int priority,
int offsetTime,
int responseTime,
int cycleTime,
StochasticsInterface *stochastics,
WorldInterface *world,
const ParameterInterface *parameters,
PublisherInterface * const publisher,
const CallbackInterface *callbacks,
AgentInterface *agent);
AlgorithmBicyclistImplementation(const AlgorithmBicyclistImplementation&) = delete;
AlgorithmBicyclistImplementation(AlgorithmBicyclistImplementation&&) = delete;
AlgorithmBicyclistImplementation& operator=(const AlgorithmBicyclistImplementation&) = delete;
AlgorithmBicyclistImplementation& operator=(AlgorithmBicyclistImplementation&&) = delete;
virtual ~AlgorithmBicyclistImplementation();
void UpdateInput(int localLinkId, const std::shared_ptr<SignalInterface const> &data, int time);
void UpdateOutput(int localLinkId, std::shared_ptr<SignalInterface const> &data, int time);
void Trigger(int time);
//! Calculate the route based on OpenSCENARIO
//!
//! \param routeAction route set in the scenario
//!
void InitRoute(const openScenario::AssignRouteAction& routeAction);
//! Calculate output for normal driving along the road
//!
//! \param distance distance to next route point
//! \param stopAtEnd wether to stop before the next route point
//!
void NormalDriving(double distance, bool stopAtEnd);
//! Calculate output for crossing the street
//!
//! \param dismount wether the driver is dismounted
void CrossingStreet(bool dismount);
//! Calculate output for aligning after crossing
//!
//! \param turnLeft true for aligning to the left, false for right
//! \param stopAtEnd wether to stop after aligning
//! \param dismount wether the drive is dismounted
//!
void FinishCrossing(bool turnLeft, bool stopAtEnd, bool dismount);
//!Bring the bike to hold
void Stop();
private:
int GetTargetLane(const openScenario::RoadPosition& position);
double GetTGapForCrossing();
//!Calculated route to follow
std::vector<RoutePoint> route{};
//!Current route point
std::vector<RoutePoint>::iterator routeCurrent;
//!Routes to set for the EgoAgent
std::list<EgoRoute> egoRoutes{};
OwnVehicleInformation ownVehicleInformation;
GeometryInformation geometryInformation;
SurroundingObjects surroundingObjects;
ComponentState componentState = ComponentState::Acting;
// constants from IDM paper
//! desired speed
double vWish = 20.0/3.6;
//! desired speed
double vWishDismounted = 5.0/3.6;
//! free acceleration exponent characterizing how the acceleration decreases with velocity (1: linear, infinity: constant)
double delta = 4.0;
//! desired time gap between ego and front agent
double tGapWish = 1.5;
//! minimum distance between ego and front (used at slow speeds) also called jam distance
double minDistance = 2.0;
//! maximum acceleration in satisfactory way, not vehicle possible acceleration
double maxAcceleration = 1.4;
//! desired deceleration
double decelerationWish = 2.0;
double MAX_STEERING_PER_SECOND = 360.0;
double desiredTGapForCrossing = 2.0;
//! propability that the driver dismounts when using a crosswalk
double p_dismountAtCrosswalk = 1.0;
//! propability that the driver uses a crosswalk to cross the street
double p_useCrosswalk = 1.0;
double wheelBase = 1.0; //TODO
double out_steeringwheelangle = 0;
//! The longitudinal acceleration of the vehicle [m/s^2].
double out_longitudinal_acc = 0;
bool waitForCrossing = false;
};
......@@ -18,10 +18,12 @@ add_subdirectory(AgentUpdater)
add_subdirectory(AlgorithmAFDM)
add_subdirectory(Algorithm_AEB)
#add_subdirectory(Algorithm_CruiseControlByDistance)
add_subdirectory(Algorithm_Bicyclist)
add_subdirectory(Algorithm_FmuWrapper)
add_subdirectory(Algorithm_Lateral)
add_subdirectory(Algorithm_Longitudinal)
add_subdirectory(ComponentController)
add_subdirectory(Dynamics_Bicycle)
add_subdirectory(Dynamics_Collision)
add_subdirectory(Dynamics_CollisionPostCrash)
#add_subdirectory(Dynamics_Longitudinal_Basic)
......@@ -35,6 +37,7 @@ add_subdirectory(SensorAggregation_OSI)
add_subdirectory(SensorFusionErrorless_OSI)
#add_subdirectory(Sensor_Distance)
add_subdirectory(Sensor_Driver)
add_subdirectory(Sensor_DriverBicyclist)
add_subdirectory(Sensor_OSI)
add_subdirectory(Sensor_RecordState)
add_subdirectory(SignalPrioritizer)
set(COMPONENT_NAME Dynamics_Bicycle)
add_compile_definitions(DYNAMICS_BICYCLE_LIBRARY)
add_openpass_target(
NAME ${COMPONENT_NAME} TYPE library LINKAGE shared COMPONENT core
HEADERS
dynamics_bicycle.h
src/bicycle.h
SOURCES
dynamics_bicycle.cpp
src/bicycle.cpp
LIBRARIES
Qt5::Core
)
# /*********************************************************************
# * Copyright (c) 2021 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 Dynamics_RegularDriving.pro
# \brief This file contains the information for the QtCreator-project of the
# module Dynamics_RegularDriving
#-----------------------------------------------------------------------------/
DEFINES += DYNAMICS_REGULAR_DRIVING_LIBRARY
CONFIG += OPENPASS_LIBRARY
include(../../../global.pri)
SUBDIRS += .\
src
INCLUDEPATH += \
$$SUBDIRS \
../../.. \
../..
SOURCES += \
dynamics_regularDriving.cpp \
src/regularDriving.cpp
HEADERS += \
dynamics_regularDriving.h \
src/regularDriving.h
/*******************************************************************************
* Copyright (c) 2021 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 dynamics_regularDriving.cpp */
//-----------------------------------------------------------------------------
#include "dynamics_bicycle.h"
#include "src/bicycle.h"
const std::string Version = "0.0.1";
static const CallbackInterface *Callbacks = nullptr;
extern "C" DYNAMICS_BICYCLE_SHARED_EXPORT const std::string &OpenPASS_GetVersion()
{
return Version;
}
extern "C" DYNAMICS_BICYCLE_SHARED_EXPORT ModelInterface *OpenPASS_CreateInstance(
std::string componentName,
bool isInit,
int priority,
int offsetTime,
int responseTime,
int cycleTime,
StochasticsInterface *stochastics,
WorldInterface *world,
const ParameterInterface *parameters,
PublisherInterface * const publisher,
AgentInterface *agent,
const CallbackInterface *callbacks)
{
Callbacks = callbacks;
try
{
return (ModelInterface*)(new (std::nothrow) DynamicsBicycleImplementation(
componentName,
isInit,
priority,
offsetTime,
responseTime,
cycleTime,
stochastics,
world,
parameters,
publisher,
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" DYNAMICS_BICYCLE_SHARED_EXPORT void OpenPASS_DestroyInstance(ModelInterface *implementation)
{
delete (DynamicsBicycleImplementation*)implementation;
}
extern "C" DYNAMICS_BICYCLE_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" DYNAMICS_BICYCLE_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" DYNAMICS_BICYCLE_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(...)
{
if(Callbacks != nullptr)
{
Callbacks->Log(CbkLogLevel::Error, __FILE__, __LINE__, "unexpected exception");
}
return false;
}
return true;
}
/*******************************************************************************
* Copyright (c) 2021 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 dynamics_bicycle.h
* @brief This file provides the exported methods.
*
* This file provides the exported methods which are available outside of the library. */
//-----------------------------------------------------------------------------
#pragma once
#include <QtCore/qglobal.h>
#if defined(DYNAMICS_BICYCLE_LIBRARY)
# define DYNAMICS_BICYCLE_SHARED_EXPORT Q_DECL_EXPORT
#else
# define DYNAMICS_BICYCLE_SHARED_EXPORT Q_DECL_IMPORT
#endif
#include "include/modelInterface.h"
/*******************************************************************************
* Copyright (c) 2021 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 dynamics_regularDrivingImplementation.cpp
//! @brief This file contains the implementation header file
//-----------------------------------------------------------------------------
#include "common/opMath.h"
#include <memory>
#include <qglobal.h>
#include <cassert>
#include "bicycle.h"
#include "common/longitudinalSignal.h"
#include "common/commonTools.h"
#include "common/accelerationSignal.h"
#include "common/steeringSignal.h"
#include "common/parametersVehicleSignal.h"
#include "common/globalDefinitions.h"
#include "include/worldInterface.h"
//! value of earth gravity [m/s²].
constexpr double ONE_G = 9.81;
//! air density.
constexpr double RHO = 1.23;
//! Conversion factor degree to radiant.
constexpr double DegreeToRadiant = M_PI / 180;
void DynamicsBicycleImplementation::UpdateInput(int localLinkId, const std::shared_ptr<SignalInterface const> &data,[[maybe_unused]] int time)
{
if (localLinkId == 0)
{
const std::shared_ptr<ComponentStateSignalInterface const> stateSignal = std::dynamic_pointer_cast<ComponentStateSignalInterface const>(data);
if(stateSignal->componentState == ComponentState::Acting)
{
const std::shared_ptr<AccelerationSignal const> signal = std::dynamic_pointer_cast<AccelerationSignal const>(data);
if (!signal)
{
const std::string msg = COMPONENTNAME + " invalid signaltype";
LOG(CbkLogLevel::Debug, msg);
throw std::runtime_error(msg);
}
in_accelaration = signal->acceleration;
}
}
else if (localLinkId == 1)
{
const std::shared_ptr<ComponentStateSignalInterface const> stateSignal = std::dynamic_pointer_cast<ComponentStateSignalInterface const>(data);
if(stateSignal->componentState == ComponentState::Acting)
{
const std::shared_ptr<SteeringSignal const> signal = std::dynamic_pointer_cast<SteeringSignal const>(data);
if (!signal)
{
const std::string msg = COMPONENTNAME + " invalid signaltype";
LOG(CbkLogLevel::Debug, msg);
throw std::runtime_error(msg);
}
in_steeringWheelAngle = signal->steeringWheelAngle;
}
}
else if (localLinkId == 100)
{
// from ParametersAgent
const std::shared_ptr<ParametersVehicleSignal const> signal = std::dynamic_pointer_cast<ParametersVehicleSignal const>(data);
if (!signal)
{
const std::string msg = COMPONENTNAME + " invalid signaltype";
LOG(CbkLogLevel::Debug, msg);
throw std::runtime_error(msg);
}
vehicleModelParameters = signal->vehicleParameters;
}
else
{
const std::string msg = COMPONENTNAME + " invalid link";
LOG(CbkLogLevel::Debug, msg);
throw std::runtime_error(msg);
}
}
void DynamicsBicycleImplementation::UpdateOutput(int localLinkId, std::shared_ptr<SignalInterface const> &data,[[maybe_unused]] int time)
{
if(localLinkId == 0)
{
try {
data = std::make_shared<DynamicsSignal const>(dynamicsSignal);
}
catch(const std::bad_alloc&)
{
const std::string msg = COMPONENTNAME + " could not instantiate signal";
LOG(CbkLogLevel::Debug, msg);
throw std::runtime_error(msg);
}
}
else
{
const std::string msg = COMPONENTNAME + " invalid link";
LOG(CbkLogLevel::Debug, msg);
throw std::runtime_error(msg);
}
}
double DynamicsBicycleImplementation::GetAccelerationFromRollingResistance()
{
double rollingResistanceCoeff = .0125; // Dummy value, get via vehicle Parameters (vehicleModelParameters.rollingDragCoefficient)
double accDueToRollingResistance = -rollingResistanceCoeff * ONE_G;
return accDueToRollingResistance;
}
double DynamicsBicycleImplementation::GetAccelerationFromAirResistance(double velocity)
{
double forceAirResistance = -.5 * RHO * vehicleModelParameters.airDragCoefficient *
vehicleModelParameters.frontSurface * velocity * velocity;
double accDueToAirResistance = forceAirResistance / vehicleModelParameters.weight;
return accDueToAirResistance;
}
double DynamicsBicycleImplementation::GetFrictionCoefficient()
{
return GetWorld()->GetFriction() * vehicleModelParameters.frictionCoeff;
}
double DynamicsBicycleImplementation::GetAccVehicle(double accelarationDriver)
{
const double accelerationDueToAirResistance = GetAccelerationFromAirResistance(GetAgent()->GetVelocity());
const double accelerationDueToRollingResistance = GetAccelerationFromRollingResistance();
return accelarationDriver + accelerationDueToAirResistance + accelerationDueToRollingResistance;
}
void DynamicsBicycleImplementation::Trigger(int time)
{
Q_UNUSED(time);
const auto agent = GetAgent();
//Lateral behavior
double maxDecel = ONE_G * GetFrictionCoefficient() * -1;
agent->SetMaxDeceleration(maxDecel);
double v;
double yawAngle = agent->GetYaw();
double accVehicle = GetAccVehicle(in_accelaration);
v = agent->GetVelocity() + accVehicle*GetCycleTime()/1000.;
if(v < VLowerLimit)
{
accVehicle = 0.0;
v = VLowerLimit;
}
// change of path coordinate since last time step [m]
double ds = v * GetCycleTime() * 0.001;
// change of inertial x-position due to ds and yaw [m]
double dx = ds * std::cos(yawAngle);
// change of inertial y-position due to ds and yaw [m]
double dy = ds * std::sin(yawAngle);
// new inertial x-position [m]
double x = agent->GetPositionX() + dx;
// new inertial y-position [m]
double y = agent->GetPositionY() + dy;
dynamicsSignal.acceleration = accVehicle;
dynamicsSignal.velocity = v;
dynamicsSignal.positionX = x;
dynamicsSignal.positionY = y;
dynamicsSignal.travelDistance = ds;
// convert steering wheel angle to steering angle of front wheels [degree]
double steering_angle_degrees = TrafficHelperFunctions::ValueInBounds(-vehicleModelParameters.maximumSteeringWheelAngleAmplitude, in_steeringWheelAngle, vehicleModelParameters.maximumSteeringWheelAngleAmplitude);
dynamicsSignal.steeringWheelAngle = steering_angle_degrees;
GetPublisher()->Publish("SteeringAngle", steering_angle_degrees);
// calculate curvature (Ackermann model; reference point of yawing = rear axle!) [radiant]
double steeringCurvature = std::tan(DegreeToRadiant * steering_angle_degrees) / vehicleModelParameters.wheelbase;
// change of yaw angle due to ds and curvature [radiant]
double dpsi = std::atan(steeringCurvature*ds);
dynamicsSignal.yawRate = dpsi / (GetCycleTime() * 0.001);
dynamicsSignal.centripetalAcceleration = dynamicsSignal.yawRate * v;
// new yaw angle in current time step [radiant]
double psi = agent->GetYaw() + dpsi;
dynamicsSignal.yaw = psi;
}
/*******************************************************************************
* Copyright (c) 2021 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
*******************************************************************************/
#pragma once
#include "common/opMath.h"
#include "include/modelInterface.h"
#include "include/observationInterface.h"
#include "common/primitiveSignals.h"
#include "common/dynamicsSignal.h"
#include "common/globalDefinitions.h"
/** \addtogroup Dynamics_Bicycle
* @{
* \brief models the longitudinal and lateral dynamics of the vehicle coresponding to the accelaration and steering inputs
*
* \section Dynamics_Bicycle_Inputs Inputs
* Input variables:
* name | meaning
* -----|------
* in_accelaration | Acceleleration
* in_steeringWheelAngle | Steering wheel angle of the agent
*
*
* Input channel IDs:
* Input ID | signal class | contained variables
* ----------|-------------------------------------------|-------------
* 1 | AccelerationSignal | in_accelaration
* 2 | SteeringSignal | in_steeringWheelAngle
*
*
** \section Dynamics_Bicycle_InitInputs Init Inputs
* Init input variables:
* name | meaning
* --------------|------
* vehicleModelParameters | VehicleModelParameters
*
*
* * Init input channel IDs:
* Input Id | signal class | contained variables
* --------------|-----------------------------|------------
* 100 | ParametersVehicleSignal | vehicleModelParameters
*
*
*
* * \section Dynamics_Bicycle_Outputs Outputs
* Output variables:
* name | meaning
* ---------------------------|------
* acceleration | Acceleration of the current agent coresponding to the pedalpositions [m/s²]
* velocity | The driven curvature of the car coresponding to the steering wheel angle based on Ackermann [radiant]
* positionX | new inertial x-position [m]
* positionY | new inertial y-position [m]
* yaw | new yaw angle in current time step [radiant]
* yawRate | change of yaw angle due to ds and curvature [radiant]
* steeringWheelAngle | new angle of the steering wheel angle [degree]
* travelDistance | distance traveled by the agent during this timestep [m]
*
*
* Output channel IDs:
* Output Id | signal class | contained variables
* ---------------------------|-----------------|-------------
* 0 | DynamicsSignal | acceleration, velocity, positionX, positionY, yaw, yawRate, steeringWheelAngle, travelDistance
*
* \section Dynamics_Bicycle_ExternalParameters External parameters
* none
*
* \section Dynamics_Bicycle_InternalParameters Internal paramters
* name | value | meaning
* ----------------------|--------|-------------
* VLowerLimit | 0 | The minimal velocity of the agent in m/s.
*
* @} */
//! \ingroup Dynamics_Bicycle
class DynamicsBicycleImplementation : public DynamicsInterface
{
public:
//! Name of the current component
const std::string COMPONENTNAME = "DynamicsBicycle";
DynamicsBicycleImplementation(
std::string componentName,
bool isInit,
int priority,
int offsetTime,
int responseTime,
int cycleTime,
StochasticsInterface *stochastics,
WorldInterface *world,
const ParameterInterface *parameters,
PublisherInterface * const publisher,
const CallbackInterface *callbacks,
AgentInterface *agent) :
DynamicsInterface(
componentName,
isInit,
priority,
offsetTime,
responseTime,
cycleTime,
stochastics,
world,
parameters,
publisher,
callbacks,
agent),
dynamicsSignal {ComponentState::Acting}
{
if (GetPublisher() == nullptr)
{
std::string msg = "DynamicsBicycle requires a publisher";
LOG(CbkLogLevel::Error, msg);
throw std::runtime_error(msg);
}
}
DynamicsBicycleImplementation(const DynamicsBicycleImplementation&) = delete;
DynamicsBicycleImplementation(DynamicsBicycleImplementation&&) = delete;
DynamicsBicycleImplementation& operator=(const DynamicsBicycleImplementation&) = delete;
DynamicsBicycleImplementation& operator=(DynamicsBicycleImplementation&&) = delete;
virtual ~DynamicsBicycleImplementation() = default;
/*!
* \brief Update Inputs
*
* Function is called by framework when another component delivers a signal over
* a channel to this component (scheduler calls update taks of other component).
*
* Refer to module description for input channels and input ids.
*
* @param[in] localLinkId Corresponds to "id" of "ComponentInput"
* @param[in] data Referenced signal (copied by sending component)
* @param[in] time Current scheduling time
*/
virtual void UpdateInput(int localLinkId, const std::shared_ptr<SignalInterface const> &data, int time);
/*!
* \brief Update outputs.
*
* Function is called by framework when this component has to deliver a signal over
* a channel to another component (scheduler calls update task of this component).
*
* Refer to module description for output channels and output ids.
*
* @param[in] localLinkId Corresponds to "id" of "ComponentOutput"
* @param[out] data Referenced signal (copied by this component)
* @param[in] time Current scheduling time
*/
virtual void UpdateOutput(int localLinkId, std::shared_ptr<SignalInterface const> &data, int time);
/*!
* \brief Process data within component.
*
* Function is called by framework when the scheduler calls the trigger task
* of this component
*
* @param[in] time Current scheduling time
*/
virtual void Trigger(int time);
private:
double GetAccVehicle(double accelarationDriver);
//! Get the acceleration (negative) caused by the air resistance of the vehicle.
//! @param [in] velocity absolute vehicle speed [m/s]
//! @return acceleration due to rolling resistance [m/s^2]
//-----------------------------------------------------------------------------
double GetAccelerationFromAirResistance(double velocity);
//! Get the acceleration (negative) caused by the rolling resistance of the wheels.
//! @return acceleration due to rolling resistance [m/s^2]
//-----------------------------------------------------------------------------
double GetAccelerationFromRollingResistance();
//! Returns the friction coefficient according to enviroment and car conditions
//! @return friction coefficient (currently ALWAYS 1)
double GetFrictionCoefficient();
/** @name Private Variables
* @{
* */
/** @name Internal Parameters
* @{
*/
//! The minimal velocity of the agent [m/s].
const double VLowerLimit = 0;
/**
* @} */ // End of Internal Parameters
/** @name External Parameters
* @{
*/
// --- Inputs
double in_accelaration = 0;
//! The steering wheel angle [°].
double in_steeringWheelAngle = 0;
// --- Outputs
//! Output Signal containing (aLong,v,x,y,dpsi,psi)
DynamicsSignal dynamicsSignal;
// --- Init Inputs
//! Containing the vehicle parameters e.g. double carMass; double rDyn and more.
VehicleModelParameters vehicleModelParameters;
};
/** @} */ // End of group DynamicsBicycle