DDS Exclusive Ownership QoS (#56)

* Refs #14570. Add Ownership DDS QoS

Signed-off-by: Ricardo González Moreno <[email protected]>

* Refs #14570. Add disposed shape draw

Signed-off-by: Ricardo González Moreno <[email protected]>

* Refs #14570. Add incompatible_qos callbacks

Signed-off-by: Ricardo González Moreno <[email protected]>

Signed-off-by: Ricardo González Moreno <[email protected]>

include/eprosimashapesdemo/shapesdemo/ShapeHistory.h

@@ -63,7 +63,6 @@ class ShapeHistory
 
     ShapeHistory()
         : m_history_depth(0)
-        , m_isExclusiveOwnership(false)
     {
     }
 
@@ -105,12 +104,9 @@ class ShapeHistory
             const SD_COLOR color);
     void adjustContentFilter(
             ShapeFilter& filter);
-    void removedOwner(
-            const rtps::GUID_t& guid);
 
     uint32_t m_history_depth;
     ShapeFilter m_filter;
-    bool m_isExclusiveOwnership;
 };
 
 #endif // SHAPEHISTORY_H

include/eprosimashapesdemo/shapesdemo/ShapeInfo.h

@@ -43,9 +43,7 @@ class Shape
 public:
 
     Shape()
-        : m_strength(0)
-        , m_hasOwner(true)
-        , m_dirX(0)
+        : m_dirX(0)
         , m_dirY(0)
         , m_changeDir(true)
         , m_max_speed_variance(0.1f)
@@ -66,19 +64,17 @@ class Shape
         m_shape.x(x);
         m_shape.y(y);
         m_shape.shapesize(size);
-        m_strength = 0;
     }
 
     ShapeType m_shape;
     TYPESHAPE m_type;
     Time_t m_time;
-    rtps::GUID_t m_writerGuid;
-    uint32_t m_strength;
-    bool m_hasOwner;
     float m_dirX;
     float m_dirY;
     bool m_changeDir;
     float m_max_speed_variance;
+
+    bool dispose = false;
 };
 
 typedef std::pair<SD_COLOR, rtps::InstanceHandle_t> colorPair;

include/eprosimashapesdemo/shapesdemo/ShapePublisher.h

@@ -100,6 +100,10 @@ class ShapePublisher : public DataWriterListener
                 DataWriter* writer,
                 const LivelinessLostStatus& status) override;
 
+        void on_offered_incompatible_qos(
+                DataWriter* writer,
+                const OfferedIncompatibleQosStatus& status) override;
+
     private:
 
         ShapePublisher* parent_;

include/eprosimashapesdemo/shapesdemo/ShapeSubscriber.h

@@ -95,10 +95,6 @@ class ShapeSubscriber
         void on_data_available(
                 DataReader* reader) override;
 
-        void on_subscription_matched(
-                DataReader* reader,
-                const eprosima::fastdds::dds::SubscriptionMatchedStatus& info) override;
-
         void on_requested_deadline_missed(
                 DataReader* reader,
                 const eprosima::fastrtps::RequestedDeadlineMissedStatus& status) override;
@@ -107,6 +103,10 @@ class ShapeSubscriber
                 DataReader* reader,
                 const eprosima::fastrtps::LivelinessChangedStatus& status) override;
 
+        void on_requested_incompatible_qos(
+                DataReader* reader,
+                const RequestedIncompatibleQosStatus& status) override;
+
     private:
 
         ShapeSubscriber* parent_;
@@ -121,7 +121,6 @@ class ShapeSubscriber
 
     bool hasReceived;
     QMutex m_mutex;
-    std::vector<rtps::GUID_t> m_remoteWriters;
     ShapeHistory m_shapeHistory;
     TYPESHAPE m_shapeType;
     ContentFilterSelector* mp_contentFilter;

include/eprosimashapesdemo/shapesdemo/ShapesDemo.h

@@ -24,6 +24,7 @@
 #define SHAPESDEMO_H_
 
 #include <map>
+#include <string>
 
 #include <QMutex>
 
@@ -42,6 +43,9 @@
 
 using namespace eprosima::fastdds::dds;
 
+std::string qos_policy_id_to_string(
+        QosPolicyId_t policy_id);
+
 /**
  * @brief The ShapesDemoOptions class, used to transmit the options between the options menu and the ShapesDemo class.
  */
@@ -211,26 +215,8 @@ class ShapesDemo
         return m_data_sharing_enable;
     }
 
