io.cpp
Go to the documentation of this file.
1/**
2 * This file is part of ArmarX.
3 *
4 * ArmarX is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License version 2 as
6 * published by the Free Software Foundation.
7 *
8 * ArmarX is distributed in the hope that it will be useful, but
9 * WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * GNU General Public License for more details.
12 *
13 * You should have received a copy of the GNU General Public License
14 * along with this program. If not, see <http://www.gnu.org/licenses/>.
15 */
16
17#include "io.h"
18
19#include <fstream>
20#include <sstream>
21#include <stdexcept>
22
23#include <ArmarXCore/core/PackagePath.h>
24
25namespace armarx::navigation::algorithms::spfa::smoothing::io
26{
27
28 SmoothingParams
29 loadSmoothingParams(const std::string& filePath)
30 {
31 SmoothingParams params;
32
33 const auto fullFilePath = armarx::PackagePath("armarx_navigation", filePath).toSystemPath();
34 std::ifstream file(fullFilePath);
35 if (!file.is_open())
36 {
37 throw std::runtime_error("Could not open config file: " + fullFilePath.string());
38 }
39
40 std::string line;
41 while (std::getline(file, line))
42 {
43 auto commentPos = line.find('#');
44 if (commentPos != std::string::npos)
45 line = line.substr(0, commentPos);
46
47 bool allSpace = true;
48 for (char c : line)
49 {
50 if (!std::isspace(static_cast<unsigned char>(c)))
51 {
52 allSpace = false;
53 break;
54 }
55 }
56 if (allSpace)
57 continue;
58
59 std::istringstream iss(line);
60 std::string key;
61 if (!std::getline(iss, key, '='))
62 continue;
63
64 {
65 size_t end = key.find_last_not_of(" \t\r\n");
66 if (end != std::string::npos)
67 key = key.substr(0, end + 1);
68 else
69 key.clear();
70 }
71 if (key.empty())
72 continue;
73
74 std::string value;
75 if (!std::getline(iss, value))
76 continue;
77
78 {
79 size_t start = value.find_first_not_of(" \t\r\n");
80 if (start != std::string::npos)
81 {
82 size_t end = value.find_last_not_of(" \t\r\n");
83 value = value.substr(start, end - start + 1);
84 }
85 else
86 {
87 value.clear();
88 }
89 }
90
91 try
92 {
93 if (key == "clearance")
94 params.clearance = std::stof(value);
95 else if (key == "hillClimbStepSize")
96 params.hill_climb_step_size = std::stof(value);
97 else if (key == "hillClimbMaxIterations")
98 params.hill_climb_max_iterations = std::stoi(value);
99 else if (key == "obsMaxDistance")
100 params.obs_max_distance = std::stof(value);
101 else if (key == "segmentObstacleSamples")
102 params.segment_obstacle_samples = std::stoi(value);
103 else if (key == "pass1WeightObstacle")
104 params.pass1_w_obs = std::stof(value);
105 else if (key == "pass1WeightPoseSmooth")
106 params.pass1_w_pose_smooth = std::stof(value);
107 else if (key == "pass1WeightSpacing")
108 params.pass1_w_spacing = std::stof(value);
109 else if (key == "pass1WeightTracking")
110 params.pass1_w_tracking = std::stof(value);
111 else if (key == "pass1WeightSafeTracking")
112 params.pass1_w_safe_tracking = std::stof(value);
113 else if (key == "pass2WeightObstacle")
114 params.pass2_w_obs = std::stof(value);
115 else if (key == "pass2WeightPoseSmooth")
116 params.pass2_w_pose_smooth = std::stof(value);
117 else if (key == "pass2WeightSpacing")
118 params.pass2_w_spacing = std::stof(value);
119 else if (key == "pass2WeightTracking")
120 params.pass2_w_tracking = std::stof(value);
121 else if (key == "pass2WeightSafeTracking")
122 params.pass2_w_safe_tracking = std::stof(value);
123 else if (key == "pass3WeightObstacle")
124 params.pass3_w_obs = std::stof(value);
125 else if (key == "pass3WeightPoseSmooth")
126 params.pass3_w_pose_smooth = std::stof(value);
127 else if (key == "pass3WeightSpacing")
128 params.pass3_w_spacing = std::stof(value);
129 else if (key == "pass3WeightTracking")
130 params.pass3_w_tracking = std::stof(value);
131 else if (key == "pass3WeightSafeTracking")
132 params.pass3_w_safe_tracking = std::stof(value);
133 else if (key == "repairWeightObstacle")
134 params.repair_w_obs = std::stof(value);
135 else if (key == "repairWeightPoseSmooth")
136 params.repair_w_pose_smooth = std::stof(value);
137 else if (key == "repairWeightSpacing")
138 params.repair_w_spacing = std::stof(value);
139 else if (key == "repairWeightTracking")
140 params.repair_w_tracking = std::stof(value);
141 else if (key == "repairWeightSafeTracking")
142 params.repair_w_safe_tracking = std::stof(value);
143 else if (key == "repairMaxIterations")
144 params.repair_max_iterations = std::stoi(value);
145 else if (key == "trackingDeadzone")
146 params.tracking_deadzone = std::stof(value);
147 else if (key == "trackingMaxDeviation")
148 params.tracking_max_deviation = std::stof(value);
149 else if (key == "weightTrackingInBounds")
150 params.w_tracking_in_bounds = std::stof(value);
151 else if (key == "weightTrackingHard")
152 params.w_tracking_hard = std::stof(value);
153 else if (key == "maxIterations")
154 params.max_iterations = std::stoi(value);
155 else if (key == "numThreads")
156 params.num_threads = std::stoi(value);
157 else if (key == "initialTrustRegionRadius")
158 params.initial_trust_region_radius = std::stof(value);
159 else if (key == "maxTrustRegionRadius")
160 params.max_trust_region_radius = std::stof(value);
161 else if (key == "minTrustRegionRadius")
162 params.min_trust_region_radius = std::stof(value);
163 }
164 catch (const std::invalid_argument&)
165 {
166 throw std::runtime_error("Invalid value for key " + key + ": " + value);
167 }
168 }
169
170 return params;
171 }
172
173} // namespace armarx::navigation::algorithms::spfa::smoothing::io
c
constexpr T c
Definition UnscentedKalmanFilterTest.cpp:46
armarx::PackagePath::toSystemPath
static std::filesystem::path toSystemPath(const data::PackagePath &pp)
Definition PackagePath.cpp:47
armarx::navigation::algorithms::spfa::smoothing::io::loadSmoothingParams
SmoothingParams loadSmoothingParams(const std::string &filePath)
Definition io.cpp:29