linerobot/pythonDDS/read_colorsensor.py

# -*- coding: utf-8 -*-
"""
Created on 23 Jan 2022

@author: Edward Hage
"""

import numpy as np

from dds import Listener, DomainParticipant, Topic, QosProvider, Qos
from dds import PresentationQosPolicy, PartitionQosPolicy, DDSPresentationAccessScopeKind, DurabilityQosPolicy,DDSDurabilityKind
from idlpython.std_msgs.Float32_.std_msgs.msg.dds_ import Float32___TypeSupport

import time

class ColorReader():
   
    class Float32Listener(Listener):
        
        def __init__(self):
            Listener.__init__(self)
    
        def on_data_available(self, entity):
            l = entity.take(10)
          
            for (sd, si) in l:
                #sd.print_vars()
                print("red sensor value: " + str(sd.data_))

    listener = Float32Listener()
    
    def __init__(self, topicname):
        
        print("in init")
        qp = QosProvider('file://DDS_ROSQoS.xml', 'DDS ROSQosProfile')
        dp = DomainParticipant(qos = qp.get_participant_qos())
        sub = dp.create_subscriber(Qos([PresentationQosPolicy(DDSPresentationAccessScopeKind.INSTANCE, False, False), 
                                      PartitionQosPolicy(['']),
                                      DurabilityQosPolicy(DDSDurabilityKind.VOLATILE)]))
        topic = Topic(dp, topicname, Float32___TypeSupport(), qp.get_topic_qos(), None)
        self.reader = sub.create_datareader(topic, qp.get_reader_qos(), self.listener)

reader = ColorReader("rt/redsensor")
input("Press a key to stop ...")