scenario.cpp

Go to the documentation of this file.

#include "compile_scenario/scenario.h"
#include "coordinate_converter/coordinate_converter.h"
#include "map_management/map_manager.h"
 
namespace CompileScenario
{
 
    // The constructor to the scenario class update the size and origin to
    // coordinate converter, map manager and routemaker so the whole system uses
    // the same values.
    Scenario::Scenario(std::string name, Core::GeographicalCoordinate origin,
                       int size)
        : m_Name(name), m_Size(size), m_Origin(origin),
          m_KeyframeManager(KeyframeManagement::KeyframeManager::Instance())
    {
        CoordinateConverter::CoordConv::ResetOrigin(origin, size);
        MapManagement::MapManager::GetMap(
            CoordinateConverter::CoordConv::GeographicToUTM(origin), size);
        m_Routemaker = std::make_unique<Routemaker::Routemaker>(
            CoordinateConverter::CoordConv::GeographicToUTM(origin), size);
    }
 
    // SetOrigin to the scenario class update the size and origin to
    // coordinate converter, map manager and routemaker so the whole system uses
    // the same values.
    void
    Scenario::SetOrigin(Core::GeographicalCoordinate GeoCoord, int size)
    {
        m_Origin = GeoCoord;
        m_Size = size;
        CoordinateConverter::CoordConv::ResetOrigin(GeoCoord, size);
        m_Routemaker->UpdateOrigin(
            CoordinateConverter::CoordConv::GeographicToUTM(GeoCoord), size);
        MapManagement::MapManager::GetMap(
            CoordinateConverter::CoordConv::GeographicToUTM(GeoCoord), size);
    }
 
    Scenario::RouteMap&
    Scenario::Compile()
    {
        if (m_KeyframeManager.GetKeyframes().empty()) {
            return m_Routes;
        }
 
        m_Routes.clear();
        auto keyframes = m_KeyframeManager.GetKeyframes();
        // Keyframes need to be sorted by agentID and timestamp before planning
        // routes between them.
        std::sort(keyframes.begin(), keyframes.end(),
                  [](Core::Keyframe a, Core::Keyframe b) {
                      if (a.AgentId != b.AgentId) {
                          return a.AgentId < b.AgentId;
                      }
                      return a.TimeStamp < b.TimeStamp;
                  });
        // The routes generated by the routemaker is stored in a map where the
        // key is the agent ID and the value is a vector with the coordinates
        // the routemaker returns.
        for (int i = 0; i < keyframes.size() - 1; i++) {
            // Check if the current and next keyframe belongs to the same agent
            // and search for the agent Id in the map. If this is the first
            // agent with this ID the routes will be inserted as a new element
            // in the map. If the agent ID is already in the map, the returned
            // values from the routemaker will be pushed back into the place for
            // the agent ID to the agent.
            if (keyframes[i].AgentId == keyframes[i + 1].AgentId) {
                auto iter = m_Routes.find(keyframes[i].AgentId);
                if (iter == m_Routes.end()) {
                    std::vector<std::vector<Core::CartesianCoordinate>> vec;
                    vec.push_back(m_Routemaker->MakeRoute(keyframes[i],
                                                          keyframes[i + 1]));
                    m_Routes.insert(std::make_pair(keyframes[i].AgentId, vec));
                } else {
                    iter->second.push_back(m_Routemaker->MakeRoute(
                        keyframes[i], keyframes[i + 1]));
                }
            }
        }
 
        return m_Routes;
    }
 
    void
    Scenario::AddAgent(Core::Agent newAgent)
    {
        m_Agents.push_back(newAgent);
    }
 
    void
    Scenario::save(std::string filename)
    {
        Json::serialize(filename, this);
    }
 
    void
    Scenario::load(std::string filename)
    {
        Json::deserialize(filename, this);
        SetOrigin(m_Origin, m_Size);
    }
 
} // namespace CompileScenario