GCC Code Coverage Report

Directory: ./
File: src/tools/trajectory.cpp
Date: 2024-11-13 12:35:17
Exec Total Coverage
Lines: 0 193 0.0%
Branches: 0 404 0.0%
Line Branch Exec Source
1 /*
2 * Copyright 2013,
3 * Olivier Stasse,
4 *
5 * CNRS/AIST
6 *
7 */
8 #define VP_DEBUG
9 #define VP_DEBUG_MODE 45
10 #include <iostream>
11 #include <sot/core/debug.hh>
12 // #ifdef VP_DEBUG
13 // class sotJTE__INIT
14 // {
15 // public:sotJTE__INIT( void ) { dynamicgraph::sot::DebugTrace::openFile(); }
16 // };
17 // sotJTE__INIT sotJTE_initiator;
18 // #endif //#ifdef VP_DEBUG
19
20 #include <sot/core/trajectory.hh>
21
22 /************************/
23 /* JointTrajectoryPoint */
24 /************************/
25
26 /**************/
27 /* Trajectory */
28 /**************/
29 namespace dynamicgraph {
30 namespace sot {
31
32 ✗ RulesJointTrajectory::RulesJointTrajectory(Trajectory &aTrajectoryToFill)
33 ✗ : TrajectoryToFill_(aTrajectoryToFill),
34 ✗ dbg_level(0),
35 ✗ float_str_re("[-0-9]+\\.[0-9]*"),
36
37 // Header Regular Expression
38 ✗ seq_str_re("([0-9]+)"),
39 ✗ timestamp_str_re("(" + float_str_re + "),(" + float_str_re + ")"),
40 ✗ frame_id_str_re("[a-zA-z_0-9]*"),
41 ✗ header_str_re("\\(" + seq_str_re + "\\,\\(" + timestamp_str_re + "\\),(" +
42 ✗ frame_id_str_re + ")\\)\\,\\("),
43
44 // List of Joint Names
45 ✗ joint_name_str_re("([a-zA-Z0-9_]+)"),
46 ✗ list_of_jn_str_re(joint_name_str_re + "(\\,|\\))"),
47
48 // Point
49 ✗ point_value_str_re("(" + float_str_re + "+)|(?:)"),
50 ✗ list_of_pv_str_re(point_value_str_re + "(\\,|\\))"),
51 ✗ bg_pt_str_re("\\("),
52 ✗ end_pt_str_re("\\)"),
53 ✗ comma_pt_str_re("\\,\\("),
54
55 // Liste of points
56 ✗ bg_liste_of_pts_str_re("\\,\\("),
57
58 // Reg Exps
59 ✗ header_re(header_str_re),
60 ✗ list_of_jn_re(list_of_jn_str_re),
61 ✗ list_of_pv_re(list_of_pv_str_re),
62 ✗ bg_pt_re(bg_pt_str_re),
63 ✗ end_pt_re(end_pt_str_re),
64 ✗ comma_pt_re(comma_pt_str_re),
65 ✗ bg_liste_of_pts_re(bg_liste_of_pts_str_re) {}
66
67 ✗ bool RulesJointTrajectory::search_exp_sub_string(
68 std::string &text, boost::match_results<std::string::const_iterator> &what,
69 boost::regex &e, std::string &sub_text) {
70 ✗ unsigned nb_failures = 0;
71
72 ✗ boost::match_flag_type flags = boost::match_extra;
73 ✗ if (boost::regex_search(text, what, e, flags)) {
74 ✗ if (dbg_level > 5) {
75 ✗ std::cout << "** Match found **\n Sub-Expressions:" << what.size()
76 ✗ << std::endl;
77 ✗ for (unsigned int i = 0; i < what.size(); ++i)
78 ✗ std::cout << " $" << i << " = \"" << what[i] << "\" "
79 ✗ << what.position(i) << " " << what.length(i) << "\n";
80 }
81 ✗ if (what.size() >= 1) {
82 ✗ unsigned int all_text = 0;
83 boost::match_results<std::string::const_iterator>::difference_type pos =
84 ✗ what.position(all_text);
85 boost::match_results<std::string::const_iterator>::difference_type len =
86 ✗ what.length(all_text);
87 ✗ sub_text = text.substr(pos + len);
88 ✗ return true;
89 }
90 } else {
91 ✗ if (dbg_level > 5) std::cout << "** No Match found **\n";
92 ✗ sub_text = text;
93 ✗ nb_failures++;
94 ✗ if (nb_failures > 100) return false;
95 }
96 ✗ return false;
97 }
98
99 ✗ void RulesJointTrajectory::parse_header(std::string &trajectory,
100 std::string &sub_text1) {
101 ✗ std::istringstream is;
102 ✗ boost::match_results<std::string::const_iterator> what;
103
104 ✗ if (search_exp_sub_string(trajectory, what, header_re, sub_text1)) {
105 ✗ is.str(what[1]);
106 ✗ is >> TrajectoryToFill_.header_.seq_;
107 ✗ is.str(what[2]);
108 ✗ is >> TrajectoryToFill_.header_.stamp_.secs_;
109 ✗ is.str(what[3]);
110 ✗ is >> TrajectoryToFill_.header_.stamp_.nsecs_;
111 ✗ TrajectoryToFill_.header_.frame_id_ = what[4];
112
113 ✗ if (dbg_level > 5) {
114 ✗ std::cout << "seq: " << TrajectoryToFill_.header_.seq_ << std::endl;
115 ✗ std::cout << "ts:" << TrajectoryToFill_.header_.stamp_.secs_ << " "
116 ✗ << what[2] << " " << TrajectoryToFill_.header_.stamp_.nsecs_
117 ✗ << " " << is.str() << std::endl;
118 ✗ std::cout << "frame_id:" << TrajectoryToFill_.header_.frame_id_
119 ✗ << std::endl;
120
121 ✗ std::cout << "sub_text1:" << sub_text1 << std::endl;
122 }
123 }
124 }
125
126 ✗ void RulesJointTrajectory::parse_joint_names(
127 std::string &trajectory, std::string &sub_text1,
128 std::vector<std::string> &joint_names) {
129 ✗ std::istringstream is;
130 ✗ boost::match_results<std::string::const_iterator> what;
131 ✗ bool joint_names_loop = true;
132 ✗ do {
133 ✗ if (search_exp_sub_string(trajectory, what, list_of_jn_re, sub_text1)) {
134 ✗ std::string joint_name;
135 ✗ is.str(what[1]);
136 ✗ joint_name = what[1];
137 ✗ joint_names.push_back(joint_name);
138
139 ✗ std::string sep_char;
140 ✗ sep_char = what[2];
141
142 ✗ if (sep_char == ")") joint_names_loop = false;
143 ✗ if (dbg_level > 5) {
144 ✗ std::cout << "joint_name:" << joint_name << " " << sep_char
145 ✗ << std::endl;
146 ✗ std::cout << "sub_text1:" << sub_text1 << std::endl;
147 }
148 }
149 ✗ trajectory = sub_text1;
150
151 } while (joint_names_loop);
152 }
153
154 ✗ bool RulesJointTrajectory::parse_seq(std::string &trajectory,
155 std::string &sub_text1,
156 std::vector<double> &seq) {
157 ✗ boost::match_results<std::string::const_iterator> what;
158 ✗ bool joint_seq_loop = true;
159 ✗ std::istringstream is;
160 ✗ std::string sub_text2 = trajectory;
161 ✗ sub_text1 = trajectory;
162 ✗ do {
163 ✗ if (search_exp_sub_string(sub_text2, what, list_of_pv_re, sub_text1)) {
164 ✗ std::string sep_char;
165 ✗ if (dbg_level > 5) {
166 ✗ std::cout << "size:" << what.size() << std::endl;
167 }
168
169 ✗ if (what.size() == 3) {
170 ✗ std::string aString(what[1]);
171 ✗ if (aString.size() > 0) {
172 ✗ is.clear();
173 ✗ is.str(aString);
174 double aValue;
175 ✗ is >> aValue;
176 ✗ if (dbg_level > 5) {
177 ✗ std::cout << aString << " | " << aValue << std::endl;
178 }
179
180 ✗ seq.push_back(aValue);
181 }
182 ✗ sep_char = what[2];
183 ✗ } else if (what.size() == 1)
184 ✗ sep_char = what[0];
185
186 ✗ if (sep_char == ")") joint_seq_loop = false;
187
188 ✗ } else {
189 ✗ return true;
190 }
191 ✗ sub_text2 = sub_text1;
192 } while (joint_seq_loop);
193 ✗ return true;
194 }
195
196 ✗ bool RulesJointTrajectory::parse_point(std::string &trajectory,
197 std::string &sub_text1) {
198 ✗ std::vector<double> position, velocities, acceleration, effort;
199 ✗ std::string sub_text2;
200 ✗ boost::match_results<std::string::const_iterator> what;
201 ✗ JointTrajectoryPoint aJTP;
202
203 ✗ if (!search_exp_sub_string(trajectory, what, bg_pt_re, sub_text1))
204 ✗ return false;
205 ✗ sub_text2 = sub_text1;
206
207 ✗ if (!parse_seq(sub_text2, sub_text1, aJTP.positions_)) return false;
208 ✗ sub_text2 = sub_text1;
209
210 ✗ if (!search_exp_sub_string(sub_text2, what, comma_pt_re, sub_text1))
211 ✗ return false;
212 ✗ sub_text2 = sub_text1;
213
214 ✗ if (!parse_seq(sub_text2, sub_text1, aJTP.velocities_)) return false;
215 ✗ sub_text2 = sub_text1;
216
217 ✗ if (!search_exp_sub_string(sub_text2, what, comma_pt_re, sub_text1))
218 ✗ return false;
219 ✗ sub_text2 = sub_text1;
220
221 ✗ if (!parse_seq(sub_text2, sub_text1, aJTP.accelerations_)) return false;
222 ✗ sub_text2 = sub_text1;
223
224 ✗ if (!search_exp_sub_string(sub_text2, what, comma_pt_re, sub_text1))
225 ✗ return false;
226 ✗ sub_text2 = sub_text1;
227 ✗ if (!parse_seq(sub_text2, sub_text1, aJTP.efforts_)) return false;
228
229 ✗ TrajectoryToFill_.points_.push_back(aJTP);
230 ✗ return true;
231 }
232
233 ✗ bool RulesJointTrajectory::parse_points(std::string &trajectory,
234 std::string &sub_text1) {
235 ✗ boost::match_results<std::string::const_iterator> what;
236 ✗ bool joint_points_loop = true;
237 ✗ std::istringstream is;
238
239 ✗ if (!search_exp_sub_string(trajectory, what, bg_liste_of_pts_re, sub_text1))
240 ✗ return false;
241 ✗ std::string sub_text2 = sub_text1;
242
243 ✗ do {
244 ✗ if (!search_exp_sub_string(sub_text2, what, bg_pt_re, sub_text1))
245 ✗ return false;
246 ✗ sub_text2 = sub_text1;
247
248 ✗ if (!parse_point(sub_text2, sub_text1)) return false;
249 ✗ sub_text2 = sub_text1;
250
251 ✗ if (!search_exp_sub_string(sub_text2, what, end_pt_re, sub_text1))
252 ✗ return false;
253 ✗ sub_text2 = sub_text1;
254
255 ✗ if (!search_exp_sub_string(sub_text2, what, list_of_pv_re, sub_text1))
256 ✗ return false;
257 ✗ sub_text2 = sub_text1;
258 ✗ std::string sep_char;
259 ✗ sep_char = what[1];
260
261 ✗ if (sep_char == ")") joint_points_loop = false;
262
263 ✗ } while (joint_points_loop);
264
265 ✗ return true;
266 }
267
268 ✗ void RulesJointTrajectory::parse_string(std::string &atext) {
269 ✗ std::string sub_text1, sub_text2;
270 ✗ parse_header(atext, sub_text2);
271 ✗ sub_text1 = sub_text2;
272
273 ✗ parse_joint_names(sub_text1, sub_text2, TrajectoryToFill_.joint_names_);
274
275 ✗ if (dbg_level > 5) {
276 ✗ for (std::vector<std::string>::size_type i = 0; i < joint_names.size(); i++)
277 ✗ std::cout << joint_names[i] << std::endl;
278 }
279
280 ✗ sub_text1 = sub_text2;
281 ✗ parse_points(sub_text1, sub_text2);
282 }
283
284 ✗ Trajectory::Trajectory(void) {}
285
286 ✗ Trajectory::Trajectory(const Trajectory &copy) {
287 ✗ header_ = copy.header_;
288 ✗ time_from_start_ = copy.time_from_start_;
289 ✗ points_ = copy.points_;
290 }
291
292 ✗ Trajectory::~Trajectory(void) {}
293
294 ✗ int Trajectory::deserialize(std::istringstream &is) {
295 ✗ std::string aStr = is.str();
296 ✗ RulesJointTrajectory aRJT(*this);
297 ✗ aRJT.parse_string(aStr);
298
299 ✗ return 0;
300 }
301
302 ✗ void Trajectory::display(std::ostream &os) const {
303 ✗ unsigned int index = 0;
304 ✗ os << "-- Trajectory --" << std::endl;
305 ✗ for (std::vector<std::string>::const_iterator it_joint_name =
306 ✗ joint_names_.begin();
307 ✗ it_joint_name != joint_names_.end(); it_joint_name++, index++)
308 ✗ os << "Joint(" << index << ")=" << *(it_joint_name) << std::endl;
309
310 ✗ os << " Number of points: " << points_.size() << std::endl;
311 ✗ for (std::vector<JointTrajectoryPoint>::const_iterator it_point =
312 ✗ points_.begin();
313 ✗ it_point != points_.end(); it_point++) {
314 ✗ it_point->display(os);
315 }
316 }
317
318 } // namespace sot
319 } // namespace dynamicgraph
320