GCC Code Coverage Report

Directory: ./
File: include/sot/core/trajectory.hh
Date: 2024-10-13 12:22:59
Exec Total Coverage
Lines: 0 31 0.0%
Branches: 0 37 0.0%
Line Branch Exec Source
1 /*
2 * Copyright 2013,
3 * Olivier Stasse,
4 *
5 * CNRS
6 *
7 */
8
9 #ifndef SOT_TRAJECTORY_H__
10 #define SOT_TRAJECTORY_H__
11
12 // Matrix
13 #include <dynamic-graph/linear-algebra.h>
14 #include <dynamic-graph/signal-caster.h>
15
16 #include <boost/array.hpp>
17 #include <boost/assign/list_of.hpp>
18 #include <boost/regex.hpp>
19 #include <sot/core/api.hh>
20
21 namespace dg = dynamicgraph;
22 namespace ba = boost::assign;
23
24 namespace dynamicgraph {
25 namespace sot {
26
27 class Trajectory;
28
29 class RulesJointTrajectory {
30 protected:
31 Trajectory &TrajectoryToFill_;
32
33 public:
34 unsigned int dbg_level;
35
36 /// \brief Strings specifying the grammar of the structure.
37 std::string float_str_re, seq_str_re, timestamp_str_re, frame_id_str_re,
38 header_str_re, joint_name_str_re, list_of_jn_str_re, point_value_str_re,
39 list_of_pv_str_re, bg_pt_str_re, end_pt_str_re, comma_pt_str_re,
40 bg_liste_of_pts_str_re;
41 /// \brief Boost regular expressions implementing the grammar.
42 boost::regex header_re, list_of_jn_re, list_of_pv_re, bg_pt_re, end_pt_re,
43 comma_pt_re, bg_liste_of_pts_re;
44 std::vector<std::string> joint_names;
45
46 /// \brief Constructor TrajectoryToFill is the structure where to store the
47 /// parsed information.
48 RulesJointTrajectory(Trajectory &TrajectoryToFill);
49
50 /// \brief parse_string will fill TrajectoryToFill with string atext.
51 void parse_string(std::string &atext);
52
53 protected:
54 /// \brief General parsing method of text with regexp e. The results are given
55 /// in what. The remaining text is left in sub_text.
56 bool search_exp_sub_string(
57 std::string &text,
58 boost::match_results<std::string::const_iterator> &what, boost::regex &e,
59 std::string &sub_text);
60 /// \brief Find and store the header.
61 /// This method is looking for:
62 /// unsigned int seq.
63 /// unsigned int sec, unsigned int nsec.
64 /// string format_id
65 void parse_header(std::string &text, std::string &sub_text1);
66
67 /// \brief Understand joint_names.
68 /// Extract a list of strings.
69 void parse_joint_names(std::string &text, std::string &sub_text1,
70 std::vector<std::string> &joint_names);
71
72 /// \brief Extract a sequence of doubles.
73 /// To be used for position, velocities, accelerations and effort.
74 bool parse_seq(std::string &text, std::string &sub_text1,
75 std::vector<double> &seq);
76
77 /// \brief Extract a point description.
78 bool parse_point(std::string &trajectory, std::string &sub_text1);
79
80 /// \brief Extract a sequence of points.
81 bool parse_points(std::string &trajectory, std::string &sub_text1);
82 };
83
84 class SOT_CORE_EXPORT timestamp {
85 public:
86 unsigned long int secs_;
87 unsigned long int nsecs_;
88 timestamp() : secs_(0), nsecs_(0) {}
89 timestamp(const timestamp &ats) {
90 secs_ = ats.secs_;
91 nsecs_ = ats.nsecs_;
92 }
93 ✗ timestamp(unsigned long int lsecs, unsigned long int lnsecs) {
94 ✗ secs_ = lsecs;
95 ✗ nsecs_ = lnsecs;
96 }
97 ✗ bool operator==(const timestamp &other) const {
98 ✗ if ((secs_ != other.secs_) || (nsecs_ != other.nsecs_)) return false;
99 ✗ return true;
100 }
101 friend std::ostream &operator<<(std::ostream &stream, const timestamp &ats) {
102 stream << ats.secs_ + 0.000001 * (long double)ats.nsecs_;
103 return stream;
104 }
105 };
106
107 class SOT_CORE_EXPORT Header {
108 public:
109 unsigned int seq_;
110 timestamp stamp_;
111 std::string frame_id_;
112 ✗ Header() : seq_(0), stamp_(0, 0), frame_id_("initial_trajectory") {}
113 };
114
115 class SOT_CORE_EXPORT JointTrajectoryPoint {
116 public:
117 std::vector<double> positions_;
118 std::vector<double> velocities_;
119 std::vector<double> accelerations_;
120 std::vector<double> efforts_;
121
122 typedef std::vector<double> vec_ref;
123
124 ✗ void display(std::ostream &os) const {
125 ✗ boost::array<std::string, 4> names = boost::assign::list_of("Positions")(
126 ✗ "Velocities")("Accelerations")("Effort");
127
128 ✗ const std::vector<double> *points = 0;
129
130 ✗ for (std::size_t arrayId = 0; arrayId < names.size(); ++arrayId) {
131 ✗ switch (arrayId) {
132 ✗ case (0):
133 ✗ points = &positions_;
134 ✗ break;
135 ✗ case (1):
136 ✗ points = &velocities_;
137 ✗ break;
138 ✗ case (2):
139 ✗ points = &accelerations_;
140 ✗ break;
141 ✗ case (3):
142 ✗ points = &efforts_;
143 ✗ break;
144 ✗ default:
145 ✗ assert(0);
146 }
147
148 ✗ std::vector<double>::const_iterator it_db;
149 ✗ os << names[arrayId] << std::endl << "---------" << std::endl;
150 ✗ for (it_db = points->begin(); it_db != points->end(); it_db++) {
151 ✗ os << *it_db << std::endl;
152 }
153 }
154 }
155
156 void transfer(const std::vector<double> &src, unsigned int vecId) {
157 switch (vecId) {
158 case (0):
159 positions_ = src;
160 break;
161 case (1):
162 velocities_ = src;
163 break;
164 case (2):
165 accelerations_ = src;
166 break;
167 case (3):
168 efforts_ = src;
169 break;
170 default:
171 assert(0);
172 }
173 }
174 };
175
176 class SOT_CORE_EXPORT Trajectory {
177 public:
178 Trajectory();
179 Trajectory(const Trajectory &copy);
180 virtual ~Trajectory();
181
182 std::vector<std::string> joint_names_;
183
184 Header header_;
185 double time_from_start_;
186
187 std::vector<JointTrajectoryPoint> points_;
188
189 int deserialize(std::istringstream &is);
190 void display(std::ostream &) const;
191 };
192 } // namespace sot
193
194 template <>
195 struct signal_io<sot::Trajectory> : signal_io_unimplemented<sot::Trajectory> {};
196
197 } // namespace dynamicgraph
198
199 #endif /* #ifndef SOT_TRAJECTORY_H__ */
200