Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/sot_to_ros.hh	Lines:	0	46	0.0 %
Date:	2022-09-08 07:44:08	Branches:	0	60	0.0 %


#ifndef DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
#define DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#include <std_msgs/UInt32.h>

#include <boost/format.hpp>
#include <sot/core/matrix-geometry.hh>
#include <utility>
#include <vector>

#include "dynamic_graph_bridge_msgs/Matrix.h"
#include "dynamic_graph_bridge_msgs/Vector.h"
#include "geometry_msgs/Transform.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/TwistStamped.h"
#include "geometry_msgs/Vector3Stamped.h"

#define MAKE_SIGNAL_STRING(NAME, IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME)  \
  makeSignalString(typeid(*this).name(), NAME, IS_INPUT, OUTPUT_TYPE, \
                   SIGNAL_NAME)

namespace dynamicgraph {

/// \brief Types dedicated to identify pairs of (dg,ros) data.
///
/// They do not contain any data but allow to make the difference
/// between different types with the same structure.
/// For instance a vector of six elements vs a twist coordinate.
namespace specific {
class Vector3 {};
class Twist {};
}  // end of namespace specific.

/// \brief SotToRos trait type.
///
/// This trait provides types associated to a dynamic-graph type:
/// - ROS corresponding type,
/// - signal type / input signal type,
/// - ROS callback type.
template <typename SotType>
class SotToRos;

template <>
struct SotToRos<bool> {
  typedef bool sot_t;
  typedef std_msgs::Bool ros_t;
  typedef std_msgs::BoolConstPtr ros_const_ptr_t;
  typedef dynamicgraph::Signal<sot_t, int> signal_t;
  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
  typedef boost::function<sot_t&(sot_t&, int)> callback_t;

  static const char* signalTypeName;

  template <typename S>
  static void setDefault(S& s) {
    s.setConstant(false);
  }

  static void setDefault(sot_t& s) { s = false; }
};

template <>
struct SotToRos<double> {
  typedef double sot_t;
  typedef std_msgs::Float64 ros_t;
  typedef std_msgs::Float64ConstPtr ros_const_ptr_t;
  typedef dynamicgraph::Signal<sot_t, int> signal_t;
  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
  typedef boost::function<sot_t&(sot_t&, int)> callback_t;

  static const char* signalTypeName;

  template <typename S>
  static void setDefault(S& s) {
    s.setConstant(0.);
  }

  static void setDefault(sot_t& s) { s = 0.; }
};

template <>
struct SotToRos<unsigned int> {
  typedef unsigned int sot_t;
  typedef std_msgs::UInt32 ros_t;
  typedef std_msgs::UInt32ConstPtr ros_const_ptr_t;
  typedef dynamicgraph::Signal<sot_t, int> signal_t;
  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
  typedef boost::function<sot_t&(sot_t&, int)> callback_t;

  static const char* signalTypeName;

  template <typename S>
  static void setDefault(S& s) {
    s.setConstant(0);
  }

  static void setDefault(sot_t& s) { s = 0; }
};

template <>
struct SotToRos<int> {
  typedef int sot_t;
  typedef std_msgs::Int32 ros_t;
  typedef std_msgs::Int32ConstPtr ros_const_ptr_t;
  typedef dynamicgraph::Signal<sot_t, int> signal_t;
  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
  typedef boost::function<sot_t&(sot_t&, int)> callback_t;

  static const char* signalTypeName;

  template <typename S>
  static void setDefault(S& s) {
    s.setConstant(0);
  }

  static void setDefault(sot_t& s) { s = 0; }
};

template <>
struct SotToRos<std::string> {
  typedef std::string sot_t;
  typedef std_msgs::String ros_t;
  typedef std_msgs::StringConstPtr ros_const_ptr_t;
  typedef dynamicgraph::Signal<sot_t, int> signal_t;
  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
  typedef boost::function<sot_t&(sot_t&, int)> callback_t;

