Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/sot/core/trajectory.hh	Lines:	0	31	0.0 %
Date:	2023-03-13 12:09:37	Branches:	0	37	0.0 %


/*
 * Copyright 2013,
 * Olivier Stasse,
 *
 * CNRS
 *
 */

#ifndef SOT_TRAJECTORY_H__
#define SOT_TRAJECTORY_H__

// Matrix
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/signal-caster.h>

#include <boost/array.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/regex.hpp>
#include <sot/core/api.hh>

namespace dg = dynamicgraph;
namespace ba = boost::assign;

namespace dynamicgraph {
namespace sot {

class Trajectory;

class RulesJointTrajectory {
 protected:
  Trajectory &TrajectoryToFill_;

 public:
  unsigned int dbg_level;

  /// \brief Strings specifying the grammar of the structure.
  std::string float_str_re, seq_str_re, timestamp_str_re, frame_id_str_re,
      header_str_re, joint_name_str_re, list_of_jn_str_re, point_value_str_re,
      list_of_pv_str_re, bg_pt_str_re, end_pt_str_re, comma_pt_str_re,
      bg_liste_of_pts_str_re;
  /// \brief Boost regular expressions implementing the grammar.
  boost::regex header_re, list_of_jn_re, list_of_pv_re, bg_pt_re, end_pt_re,
      comma_pt_re, bg_liste_of_pts_re;
  std::vector<std::string> joint_names;

  /// \brief Constructor TrajectoryToFill is the structure where to store the
  /// parsed information.
  RulesJointTrajectory(Trajectory &TrajectoryToFill);

  /// \brief parse_string will fill TrajectoryToFill with string atext.
  void parse_string(std::string &atext);

 protected:
  /// \brief General parsing method of text with regexp e. The results are given
  /// in what. The remaining text is left in sub_text.
  bool search_exp_sub_string(
      std::string &text,
      boost::match_results<std::string::const_iterator> &what, boost::regex &e,
      std::string &sub_text);
  /// \brief Find and store the header.
  /// This method is looking for:
  /// unsigned int seq.
  /// unsigned int sec, unsigned int nsec.
  /// string format_id
  void parse_header(std::string &text, std::string &sub_text1);

  /// \brief Understand joint_names.
  /// Extract a list of strings.
  void parse_joint_names(std::string &text, std::string &sub_text1,
                         std::vector<std::string> &joint_names);

  /// \brief Extract a sequence of doubles.
  /// To be used for position, velocities, accelerations and effort.
  bool parse_seq(std::string &text, std::string &sub_text1,
                 std::vector<double> &seq);

  /// \brief Extract a point description.
  bool parse_point(std::string &trajectory, std::string &sub_text1);

  /// \brief Extract a sequence of points.
  bool parse_points(std::string &trajectory, std::string &sub_text1);
};

class SOT_CORE_EXPORT timestamp {
 public:
  unsigned long int secs_;
  unsigned long int nsecs_;
  timestamp() : secs_(0), nsecs_(0) {}
  timestamp(const timestamp &ats) {
    secs_ = ats.secs_;
    nsecs_ = ats.nsecs_;
  }
  timestamp(unsigned long int lsecs, unsigned long int lnsecs) {
    secs_ = lsecs;
    nsecs_ = lnsecs;
  }
  bool operator==(const timestamp &other) const {
    if ((secs_ != other.secs_) || (nsecs_ != other.nsecs_)) return false;
    return true;
  }
  friend std::ostream &operator<<(std::ostream &stream, const timestamp &ats) {
    stream << ats.secs_ + 0.000001 * (long double)ats.nsecs_;
    return stream;
  }
};

class SOT_CORE_EXPORT Header {
 public:
  unsigned int seq_;
  timestamp stamp_;
  std::string frame_id_;
  Header() : seq_(0), stamp_(0, 0), frame_id_("initial_trajectory") {}
};

class SOT_CORE_EXPORT JointTrajectoryPoint {
 public:
  std::vector<double> positions_;
  std::vector<double> velocities_;
  std::vector<double> accelerations_;
  std::vector<double> efforts_;

  typedef std::vector<double> vec_ref;

  void display(std::ostream &os) const {
    boost::array<std::string, 4> names = boost::assign::list_of("Positions")(
        "Velocities")("Accelerations")("Effort");

    const std::vector<double> *points = 0;

    for (std::size_t arrayId = 0; arrayId < names.size(); ++arrayId) {
      switch (arrayId) {
        case (0):
          points = &positions_;
          break;
        case (1):
          points = &velocities_;
          break;
        case (2):
          points = &accelerations_;
          break;
        case (3):
          points = &efforts_;
          break;
        default:
          assert(0);
      }

      std::vector<double>::const_iterator it_db;
      os << names[arrayId] << std::endl << "---------" << std::endl;
      for (it_db = points->begin(); it_db != points->end(); it_db++) {
        os << *it_db << std::endl;
      }
    }
  }

  void transfer(const std::vector<double> &src, unsigned int vecId) {
    switch (vecId) {
      case (0):
        positions_ = src;
        break;
      case (1):
        velocities_ = src;
        break;
      case (2):
        accelerations_ = src;
        break;
      case (3):
        efforts_ = src;
        break;
      default:
        assert(0);
    }
  }
};

class SOT_CORE_EXPORT Trajectory {
 public:
  Trajectory();
  Trajectory(const Trajectory &copy);
  virtual ~Trajectory();

  std::vector<std::string> joint_names_;

  Header header_;
  double time_from_start_;

  std::vector<JointTrajectoryPoint> points_;

  int deserialize(std::istringstream &is);
  void display(std::ostream &) const;
};
}  // namespace sot

template <>
struct signal_io<sot::Trajectory> : signal_io_unimplemented<sot::Trajectory> {};

}  // namespace dynamicgraph

#endif /* #ifndef SOT_TRAJECTORY_H__ */


Generated by: GCOVR (Version 4.2)

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__ */