Skip to content

Commit dbb82ee

Browse files
berndpfrommerberndpfrommer
committed
added LiveSync for combined subscription + sync
1 parent 050b9ba commit dbb82ee

File tree

4 files changed

+151
-4
lines changed

4 files changed

+151
-4
lines changed

‎include/flex_sync/approximate_sync.h

+3
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
#ifndef FLEX_SYNC_APPROXIMATE_SYNC_H
55
#define FLEX_SYNC_APPROXIMATE_SYNC_H
66

7+
#include <flex_sync/msg_pack.h>
78
#include <ros/ros.h>
89
#include <functional>
910
#include <string>
@@ -23,6 +24,8 @@ namespace flex_sync {
2324
template <typename ... MsgTypes>
2425
class ApproximateSync {
2526
public:
27+
// Expose the messsage types
28+
using message_types = MsgPack<MsgTypes...>;
2629
// the signature of the callback function depends on the MsgTypes template
2730
// parameter.
2831
typedef std::function<void(const std::vector<boost::shared_ptr<

‎include/flex_sync/exact_sync.h

+3-4
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
#ifndef FLEX_SYNC_EXACT_SYNC_H
55
#define FLEX_SYNC_EXACT_SYNC_H
66

7+
#include <flex_sync/msg_pack.h>
78
#include <ros/ros.h>
89
#include <functional>
910
#include <string>
@@ -19,12 +20,8 @@
1920
*/
2021

2122
namespace flex_sync {
22-
2323
template <typename ... MsgTypes>
2424
class ExactSync {
25-
template <typename MsgType>
26-
using TopicVec = std::vector<boost::shared_ptr<MsgType>>;
27-
2825
// CallbackArg has arguments of the callback as a tuple
2926
typedef std::tuple<std::vector<boost::shared_ptr<const MsgTypes>> ...>
3027
CallbackArg;
@@ -42,6 +39,8 @@ namespace flex_sync {
4239
typedef std::tuple<TypeInfo<const MsgTypes>...> TupleOfTypeInfo;
4340

4441
public:
42+
// Expose the messsage types
43+
using message_types = MsgPack<MsgTypes...>;
4544
// the signature of the callback function depends on the MsgTypes template
4645
// parameter.
4746
typedef std::function<void(const std::vector<boost::shared_ptr<

‎include/flex_sync/live_sync.h

+130
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,130 @@
1+
/*-*-c++-*--------------------------------------------------------------------
2+
* 2020 Bernd Pfrommer [email protected]
3+
*/
4+
5+
#pragma once
6+
7+
#include <ros/ros.h>
8+
#include <flex_sync/msg_pack.h>
9+
10+
/*
11+
* Class for synchronized ros-subscriber
12+
*/
13+
14+
namespace flex_sync {
15+
// use this trick to get to the parameter pack
16+
// https://stackoverflow.com/questions/22968182/
17+
// is-it-possible-to-typedef-a-parameter-pack
18+
template <typename SyncT, typename = typename SyncT::message_types>
19+
class LiveSync;
20+
// now partial specialization
21+
template <typename SyncT, typename ...MsgTypes>
22+
class LiveSync <SyncT, MsgPack<MsgTypes...> > {
23+
// live topic class has the subscribers
24+
template <class T>
25+
class LiveTopic {
26+
public:
27+
typedef boost::shared_ptr<T const> TConstPtr;
28+
LiveTopic(const std::string &topic, ros::NodeHandle &nh,
29+
unsigned int qs, const std::shared_ptr<SyncT> &sync) :
30+
topic_(topic),
31+
sync_(sync) {
32+
sub_ = nh.subscribe(topic, qs, &LiveTopic::callback, this);
33+
}
34+
void callback(TConstPtr const &msg) {
35+
sync_->process(topic_, msg);
36+
}
37+
private:
38+
std::string topic_;
39+
std::shared_ptr<SyncT> sync_;
40+
ros::Subscriber sub_;
41+
};
42+
43+
44+
public:
45+
using string = std::string;
46+
using Time = ros::Time;
47+
typedef std::tuple<
48+
std::vector<
49+
std::shared_ptr<
50+
LiveTopic<const MsgTypes>>>...> TupleOfTopicVec;
51+
typedef typename SyncT::Callback Callback;
52+
53+
LiveSync(const ros::NodeHandle &nh,
54+
const std::vector<std::vector<string>> &topics,
55+
const Callback &callback,
56+
unsigned int maxQueueSize = 5) :
57+
nh_(nh) {
58+
sync_.reset(new SyncT(topics, callback, maxQueueSize));
59+
// initialize topics
60+
TopicInitializer ti;
61+
(void) for_each(topics_, &ti);
62+
}
63+
64+
std::shared_ptr<SyncT> getSync() {
65+
return (sync_);
66+
}
67+
68+
ros::NodeHandle &getNodeHandle() {
69+
return (nh_);
70+
}
71+
72+
private:
73+
struct TopicInitializer {
74+
template<std::size_t I>
75+
int operate(LiveSync<SyncT> *liveSync) const
76+
{
77+
std::shared_ptr<SyncT> sync = liveSync->getSync();
78+
auto &topics = sync->getTopics();
79+
auto &topic_vec = std::get<I>(liveSync->topics_);
80+
//std::cout << "creating topic for " << I
81+
//<< " num: " << topics[I].size() << std::endl;
82+
ros::NodeHandle &nh = liveSync->getNodeHandle();
83+
const unsigned int qs = sync->getQueueSize();
84+
for (const std::string &topic: topics[I]) {
85+
// get vector type-> pointer type -> pointer element type
86+
typedef typename get_type<
87+
I, TupleOfTopicVec>::type::value_type::element_type LiveTopicT;
88+
std::shared_ptr<LiveTopicT> lt(
89+
new LiveTopicT(topic, nh, qs, sync));
90+
topic_vec.push_back(lt);
91+
}
92+
return (topic_vec.size());
93+
}
94+
};
95+
96+
// some neat template tricks picked up here:
97+
// https://stackoverflow.com/questions/18063451/get-index-of-a-tuple-elements -type
98+
// This template terminates the recursion
99+
template<std::size_t I = 0, typename FuncT, typename... Tp>
100+
inline typename std::enable_if<I == sizeof...(Tp), int>::type
101+
for_each(std::tuple<Tp...> &, FuncT *) // Unused arg needs no name
102+
{ return 0; } // do nothing
103+
104+
// This template recursively calls itself, thereby iterating
105+
template<std::size_t I = 0, typename FuncT, typename... Tp>
106+
inline typename std::enable_if<I < sizeof...(Tp), int>::type
107+
for_each(std::tuple<Tp...>& t, FuncT *f) {
108+
const int rv = (*f).template operate<I>(this);
109+
return (rv + for_each<I + 1, FuncT, Tp...>(t, f));
110+
}
111+
// The following templates return the N'th type of a tuple.
112+
// source:
113+
// https://stackoverflow.com/questions/16928669/how-to-get-n-th-type-from-a-tuple
114+
template <int N, typename... Ts> struct get_type;
115+
template <int N, typename T, typename... Ts>
116+
struct get_type<N, std::tuple<T, Ts...>> {
117+
using type = typename get_type<N - 1, std::tuple<Ts...>>::type;
118+
};
119+
template <typename T, typename... Ts>
120+
struct get_type<0, std::tuple<T, Ts...>> {
121+
using type = T;
122+
};
123+
124+
// -------------- variables -------------
125+
std::shared_ptr<SyncT> sync_;
126+
TupleOfTopicVec topics_;
127+
ros::NodeHandle nh_;
128+
};
129+
130+
}

‎include/flex_sync/msg_pack.h

+15
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
/* -*-c++-*--------------------------------------------------------------------
2+
* 2020 Bernd Pfrommer [email protected]
3+
*/
4+
#ifndef FLEX_SYNC_MSG_PACK_H
5+
#define FLEX_SYNC_MSG_PACK_H
6+
7+
/*
8+
* helper class for type deduction
9+
*/
10+
11+
namespace flex_sync {
12+
template <typename... Args>
13+
struct MsgPack {}; // for exposing message types
14+
}
15+
#endif // FLEX_SYNC_MSG_PACK_H

0 commit comments

Comments
 (0)