-    // NOTE: there is no other way to check ownership strength unless using DomainParticipant callback listener
-    // on_publisher_discovery and storing to a map
-
-    //! Add ownership strength to a writer
-    bool add_writer_strength(
-            const eprosima::fastrtps::rtps::GUID_t& guid,
-            uint32_t strength);
-
-    //! Erase ownership strength from a writer (When writer drops)
-    bool remove_writer_strength(
-            eprosima::fastrtps::rtps::GUID_t guid);
-
-    //! Get writer strength
-    uint32_t writer_strength(
-            eprosima::fastrtps::rtps::GUID_t guid);
-
 private:
 
-    std::map<eprosima::fastrtps::rtps::GUID_t, uint32_t> m_ownership_strength_map;
-
     std::vector<ShapePublisher*> m_publishers;
     std::vector<ShapeSubscriber*> m_subscribers;
     eprosima::fastdds::dds::DomainParticipant* mp_participant;
@@ -257,40 +243,6 @@ class ShapesDemo
 #endif // ifdef ENABLE_ROS_COMPONENTS
     std::map<std::string, Topic*> m_topics;
     bool m_data_sharing_enable;
-
-    class Listener : public eprosima::fastdds::dds::DomainParticipantListener
-    {
-    public:
-
-        Listener(
-                ShapesDemo* participant)
-            : mp_parent_(participant)
-        {
-        }
-
-        /* Custom Callback on_publisher_discovery */
-        virtual void on_publisher_discovery(
-                eprosima::fastdds::dds::DomainParticipant*,
-                eprosima::fastrtps::rtps::WriterDiscoveryInfo&& info)
-        {
-            switch (info.status){
-                case eprosima::fastrtps::rtps::WriterDiscoveryInfo::DISCOVERED_WRITER:
-                case eprosima::fastrtps::rtps::WriterDiscoveryInfo::CHANGED_QOS_WRITER:
-                    // Add strength to Participant map
-                    mp_parent_->add_writer_strength(info.info.guid(), info.info.m_qos.m_ownershipStrength.value);
-                    break;
-                case eprosima::fastrtps::rtps::WriterDiscoveryInfo::REMOVED_WRITER:
-                    // Erase strength to Participant map
-                    mp_parent_->remove_writer_strength(info.info.guid());
-                    break;
-            }
-        }
-
-    private:
-
-        ShapesDemo* mp_parent_;
-    }
-    m_listener;
 };
 
 #endif /* SHAPESDEMO_H_ */

src/qt/DrawArea.cpp

@@ -132,14 +132,11 @@ void DrawArea::drawShapes(
                 {
                     size_t total = vit->size();
                     int index = 0;
-                    if (vit->begin()->m_writerGuid != rtps::c_Guid_Unknown)
+                    for (std::list<Shape>::reverse_iterator sit = vit->rbegin();
+                            sit != vit->rend(); ++sit)
                     {
-                        for (std::list<Shape>::reverse_iterator sit = vit->rbegin();
-                                sit != vit->rend(); ++sit)
-                        {
-                            paintShape(painter, *sit, getAlpha(index, total), true);
-                            ++index;
-                        }
+                        paintShape(painter, *sit, getAlpha(index, total), true);
+                        ++index;
                     }
                 }
             }
@@ -165,15 +162,27 @@ void DrawArea::paintShape(
     painter->save();
     m_pen.setColor(SD_QT_BLACK);
     m_pen.setStyle(isHistory ? Qt::DotLine : Qt::SolidLine);
-    painter->setPen(m_pen);
     QColor auxc = SD_QT_BLACK;
-    if (shape.m_shape.color().size() >= 3)
+    if (!shape.dispose)
     {
-        auxc = SD_COLOR2QColor(getColor(shape.m_shape.color().at(0), shape.m_shape.color().at(2)));
+        if (shape.m_shape.color().size() >= 3)
+        {
+            auxc = SD_COLOR2QColor(getColor(shape.m_shape.color().at(0), shape.m_shape.color().at(2)));
+        }
+        auxc.setAlpha(alpha);
+        m_brush.setColor(auxc);
+    }
+    else
+    {
+        auxc = SD_QT_WHITE;
+        m_brush.setColor(auxc);
+        if (shape.m_shape.color().size() >= 3)
+        {
+            m_pen.setColor(SD_COLOR2QColor(getColor(shape.m_shape.color().at(0), shape.m_shape.color().at(2))));
+        }
     }
-    auxc.setAlpha(alpha);
-    m_brush.setColor(auxc);
     m_brush.setStyle(Qt::SolidPattern);
+    painter->setPen(m_pen);
     painter->setBrush(m_brush);
     switch (shape.m_type)
     {
@@ -184,6 +193,19 @@ void DrawArea::paintShape(
                     shape.m_shape.shapesize(),
                     shape.m_shape.shapesize());
             painter->drawRect(rect);
+            if (shape.dispose)
+            {
+                QLine line(shape.m_shape.x() - shape.m_shape.shapesize() / 2,
+                        shape.m_shape.y() - shape.m_shape.shapesize() / 2,
+                        shape.m_shape.x() + shape.m_shape.shapesize() / 2,
+                        shape.m_shape.y() + shape.m_shape.shapesize() / 2);
+                painter->drawLine(line);
+                QLine line2(shape.m_shape.x() + shape.m_shape.shapesize() / 2,
+                        shape.m_shape.y() - shape.m_shape.shapesize() / 2,
+                        shape.m_shape.x() - shape.m_shape.shapesize() / 2,
+                        shape.m_shape.y() + shape.m_shape.shapesize() / 2);
+                painter->drawLine(line2);
+            }
             break;
         }
         case TRIANGLE:

src/qt/subscribedialog.cpp

@@ -172,7 +172,6 @@ void SubscribeDialog::on_buttonBox_accepted()
             case 1:
             {
                 SSub->m_dr_qos.ownership().kind = eprosima::fastdds::dds::EXCLUSIVE_OWNERSHIP_QOS;
-                SSub->m_shapeHistory.m_isExclusiveOwnership = true;
                 break;
             }
         }

