Add sequences support

Signed-off-by: Juan López Fernández <juanlopez@eprosima.com>

plugins/datastreamer_plugin/datastreamer/FastDdsDataStreamer.cpp

@@ -97,23 +97,7 @@ bool FastDdsDataStreamer::start(
     }
 
     // Get all series from topics and create them
-    // NUMERIC
-    std::vector<types::DatumLabel> numeric_series = fastdds_handler_.numeric_data_series_names();
-    for (const auto& series : numeric_series)
-    {
-        // Create a series
-        DEBUG("Creating numeric series: " << series);
-        dataMap().addNumeric(series);
-    }
-
-    // STRING
-    std::vector<types::DatumLabel> string_series = fastdds_handler_.string_data_series_names();
-    for (const auto& series : string_series)
-    {
-        // Create a series
-        DEBUG("Creating string series: " << series);
-        dataMap().addStringSeries(series);
-    }
+    create_series_();
 
     running_ = true;
     return true;
@@ -161,6 +145,20 @@ bool FastDdsDataStreamer::xmlLoadState(
 // FASTDDS LISTENER METHODS
 ////////////////////////////////////////////////////
 
+void FastDdsDataStreamer::on_data_available()
+{
+    DEBUG("FastDdsDataStreamer on_data_available");
+
+    // Locking DataStream
+    std::lock_guard<std::mutex> lock(mutex());
+
+    // Clear data created from previous sample
+    dataMap().clear();
+
+    // Create series from new received sample
+    create_series_();
+}
+
 void FastDdsDataStreamer::on_double_data_read(
         const std::vector<std::pair<std::string, double>>& data_per_topic_value,
         double timestamp)
@@ -255,6 +253,28 @@ void FastDdsDataStreamer::connect_to_domain_(
     select_topics_dialog_.connect_to_domain(domain_id);
 }
 
+void FastDdsDataStreamer::create_series_()
+{
+    // Get all series from topics and create them
+    // NUMERIC
+    std::vector<types::DatumLabel> numeric_series = fastdds_handler_.numeric_data_series_names();
+    for (const auto& series : numeric_series)
+    {
+        // Create a series
+        DEBUG("Creating numeric series: " << series);
+        dataMap().addNumeric(series);
+    }
+
+    // STRING
+    std::vector<types::DatumLabel> string_series = fastdds_handler_.string_data_series_names();
+    for (const auto& series : string_series)
+    {
+        // Create a series
+        DEBUG("Creating string series: " << series);
+        dataMap().addStringSeries(series);
+    }
+}
+
 } /* namespace datastreamer */
 } /* namespace plotjuggler */
 } /* namespace eprosima */

plugins/datastreamer_plugin/datastreamer/FastDdsDataStreamer.hpp

@@ -92,6 +92,8 @@ class FastDdsDataStreamer :
     // FASTDDS LISTENER METHODS
     ////////////////////////////////////////////////////
 
+    virtual void on_data_available() override;
+
     virtual void on_double_data_read(
             const std::vector<types::NumericDatum>& data_per_topic_value,
             double timestamp) override;
@@ -132,6 +134,8 @@ class FastDdsDataStreamer :
     void connect_to_domain_(
             unsigned int domain_id);
 
+    void create_series_();
+
 
     ////////////////////////////////////////////////////
     // INTERNAL VALUES

plugins/datastreamer_plugin/fastdds/FastDdsListener.hpp

