Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/converter.hh	Lines:	0	94	0.0 %
Date:	2022-09-08 07:44:08	Branches:	0	102	0.0 %


#ifndef DYNAMIC_GRAPH_ROS_CONVERTER_HH
#define DYNAMIC_GRAPH_ROS_CONVERTER_HH
#include <ros/time.h>
#include <std_msgs/Header.h>

#include <boost/date_time/date.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/static_assert.hpp>
#include <stdexcept>

#include "sot_to_ros.hh"

#define SOT_TO_ROS_IMPL(T)                       \
  template <>                                    \
  inline void converter(SotToRos<T>::ros_t& dst, \
                        const SotToRos<T>::sot_t& src)

#define ROS_TO_SOT_IMPL(T)                       \
  template <>                                    \
  inline void converter(SotToRos<T>::sot_t& dst, \
                        const SotToRos<T>::ros_t& src)

namespace dynamicgraph {
inline void makeHeader(std_msgs::Header& header) {
  header.seq = 0;
  header.stamp = ros::Time::now();
  header.frame_id = "/dynamic_graph/world";
}

/// \brief Handle ROS <-> dynamic-graph conversion.
///
/// Implements all ROS/dynamic-graph conversions required by the
/// bridge.
///
/// Relies on the SotToRos type trait to make sure valid pair of
/// types are used.
template <typename D, typename S>
void converter(D& dst, const S& src);

// Boolean
SOT_TO_ROS_IMPL(bool) { dst.data = src; }

ROS_TO_SOT_IMPL(bool) { dst = src.data; }

// Double
SOT_TO_ROS_IMPL(double) { dst.data = src; }

ROS_TO_SOT_IMPL(double) { dst = src.data; }

// Int
SOT_TO_ROS_IMPL(int) { dst.data = src; }

ROS_TO_SOT_IMPL(int) { dst = src.data; }

// Unsigned
SOT_TO_ROS_IMPL(unsigned int) { dst.data = src; }

ROS_TO_SOT_IMPL(unsigned int) { dst = src.data; }

// String
SOT_TO_ROS_IMPL(std::string) { dst.data = src; }

ROS_TO_SOT_IMPL(std::string) { dst = src.data; }

// Vector
SOT_TO_ROS_IMPL(Vector) {
  dst.data.resize(src.size());
  for (int i = 0; i < src.size(); ++i) dst.data[i] = src(i);
}

ROS_TO_SOT_IMPL(Vector) {
  dst.resize(src.data.size());
  for (unsigned int i = 0; i < src.data.size(); ++i) dst(i) = src.data[i];
}

// Vector3
SOT_TO_ROS_IMPL(specific::Vector3) {
  if (src.size() > 0) {
    dst.x = src(0);
    if (src.size() > 1) {
      dst.y = src(1);
      if (src.size() > 2) dst.z = src(2);
    }
  }
}

ROS_TO_SOT_IMPL(specific::Vector3) {
  dst.resize(3);
  dst(0) = src.x;
  dst(1) = src.y;
  dst(2) = src.z;
}

// Matrix
SOT_TO_ROS_IMPL(Matrix) {
  // TODO: Confirm Ros Matrix Storage order. It changes the RosMatrix to
  // ColMajor.
  dst.width = (unsigned int)src.rows();
  dst.data.resize(src.cols() * src.rows());
  for (int i = 0; i < src.cols() * src.rows(); ++i) dst.data[i] = src.data()[i];
}

ROS_TO_SOT_IMPL(Matrix) {
  dst.resize(src.width,
             (unsigned int)src.data.size() / (unsigned int)src.width);
  for (unsigned i = 0; i < src.data.size(); ++i) dst.data()[i] = src.data[i];
}

// Homogeneous matrix.
SOT_TO_ROS_IMPL(sot::MatrixHomogeneous) {
  sot::VectorQuaternion q(src.linear());
  dst.translation.x = src.translation()(0);
  dst.translation.y = src.translation()(1);
  dst.translation.z = src.translation()(2);

  dst.rotation.x = q.x();
  dst.rotation.y = q.y();
  dst.rotation.z = q.z();
  dst.rotation.w = q.w();
}

ROS_TO_SOT_IMPL(sot::MatrixHomogeneous) {
  sot::VectorQuaternion q(src.rotation.w, src.rotation.x, src.rotation.y,
                          src.rotation.z);
  dst.linear() = q.matrix();

  // Copy the translation component.
  dst.translation()(0) = src.translation.x;
  dst.translation()(1) = src.translation.y;
  dst.translation()(2) = src.translation.z;
}

// Twist.
SOT_TO_ROS_IMPL(specific::Twist) {
  if (src.size() != 6)
    throw std::runtime_error("failed to convert invalid twist");
  dst.linear.x = src(0);
  dst.linear.y = src(1);
  dst.linear.z = src(2);
  dst.angular.x = src(3);
  dst.angular.y = src(4);
  dst.angular.z = src(5);
}

ROS_TO_SOT_IMPL(specific::Twist) {
  dst.resize(6);
  dst(0) = src.linear.x;
  dst(1) = src.linear.y;
  dst(2) = src.linear.z;
  dst(3) = src.angular.x;
  dst(4) = src.angular.y;
  dst(5) = src.angular.z;
}

/// \brief This macro generates a converter for a stamped type from
/// dynamic-graph to ROS.  I.e. A data associated with its
/// timestamp.
#define DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(T, ATTRIBUTE, EXTRA)              \
  template <>                                                                \
  inline void converter(SotToRos<std::pair<T, Vector> >::ros_t& dst,         \
                        const SotToRos<std::pair<T, Vector> >::sot_t& src) { \
    makeHeader(dst.header);                                                  \
    converter<SotToRos<T>::ros_t, SotToRos<T>::sot_t>(dst.ATTRIBUTE, src);   \
    do {                                                                     \
      EXTRA                                                                  \
    } while (0);                                                             \
  }                                                                          \
  struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n

DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Vector3, vector, ;);
DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform,
                                   dst.child_frame_id = "";);
DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);

/// \brief This macro generates a converter for a shared pointer on
///        a ROS type to a dynamic-graph type.
///
///        A converter for the underlying type is required.  I.e. to
///        convert a shared_ptr<T> to T', a converter from T to T'
///        is required.
#define DG_BRIDGE_MAKE_SHPTR_IMPL(T)                              \
  template <>                                                     \
  inline void converter(                                          \
      SotToRos<T>::sot_t& dst,                                    \
      const boost::shared_ptr<SotToRos<T>::ros_t const>& src) {   \
    converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, *src); \
  }                                                               \
  struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n

DG_BRIDGE_MAKE_SHPTR_IMPL(bool);
DG_BRIDGE_MAKE_SHPTR_IMPL(double);
DG_BRIDGE_MAKE_SHPTR_IMPL(int);
DG_BRIDGE_MAKE_SHPTR_IMPL(unsigned int);
DG_BRIDGE_MAKE_SHPTR_IMPL(std::string);
DG_BRIDGE_MAKE_SHPTR_IMPL(Vector);
DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Vector3);
DG_BRIDGE_MAKE_SHPTR_IMPL(Matrix);
DG_BRIDGE_MAKE_SHPTR_IMPL(sot::MatrixHomogeneous);
DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Twist);

/// \brief This macro generates a converter for a stamped type.
/// I.e. A data associated with its timestamp.
///
/// FIXME: the timestamp is not yet forwarded to the dg signal.
#define DG_BRIDGE_MAKE_STAMPED_IMPL(T, ATTRIBUTE, EXTRA)                     \
  template <>                                                                \
  inline void converter(SotToRos<std::pair<T, Vector> >::sot_t& dst,         \
                        const SotToRos<std::pair<T, Vector> >::ros_t& src) { \
    converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, src.ATTRIBUTE);   \
    do {                                                                     \
      EXTRA                                                                  \
    } while (0);                                                             \
  }                                                                          \
  struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n

DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Vector3, vector, ;);
DG_BRIDGE_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform, ;);
DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);

/// \brief This macro generates a converter for a shared pointer on
/// a stamped type.  I.e. A data associated with its timestamp.
///
/// FIXME: the timestamp is not yet forwarded to the dg signal.
#define DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(T, ATTRIBUTE, EXTRA)               \
  template <>                                                                \
  inline void converter(                                                     \
      SotToRos<std::pair<T, Vector> >::sot_t& dst,                           \
      const boost::shared_ptr<SotToRos<std::pair<T, Vector> >::ros_t const>& \
          src) {                                                             \
    converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, src->ATTRIBUTE);  \
    do {                                                                     \
      EXTRA                                                                  \
    } while (0);                                                             \
  }                                                                          \
  struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n

DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(specific::Vector3, vector, ;);
DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(sot::MatrixHomogeneous, transform, ;);
DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(specific::Twist, twist, ;);

/// \brief If an impossible/unimplemented conversion is required, fail.
///
/// IMPORTANT, READ ME:
///
/// If the compiler generates an error in the following function,
/// this is /normal/.
///
/// This error means that either you try to use an undefined
/// conversion.  You can either fix your code or provide the wanted
/// conversion by updating this header.
template <typename U, typename V>
inline void converter(U& dst, V& src) {
  // This will always fail if instantiated.
  BOOST_STATIC_ASSERT(sizeof(U) == 0);
}

typedef boost::posix_time::ptime ptime;
typedef boost::posix_time::seconds seconds;
typedef boost::posix_time::microseconds microseconds;
typedef boost::posix_time::time_duration time_duration;
typedef boost::gregorian::date date;

boost::posix_time::ptime rosTimeToPtime(const ros::Time& rosTime) {
  ptime time(date(1970, 1, 1),
             seconds(rosTime.sec) + microseconds(rosTime.nsec / 1000));
  return time;
}

ros::Time pTimeToRostime(const boost::posix_time::ptime& time) {
  static ptime timeStart(date(1970, 1, 1));
  time_duration diff = time - timeStart;

  uint32_t sec = (unsigned int)diff.ticks() /
                 (unsigned int)time_duration::rep_type::res_adjust();
  uint32_t nsec = (unsigned int)diff.fractional_seconds();

  return ros::Time(sec, nsec);
}
}  // end of namespace dynamicgraph.

