added input datatypes and necessary functionality for RTDE write to the data package class

ur_rtde_driver/include/ur_rtde_driver/rtde/data_package.h

@@ -66,6 +66,22 @@ struct StringVisitor : public boost::static_visitor<std::string>
     return ss.str();
   }
 };
+struct SizeVisitor : public boost::static_visitor<uint16_t>
+{
+  template <typename T>
+  uint16_t operator()(T& d) const
+  {
+    return sizeof(d);
+  }
+};
+struct SerializeVisitor : public boost::static_visitor<size_t>
+{
+  template <typename T>
+  size_t operator()(T& d, uint8_t* buffer) const
+  {
+    return comm::PackageSerializer::serialize(buffer, d);
+  }
+};
 
 class DataPackage : public RTDEPackage
 {
@@ -79,9 +95,13 @@ public:
   }
   virtual ~DataPackage() = default;
 
+  void initEmpty();
+
   virtual bool parseWith(comm::BinParser& bp);
   virtual std::string toString() const;
 
+  size_t serializePackage(uint8_t* buffer);
+
   /*!
    * \brief Get a data field from the DataPackage.
    *
@@ -133,6 +153,35 @@ public:
     return true;
   }
 
+  /*!
+   * \brief Set a data field in the DataPackage.
+   *
+   * 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 Value to set. Make sure, it's the correct type.
+   *
+   * \returns True on success, false if the field cannot be found inside the package.
+   */
+  template <typename T>
+  bool setData(const std::string& name, T& val)
+  {
+    if (data_.find(name) != data_.end())
+    {
+      data_[name] = val;
+    }
+    else
+    {
+      return false;
+    }
+    return true;
+  }
+
+  void setRecipeID(const uint8_t& recipe_id)
+  {
+    recipe_id_ = recipe_id;
+  }
+
 private:
   // Const would be better here
   static std::unordered_map<std::string, _rtde_type_variant> g_type_list;

ur_rtde_driver/src/rtde/data_package.cpp

@@ -138,6 +138,30 @@ std::unordered_map<std::string, DataPackage::_rtde_type_variant> DataPackage::g_
   { "output_double_register_23", double() },
   { "input_bit_registers0_to_31", uint32_t() },
   { "input_bit_registers32_to_63", uint32_t() },
+  { "input_bit_register_0", bool() },
+  { "input_bit_register_1", bool() },
+  { "input_bit_register_2", bool() },
+  { "input_bit_register_3", bool() },
+  { "input_bit_register_4", bool() },
+  { "input_bit_register_5", bool() },
+  { "input_bit_register_6", bool() },
+  { "input_bit_register_7", bool() },
+  { "input_bit_register_8", bool() },
+  { "input_bit_register_9", bool() },
+  { "input_bit_register_10", bool() },
+  { "input_bit_register_11", bool() },
+  { "input_bit_register_12", bool() },
+  { "input_bit_register_13", bool() },
+  { "input_bit_register_14", bool() },
+  { "input_bit_register_15", bool() },
+  { "input_bit_register_16", bool() },
+  { "input_bit_register_17", bool() },
+  { "input_bit_register_18", bool() },
+  { "input_bit_register_19", bool() },
+  { "input_bit_register_20", bool() },
+  { "input_bit_register_21", bool() },
+  { "input_bit_register_22", bool() },
+  { "input_bit_register_23", bool() },
   { "input_int_register_0", int32_t() },
   { "input_int_register_1", int32_t() },
   { "input_int_register_2", int32_t() },
@@ -185,9 +209,31 @@ std::unordered_map<std::string, DataPackage::_rtde_type_variant> DataPackage::g_
   { "input_double_register_20", double() },
   { "input_double_register_21", double() },
   { "input_double_register_22", double() },
-  { "input_double_register_23", double() }
+  { "input_double_register_23", double() },
+  { "speed_slider_mask", uint32_t() },
+  { "speed_slider_fraction", double() },
+  { "standard_digital_output_mask", uint8_t() },
+  { "standard_digital_output", uint8_t() },
+  { "configurable_digital_output_mask", uint8_t() },
+  { "configurable_digital_output", uint8_t() },
+  { "tool_digital_output_mask", uint8_t() },
+  { "tool_digital_output", uint8_t() },
+  { "standard_analog_output_mask", uint8_t() },
+  { "standard_analog_output_type", uint8_t() },
 };
 
+void rtde_interface::DataPackage::initEmpty()
+{
+  for (auto& item : recipe_)
+  {
+    if (g_type_list.find(item) != g_type_list.end())
+    {
+      _rtde_type_variant entry = g_type_list[item];
+      data_[item] = entry;
+    }
+  }
+}
+
 bool rtde_interface::DataPackage ::parseWith(comm::BinParser& bp)
 {
   bp.parse(recipe_id_);
@@ -218,5 +264,25 @@ std::string rtde_interface::DataPackage::toString() const
   }
   return ss.str();
 }
+
+size_t rtde_interface::DataPackage::serializePackage(uint8_t* buffer)
+{
+  uint16_t payload_size = sizeof(recipe_id_);
+
+  for (auto& item : data_)
+  {
+    payload_size += boost::apply_visitor(SizeVisitor{}, item.second);
+  }
+  size_t size = 0;
+  size += PackageHeader::serializeHeader(buffer, PackageType::RTDE_DATA_PACKAGE, payload_size);
+  size += comm::PackageSerializer::serialize(buffer + size, recipe_id_);
+  for (auto& item : recipe_)
+  {
+    auto bound_visitor = std::bind(SerializeVisitor(), std::placeholders::_1, buffer + size);
+    size += boost::apply_visitor(bound_visitor, data_[item]);
+  }
+
+  return size;
+}
 }  // namespace rtde_interface
 }  // namespace ur_driver