@@ -41,6 +41,11 @@ class FastDdsListener
 {
 public:
 
+    virtual void on_data_available()
+    {
+        DEBUG("Calling on_data_available");
+    }
+
     virtual void on_double_data_read(
             const std::vector<types::NumericDatum>& numeric_data,
             double timestamp)

plugins/datastreamer_plugin/fastdds/ReaderHandler.cpp

@@ -48,36 +48,18 @@ ReaderHandler::ReaderHandler(
     , type_(type)
     , listener_(listener)
     , stop_(false)
+    , data_type_configuration_(data_type_configuration)
 {
     // Create data so it is not required to create it each time and avoid reallocation if possible
     data_ = eprosima::fastrtps::types::DynamicDataFactory::get_instance()->create_data(type_);
 
-    // Create the static structures to store the data introspection information AND the data itself
-    utils::get_introspection_type_names(
-        topic_name(),
-        type_,
-        data_type_configuration,
-        numeric_data_info_,
-        string_data_info_);
+    // Determine whether the data tree will be recreated for every received sample
+    static_type_ = utils::is_type_static(type);
 
-    // Create the data structures so they are not copied in the future
-    for (const auto& info : numeric_data_info_)
+    if (static_type_)
     {
-        numeric_data_.push_back({ std::get<0>(info), 0});
-    }
-    for (const auto& info : string_data_info_)
-    {
-        string_data_.push_back({ std::get<0>(info), "-"});
-    }
-
-    DEBUG("Reader created in topic: " << topic_name() << " with types: ");
-    for (const auto& info : numeric_data_info_)
-    {
-        DEBUG("\tNumeric: " << std::get<0>(info));
-    }
-    for (const auto& info : string_data_info_)
-    {
-        DEBUG("\tString: " << std::get<0>(info));
+        // Create the static structures to store the data introspection information AND the data itself
+        create_data_structures_();
     }
 
     // Set this object as this reader's listener
@@ -89,7 +71,7 @@ ReaderHandler::~ReaderHandler()
     // Stop the reader
     stop();
 
-    // Delete created data 
+    // Delete created data
     eprosima::fastrtps::types::DynamicDataFactory::get_instance()->delete_data(data_);
 }
 
@@ -115,6 +97,13 @@ void ReaderHandler::on_data_available(
     eprosima::fastrtps::types::ReturnCode_t read_ret =
             eprosima::fastrtps::types::ReturnCode_t::RETCODE_OK;
 
+    // Non-fixed size types require data to be recreated (e.g. to avoid having sequence members from previous samples)
+    if (!static_type_)
+    {
+        eprosima::fastrtps::types::DynamicDataFactory::get_instance()->delete_data(data_);
+        data_ = eprosima::fastrtps::types::DynamicDataFactory::get_instance()->create_data(type_);
+    }
+
     // Read Data from reader while there is data available and not should stop
     while (!stop_ && read_ret == eprosima::fastrtps::types::ReturnCode_t::RETCODE_OK)
     {
@@ -125,6 +114,23 @@ void ReaderHandler::on_data_available(
         if (read_ret == eprosima::fastrtps::types::ReturnCode_t::RETCODE_OK &&
                 info.instance_state == eprosima::fastdds::dds::InstanceStateKind::ALIVE_INSTANCE_STATE)
         {
+            if (!static_type_)
+            {
+                // Reset stored data info
+                numeric_data_info_.clear();
+                string_data_info_.clear();
+
+                // Reset stored data
+                numeric_data_.clear();
+                string_data_.clear();
+
+                // Recreate the structures to store the data introspection information AND the data itself
+                create_data_structures_(data_);
+
+                // Update previous data view according to new received data structure
+                listener_->on_data_available();
+            }
+
             // Get timestamp
             double timestamp = utils::get_timestamp_seconds_numeric_value(info.reception_timestamp);
 
@@ -172,6 +178,53 @@ const std::string& ReaderHandler::type_name() const
     return topic_->get_type_name();
 }
 
+std::vector<std::string> ReaderHandler::numeric_data_series_names() const
+{
+    return utils::get_introspection_type_names(numeric_data_info_);
+}
+
+std::vector<std::string> ReaderHandler::string_data_series_names() const
+{
+    return utils::get_introspection_type_names(string_data_info_);
+}
+
+////////////////////////////////////////////////////
+// AUXILIAR METHODS
+////////////////////////////////////////////////////
+
+void ReaderHandler::create_data_structures_(
+        eprosima::fastrtps::types::DynamicData* data /* = nullptr */)
+{
+    // Create the structures to store the data introspection information AND the data itself
+    utils::get_introspection_type_names(
+        topic_name(),
+        type_,
+        data_type_configuration_,
+        numeric_data_info_,
+        string_data_info_,
+        data);
+
+    // Create the data structures
+    for (const auto& info : numeric_data_info_)
+    {
+        numeric_data_.push_back({ std::get<0>(info), 0});
+    }
+    for (const auto& info : string_data_info_)
+    {
+        string_data_.push_back({ std::get<0>(info), "-"});
+    }
+
+    DEBUG("Completed type introspection created in topic: " << topic_name() << " with types: ");
+    for (const auto& info : numeric_data_info_)
+    {
+        DEBUG("\tNumeric: " << std::get<0>(info));
+    }
+    for (const auto& info : string_data_info_)
+    {
+        DEBUG("\tString: " << std::get<0>(info));
+    }
+}
+
 ////////////////////////////////////////////////////
 // AUXILIAR STATIC METHODS
 ////////////////////////////////////////////////////
@@ -187,16 +240,6 @@ eprosima::fastdds::dds::StatusMask ReaderHandler::default_listener_mask_()
     return mask;
 }
 
-std::vector<std::string> ReaderHandler::numeric_data_series_names() const
-{
-    return utils::get_introspection_type_names(numeric_data_info_);
-}
-
-std::vector<std::string> ReaderHandler::string_data_series_names() const
-{
-    return utils::get_introspection_type_names(string_data_info_);
-}
-
 } /* namespace fastdds */
 } /* namespace plotjuggler */
 } /* namespace eprosima */

plugins/datastreamer_plugin/fastdds/ReaderHandler.hpp

@@ -92,6 +92,14 @@ struct ReaderHandler : public eprosima::fastdds::dds::DataReaderListener
     std::vector<types::DatumLabel> string_data_series_names() const;
 
 
+    ////////////////////////////////////////////////////
+    // AUXILIAR METHODS
+    ////////////////////////////////////////////////////
+
+    void create_data_structures_(
+            eprosima::fastrtps::types::DynamicData* data = nullptr);
+
+
     ////////////////////////////////////////////////////
     // AUXILIAR STATIC METHODS
     ////////////////////////////////////////////////////
@@ -127,6 +135,9 @@ struct ReaderHandler : public eprosima::fastdds::dds::DataReaderListener
     //! Type Informantion
     eprosima::fastrtps::types::DynamicType_ptr type_;
 
+    //! Whether it is composed of variable sized types (e.g. sequences)
+    bool static_type_;
+
     //! Data Type element
     eprosima::fastrtps::types::DynamicData* data_;
 
@@ -137,6 +148,8 @@ struct ReaderHandler : public eprosima::fastdds::dds::DataReaderListener
 
     utils::TypeIntrospectionNumericStruct numeric_data_;
     utils::TypeIntrospectionStringStruct string_data_;
+
+    DataTypeConfiguration data_type_configuration_;
 };
 
 } /* namespace fastdds */