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::smoothing::io
{
    struct SmoothingParams
    {
        double w_boundary = 10.0;
        double w_pose_smooth = 10.0;
        double w_pose_smooth_ori = 100.0;
        double w_vel_smooth = 0.0;
        double w_pose_jerk = 0.0;
        double w_robot_smooth = 0.0;
        double w_obs = 2000.0;
        double w_track = 0.05;
        double w_vel_limit = 0.0;
        double w_spacing = 1.0;
        double clearance = 50.0;
        double obsCollisionResidual = 1000.0;
        double obsMaxDistance = 500.0;
        double vmax = 1.0;
        bool use_tracking = true;
        bool use_robot_smooth = true;
        bool use_jerk = false;
        bool use_obs = true;
        int max_iterations = 15;
        int num_threads = 4;
    };
 
    SmoothingParams
    loadSmoothingParams(const std::string& filePath = "algorithms/orientation-aware-smoothing.cfg")
    {
        // We expect the file to have a line for each config parameter, in the format "key=value"
        SmoothingParams 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
                    {
                        // add all keys
                        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& e)
                    {
                        throw std::runtime_error("Invalid value for key " + key + ": " + value);
                    }
                }
            }
        }
 
        return params;
    }
 
} // namespace armarx::navigation::algorithms::orientation_aware::smoothing::io