trajectory.cpp
Go to the documentation of this file.
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 
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 
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]);
107  is.str(what[2]);
109  is.str(what[3]);
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 
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;
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 
285 
287  header_ = copy.header_;
288  time_from_start_ = copy.time_from_start_;
289  points_ = copy.points_;
290 }
291 
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
int deserialize(std::istringstream &is)
Definition: trajectory.cpp:294
int i
index
pos
std::vector< std::string > joint_names
Definition: trajectory.hh:44
bool parse_points(std::string &trajectory, std::string &sub_text1)
Extract a sequence of points.
Definition: trajectory.cpp:233
std::vector< std::string > joint_names_
Definition: trajectory.hh:182
void parse_string(std::string &atext)
parse_string will fill TrajectoryToFill with string atext.
Definition: trajectory.cpp:268
boost::regex header_re
Boost regular expressions implementing the grammar.
Definition: trajectory.hh:42
bool search_exp_sub_string(std::string &text, boost::match_results< std::string::const_iterator > &what, boost::regex &e, std::string &sub_text)
General parsing method of text with regexp e. The results are given in what. The remaining text is le...
Definition: trajectory.cpp:67
void display(std::ostream &) const
Definition: trajectory.cpp:302
string joint_name
std::vector< JointTrajectoryPoint > points_
Definition: trajectory.hh:187
bool parse_point(std::string &trajectory, std::string &sub_text1)
Extract a point description.
Definition: trajectory.cpp:196
unsigned long int secs_
Definition: trajectory.hh:86
void parse_header(std::string &text, std::string &sub_text1)
Find and store the header. This method is looking for: unsigned int seq. unsigned int sec...
Definition: trajectory.cpp:99
RulesJointTrajectory(Trajectory &TrajectoryToFill)
Constructor TrajectoryToFill is the structure where to store the parsed information.
Definition: trajectory.cpp:32
void parse_joint_names(std::string &text, std::string &sub_text1, std::vector< std::string > &joint_names)
Understand joint_names. Extract a list of strings.
Definition: trajectory.cpp:126
bool parse_seq(std::string &text, std::string &sub_text1, std::vector< double > &seq)
Extract a sequence of doubles. To be used for position, velocities, accelerations and effort...
Definition: trajectory.cpp:154
std::vector< double > accelerations_
Definition: trajectory.hh:119
unsigned long int nsecs_
Definition: trajectory.hh:87


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Wed Jun 21 2023 02:51:26