15 #include <boost/bind.hpp> 16 #include <boost/bind/placeholders.hpp> 26 #include <topic_tools/shape_shifter.h> 37 const size_t inQueueSize,
const size_t outQueueSize, CallbackType callback,
40 topicIn(topicIn), topicOut(topicOut), callback(
std::move(callback)), subscribeOptions(
std::move(subscribeOptions)),
41 inQueueSize(inQueueSize), outQueueSize(outQueueSize), nhIn(nhIn), nhOut(nhOut)
44 std::lock_guard<std::mutex> lock(this->connectMutex);
45 this->connectNoLock();
51 opts.template initByFullCallbackType<const ros::MessageEvent<topic_tools::ShapeShifter const>&>(
53 sub = this->nhIn.subscribe(opts);
60 return this->pub.getNumSubscribers() > 0;
67 this->updateSubscription();
72 const auto&
msg =
event.getConstMessage();
75 std::lock_guard<std::mutex> pubLock(this->pubCreateMutex);
79 CRAS_INFO(
"Creating%s publisher on %s with type %s.",
80 (this->advertiseOptions->latch ?
" latched" :
""),
81 this->nhOut.resolveName(this->topicOut).c_str(),
82 msg->getDataType().c_str());
85 std::lock_guard<std::mutex> lock(this->connectMutex);
86 this->pub = this->nhOut.advertise(this->advertiseOptions.value());
90 for (
size_t i = 0; i < 100 &&
ros::ok() && this->pub.getNumSubscribers() == 0; ++i)
92 this->updateSubscription();
95 this->callback(event, this->pub);
101 const auto&
msg =
event.getConstMessage();
104 msg->getMessageDefinition(),
cb,
cb);
108 const auto header =
event.getConnectionHeader();
109 opts.latch =
header.find(
"latching") !=
header.end() &&
event.getConnectionHeader()[
"latching"] ==
"1";
115 const ros::NodeHandle& nhOut,
const std::string& topicOut,
size_t queueSize,
119 nhIn, topicIn, nhOut, topicOut, queueSize, queueSize, std::move(callback), std::move(subscribeOptions), logHelper)
126 GenericLazyPubSub(nhIn, topicIn, nhOut, topicOut, 10, std::move(callback), std::move(subscribeOptions), logHelper)
134 nh, topicIn, nh, topicOut, inQueueSize, outQueueSize, std::move(callback), std::move(subscribeOptions), logHelper)
142 nh, topicIn, topicOut, queueSize, queueSize, std::move(callback), std::move(subscribeOptions), logHelper)
149 GenericLazyPubSub(nh, topicIn, topicOut, 10, std::move(callback), std::move(subscribeOptions), logHelper)
void cb(const ::ros::MessageEvent<::topic_tools::ShapeShifter const > &event)
Callback for the received messages from subscriber. It also handles publisher creation if none exists...
void subscribe(::ros::Subscriber &sub)
Perform the subscription to the input topic.
virtual void cb(const ::ros::MessageEvent<::topic_tools::ShapeShifter const > &event)
Callback for counting the messages.
Tools for more convenient working with ShapeShifter objects.
A pair of lazy subscriber and publisher which use the same message type (unknown at compile time)...
auto bind_front(F &&f, Args &&... args)
Lazy subscriber that subscribes only when a paired publisher has subscribers. Version for unknown mes...
virtual ::ros::AdvertiseOptions createAdvertiseOptions(const ::ros::MessageEvent<::topic_tools::ShapeShifter const > &event)
Create ros::AdvertiseOptions from a message event.
::ros::Subscriber sub
The message subscriber.
::boost::function< void(::cras::GenericLazyPubSub::CallbackMessageType, ::ros::Publisher &)> CallbackType
Type of the user callback passed to this class.
Lazy subscriber that subscribes only when a paired publisher has subscribers.
ros::AdvertiseOptions createAdvertiseOptions(MessageInstance const &msg, uint32_t queue_size, const std::string &prefix="")
bool hasHeader(const ::topic_tools::ShapeShifter &msg)
Tell whether the given message has header field.
GenericLazyPubSub(const ::ros::NodeHandle &nhIn, const ::std::string &topicIn, const ::ros::NodeHandle &nhOut, const ::std::string &topicOut, size_t inQueueSize, size_t outQueueSize, CallbackType callback, ::ros::SubscribeOptions subscribeOptions={}, const ::cras::LogHelperPtr &logHelper=::std::make_shared<::cras::NodeLogHelper >())
Create the pair of lazy subscriber and publisher.
const boost::shared_ptr< M_string > & getConnectionHeaderPtr() const
void connectCb(const ::ros::SingleSubscriberPublisher &)
Callback that is called whenever someone (un)subscribes from the publisher.
bool shouldBeSubscribed() const override
Returns true when the subscriber should be connected - i.e. either at the start, or when pub has clie...
::cras::LogHelper::Ptr LogHelperPtr