Skip to content
Snippets Groups Projects
Commit 1382c735 authored by René Paris's avatar René Paris
Browse files

mod: remote lenght sorting of entities and enable weighting based on DistributionEntries

parent e2054055
No related branches found
No related tags found
No related merge requests found
......@@ -64,14 +64,19 @@ struct ToTrafficDistribution
TrafficDistributionEntry operator()(
const NET_ASAM_OPENSCENARIO::v1_3::ITrafficDistributionEntry& entry) const
{
return {
TransformSharedPointers(
entry.GetEntityDistribution()->GetEntityDistributionEntry(), //
[](const NET_ASAM_OPENSCENARIO::v1_3::IEntityDistributionEntry& entry)
{ return entry.GetScenarioObjectTemplate(); }),
ParseProperites(stochastics, entry.GetProperties()),
entry.GetWeight(),
};
if (const auto weight = entry.GetWeight(); weight != 0.0)
{
return {
TransformSharedPointers(
entry.GetEntityDistribution()->GetEntityDistributionEntry(), //
[weight](const NET_ASAM_OPENSCENARIO::v1_3::IEntityDistributionEntry& entry) -> EntityDistrubutionEntry
{ return {entry.GetScenarioObjectTemplate(), weight}; }),
ParseProperites(stochastics, entry.GetProperties()),
entry.GetWeight(),
};
}
throw std::domain_error("ToTrafficDistribution: TrafficDistributionEntry with weight 0 detected. Please adjust the scenario.");
}
StochasticsInterface& stochastics;
......
......@@ -24,7 +24,11 @@
namespace OpenScenarioEngine::v1_3
{
using EntityDistrubutionEntry = std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IScenarioObjectTemplate>;
struct EntityDistrubutionEntry
{
std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IScenarioObjectTemplate> scenario_object_template;
units::dimensionless::scalar_t weight;
};
struct TrafficDistributionEntry
{
......
......@@ -115,8 +115,8 @@ public:
protected:
std::unique_ptr<Implementation<Instance>> impl_{nullptr};
std::shared_ptr<StochasticsInterface> stochastics_;
std::shared_ptr<Interface<Instance>> action_;
std::shared_ptr<StochasticsInterface> stochastics_;
};
} // namespace Node
} // namespace OpenScenarioEngine::v1_3
......@@ -15,6 +15,7 @@
#include <memory>
#include "Conversion/OscToMantle/ConvertScenarioTrafficArea.h"
#include "Conversion/OscToMantle/ConvertScenarioTrafficDistribution.h"
#include "Utils/ControllerCreator.h"
#include "Utils/EntityCreator.h"
#include "Utils/Logger.h"
......@@ -64,13 +65,9 @@ bool TrafficAreaAction::Step()
EntityCreator spawner{mantle.environment};
SequentialSpawnSpace spawnSpace{trafficAreas, values.trafficDistributions};
//// HACK!
StochasticsInterface* rng;
//// HACK!
while (values.numberOfEntities)
{
if (!spawnSpace.Spawn(spawner, mantle, *rng))
if (!spawnSpace.Spawn(spawner, mantle, *mantle.stochastics))
{
return false;
}
......@@ -107,7 +104,7 @@ void TrafficAreaAction::lookupAndRegisterData(yase::Blackboard& blackboard)
action_->GetNumberOfEntities(),
ConvertScenarioTrafficDistribution(*stochastics_, action_->GetTrafficDistribution()),
ConvertScenarioTrafficArea(action_->GetTrafficArea())},
Interfaces<OpenScenarioEngine::v1_3::TrafficAreaAction>{environment, controller_service});
Interfaces<OpenScenarioEngine::v1_3::TrafficAreaAction>{environment, controller_service, stochastics_});
}
} // namespace Node
} // namespace OpenScenarioEngine::v1_3
......@@ -44,7 +44,7 @@ struct Interfaces<TrafficAreaAction>
{
std::shared_ptr<mantle_api::IEnvironment> environment;
std::shared_ptr<ControllerService> controller_service;
std::shared_ptr<PropertyInterpreter> property_interpreter;
std::shared_ptr<StochasticsInterface> stochastics;
mantle_api::IController& operator()(mantle_api::IEntity& entity, const std::vector<std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IObjectController>>& controllers);
};
......@@ -64,7 +64,7 @@ private:
/// The accumulated size of all traffic areas up to and including the index of the respective element.
/// Does not account for overlapping areas.
std::vector<units::length::meter_t> lengths;
std::shared_ptr<StochasticsInterface> property_interpreter;
std::shared_ptr<StochasticsInterface> stochastics_;
};
namespace Node
......
......@@ -17,12 +17,15 @@
#include <cstddef>
#include <iterator>
#include <numeric>
#include <stdexcept>
#include <tuple>
#include <utility>
#include <vector>
#include "Conversion/OscToMantle/ConvertScenarioTrafficDistribution.h"
#include "Length.h"
#include "Utils/PropertyInterpreter.h"
#include "Utils/Spawning/Common.h"
#include "Utils/Spawning/SpawnZone.h"
#include "Utils/Spawning/Transform.h"
#include "Utils/Spawning/Weighted.h"
......@@ -31,31 +34,9 @@
namespace OpenScenarioEngine::v1_3
{
SpawnSpace::SpawnSpace(const OpenScenarioEngine::v1_3::TrafficDistributions &distributions)
: weights{{}}
: weights{{}}, distributions_{distributions}
{
const auto [totalWeight, totalEntries] =
std::transform_reduce(
distributions.begin(),
distributions.end(),
std::make_tuple(double{}, size_t{}),
[](const auto &lhs, const auto &rhs)
{ return std::make_tuple(
std::get<0>(lhs) + std::get<0>(rhs),
std::get<1>(lhs) + std::get<1>(rhs)); },
[](const auto &d)
{ return std::make_tuple(d.weight, d.entity_distrubutions.size()); });
entities.reserve(totalEntries);
for (const auto &distribution : distributions)
{
for (const auto &entity_distrubution : distribution.entity_distrubutions)
{
entities.emplace_back(
Weighted<ObjectTemplate>{entity_distrubution, distribution.weight / totalWeight});
}
}
std::sort(entities.begin(), entities.end(), Less<ToLength>{});
Transform(entities, weights, ToAccumulatedWeight{});
Transform(distributions_, weights, ToAccumulatedWeight{});
} // namespace OpenScenarioEngine::v1_3
SequentialSpawnSpace::SequentialSpawnSpace(const std::vector<mantle_api::TrafficArea> &areas, const OpenScenarioEngine::v1_3::TrafficDistributions &distributions)
......@@ -68,15 +49,37 @@ SequentialSpawnSpace::SequentialSpawnSpace(const std::vector<mantle_api::Traffic
}); // clang-format on
}
ObjectTemplate SequentialSpawnSpace::SampleObjectTemplate(StochasticsInterface &rng) const
// TODO: prevent calling with no distirbution
const TrafficDistributionEntry &SequentialSpawnSpace::SampleDistribution(StochasticsInterface &stochastics) const
{
if (entities.size() <= 1)
if (distributions_.size() == 1)
{
return entities.empty() ? ObjectTemplate{} : entities.front().handle;
return distributions_.front();
}
assert(weights.size() == entities.size() + 1);
const units::dimensionless::scalar_t sampled_weight{rng.GetUniformDistributed(.0, weights.back().value())};
return Sample(entities, weights, sampled_weight).handle;
assert(weights.size() == distributions_.size() + 1);
// add 0th element with zero for sampler
const units::dimensionless::scalar_t sampled_weight{stochastics.GetUniformDistributed(.0, weights.back().value())};
return Sample(distributions_, weights, sampled_weight);
}
std::tuple<ObjectTemplatePtr, PropertyInterpreter &> SequentialSpawnSpace::SampleObjectTemplate(StochasticsInterface &stochastics) const
{
auto distribution = SampleDistribution(stochastics);
auto &entityDistributions = distribution.entity_distrubutions;
if (entityDistributions.size() == 1)
{
return {entityDistributions.front().scenario_object_template, distribution.properties};
}
// add 0th element with zero for sampler (similar to constructor for distributions)
std::vector<units::dimensionless::scalar_t> entity_weights{0};
Transform(entityDistributions, entity_weights, ToAccumulatedWeight{});
assert(entity_weights.size() == entityDistributions.size() + 1);
const units::dimensionless::scalar_t sampled_weight{stochastics.GetUniformDistributed(.0, weights.back().value())};
return {Sample(entityDistributions, entity_weights, sampled_weight).scenario_object_template,
distribution.properties};
}
std::tuple<std::vector<std::vector<std::vector<SpawnSpot>>>::iterator,
......@@ -131,17 +134,17 @@ DistributedSpawnSpace::DistributedSpawnSpace(const OpenScenarioEngine::v1_3::Tra
}
}
ObjectTemplate DistributedSpawnSpace::SampleObjectTemplate(StochasticsInterface &rng) const
ObjectTemplatePtr DistributedSpawnSpace::SampleObjectTemplate(StochasticsInterface &stochastics) const
{
const units::length::meter_t max_length{GetMaxIntervalLength()};
const auto entity_end{FindSpawnableEntityEnd(max_length)};
if (std::distance(entities.begin(), entity_end) <= 1)
{
return entity_end == entities.begin() ? ObjectTemplate{} : entities.front().handle;
return entity_end == entities.begin() ? ObjectTemplatePtr{} : entities.front().handle;
}
assert(weights.size() == entities.size() + 1);
const units::dimensionless::scalar_t max_weight{weights[static_cast<size_t>(std::distance(entities.begin(), entity_end))]};
const units::dimensionless::scalar_t sampled_weight{rng.GetUniformDistributed(.0, max_weight.value())};
const units::dimensionless::scalar_t sampled_weight{stochastics.GetUniformDistributed(.0, max_weight.value())};
return Sample(entities, weights, sampled_weight).handle;
}
......@@ -150,18 +153,18 @@ units::length::meter_t DistributedSpawnSpace::GetMaxIntervalLength() const
return spots.empty() ? units::length::meter_t{} : GetLength(*spots.rbegin());
}
std::vector<Weighted<ObjectTemplate>>::const_iterator DistributedSpawnSpace::FindSpawnableEntityEnd(units::length::meter_t threshold) const
std::vector<Weighted<ObjectTemplatePtr>>::const_iterator DistributedSpawnSpace::FindSpawnableEntityEnd(units::length::meter_t threshold) const
{
return std::upper_bound(entities.begin(), entities.end(), threshold, [](units::length::meter_t threshold, const Weighted<ObjectTemplate> &entity)
return std::upper_bound(entities.begin(), entities.end(), threshold, [](units::length::meter_t threshold, const Weighted<ObjectTemplatePtr> &entity)
{ return threshold < GetLength(entity); });
}
std::vector<Weighted<ObjectTemplate>>::iterator DistributedSpawnSpace::FindSpawnableEntityEnd(units::length::meter_t threshold)
std::vector<Weighted<ObjectTemplatePtr>>::iterator DistributedSpawnSpace::FindSpawnableEntityEnd(units::length::meter_t threshold)
{
return std::upper_bound(entities.begin(), entities.end(), threshold, [](units::length::meter_t threshold, const Weighted<ObjectTemplate> &entity)
return std::upper_bound(entities.begin(), entities.end(), threshold, [](units::length::meter_t threshold, const Weighted<ObjectTemplatePtr> &entity)
{ return threshold < GetLength(entity); });
}
std::set<SpawnSpot, Less<ToLength>>::const_iterator DistributedSpawnSpace::SampleSpot(units::length::meter_t minLength, VelocityRange velocity, StochasticsInterface &rng) const
std::set<SpawnSpot, Less<ToLength>>::const_iterator DistributedSpawnSpace::SampleSpot(units::length::meter_t minLength, VelocityRange velocity, StochasticsInterface &stochastics) const
{
auto spot{spots.lower_bound(minLength)};
if (spot == spots.end())
......@@ -188,7 +191,7 @@ std::set<SpawnSpot, Less<ToLength>>::const_iterator DistributedSpawnSpace::Sampl
Transform(candidates, lengths, [&accumulator](const auto iterator) { //
return accumulator(*iterator);
});
const units::length::meter_t sampledLength{rng.GetUniformDistributed(.0, lengths.back().value())};
const units::length::meter_t sampledLength{stochastics.GetUniformDistributed(.0, lengths.back().value())};
return Sample(candidates, lengths, sampledLength);
}
......
......@@ -13,8 +13,10 @@
#include <Stochastics/StochasticsInterface.h>
#include <units.h>
#include <cstddef>
#include <set>
#include <utility>
#include <string>
#include <tuple>
#include <vector>
#include "Conversion/OscToMantle/ConvertScenarioTrafficDistribution.h"
......@@ -22,6 +24,7 @@
#include "SpawnSpot.h"
#include "SpawnZone.h"
#include "Utils/Geometry.h"
#include "Utils/PropertyInterpreter.h"
#include "VelocityRange.h"
#include "Weighted.h"
......@@ -31,8 +34,8 @@ struct SpawnSpace
{
SpawnSpace(const OpenScenarioEngine::v1_3::TrafficDistributions &);
std::vector<Weighted<ObjectTemplate>> entities; // Object templates (entity + controllers) sorted by ascending length
std::vector<units::dimensionless::scalar_t> weights; // Used to sample an entity
std::vector<units::dimensionless::scalar_t> weights; // Used to sample an entity
const OpenScenarioEngine::v1_3::TrafficDistributions &distributions_; // Distributions to differentiate between individual entity groups
};
/// Space that spawns entities naively & greedily by design as defined here:
......@@ -44,10 +47,13 @@ struct SequentialSpawnSpace : SpawnSpace
{
SequentialSpawnSpace(const std::vector<mantle_api::TrafficArea> &, const OpenScenarioEngine::v1_3::TrafficDistributions &);
// TODO: Documentation
const TrafficDistributionEntry &SampleDistribution(StochasticsInterface &stochastics) const;
/// Samples and returns a random entity without considering the remaining space.
///
/// \return Random entity out of this space's entities. If it has none, a nullptr is returned.
ObjectTemplate SampleObjectTemplate(StochasticsInterface &) const;
std::tuple<ObjectTemplatePtr, PropertyInterpreter &> SampleObjectTemplate(StochasticsInterface &) const;
/// Tries to spawn an entity and returns whether spawning was successful.
/// If successful, also adjusts the spawn space's intervals.
......@@ -82,13 +88,13 @@ struct DistributedSpawnSpace : SpawnSpace
///
/// \param sample Value within the accumulated weights of each entity of this space
/// \return Blueprint for spawning an entity alongside controllers for that entity
ObjectTemplate GetObjectTemplate(units::dimensionless::scalar_t sample);
ObjectTemplatePtr GetObjectTemplate(units::dimensionless::scalar_t sample);
//! Returns a random object template (entity + controllers) using the given number generator
//!
//! \param StochasticsInterface Random number generator used to sample a weight
//! \return Randomly selected object template
ObjectTemplate SampleObjectTemplate(StochasticsInterface &) const;
ObjectTemplatePtr SampleObjectTemplate(StochasticsInterface &) const;
std::set<SpawnSpot, Less<ToLength>>::const_iterator SampleSpot(units::length::meter_t min_length, VelocityRange, StochasticsInterface &) const;
......@@ -102,8 +108,8 @@ struct DistributedSpawnSpace : SpawnSpace
units::length::meter_t GetMaxIntervalLength() const;
typename std::vector<Weighted<ObjectTemplate>>::const_iterator FindSpawnableEntityEnd(units::length::meter_t upper_bound) const;
typename std::vector<Weighted<ObjectTemplate>>::iterator FindSpawnableEntityEnd(units::length::meter_t upper_bound);
typename std::vector<Weighted<ObjectTemplatePtr>>::const_iterator FindSpawnableEntityEnd(units::length::meter_t upper_bound) const;
typename std::vector<Weighted<ObjectTemplatePtr>>::iterator FindSpawnableEntityEnd(units::length::meter_t upper_bound);
std::set<SpawnSpot, Less<ToLength>> spots;
};
......@@ -125,7 +131,7 @@ void UpdateSpot(std::vector<OpenScenarioEngine::v1_3::SpawnSpot> &stream,
namespace OpenScenarioEngine::v1_3
{
template <typename EntityCreator, typename AttachControllers>
bool SequentialSpawnSpace::Spawn(EntityCreator &&spawner, AttachControllers &&attach_controllers, StochasticsInterface &rng)
bool SequentialSpawnSpace::Spawn(EntityCreator &&spawner, AttachControllers &&attach_controllers, StochasticsInterface &stochastics)
{
using namespace units::literals;
......@@ -135,27 +141,36 @@ bool SequentialSpawnSpace::Spawn(EntityCreator &&spawner, AttachControllers &&at
{
return false;
}
ObjectTemplate blueprint{SampleObjectTemplate(rng)};
if (blueprint == nullptr)
if (distributions_.empty())
{
return false;
}
const auto velocity_range{GetSpawnVelocityRange(blueprint)};
auto spawn_velocity{SampleVelocity(velocity_range, rng)};
auto [objectTemplate, propertyInterpreter] = SampleObjectTemplate(stochastics);
if (objectTemplate == nullptr)
{
return false;
}
const auto velocity_range{GetSpawnVelocityRange(objectTemplate)};
auto parameters = propertyInterpreter.GenerateParameters();
units::velocity::meters_per_second_t spawn_velocity{std::stod(parameters["velocity"])};
const units::time::second_t minimum_time_to_collision{2_s};
const Padding padding{GetLengthPadding(blueprint)};
const Padding padding{GetLengthPadding(objectTemplate)};
if (auto [spot_area, spot_stream, spot, interval]{FindSpot(padding, spawn_velocity, minimum_time_to_collision)}; spot_area != spots.end())
{
const auto area{std::next(areas.begin(), std::distance(spots.begin(), spot_area))};
const auto &stream{*std::next(area->begin(), std::distance(spot_area->begin(), spot_stream))};
const mantle_api::Pose pose{stream->Convert(mantle_api::ITrafficAreaStream::StreamPose{{interval.max, {}}, {}}).value()};
std::string name{"Common" + std::to_string(++i)};
mantle_api::IEntity &object{spawner.CreateEntity(GetEntity(blueprint), name)};
mantle_api::IEntity &object{spawner.CreateEntity(GetEntity(objectTemplate), name)};
object.SetName(name);
object.SetPosition(pose.position);
object.SetOrientation(pose.orientation);
object.SetVelocity(util::Rotate(spawn_velocity, pose.orientation));
attach_controllers(object, blueprint->GetObjectController());
attach_controllers(object, objectTemplate->GetObjectController());
const auto occupied_space{Interval{interval.max, interval.max}.OffsetBy(-padding.rear, padding.front)};
UpdateSpot(*spot_stream, spot, occupied_space, spawn_velocity);
return true;
......@@ -164,20 +179,20 @@ bool SequentialSpawnSpace::Spawn(EntityCreator &&spawner, AttachControllers &&at
}
template <typename EntityCreator>
bool DistributedSpawnSpace::Spawn(EntityCreator &&spawner, StochasticsInterface &rng)
bool DistributedSpawnSpace::Spawn(EntityCreator &&spawner, StochasticsInterface &stochastics)
{
ObjectTemplate blueprint{SampleObjectTemplate(rng)};
ObjectTemplatePtr blueprint{SampleObjectTemplate(stochastics)};
if (blueprint == nullptr)
{
return false;
}
const auto velocity_range{GetSpawnVelocityRange(blueprint)};
const auto spot{SampleSpot(GetLength(blueprint), velocity_range, rng)};
const auto spot{SampleSpot(GetLength(blueprint), velocity_range, stochastics)};
if (spot == spots.end())
{
return false;
}
auto spawn_velocity{SampleVelocity(velocity_range, rng)};
auto spawn_velocity{SampleVelocity(velocity_range, stochastics)};
// TODO: Find last spawnable spot on the zone of the sampled interval
// spawner.CreateEntity(*entity, place.zone->GetPosition(distance));
// TODO: Remove/replace interval
......
......@@ -15,6 +15,8 @@
#include <algorithm>
#include <stdexcept>
#include "Conversion/OscToMantle/ConvertScenarioTrafficDistribution.h"
namespace OpenScenarioEngine::v1_3
{
units::dimensionless::scalar_t ToWeight::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IScenarioObjectTemplate& object) const
......@@ -27,47 +29,9 @@ units::dimensionless::scalar_t ToWeight::operator()(const NET_ASAM_OPENSCENARIO:
return units::dimensionless::scalar_t{entry.GetWeight()};
}
units::dimensionless::scalar_t ToWeight::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IEntityObject& object) const
{
if (object.IsSetVehicle())
{
return ToWeight{}(object.GetVehicle());
}
if (object.IsSetPedestrian())
{
return ToWeight{}(object.GetPedestrian());
}
if (object.IsSetMiscObject())
{
return ToWeight{}(object.GetMiscObject());
}
throw std::runtime_error("ToWeight: Invalid entity. Is neither a vehicle, pedestrian nor a misc. object");
}
units::dimensionless::scalar_t ToWeight::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IPedestrian& pedestrian) const
{
return ToWeight{}(pedestrian.GetProperties());
}
units::dimensionless::scalar_t ToWeight::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IVehicle& vehicle) const
{
return ToWeight{}(vehicle.GetProperties());
}
units::dimensionless::scalar_t ToWeight::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IMiscObject& object) const
units::dimensionless::scalar_t ToWeight::operator()(const EntityDistrubutionEntry& entityDistrubutionEntry) const
{
return ToWeight{}(object.GetProperties());
return entityDistrubutionEntry.weight;
}
units::dimensionless::scalar_t ToWeight::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IProperties& properties) const
{
const auto& entries{properties.GetProperties()};
auto match{std::find_if(entries.begin(), entries.end(), [](const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IProperty>& entry)
{ return entry->GetName() == "Weight"; })};
if (match != entries.end())
{
return units::dimensionless::scalar_t{std::stod((*match)->GetValue())};
}
throw std::runtime_error("ToWeight: 'Weight' not found in properties");
}
} // namespace OpenScenarioEngine::v1_3
......@@ -19,6 +19,8 @@
namespace OpenScenarioEngine::v1_3
{
class EntityDistrubutionEntry;
struct ToWeight
{
constexpr units::dimensionless::scalar_t operator()(units::dimensionless::scalar_t) const;
......@@ -30,15 +32,7 @@ struct ToWeight
units::dimensionless::scalar_t operator()(const NET_ASAM_OPENSCENARIO::v1_3::ITrafficDistributionEntry&) const;
units::dimensionless::scalar_t operator()(const NET_ASAM_OPENSCENARIO::v1_3::IEntityObject&) const;
units::dimensionless::scalar_t operator()(const NET_ASAM_OPENSCENARIO::v1_3::IPedestrian&) const;
units::dimensionless::scalar_t operator()(const NET_ASAM_OPENSCENARIO::v1_3::IVehicle&) const;
units::dimensionless::scalar_t operator()(const NET_ASAM_OPENSCENARIO::v1_3::IMiscObject&) const;
units::dimensionless::scalar_t operator()(const NET_ASAM_OPENSCENARIO::v1_3::IProperties&) const;
units::dimensionless::scalar_t operator()(const EntityDistrubutionEntry&) const;
template <typename Type>
units::dimensionless::scalar_t operator()(const Weighted<Type>&) const;
......
......@@ -17,7 +17,7 @@
namespace OpenScenarioEngine::v1_3
{
//! \brief Shared point of a scenario object template, which is an entity paired with an arbitrary number of controllers
using ObjectTemplate = typename std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IScenarioObjectTemplate>;
using ObjectTemplatePtr = typename std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IScenarioObjectTemplate>;
template <typename Type>
struct Weighted
......
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