  static const char* signalTypeName;

  template <typename S>
  static void setDefault(S& s) {
    s.setConstant("");
  }

  static void setDefault(sot_t& s) { s = std::string(); }
};

template <>
struct SotToRos<Matrix> {
  typedef Matrix sot_t;
  typedef dynamic_graph_bridge_msgs::Matrix ros_t;
  typedef dynamic_graph_bridge_msgs::MatrixConstPtr ros_const_ptr_t;
  typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
  typedef boost::function<sot_t&(sot_t&, int)> callback_t;

  static const char* signalTypeName;

  template <typename S>
  static void setDefault(S& s) {
    Matrix m;
    m.resize(0, 0);
    s.setConstant(m);
  }

  static void setDefault(sot_t& s) { s.resize(0, 0); }
};

template <>
struct SotToRos<Vector> {
  typedef Vector sot_t;
  typedef dynamic_graph_bridge_msgs::Vector ros_t;
  typedef dynamic_graph_bridge_msgs::VectorConstPtr ros_const_ptr_t;
  typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
  typedef boost::function<sot_t&(sot_t&, int)> callback_t;

  static const char* signalTypeName;

  template <typename S>
  static void setDefault(S& s) {
    Vector v;
    v.resize(0);
    s.setConstant(v);
  }

  static void setDefault(sot_t& s) { s.resize(0); }
};

template <>
struct SotToRos<specific::Vector3> {
  typedef Vector sot_t;
  typedef geometry_msgs::Vector3 ros_t;
  typedef geometry_msgs::Vector3ConstPtr ros_const_ptr_t;
  typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
  typedef boost::function<sot_t&(sot_t&, int)> callback_t;

  static const char* signalTypeName;

  template <typename S>
  static void setDefault(S& s) {
    Vector v(Vector::Zero(3));
    s.setConstant(v);
  }

  static void setDefault(sot_t& s) { s = Vector::Zero(3); }
};

template <>
struct SotToRos<sot::MatrixHomogeneous> {
  typedef sot::MatrixHomogeneous sot_t;
  typedef geometry_msgs::Transform ros_t;
  typedef geometry_msgs::TransformConstPtr ros_const_ptr_t;
  typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
  typedef boost::function<sot_t&(sot_t&, int)> callback_t;

  static const char* signalTypeName;

  template <typename S>
  static void setDefault(S& s) {
    sot::MatrixHomogeneous m;
    s.setConstant(m);
  }

  static void setDefault(sot_t& s) { s.setIdentity(); }
};

template <>
struct SotToRos<specific::Twist> {
  typedef Vector sot_t;
  typedef geometry_msgs::Twist ros_t;
  typedef geometry_msgs::TwistConstPtr ros_const_ptr_t;
  typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
  typedef boost::function<sot_t&(sot_t&, int)> callback_t;

  static const char* signalTypeName;

  template <typename S>
  static void setDefault(S& s) {
    Vector v(6);
    v.setZero();
    s.setConstant(v);
  }

  static void setDefault(sot_t& s) { s = Vector::Zero(6); }
};

// Stamped vector3
template <>
struct SotToRos<std::pair<specific::Vector3, Vector> > {
  typedef Vector sot_t;
  typedef geometry_msgs::Vector3Stamped ros_t;
  typedef geometry_msgs::Vector3StampedConstPtr ros_const_ptr_t;
  typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
  typedef boost::function<sot_t&(sot_t&, int)> callback_t;

  static const char* signalTypeName;

  template <typename S>
  static void setDefault(S& s) {
    SotToRos<sot_t>::setDefault(s);
  }
};

// Stamped transformation
template <>
struct SotToRos<std::pair<sot::MatrixHomogeneous, Vector> > {
  typedef sot::MatrixHomogeneous sot_t;
  typedef geometry_msgs::TransformStamped ros_t;
  typedef geometry_msgs::TransformStampedConstPtr ros_const_ptr_t;
  typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
  typedef boost::function<sot_t&(sot_t&, int)> callback_t;

