io.h

Go to the documentation of this file.

#pragma once
 
#include <fstream>
#include <sstream>
#include <stdexcept>
#include <string>
 
#include <ArmarXCore/core/PackagePath.h>
 
namespace armarx::navigation::algorithms::orientation_aware::io
{
    struct AStarWithOrientationParams
    {
        float obstacleDistanceWeightFactor = 0.2;
        float obstacleDistanceContinuousWeightFactor = 0;
        float orientationWeightFactor = 0.0;
        float forwardWeightFactor = 80.0;
        float smoothnessWeightFactor = 200.0;
 
        float maxObstacleDistance = 1300; // [mm]
    };
 
    AStarWithOrientationParams
    loadAStarWithOrientationParams(
        const std::string& filePath = "algorithms/astarwithorientation.cfg")
    {
        // We expect the file to have a line for each config parameter, in the format "key=value"
        AStarWithOrientationParams params;
 
        // Create full file path with ArmarX PackagePath
        const auto fullFilePath = armarx::PackagePath("armarx_navigation", filePath).toSystemPath();
        std::ifstream file(fullFilePath);
        if (!file.is_open())
        {
            throw std::runtime_error("Could not open config file: " + fullFilePath.string());
        }
        std::string line;
        while (std::getline(file, line))
        {
            std::istringstream iss(line);
            std::string key;
            if (std::getline(iss, key, '='))
            {
                std::string value;
                if (std::getline(iss, value))
                {
                    try
                    {
                        if (key == "maxObstacleDistance")
                        {
                            params.maxObstacleDistance = std::stof(value);
                        }
                        else if (key == "obstacleDistanceWeightFactor")
                        {
                            params.obstacleDistanceWeightFactor = std::stof(value);
                        }
                        else if (key == "obstacleDistanceContinuousWeightFactor")
                        {
                            params.obstacleDistanceContinuousWeightFactor = std::stof(value);
                        }
                        else if (key == "orientationWeightFactor")
                        {
                            params.orientationWeightFactor = std::stof(value);
                        }
                        else if (key == "forwardWeightFactor")
                        {
                            params.forwardWeightFactor = std::stof(value);
                        }
                        else if (key == "smoothnessWeightFactor")
                        {
                            params.smoothnessWeightFactor = std::stof(value);
                        }
                    }
                    catch (const std::invalid_argument& e)
                    {
                        throw std::runtime_error("Invalid value for key " + key + ": " + value);
                    }
                }
            }
        }
 
        return params;
    }
 
} // namespace armarx::navigation::algorithms::orientation_aware::io