io.cpp

Go to the documentation of this file.

#include "io.h"
 
#include <fstream>
#include <sstream>
#include <stdexcept>
 
#include <ArmarXCore/core/PackagePath.h>
 
namespace armarx::navigation::algorithms::orientation_aware::io
{
 
    AStarWithOrientationParams
    loadAStarWithOrientationParams(const std::string& filePath)
    {
        AStarWithOrientationParams params;
 
        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 == "obstacleDistanceContinuousExponent")
                        {
                            params.obstacleDistanceContinuousExponent = std::stof(value);
                        }
                        else if (key == "orientationWeightFactor")
                        {
                            params.orientationWeightFactor = std::stof(value);
                        }
                        else if (key == "forwardWeightFactor")
                        {
                            params.forwardWeightFactor = std::stof(value);
                        }
                        else if (key == "forwardWeightObstacleScale")
                        {
                            params.forwardWeightObstacleScale = std::stof(value);
                        }
                        else if (key == "smoothnessWeightFactor")
                        {
                            params.smoothnessWeightFactor = std::stof(value);
                        }
                        else if (key == "clearance")
                        {
                            params.clearance = std::stof(value);
                        }
                    }
                    catch (const std::invalid_argument&)
                    {
                        throw std::runtime_error("Invalid value for key " + key + ": " + value);
                    }
                }
            }
        }
 
        return params;
    }
 
} // namespace armarx::navigation::algorithms::orientation_aware::io