diff --git a/src/ros_queued_subscribe-python-module-py.cc b/src/ros_queued_subscribe-python-module-py.cc index f5c9632bb31da513d2874d5ba52a47c3c3e43721..fa2936a61818596bf55c0d422f05dec6ef2033c9 100644 --- a/src/ros_queued_subscribe-python-module-py.cc +++ b/src/ros_queued_subscribe-python-module-py.cc @@ -24,5 +24,11 @@ BOOST_PYTHON_MODULE(wrap) { "Return the queue size of a given signal", bp::args("signal_name")) .def("readQueue", &dg::RosQueuedSubscribe::readQueue, "Whether signals should read values from the queues, and when.", - bp::args("start_time")); + bp::args("start_time")) + .def("queueReceivedData", &dg::RosQueuedSubscribe::queueReceivedData, + "Check whether the queue of a given signal has received atleast one data point", + bp::args("signal_name")) + .def("setQueueReceivedData", &dg::RosQueuedSubscribe::setQueueReceivedData, + "Set the data reception status of the queue corresponding to a given signal", + bp::args("signal_name","signal_data_acq_status")); } diff --git a/src/ros_queued_subscribe.cpp b/src/ros_queued_subscribe.cpp index d67f69d997da48c960ef5428fa4ee7d8a66ce019..35b82a60a040bcac0f05e1234b792809221a493a 100644 --- a/src/ros_queued_subscribe.cpp +++ b/src/ros_queued_subscribe.cpp @@ -147,4 +147,21 @@ void RosQueuedSubscribe::readQueue(int beginReadingAt) { } std::string RosQueuedSubscribe::getDocString() const { return docstring_; } -} // end of namespace dynamicgraph. + +bool RosQueuedSubscribe::queueReceivedData(const std::string& signal) const{ + std::map<std::string, bindedSignal_t>::const_iterator _bs = bindedSignal_.find(signal); + if (_bs != bindedSignal_.end()) { + return _bs->second->receivedData(); + } + + return false; +} + +void RosQueuedSubscribe::setQueueReceivedData(const std::string& signal, bool status) { + std::map<std::string, bindedSignal_t>::const_iterator _bs = bindedSignal_.find(signal); + if (_bs != bindedSignal_.end()) { + _bs->second->receivedData(status); + } +} + +} // end of namespace dynamicgraph. \ No newline at end of file diff --git a/src/ros_queued_subscribe.hh b/src/ros_queued_subscribe.hh index a97e9a211086c4a6679565c7a699dc91559617d8..46a09906d80b53c16a24c761b1e413ce23e48da3 100644 --- a/src/ros_queued_subscribe.hh +++ b/src/ros_queued_subscribe.hh @@ -57,6 +57,9 @@ struct BindedSignalBase { virtual void clear() = 0; virtual std::size_t size() const = 0; + virtual bool receivedData() const = 0; + virtual void receivedData(bool) = 0; + Subscriber_t subscriber; RosQueuedSubscribe* entity; }; @@ -73,7 +76,8 @@ struct BindedSignal : BindedSignalBase { frontIdx(0), backIdx(0), buffer(BufferSize), - init(false) {} + init(false), + receivedData_(false) {} ~BindedSignal() { signal.reset(); clear(); @@ -107,6 +111,15 @@ struct BindedSignal : BindedSignalBase { return backIdx + BufferSize - frontIdx; } + /// @brief Returns the value stored in receivedData_ i.e. + /// whether the signal has received atleast one data point + /// or not + bool receivedData() const {return receivedData_;} + + /// @brief Set the value of data acquisition status of the signal + /// @param status + void receivedData(bool status) {receivedData_ = status;} + SignalPtr_t signal; /// Index of the next value to be read. size_type frontIdx; @@ -120,6 +133,10 @@ struct BindedSignal : BindedSignalBase { template <typename R> void writer(const R& data); T& reader(T& val, int time); + + private: + /// Indicates whether the signal has received atleast one data point + bool receivedData_; }; } // namespace internal @@ -144,6 +161,8 @@ class RosQueuedSubscribe : public dynamicgraph::Entity { void clearQueue(const std::string& signal); void readQueue(int beginReadingAt); std::size_t queueSize(const std::string& signal) const; + bool queueReceivedData(const std::string& signal) const; + void setQueueReceivedData(const std::string& signal, bool status); template <typename T> void add(const std::string& type, const std::string& signal, diff --git a/src/ros_queued_subscribe.hxx b/src/ros_queued_subscribe.hxx index 7a628fc8a9d5b8a557a94274835389582c2b2456..c1ebc461e7309dffec6cddd80cfca4934a152717 100644 --- a/src/ros_queued_subscribe.hxx +++ b/src/ros_queued_subscribe.hxx @@ -82,6 +82,12 @@ void BindedSignal<T, N>::writer(const R& data) { last = buffer[backIdx]; init = true; } + // Updating the status flag to indicate that the signal + // has received atleast one data point. + if (!receivedData_) { + receivedData_ = true; + } + backIdx = (backIdx + 1) % N; }