GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/tools/trajectory.cpp Lines: 0 181 0.0 %
Date: 2023-03-13 12:09:37 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