src/shapesdemo/ShapeHistory.cpp

@@ -76,33 +76,6 @@ void ShapeHistory::addShapeToList(
         std::list<Shape>& list)
 {
     if (passTimeFilter(sh, list.front()))
-    {
-        if (this->m_isExclusiveOwnership)
-        {
-            addShapeExclusive(sh, list);
-        }
-        else
-        {
-            addShape(sh, list);
-        }
-    }
-}
-
-void ShapeHistory::addShapeExclusive(
-        Shape& sh,
-        std::list<Shape>& list)
-{
-    if (sh.m_strength > list.front().m_strength || list.front().m_hasOwner == false)
-    {
-        list.pop_front();
-        addShape(sh, list);
-    }
-    else if (sh.m_strength == list.front().m_strength && sh.m_writerGuid == list.front().m_writerGuid )
-    {
-        addShape(sh, list);
-    }
-    else if (sh.m_strength == list.front().m_strength &&
-            compareGUID(sh.m_writerGuid, list.front().m_writerGuid))
     {
         addShape(sh, list);
     }
@@ -191,7 +164,7 @@ void ShapeHistory::dispose(
     {
         if (it->front().m_shape.color() == getColorStr(color))
         {
-            it->front().m_hasOwner = false;
+            it->front().dispose = true;
             return;
         }
     }
@@ -224,18 +197,3 @@ void ShapeHistory::adjustContentFilter(
         m_filter.m_minY = filter.m_minY;
     }
 }
-
-void ShapeHistory::removedOwner(
-        const GUID_t& guid)
-{
-    for (std::vector<std::list<Shape>>::iterator it = m_history.begin();
-            it != m_history.end(); ++it)
-    {
-        if (it->front().m_writerGuid == guid)
-        {
-            it->front().m_hasOwner = false;
-            return;
-        }
-    }
-    return;
-}

src/shapesdemo/ShapePublisher.cpp

@@ -121,16 +121,35 @@ void ShapePublisher::PubListener::on_offered_deadline_missed(
         DataWriter* writer,
         const OfferedDeadlineMissedStatus& status)
 {
-    static_cast<void>(writer);
-    static_cast<void>(status);
-    parent_->m_mainWindow->addMessageToOutput(QString("Offered deadline missed"));
+    if (0 < status.total_count_change)
+    {
+        std::stringstream str;
+        str << "DataWriter " << writer->guid() << " detects deadline missed";
+        parent_->m_mainWindow->addMessageToOutput(QString(str.str().c_str()));
+    }
 }
 
 void ShapePublisher::PubListener::on_liveliness_lost(
         DataWriter* writer,
         const LivelinessLostStatus& status)
 {
-    static_cast<void>(writer);
-    static_cast<void>(status);
-    parent_->m_mainWindow->addMessageToOutput(QString("Publisher lost liveliness"));
+    if (0 < status.total_count_change)
+    {
+        std::stringstream str;
+        str << "DataWriter " << writer->guid() << " detects liveliness lost";
+        parent_->m_mainWindow->addMessageToOutput(QString(str.str().c_str()));
+    }
+}
+
+void ShapePublisher::PubListener::on_offered_incompatible_qos(
+        DataWriter* writer,
+        const OfferedIncompatibleQosStatus& status)
+{
+    if (0 < status.total_count_change)
+    {
+        std::stringstream str;
+        str << "DataWriter " << writer->guid() << " detects incompatible QoS " << qos_policy_id_to_string(
+            status.last_policy_id);
+        parent_->m_mainWindow->addMessageToOutput(QString(str.str().c_str()));
+    }
 }