From a1c53caef33df4ba1f391bf968a8c6890eefbde0 Mon Sep 17 00:00:00 2001
From: Felix Mauch <mauch@fzi.de>
Date: Thu, 18 Jul 2019 15:11:57 +0200
Subject: [PATCH] added function to get rtde data as bitset

---
 .../ur_rtde_driver/rtde/data_package.h        | 28 ++++++++++++++++++-
 1 file changed, 27 insertions(+), 1 deletion(-)

diff --git a/ur_rtde_driver/include/ur_rtde_driver/rtde/data_package.h b/ur_rtde_driver/include/ur_rtde_driver/rtde/data_package.h
index 2a7cab8..fbbe8e0 100644
--- a/ur_rtde_driver/include/ur_rtde_driver/rtde/data_package.h
+++ b/ur_rtde_driver/include/ur_rtde_driver/rtde/data_package.h
@@ -82,7 +82,6 @@ public:
   virtual bool parseWith(comm::BinParser& bp);
   virtual std::string toString() const;
 
-  template <typename T>
   /*!
    * \brief Get a data field from the DataPackage.
    *
@@ -93,6 +92,7 @@ public:
    *
    * \returns True on success, false if the field cannot be found inside the package.
    */
+  template <typename T>
   bool getData(const std::string& name, T& val)
   {
     if (data_.find(name) != data_.end())
@@ -107,6 +107,32 @@ public:
     return true;
   }
 
+  /*!
+   * \brief Get a data field from the DataPackage as bitset
+   *
+   * The data package contains a lot of different data fields, depending on the recipe.
+   *
+   * \param name The string identifier for the data field as used in the documentation.
+   * \param val Target variable. Make sure, it's the correct type.
+   *
+   * \returns True on success, false if the field cannot be found inside the package.
+   */
+  template <typename T, size_t N>
+  bool getData(const std::string& name, std::bitset<N>& val)
+  {
+    static_assert(sizeof(T) * 8 >= N, "Bitset is too large for underlying variable");
+
+    if (data_.find(name) != data_.end())
+    {
+      val = std::bitset<N>(boost::strict_get<T>(data_[name]));
+    }
+    else
+    {
+      return false;
+    }
+    return true;
+  }
+
 private:
   // Const would be better here
   static std::unordered_map<std::string, _rtde_type_variant> g_type_list;