From 40106a81924e452876deaeef0fd3ed764f8d73c1 Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Thu, 19 Sep 2019 17:58:31 +0200 Subject: [PATCH] Doxygen updates comm part 1 --- .../include/ur_rtde_driver/comm/bin_parser.h | 141 +++++++++++++++++- .../include/ur_rtde_driver/comm/package.h | 13 ++ .../ur_rtde_driver/comm/package_serializer.h | 29 ++++ .../include/ur_rtde_driver/comm/parser.h | 6 +- .../include/ur_rtde_driver/comm/pipeline.h | 125 ++++++++++++++++ 5 files changed, 308 insertions(+), 6 deletions(-) diff --git a/ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h b/ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h index 280f620..f20bcf3 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h +++ b/ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h @@ -36,6 +36,10 @@ namespace ur_driver { namespace comm { +/*! + * \brief The BinParser class handles a byte buffer and functionality to iteratively parse the + * content. + */ class BinParser { private: @@ -74,21 +78,44 @@ private: } public: + /*! + * \brief Creates a new BinParser object from a given buffer. + * + * \param buffer The byte buffer to parse + * \param buf_len Size of the buffer + */ BinParser(uint8_t* buffer, size_t buf_len) : buf_pos_(buffer), buf_end_(buffer + buf_len), parent_(*this) { assert(buf_pos_ <= buf_end_); } + /*! + * \brief Creates a new BinParser object for part of a buffer from a parent BinParser. + * + * \param parent Parent BinParser + * \param sub_len Size of the sub-buffer to parse + */ BinParser(BinParser& parent, size_t sub_len) : buf_pos_(parent.buf_pos_), buf_end_(parent.buf_pos_ + sub_len), parent_(parent) { assert(buf_pos_ <= buf_end_); } + /*! + * \brief Deconstructor for the BinParser. + */ ~BinParser() { parent_.buf_pos_ = buf_pos_; } + + /*! + * \brief Parses the next bytes as given type without moving the buffer pointer. + * + * @tparam T Type to parse as + * + * \returns Value of the next bytes as type T + */ template T peek() { @@ -98,6 +125,12 @@ public: return decode(val); } + /*! + * \brief Parses the next bytes as given type. + * + * @tparam T Type to parse as + * \param val Reference to write the parsed value to + */ template void parse(T& val) { @@ -105,12 +138,22 @@ public: buf_pos_ += sizeof(T); } + /*! + * \brief Parses the next bytes as a double. + * + * \param val Reference to write the parsed value to + */ void parse(double& val) { uint64_t inner; parse(inner); std::memcpy(&val, &inner, sizeof(double)); } + /*! + * \brief Parses the next bytes as a float. + * + * \param val Reference to write the parsed value to + */ void parse(float& val) { uint32_t inner; @@ -118,8 +161,15 @@ public: std::memcpy(&val, &inner, sizeof(float)); } - // UR uses 1 byte for boolean values but sizeof(bool) is implementation - // defined so we must ensure they're parsed as uint8_t on all compilers + /*! + * \brief Parses the next byte as a bool. + * + * UR uses 1 byte for boolean values but sizeof(bool) is implementation defined, so we must ensure + * they're parsed as uint8_t on all compilers + * + * + * \param val Reference to write the parsed value to + */ void parse(bool& val) { uint8_t inner; @@ -127,6 +177,11 @@ public: val = inner != 0; } + /*! + * \brief Parses the next bytes as a vector of 3 doubles. + * + * \param val Reference to write the parsed value to + */ void parse(vector3d_t& val) { for (size_t i = 0; i < val.size(); ++i) @@ -135,6 +190,11 @@ public: } } + /*! + * \brief Parses the next bytes as a vector of 6 doubles. + * + * \param val Reference to write the parsed value to + */ void parse(vector6d_t& val) { for (size_t i = 0; i < val.size(); ++i) @@ -143,6 +203,11 @@ public: } } + /*! + * \brief Parses the next bytes as a vector of 6 32 bit integers. + * + * \param val Reference to write the parsed value to + */ void parse(vector6int32_t& val) { for (size_t i = 0; i < val.size(); ++i) @@ -151,6 +216,11 @@ public: } } + /*! + * \brief Parses the next bytes as a vector of 6 unsigned 32 bit integers. + * + * \param val Reference to write the parsed value to + */ void parse(vector6uint32_t& val) { for (size_t i = 0; i < val.size(); ++i) @@ -159,6 +229,12 @@ public: } } + /*! + * \brief Writes the remaining bytes into a given buffer without parsing them. + * + * \param buffer The buffer to copy the remaining bytes to. + * \param buffer_length Reference to write the number of remaining bytes to. + */ void rawData(std::unique_ptr& buffer, size_t& buffer_length) { buffer_length = buf_end_ - buf_pos_; @@ -167,18 +243,33 @@ public: consume(); } + /*! + * \brief Parses the remaining bytes as a string. + * + * \param val Reference to write the parsed value to + */ void parseRemainder(std::string& val) { parse(val, size_t(buf_end_ - buf_pos_)); } + /*! + * \brief Parses a given number of bytes as a string. + * + * \param val Reference to write the parsed value to + * \param len Number of bytes to parse + */ void parse(std::string& val, size_t len) { val.assign(reinterpret_cast(buf_pos_), len); buf_pos_ += len; } - // Special string parse function that assumes uint8_t len followed by chars + /*! + * \brief Special string parse function that assumes uint8_t len followed by chars + * + * \param val Reference to write the parsed value to + */ void parse(std::string& val) { uint8_t len; @@ -186,6 +277,13 @@ public: parse(val, size_t(len)); } + /*! + * \brief Parses the next bytes as an array of a given type and size. + * + * @tparam T Type of the array + * @tparam N Number of elements in the array + * \param array Reference of an array to parse to. + */ template void parse(std::array& array) { @@ -195,6 +293,13 @@ public: } } + /*! + * \brief Parses the next bytes as a value of a given type, but also copies it to a bitset. + * + * @tparam T Type to parse as + * @tparam N Size of the bitset to copy to + * \param set Reference to the bitset to copy the value to. + */ template void parse(std::bitset& set) { @@ -203,30 +308,60 @@ public: set = std::bitset(val); } + /*! + * \brief Sets the current buffer position to the end of the buffer, finishing parsing. + */ void consume() { buf_pos_ = buf_end_; } + /*! + * \brief Moves the current buffer position ahead by a given amount. + * + * \param bytes Number of bytes to move the buffer position + */ void consume(size_t bytes) { buf_pos_ += bytes; } + /*! + * \brief Checks if at least a given number of bytes is still remaining unparsed in the buffer. + * + * \param bytes Number of bytes to check for + * + * \returns True, if at least the given number of bytes are unparsed, false otherwise. + */ bool checkSize(size_t bytes) { return bytes <= size_t(buf_end_ - buf_pos_); } + /*! + * \brief Checks if enough bytes for a given type remain unparsed in the buffer. + * + * @tparam T The type to check for + * + * \returns True, if enough bytes are unparsed, false otherwise. + */ template bool checkSize(void) { return checkSize(T::SIZE); } + /*! + * \brief Checks if no unparsed bytes remain in the buffer. + * + * \returns True, if all bytes were parsed, false otherwise. + */ bool empty() { return buf_pos_ == buf_end_; } + /*! + * \brief Logs debugging information about the BinParser object. + */ void debug() { LOG_DEBUG("BinParser: %p - %p (%zu bytes)", buf_pos_, buf_end_, buf_end_ - buf_pos_); diff --git a/ur_rtde_driver/include/ur_rtde_driver/comm/package.h b/ur_rtde_driver/include/ur_rtde_driver/comm/package.h index 4e2aebf..8f868b8 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/comm/package.h +++ b/ur_rtde_driver/include/ur_rtde_driver/comm/package.h @@ -49,8 +49,21 @@ public: URPackage() = default; virtual ~URPackage() = default; + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized version of the package + * + * \returns true, if the package was parsed successfully, false otherwise + */ virtual bool parseWith(BinParser& bp) = 0; + /*! + * \brief Produces a human readable representation of the package obejct. + * + * \returns A string representing the object + */ virtual std::string toString() const = 0; private: diff --git a/ur_rtde_driver/include/ur_rtde_driver/comm/package_serializer.h b/ur_rtde_driver/include/ur_rtde_driver/comm/package_serializer.h index a337ebf..a0f4cb0 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/comm/package_serializer.h +++ b/ur_rtde_driver/include/ur_rtde_driver/comm/package_serializer.h @@ -35,9 +35,22 @@ namespace ur_driver { namespace comm { +/*! + * \brief A helper class to serialize packages. Contains methods for serializing all relevant + * datatypes. + */ class PackageSerializer { public: + /*! + * \brief A generalized serialization method for arbitrary datatypes. + * + * @tparam T The type to serialize + * \param buffer The buffer to write the serialization into + * \param val The value to serialize + * + * \returns Size in byte of the serialization + */ template static size_t serialize(uint8_t* buffer, T val) { @@ -47,6 +60,14 @@ public: return size; } + /*! + * \brief A serialization method for double values. + * + * \param buffer The buffer to write the serialization into. + * \param val The value to serialize. + * + * \returns Size in byte of the serialization. + */ static size_t serialize(uint8_t* buffer, double val) { size_t size = sizeof(double); @@ -57,6 +78,14 @@ public: return size; } + /*! + * \brief A serialization method for strings. + * + * \param buffer The buffer to write the serialization into. + * \param val The string to serialize. + * + * \returns Size in byte of the serialization. + */ static size_t serialize(uint8_t* buffer, std::string val) { const uint8_t* c_val = reinterpret_cast(val.c_str()); diff --git a/ur_rtde_driver/include/ur_rtde_driver/comm/parser.h b/ur_rtde_driver/include/ur_rtde_driver/comm/parser.h index ab20909..8b0cab3 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/comm/parser.h +++ b/ur_rtde_driver/include/ur_rtde_driver/comm/parser.h @@ -41,10 +41,10 @@ public: virtual ~Parser() = default; /*! - * \brief declares the parse function. + * \brief Declares the parse function. * - * \param bp instant of class binaryParser - * \param results unique pointer + * \param bp Instance of class binaryParser + * \param results A unique pointer */ virtual bool parse(BinParser& bp, std::vector>>& results) = 0; using _header_type = HeaderT; diff --git a/ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h b/ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h index 48a64dc..94deb26 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h +++ b/ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h @@ -36,27 +36,57 @@ namespace comm // TODO: Remove these!!! using namespace moodycamel; +/*! + * \brief Parent class for for arbitrary consumers. + * + * @tparam T Type of the consumed products + */ template class IConsumer { public: + /*! + * \brief Set-up functionality of the consumer. + */ virtual void setupConsumer() { } + /*! + * \brief Fully tears down the consumer - by default no difference to stopping it. + */ virtual void teardownConsumer() { stopConsumer(); } + /*! + * \brief Stops the consumer. + */ virtual void stopConsumer() { } + /*! + * \brief Functionality for handling consumer timeouts. + */ virtual void onTimeout() { } + /*! + * \brief Consumes a product, utilizing it's contents. + * + * \param product Shared pointer to the product to be consumed. + * + * \returns Success of the consumption. + */ virtual bool consume(std::shared_ptr product) = 0; }; +/*! + * \brief Consumer, that allows one product to be consumed by multiple arbitrary + * conusmers. + * + * @tparam T Type of the consumed products + */ template class MultiConsumer : public IConsumer { @@ -64,10 +94,18 @@ private: std::vector*> consumers_; public: + /*! + * \brief Creates a new MultiConsumer object. + * + * \param consumers The list of consumers that should all consume given products + */ MultiConsumer(std::vector*> consumers) : consumers_(consumers) { } + /*! + * \brief Sets up all registered consumers. + */ virtual void setupConsumer() { for (auto& con : consumers_) @@ -75,6 +113,9 @@ public: con->setupConsumer(); } } + /*! + * \brief Tears down all registered consumers. + */ virtual void teardownConsumer() { for (auto& con : consumers_) @@ -82,6 +123,9 @@ public: con->teardownConsumer(); } } + /*! + * \brief Stops all registered consumers. + */ virtual void stopConsumer() { for (auto& con : consumers_) @@ -89,6 +133,9 @@ public: con->stopConsumer(); } } + /*! + * \brief Triggers timeout functionality for all registered consumers. + */ virtual void onTimeout() { for (auto& con : consumers_) @@ -97,6 +144,13 @@ public: } } + /*! + * \brief Consumes a given product with all registered consumers. + * + * \param product Shared pointer to the product to be consumed. + * + * \returns Success of the consumption. + */ bool consume(std::shared_ptr product) { bool res = true; @@ -109,35 +163,72 @@ public: } }; +/*! + * \brief Parent class for arbitrary producers of packages. + * + * @tparam HeaderT Header type of the produced packages + */ template class IProducer { public: + /*! + * \brief Set-up functionality of the producers. + */ virtual void setupProducer() { } + /*! + * \brief Fully tears down the producer - by default no difference to stopping it. + */ virtual void teardownProducer() { stopProducer(); } + /*! + * \brief Stops the producer. + */ virtual void stopProducer() { } + /*! + * \brief Reads packages from some source and produces corresponding objects. + * + * \param products Vector of unique pointers to be filled with produced packages. + * + * \returns Success of the package production. + */ virtual bool tryGet(std::vector>>& products) = 0; }; +/*! + * \brief Parent class for notifiers. + */ class INotifier { public: + /*! + * \brief Start notification. + */ virtual void started(std::string name) { } + /*! + * \brief Stop notification. + */ virtual void stopped(std::string name) { } }; +/*! + * \brief The Pipepline manages the production and optionally consumption of packages. Cyclically + * the producer is called and returned packages are saved in a queue. This queue is then either also + * cyclically utilized by the registered consumer or can be externally used. + * + * @tparam HeaderT Header type of the managed packages + */ template class Pipeline { @@ -145,21 +236,44 @@ public: typedef std::chrono::high_resolution_clock Clock; typedef Clock::time_point Time; using _package_type = URPackage; + /*! + * \brief Creates a new Pipeline object, registering producer, consumer and notifier. + * Additionally, an empty queue is initialized. + * + * \param producer The producer to run in the pipeline + * \param consumer The consumer to run in the pipeline + * \param name The pipeline's name + * \param notifier The notifier to use + */ Pipeline(IProducer& producer, IConsumer<_package_type>& consumer, std::string name, INotifier& notifier) : producer_(producer), consumer_(&consumer), name_(name), notifier_(notifier), queue_{ 32 }, running_{ false } { } + /*! + * \brief Creates a new Pipeline object, registering producer and notifier while no consumer is + * used. Additionally, an empty queue is initialized. + * + * \param producer The producer to run in the pipeline + * \param name The pipeline's name + * \param notifier The notifier to use + */ Pipeline(IProducer& producer, std::string name, INotifier& notifier) : producer_(producer), consumer_(nullptr), name_(name), notifier_(notifier), queue_{ 32 }, running_{ false } { } + /*! + * \brief The Pipeline object's destructor, stopping the pipeline and joining all running threads. + */ virtual ~Pipeline() { LOG_DEBUG("Destructing pipeline"); stop(); } + /*! + * \brief Starts the producer and, if existing, the consumer in new threads. + */ void run() { if (running_) @@ -173,6 +287,9 @@ public: notifier_.started(name_); } + /*! + * \brief Stops the pipeline and all running threads. + */ void stop() { if (!running_) @@ -193,6 +310,14 @@ public: notifier_.stopped(name_); } + /*! + * \brief Returns the next package in the queue. Can be used instead of registering a consumer. + * + * \param product Unique pointer to be set to the package + * \param timeout Time to wait if no package is in the queue before returning + * + * \returns + */ bool getLatestProduct(std::unique_ptr>& product, std::chrono::milliseconds timeout) { return queue_.waitDequeTimed(product, timeout);