#endif  //! DYNAMIC_GRAPH_ROS_CONVERTER_HH


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			#ifndef DYNAMIC_GRAPH_ROS_CONVERTER_HH
2			#define DYNAMIC_GRAPH_ROS_CONVERTER_HH
3			#include <ros/time.h>
4			#include <std_msgs/Header.h>
5
6			#include <boost/date_time/date.hpp>
7			#include <boost/date_time/posix_time/posix_time.hpp>
8			#include <boost/static_assert.hpp>
9			#include <stdexcept>
10
11			#include "sot_to_ros.hh"
12
13			#define SOT_TO_ROS_IMPL(T) \
14			template <> \
15			inline void converter(SotToRos<T>::ros_t& dst, \
16			const SotToRos<T>::sot_t& src)
17
18			#define ROS_TO_SOT_IMPL(T) \
19			template <> \
20			inline void converter(SotToRos<T>::sot_t& dst, \
21			const SotToRos<T>::ros_t& src)
22
23			namespace dynamicgraph {
24			inline void makeHeader(std_msgs::Header& header) {
25			header.seq = 0;
26			header.stamp = ros::Time::now();
27			header.frame_id = "/dynamic_graph/world";
28			}
29
30			/// \brief Handle ROS <-> dynamic-graph conversion.
31			///
32			/// Implements all ROS/dynamic-graph conversions required by the
33			/// bridge.
34			///
35			/// Relies on the SotToRos type trait to make sure valid pair of
36			/// types are used.
37			template <typename D, typename S>
38			void converter(D& dst, const S& src);
39
40			// Boolean
41			SOT_TO_ROS_IMPL(bool) { dst.data = src; }
42
43			ROS_TO_SOT_IMPL(bool) { dst = src.data; }
44
45			// Double
46			SOT_TO_ROS_IMPL(double) { dst.data = src; }
47
48			ROS_TO_SOT_IMPL(double) { dst = src.data; }
49
50			// Int
51			SOT_TO_ROS_IMPL(int) { dst.data = src; }
52
53			ROS_TO_SOT_IMPL(int) { dst = src.data; }
54
55			// Unsigned
56			SOT_TO_ROS_IMPL(unsigned int) { dst.data = src; }
57
58			ROS_TO_SOT_IMPL(unsigned int) { dst = src.data; }
59
60			// String
61			SOT_TO_ROS_IMPL(std::string) { dst.data = src; }
62
63			ROS_TO_SOT_IMPL(std::string) { dst = src.data; }
64
65			// Vector
66			SOT_TO_ROS_IMPL(Vector) {
67			dst.data.resize(src.size());
68			for (int i = 0; i < src.size(); ++i) dst.data[i] = src(i);
69			}
70
71			ROS_TO_SOT_IMPL(Vector) {
72			dst.resize(src.data.size());
73			for (unsigned int i = 0; i < src.data.size(); ++i) dst(i) = src.data[i];
74			}
75
76			// Vector3
77			SOT_TO_ROS_IMPL(specific::Vector3) {
78			if (src.size() > 0) {
79			dst.x = src(0);
80			if (src.size() > 1) {
81			dst.y = src(1);
82			if (src.size() > 2) dst.z = src(2);
83			}
84			}
85			}
86
87			ROS_TO_SOT_IMPL(specific::Vector3) {
88			dst.resize(3);
89			dst(0) = src.x;
90			dst(1) = src.y;
91			dst(2) = src.z;
92			}
93
94			// Matrix
95			SOT_TO_ROS_IMPL(Matrix) {
96			// TODO: Confirm Ros Matrix Storage order. It changes the RosMatrix to
97			// ColMajor.
98			dst.width = (unsigned int)src.rows();
99			dst.data.resize(src.cols() * src.rows());
100			for (int i = 0; i < src.cols() * src.rows(); ++i) dst.data[i] = src.data()[i];
101			}
102
103			ROS_TO_SOT_IMPL(Matrix) {
104			dst.resize(src.width,
105			(unsigned int)src.data.size() / (unsigned int)src.width);
106			for (unsigned i = 0; i < src.data.size(); ++i) dst.data()[i] = src.data[i];
107			}
108
109			// Homogeneous matrix.
110			SOT_TO_ROS_IMPL(sot::MatrixHomogeneous) {
111			sot::VectorQuaternion q(src.linear());
112			dst.translation.x = src.translation()(0);
113			dst.translation.y = src.translation()(1);
114			dst.translation.z = src.translation()(2);
115
116			dst.rotation.x = q.x();
117			dst.rotation.y = q.y();
118			dst.rotation.z = q.z();
119			dst.rotation.w = q.w();
120			}
121
122			ROS_TO_SOT_IMPL(sot::MatrixHomogeneous) {
123			sot::VectorQuaternion q(src.rotation.w, src.rotation.x, src.rotation.y,
124			src.rotation.z);
125			dst.linear() = q.matrix();
126
127			// Copy the translation component.
128			dst.translation()(0) = src.translation.x;
129			dst.translation()(1) = src.translation.y;
130			dst.translation()(2) = src.translation.z;
131			}
132
133			// Twist.
134			SOT_TO_ROS_IMPL(specific::Twist) {
135			if (src.size() != 6)
136			throw std::runtime_error("failed to convert invalid twist");
137			dst.linear.x = src(0);
138			dst.linear.y = src(1);
139			dst.linear.z = src(2);
140			dst.angular.x = src(3);
141			dst.angular.y = src(4);
142			dst.angular.z = src(5);
143			}
144
145			ROS_TO_SOT_IMPL(specific::Twist) {
146			dst.resize(6);
147			dst(0) = src.linear.x;
148			dst(1) = src.linear.y;
149			dst(2) = src.linear.z;
150			dst(3) = src.angular.x;
151			dst(4) = src.angular.y;
152			dst(5) = src.angular.z;
153			}
154
155			/// \brief This macro generates a converter for a stamped type from
156			/// dynamic-graph to ROS. I.e. A data associated with its
157			/// timestamp.
158			#define DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(T, ATTRIBUTE, EXTRA) \
159			template <> \
160			inline void converter(SotToRos<std::pair<T, Vector> >::ros_t& dst, \
161			const SotToRos<std::pair<T, Vector> >::sot_t& src) { \
162			makeHeader(dst.header); \
163			converter<SotToRos<T>::ros_t, SotToRos<T>::sot_t>(dst.ATTRIBUTE, src); \
164			do { \
165			EXTRA \
166			} while (0); \
167			} \
168			struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
169
170			DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Vector3, vector, ;);
171			DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform,
172			dst.child_frame_id = "";);
173			DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
174
175			/// \brief This macro generates a converter for a shared pointer on
176			/// a ROS type to a dynamic-graph type.
177			///
178			/// A converter for the underlying type is required. I.e. to
179			/// convert a shared_ptr<T> to T', a converter from T to T'
180			/// is required.
181			#define DG_BRIDGE_MAKE_SHPTR_IMPL(T) \
182			template <> \
183			inline void converter( \
184			SotToRos<T>::sot_t& dst, \
185			const boost::shared_ptr<SotToRos<T>::ros_t const>& src) { \
186			converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, *src); \
187			} \
188			struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
189
190			DG_BRIDGE_MAKE_SHPTR_IMPL(bool);
191			DG_BRIDGE_MAKE_SHPTR_IMPL(double);
192			DG_BRIDGE_MAKE_SHPTR_IMPL(int);
193			DG_BRIDGE_MAKE_SHPTR_IMPL(unsigned int);
194			DG_BRIDGE_MAKE_SHPTR_IMPL(std::string);
195			DG_BRIDGE_MAKE_SHPTR_IMPL(Vector);
196			DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Vector3);
197			DG_BRIDGE_MAKE_SHPTR_IMPL(Matrix);
198			DG_BRIDGE_MAKE_SHPTR_IMPL(sot::MatrixHomogeneous);
199			DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Twist);
200
201			/// \brief This macro generates a converter for a stamped type.
202			/// I.e. A data associated with its timestamp.
203			///
204			/// FIXME: the timestamp is not yet forwarded to the dg signal.
205			#define DG_BRIDGE_MAKE_STAMPED_IMPL(T, ATTRIBUTE, EXTRA) \
206			template <> \
207			inline void converter(SotToRos<std::pair<T, Vector> >::sot_t& dst, \
208			const SotToRos<std::pair<T, Vector> >::ros_t& src) { \
209			converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, src.ATTRIBUTE); \
210			do { \
211			EXTRA \
212			} while (0); \
213			} \
214			struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
215
216			DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Vector3, vector, ;);
217			DG_BRIDGE_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform, ;);
218			DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
219
220			/// \brief This macro generates a converter for a shared pointer on
221			/// a stamped type. I.e. A data associated with its timestamp.
222			///
223			/// FIXME: the timestamp is not yet forwarded to the dg signal.
224			#define DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(T, ATTRIBUTE, EXTRA) \
225			template <> \
226			inline void converter( \
227			SotToRos<std::pair<T, Vector> >::sot_t& dst, \
228			const boost::shared_ptr<SotToRos<std::pair<T, Vector> >::ros_t const>& \
229			src) { \
230			converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, src->ATTRIBUTE); \
231			do { \
232			EXTRA \
233			} while (0); \
234			} \
235			struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
236
237			DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(specific::Vector3, vector, ;);
238			DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(sot::MatrixHomogeneous, transform, ;);
239			DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(specific::Twist, twist, ;);
240
241			/// \brief If an impossible/unimplemented conversion is required, fail.
242			///
243			/// IMPORTANT, READ ME:
244			///
245			/// If the compiler generates an error in the following function,
246			/// this is /normal/.
247			///
248			/// This error means that either you try to use an undefined
249			/// conversion. You can either fix your code or provide the wanted
250			/// conversion by updating this header.
251			template <typename U, typename V>
252			inline void converter(U& dst, V& src) {
253			// This will always fail if instantiated.
254			BOOST_STATIC_ASSERT(sizeof(U) == 0);
255			}
256
257			typedef boost::posix_time::ptime ptime;
258			typedef boost::posix_time::seconds seconds;
259			typedef boost::posix_time::microseconds microseconds;
260			typedef boost::posix_time::time_duration time_duration;
261			typedef boost::gregorian::date date;
262
263			boost::posix_time::ptime rosTimeToPtime(const ros::Time& rosTime) {
264			ptime time(date(1970, 1, 1),
265			seconds(rosTime.sec) + microseconds(rosTime.nsec / 1000));
266			return time;
267			}
268
269			ros::Time pTimeToRostime(const boost::posix_time::ptime& time) {
270			static ptime timeStart(date(1970, 1, 1));
271			time_duration diff = time - timeStart;
272
273			uint32_t sec = (unsigned int)diff.ticks() /
274			(unsigned int)time_duration::rep_type::res_adjust();
275			uint32_t nsec = (unsigned int)diff.fractional_seconds();
276
277			return ros::Time(sec, nsec);
278			}
279			} // end of namespace dynamicgraph.
280
281			#endif //! DYNAMIC_GRAPH_ROS_CONVERTER_HH