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))
        {
            // Strip inline comments
            auto commentPos = line.find('#');
            if (commentPos != std::string::npos)
                line = line.substr(0, commentPos);
 
            // Skip blank lines (all whitespace)
            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;
 
            // Trim trailing whitespace from key
            {
                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;
 
            // Trim leading/trailing whitespace from value
            {
                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
            {
                // Pre-processing
                if (key == "clearance")
                    params.clearance = std::stof(value);
                else if (key == "hillClimbStepSize")
                    params.hill_climb_step_size = std::stof(value);
                else if (key == "hillClimbMaxIterations")
                    params.hill_climb_max_iterations = std::stoi(value);
                // Obstacle avoidance
                else if (key == "weightObstacle")
                    params.w_obs = std::stof(value);
                else if (key == "obsMaxDistance")
                    params.obs_max_distance = std::stof(value);
                else if (key == "obsBarrierEpsMm")
                    params.obs_barrier_eps_mm = std::stof(value);
                else if (key == "useObstacle")
                    params.use_obs = (value == "true");
                else if (key == "useFuzzyOrientation")
                    params.use_fuzzy_orientation = (value == "true");
                else if (key == "fuzzyOrientationWindowBins")
                    params.fuzzy_orientation_window_bins = std::stoi(value);
                else if (key == "fuzzyOrientationAlpha")
                    params.fuzzy_orientation_alpha = std::stof(value);
                else if (key == "weightThetaTrack")
                    params.w_theta_track = std::stof(value);
                else if (key == "useObstacleFreeze")
                    params.use_obstacle_freeze = (value == "true");
                else if (key == "obstacleFreezeThreshold")
                    params.obstacle_freeze_threshold = std::stof(value);
                // Pose smoothness
                else if (key == "weightBoundary")
                    params.w_boundary = std::stof(value);
                else if (key == "weightPoseSmooth")
                    params.w_pose_smooth = std::stof(value);
                else if (key == "weightOrientationSmooth")
                    params.w_orientation_smooth = std::stof(value);
                else if (key == "weightPoseJerk")
                    params.w_pose_jerk = std::stof(value);
                else if (key == "useJerk")
                    params.use_jerk = (value == "true");
                else if (key == "weightRobotSmooth")
                    params.w_robot_smooth = std::stof(value);
                else if (key == "useRobotSmooth")
                    params.use_robot_smooth = (value == "true");
                // Spacing
                else if (key == "weightSpacing")
                    params.w_spacing = std::stof(value);
                else if (key == "useSoftSpacing")
                    params.use_soft_spacing = (value == "true");
                else if (key == "spacingRelativeDeviationThreshold")
                    params.spacing_relative_deviation_threshold = std::stof(value);
                else if (key == "weightSpacingLinear")
                    params.w_spacing_linear = std::stof(value);
                else if (key == "weightSpacingQuadratic")
                    params.w_spacing_quadratic = std::stof(value);
                // Velocity profile
                else if (key == "startVelocity")
                    params.start_velocity = std::stof(value);
                else if (key == "weightAccelDecelLimit")
                    params.w_accel_decel_limit = std::stof(value);
                else if (key == "maxAccelPerSegment")
                    params.max_accel_per_segment = std::stof(value);
                else if (key == "maxDecelPerSegment")
                    params.max_decel_per_segment = std::stof(value);
                else if (key == "weightNominalVelocity")
                    params.w_nominal_velocity = std::stof(value);
                else if (key == "weightVelocityProximity")
                    params.w_velocity_proximity = std::stof(value);
                else if (key == "slowDownDistance")
                    params.slow_down_distance = std::stof(value);
                else if (key == "minVelocityNearObstacle")
                    params.min_velocity_near_obstacle = std::stof(value);
                // Tracking
                else if (key == "weightTracking")
                    params.w_track = std::stof(value);
                else if (key == "useTracking")
                    params.use_tracking = (value == "true");
                else if (key == "useTrackingClearanceAdaptive")
                    params.use_tracking_clearance_adaptive = (value == "true");
                else if (key == "trackingFadeStartClearance")
                    params.tracking_fade_start_clearance = std::stof(value);
                else if (key == "trackingFadeEndClearance")
                    params.tracking_fade_end_clearance = std::stof(value);
                // Robot-pose proximity
                else if (key == "useRobotPoseProximity")
                    params.use_robot_pose_proximity = (value == "true");
                else if (key == "robotPoseProximityThresholdMm")
                    params.robot_pose_proximity_threshold_mm = std::stof(value);
                else if (key == "weightRobotPoseProximity")
                    params.w_robot_pose_proximity = std::stof(value);
                else if (key == "robotPoseProximityEpsMm")
                    params.robot_pose_proximity_eps_mm = std::stof(value);
                // Ceres solver
                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);
                // Planner behavior
                else if (key == "alwaysReturnSmoothed")
                    params.always_return_smoothed = (value == "true");
            }
            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