MyOwnCost.h

/**
 * Copyright 2024 Institute of Automotive Engineering (FZD), Technical University of Darmstadt
 * SPDX-License-Identifier: MIT
 */

#pragma once

#include "capbasedrouting/cost/MatchingCost.h"

/**
 * MyOwnCost class which extends the MatchingCost class.
 * Use this class to override the MatchingCost events.
 */
class MyOwnCost : public capbasedrouting::cost::MatchingCost{
public:
    MyOwnCost(double laneChangeCost, double minLaneChangeLength, lanelet::LaneletMapPtr map,
        capbasedrouting::loader::FileLoader &loader, std::list<capbasedrouting::matching::MatchingCriteriaProof> &proofs,
        capbasedrouting::logging::LogLevel logging = capbasedrouting::logging::LogLevel::NONE)
        : MatchingCost(laneChangeCost, minLaneChangeLength, std::move(map), loader, proofs, logging) {}

    // Override the transition event to print all BehaviorSpace transitions.
    void transitionEvent(capbasedrouting::bssd::BehaviorSpace *from, capbasedrouting::bssd::BehaviorSpace *to)
        const override{
            std::cout << "--------------------------------------------" << std::endl;
            std::cout << "Behavior space transition" << std::endl;
            std::cout << "from " << from->id << " to " << to->id << std::endl;
            std::cout << "" << std::endl;
    };

    // Override the matching criteria event to print calculated criteria.
    void matchingCriteriaEvent(capbasedrouting::matching::MatchingCriteria &mc) const override{
        std::cout << "Check for a match:" << std::endl;
    };

    // Override the match event to print the matching result.
    void matchEvent(capbasedrouting::bssd::BehaviorSpace *from, capbasedrouting::bssd::BehaviorSpace *to,
        capbasedrouting::matching::MatchingCriteria &mc, capbasedrouting::matching::MatchingCriteriaProof &mcp)
        const override{
            // First part here is the derived driving requirements set (matching criteria)
            std::cout << "v_lim_pre: " << mc.speedLimitPrecedingBehaviorSpaces << " km/h" << std::endl;
            std::cout << "w_pre: " << mc.precedingWidth << " m" << std::endl;
            std::cout << "alpha_orig: " << mc.angle << "°" << std::endl;
            std::cout << "v_lim_ori: " << mc.speedLimitOrigin << " km/h" << std::endl;
            std::cout << "w_ori: " << mc.originWidth << " m" << std::endl;
            std::cout << "l_off,lon: " << mc.longitudinalOffset << " m" << std::endl;
            std::cout << "l_off,lat: " << mc.lateralOffset << " m" << std::endl;
            std::cout << "" << std::endl;
            // Second part here is the proof set of driving capabilities that, if applicable, matches the set of driving requirements
            std::cout << "Matching passed with following proof:" << std::endl;
            std::cout << "v_lim_pre: " << mcp.speedLimitPrecedingBehaviorSpaces << " km/h" << std::endl;
            std::cout << "w_pre: " << mcp.precedingWidth << " m" << std::endl;
            std::cout << "alpha_orig: [" << mcp.angle.getLower() << "°, " << mcp.angle.getUpper() << "°]" << std::endl;
            std::cout << "v_lim_ori: " << mcp.speedLimitOrigin << " km/h" << std::endl;
            std::cout << "w_ori: " << mcp.originWidth << " m" << std::endl;
            std::cout << "l_off,lon: [" << mcp.longitudinalOffset.getLower() << " m, "
                      << mcp.longitudinalOffset.getUpper() << " m]" << std::endl;
            std::cout << "l_off,lat: [" << mcp.lateralOffset.getLower() << " m, " << mcp.lateralOffset.getUpper() << " m]" << std::endl;
            std::cout << "" << std::endl;
    };

    // Override the failed matching event to print the matching result.
    void failedMatchingEvent(capbasedrouting::bssd::BehaviorSpace *from, capbasedrouting::bssd::BehaviorSpace *to,
        capbasedrouting::matching::MatchingCriteria &mc) const override{
            // Prints the specific set of driving requirements for which no matching proof set of driving capabilities was found
            std::cout << "v_lim_pre: " << mc.speedLimitPrecedingBehaviorSpaces << " km/h" << std::endl;
            std::cout << "w_pre: " << mc.precedingWidth << " m" << std::endl;
            std::cout << "alpha_orig: " << mc.angle << "°" << std::endl;
            std::cout << "v_lim_ori: " << mc.speedLimitOrigin << " km/h" << std::endl;
            std::cout << "w_ori: " << mc.originWidth << " m" << std::endl;
            std::cout << "l_off,lon: " << mc.longitudinalOffset << " m" << std::endl;
            std::cout << "l_off,lat: " << mc.lateralOffset << " m" << std::endl;
            std::cout << "Matching failed!" << std::endl;
            std::cout << "" << std::endl;
    };

    // Override the transition success event to print the transition result.
    void transitionSuccessEvent(capbasedrouting::bssd::BehaviorSpace *from, capbasedrouting::bssd::BehaviorSpace *to)
        const override{
            std::cout << "Transition successful!" << std::endl;
            std::cout << "" << std::endl;
    };
};