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::smoothing::io
{
 
    SmoothingParams
    loadSmoothingParams(const std::string& filePath)
    {
        SmoothingParams 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 == "weightBoundary")
                        {
                            params.w_boundary = std::stof(value);
                        }
                        else if (key == "weightPoseSmooth")
                        {
                            params.w_pose_smooth = std::stof(value);
                        }
                        else if (key == "weightPoseSmoothOrientation")
                        {
                            params.w_pose_smooth_ori = std::stof(value);
                        }
                        else if (key == "weightVelocitySmooth")
                        {
                            params.w_vel_smooth = std::stof(value);
                        }
                        else if (key == "weightPoseJerk")
                        {
                            params.w_pose_jerk = std::stof(value);
                        }
                        else if (key == "weightRobotSmooth")
                        {
                            params.w_robot_smooth = std::stof(value);
                        }
                        else if (key == "weightObstacle")
                        {
                            params.w_obs = std::stof(value);
                        }
                        else if (key == "weightTracking")
                        {
                            params.w_track = std::stof(value);
                        }
                        else if (key == "weightVelocityLimit")
                        {
                            params.w_vel_limit = std::stof(value);
                        }
                        else if (key == "weightSpacing")
                        {
                            params.w_spacing = std::stof(value);
                        }
                        else if (key == "clearance")
                        {
                            params.clearance = std::stof(value);
                        }
                        else if (key == "obsCollisionResidual")
                        {
                            params.obsCollisionResidual = std::stof(value);
                        }
                        else if (key == "obsMaxDistance")
                        {
                            params.obsMaxDistance = std::stof(value);
                        }
                        else if (key == "vmax")
                        {
                            params.vmax = std::stof(value);
                        }
                        else if (key == "useTracking")
                        {
                            params.use_tracking = (value == "true");
                        }
                        else if (key == "useRobotSmooth")
                        {
                            params.use_robot_smooth = (value == "true");
                        }
                        else if (key == "useJerk")
                        {
                            params.use_jerk = (value == "true");
                        }
                        else if (key == "useObstacle")
                        {
                            params.use_obs = (value == "true");
                        }
                        else if (key == "maxIterations")
                        {
                            params.max_iterations = std::stoi(value);
                        }
                        else if (key == "numThreads")
                        {
                            params.num_threads = std::stoi(value);
                        }
                    }
                    catch (const std::invalid_argument&)
                    {
                        throw std::runtime_error("Invalid value for key " + key + ": " + value);
                    }
                }
            }
        }
 
        return params;
    }
 
} // namespace armarx::navigation::algorithms::orientation_aware::smoothing::io