io.cpp

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */
 
#include "io.h"
 
#include <fstream>
#include <sstream>
#include <stdexcept>
 
#include <ArmarXCore/core/PackagePath.h>
 
namespace armarx::navigation::algorithms::spfa::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))
        {
            auto commentPos = line.find('#');
            if (commentPos != std::string::npos)
                line = line.substr(0, commentPos);
 
            bool allSpace = true;
            for (char c : line)
            {
                if (!std::isspace(static_cast<unsigned char>(c)))
                {
                    allSpace = false;
                    break;
                }
            }
            if (allSpace)
                continue;
 
            std::istringstream iss(line);
            std::string key;
            if (!std::getline(iss, key, '='))
                continue;
 
            {
                size_t end = key.find_last_not_of(" \t\r\n");
                if (end != std::string::npos)
                    key = key.substr(0, end + 1);
                else
                    key.clear();
            }
            if (key.empty())
                continue;
 
            std::string value;
            if (!std::getline(iss, value))
                continue;
 
            {
                size_t start = value.find_first_not_of(" \t\r\n");
                if (start != std::string::npos)
                {
                    size_t end = value.find_last_not_of(" \t\r\n");
                    value = value.substr(start, end - start + 1);
                }
                else
                {
                    value.clear();
                }
            }
 
            try
            {
                if (key == "clearance")
                    params.clearance = std::stof(value);
                else if (key == "minClearance")
                    params.min_clearance = std::stof(value);
                else if (key == "obsMaxDistance")
                    params.obs_max_distance = std::stof(value);
                else if (key == "allowDegradedInput")
                    params.allow_degraded_input = (value == "true" or value == "1");
                else if (key == "useTrackingClearanceAdaptive")
                    params.use_tracking_clearance_adaptive = (value == "true" or value == "1");
                else if (key == "trackingFadeStartClearance")
                    params.tracking_fade_start_clearance = std::stof(value);
                else if (key == "trackingFadeEndClearance")
                    params.tracking_fade_end_clearance = std::stof(value);
                else if (key == "useObstacleFreeze")
                    params.use_obstacle_freeze = (value == "true" or value == "1");
                else if (key == "obstacleFreezeThreshold")
                    params.obstacle_freeze_threshold = std::stof(value);
                else if (key == "segmentObstacleSamples")
                    params.segment_obstacle_samples = std::stoi(value);
                else if (key == "segmentSampleSpacing")
                    params.segment_sample_spacing = std::stof(value);
                else if (key == "maxSegmentObstacleSamples")
                    params.max_segment_obstacle_samples = std::stoi(value);
                else if (key == "pass1WeightObstacle")
                    params.pass1_w_obs = std::stof(value);
                else if (key == "pass1WeightPoseSmooth")
                    params.pass1_w_pose_smooth = std::stof(value);
                else if (key == "pass1WeightSpacing")
                    params.pass1_w_spacing = std::stof(value);
                else if (key == "pass1WeightTracking")
                    params.pass1_w_tracking = std::stof(value);
                else if (key == "pass1WeightSafeTracking")
                    params.pass1_w_safe_tracking = std::stof(value);
                else if (key == "pass2WeightObstacle")
                    params.pass2_w_obs = std::stof(value);
                else if (key == "pass2WeightPoseSmooth")
                    params.pass2_w_pose_smooth = std::stof(value);
                else if (key == "pass2WeightSpacing")
                    params.pass2_w_spacing = std::stof(value);
                else if (key == "pass2WeightTracking")
                    params.pass2_w_tracking = std::stof(value);
                else if (key == "pass2WeightSafeTracking")
                    params.pass2_w_safe_tracking = std::stof(value);
                else if (key == "pass3WeightObstacle")
                    params.pass3_w_obs = std::stof(value);
                else if (key == "pass3WeightPoseSmooth")
                    params.pass3_w_pose_smooth = std::stof(value);
                else if (key == "pass3WeightSpacing")
                    params.pass3_w_spacing = std::stof(value);
                else if (key == "pass3WeightTracking")
                    params.pass3_w_tracking = std::stof(value);
                else if (key == "pass3WeightSafeTracking")
                    params.pass3_w_safe_tracking = std::stof(value);
                else if (key == "repairWeightObstacle")
                    params.repair_w_obs = std::stof(value);
                else if (key == "repairWeightPoseSmooth")
                    params.repair_w_pose_smooth = std::stof(value);
                else if (key == "repairWeightSpacing")
                    params.repair_w_spacing = std::stof(value);
                else if (key == "repairWeightTracking")
                    params.repair_w_tracking = std::stof(value);
                else if (key == "repairWeightSafeTracking")
                    params.repair_w_safe_tracking = std::stof(value);
                else if (key == "repairMaxIterations")
                    params.repair_max_iterations = std::stoi(value);
                else if (key == "trackingDeadzone")
                    params.tracking_deadzone = std::stof(value);
                else if (key == "trackingMaxDeviation")
                    params.tracking_max_deviation = std::stof(value);
                else if (key == "weightTrackingInBounds")
                    params.w_tracking_in_bounds = std::stof(value);
                else if (key == "weightTrackingHard")
                    params.w_tracking_hard = std::stof(value);
                else if (key == "maxIterations")
                    params.max_iterations = std::stoi(value);
                else if (key == "numThreads")
                    params.num_threads = std::stoi(value);
                else if (key == "initialTrustRegionRadius")
                    params.initial_trust_region_radius = std::stof(value);
                else if (key == "maxTrustRegionRadius")
                    params.max_trust_region_radius = std::stof(value);
                else if (key == "minTrustRegionRadius")
                    params.min_trust_region_radius = std::stof(value);
            }
            catch (const std::invalid_argument&)
            {
                throw std::runtime_error("Invalid value for key " + key + ": " + value);
            }
        }
 
        return params;
    }
 
} // namespace armarx::navigation::algorithms::spfa::smoothing::io