  static const char* signalTypeName;

  template <typename S>
  static void setDefault(S& s) {
    SotToRos<sot_t>::setDefault(s);
  }
};

// Stamped twist
template <>
struct SotToRos<std::pair<specific::Twist, Vector> > {
  typedef Vector sot_t;
  typedef geometry_msgs::TwistStamped ros_t;
  typedef geometry_msgs::TwistStampedConstPtr ros_const_ptr_t;
  typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
  typedef boost::function<sot_t&(sot_t&, int)> callback_t;

  static const char* signalTypeName;

  template <typename S>
  static void setDefault(S& s) {
    SotToRos<sot_t>::setDefault(s);
  }
};

inline std::string makeSignalString(const std::string& className,
                                    const std::string& instanceName,
                                    bool isInputSignal,
                                    const std::string& signalType,
                                    const std::string& signalName) {
  boost::format fmt("%s(%s)::%s(%s)::%s");
  fmt % className % instanceName % (isInputSignal ? "input" : "output") %
      signalType % signalName;
  return fmt.str();
}
}  // end of namespace dynamicgraph.

#endif  //! DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			#ifndef DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
2			#define DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
3			#include <dynamic-graph/linear-algebra.h>
4			#include <dynamic-graph/signal-ptr.h>
5			#include <dynamic-graph/signal-time-dependent.h>
6			#include <std_msgs/Bool.h>
7			#include <std_msgs/Float64.h>
8			#include <std_msgs/Int32.h>
9			#include <std_msgs/String.h>
10			#include <std_msgs/UInt32.h>
11
12			#include <boost/format.hpp>
13			#include <sot/core/matrix-geometry.hh>
14			#include <utility>
15			#include <vector>
16
17			#include "dynamic_graph_bridge_msgs/Matrix.h"
18			#include "dynamic_graph_bridge_msgs/Vector.h"
19			#include "geometry_msgs/Transform.h"
20			#include "geometry_msgs/TransformStamped.h"
21			#include "geometry_msgs/Twist.h"
22			#include "geometry_msgs/TwistStamped.h"
23			#include "geometry_msgs/Vector3Stamped.h"
24
25			#define MAKE_SIGNAL_STRING(NAME, IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME) \
26			makeSignalString(typeid(*this).name(), NAME, IS_INPUT, OUTPUT_TYPE, \
27			SIGNAL_NAME)
28
29			namespace dynamicgraph {
30
31			/// \brief Types dedicated to identify pairs of (dg,ros) data.
32			///
33			/// They do not contain any data but allow to make the difference
34			/// between different types with the same structure.
35			/// For instance a vector of six elements vs a twist coordinate.
36			namespace specific {
37			class Vector3 {};
38			class Twist {};
39			} // end of namespace specific.
40
41			/// \brief SotToRos trait type.
42			///
43			/// This trait provides types associated to a dynamic-graph type:
44			/// - ROS corresponding type,
45			/// - signal type / input signal type,
46			/// - ROS callback type.
47			template <typename SotType>
48			class SotToRos;
49
50			template <>
51			struct SotToRos<bool> {
52			typedef bool sot_t;
53			typedef std_msgs::Bool ros_t;
54			typedef std_msgs::BoolConstPtr ros_const_ptr_t;
55			typedef dynamicgraph::Signal<sot_t, int> signal_t;
56			typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
57			typedef boost::function<sot_t&(sot_t&, int)> callback_t;
58
59			static const char* signalTypeName;
60
61			template <typename S>
62			static void setDefault(S& s) {
63			s.setConstant(false);
64			}
65
66			static void setDefault(sot_t& s) { s = false; }
67			};
68
69			template <>
70			struct SotToRos<double> {
71			typedef double sot_t;
72			typedef std_msgs::Float64 ros_t;
73			typedef std_msgs::Float64ConstPtr ros_const_ptr_t;
74			typedef dynamicgraph::Signal<sot_t, int> signal_t;
75			typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
76			typedef boost::function<sot_t&(sot_t&, int)> callback_t;
77
78			static const char* signalTypeName;
79
80			template <typename S>
81			static void setDefault(S& s) {
82			s.setConstant(0.);
83			}
84
85			static void setDefault(sot_t& s) { s = 0.; }
86			};
87
88			template <>
89			struct SotToRos<unsigned int> {
90			typedef unsigned int sot_t;
91			typedef std_msgs::UInt32 ros_t;
92			typedef std_msgs::UInt32ConstPtr ros_const_ptr_t;
93			typedef dynamicgraph::Signal<sot_t, int> signal_t;
94			typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
95			typedef boost::function<sot_t&(sot_t&, int)> callback_t;
96
97			static const char* signalTypeName;
98
99			template <typename S>
100			static void setDefault(S& s) {
101			s.setConstant(0);
102			}
103
104			static void setDefault(sot_t& s) { s = 0; }
105			};
106
107			template <>
108			struct SotToRos<int> {
109			typedef int sot_t;
110			typedef std_msgs::Int32 ros_t;
111			typedef std_msgs::Int32ConstPtr ros_const_ptr_t;
112			typedef dynamicgraph::Signal<sot_t, int> signal_t;
113			typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
114			typedef boost::function<sot_t&(sot_t&, int)> callback_t;
115
116			static const char* signalTypeName;
117
118			template <typename S>
119			static void setDefault(S& s) {
120			s.setConstant(0);
121			}
122
123			static void setDefault(sot_t& s) { s = 0; }
124			};
125
126			template <>
127			struct SotToRos<std::string> {
128			typedef std::string sot_t;
129			typedef std_msgs::String ros_t;
130			typedef std_msgs::StringConstPtr ros_const_ptr_t;
131			typedef dynamicgraph::Signal<sot_t, int> signal_t;
132			typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
133			typedef boost::function<sot_t&(sot_t&, int)> callback_t;
134
135			static const char* signalTypeName;
136
137			template <typename S>
138			static void setDefault(S& s) {
139			s.setConstant("");
140			}
141
142			static void setDefault(sot_t& s) { s = std::string(); }
143			};
144
145			template <>
146			struct SotToRos<Matrix> {
147			typedef Matrix sot_t;
148			typedef dynamic_graph_bridge_msgs::Matrix ros_t;
149			typedef dynamic_graph_bridge_msgs::MatrixConstPtr ros_const_ptr_t;
150			typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
151			typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
152			typedef boost::function<sot_t&(sot_t&, int)> callback_t;
153
154			static const char* signalTypeName;
155
156			template <typename S>
157			static void setDefault(S& s) {
158			Matrix m;
159			m.resize(0, 0);
160			s.setConstant(m);
161			}
162
163			static void setDefault(sot_t& s) { s.resize(0, 0); }
164			};
165
166			template <>
167			struct SotToRos<Vector> {
168			typedef Vector sot_t;
169			typedef dynamic_graph_bridge_msgs::Vector ros_t;
170			typedef dynamic_graph_bridge_msgs::VectorConstPtr ros_const_ptr_t;
171			typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
172			typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
173			typedef boost::function<sot_t&(sot_t&, int)> callback_t;
174
175			static const char* signalTypeName;
176
177			template <typename S>
178			static void setDefault(S& s) {
179			Vector v;
180			v.resize(0);
181			s.setConstant(v);
182			}
183
184			static void setDefault(sot_t& s) { s.resize(0); }
185			};
186
187			template <>
188			struct SotToRos<specific::Vector3> {
189			typedef Vector sot_t;
190			typedef geometry_msgs::Vector3 ros_t;
191			typedef geometry_msgs::Vector3ConstPtr ros_const_ptr_t;
192			typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
193			typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
194			typedef boost::function<sot_t&(sot_t&, int)> callback_t;
195
196			static const char* signalTypeName;
197
198			template <typename S>
199			static void setDefault(S& s) {
200			Vector v(Vector::Zero(3));
201			s.setConstant(v);
202			}
203
204			static void setDefault(sot_t& s) { s = Vector::Zero(3); }
205			};
206
207			template <>
208			struct SotToRos<sot::MatrixHomogeneous> {
209			typedef sot::MatrixHomogeneous sot_t;
210			typedef geometry_msgs::Transform ros_t;
211			typedef geometry_msgs::TransformConstPtr ros_const_ptr_t;
212			typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
213			typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
214			typedef boost::function<sot_t&(sot_t&, int)> callback_t;
215
216			static const char* signalTypeName;
217
218			template <typename S>
219			static void setDefault(S& s) {
220			sot::MatrixHomogeneous m;
221			s.setConstant(m);
222			}
223
224			static void setDefault(sot_t& s) { s.setIdentity(); }
225			};
226
227			template <>
228			struct SotToRos<specific::Twist> {
229			typedef Vector sot_t;
230			typedef geometry_msgs::Twist ros_t;
231			typedef geometry_msgs::TwistConstPtr ros_const_ptr_t;
232			typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
233			typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
234			typedef boost::function<sot_t&(sot_t&, int)> callback_t;
235
236			static const char* signalTypeName;
237
238			template <typename S>
239			static void setDefault(S& s) {
240			Vector v(6);
241			v.setZero();
242			s.setConstant(v);
243			}
244
245			static void setDefault(sot_t& s) { s = Vector::Zero(6); }
246			};
247
248			// Stamped vector3
249			template <>
250			struct SotToRos<std::pair<specific::Vector3, Vector> > {
251			typedef Vector sot_t;
252			typedef geometry_msgs::Vector3Stamped ros_t;
253			typedef geometry_msgs::Vector3StampedConstPtr ros_const_ptr_t;
254			typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
255			typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
256			typedef boost::function<sot_t&(sot_t&, int)> callback_t;
257
258			static const char* signalTypeName;
259
260			template <typename S>
261			static void setDefault(S& s) {
262			SotToRos<sot_t>::setDefault(s);
263			}
264			};
265
266			// Stamped transformation
267			template <>
268			struct SotToRos<std::pair<sot::MatrixHomogeneous, Vector> > {
269			typedef sot::MatrixHomogeneous sot_t;
270			typedef geometry_msgs::TransformStamped ros_t;
271			typedef geometry_msgs::TransformStampedConstPtr ros_const_ptr_t;
272			typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
273			typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
274			typedef boost::function<sot_t&(sot_t&, int)> callback_t;
275
276			static const char* signalTypeName;
277
278			template <typename S>
279			static void setDefault(S& s) {
280			SotToRos<sot_t>::setDefault(s);
281			}
282			};
283
284			// Stamped twist
285			template <>
286			struct SotToRos<std::pair<specific::Twist, Vector> > {
287			typedef Vector sot_t;
288			typedef geometry_msgs::TwistStamped ros_t;
289			typedef geometry_msgs::TwistStampedConstPtr ros_const_ptr_t;
290			typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
291			typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
292			typedef boost::function<sot_t&(sot_t&, int)> callback_t;
293
294			static const char* signalTypeName;
295
296			template <typename S>
297			static void setDefault(S& s) {
298			SotToRos<sot_t>::setDefault(s);
299			}
300			};
301
302			inline std::string makeSignalString(const std::string& className,
303			const std::string& instanceName,
304			bool isInputSignal,
305			const std::string& signalType,
306			const std::string& signalName) {
307			boost::format fmt("%s(%s)::%s(%s)::%s");
308			fmt % className % instanceName % (isInputSignal ? "input" : "output") %
309			signalType % signalName;
310			return fmt.str();
311			}
312			} // end of namespace dynamicgraph.
313
314			#endif //! DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH