def __init__(self,port=1234,sub_topic="modbus_server/write_to_registers",pub_topic="modbus_server/read_from_registers"):
        """
            Creates a Modbus TCP Server object
            .. note:: The default port for modbus is 502. This modbus server uses port 1234 by default, otherwise superuser rights are required.
            
            .. note:: Use "startServer" to start the listener.
            
            :param port: Port for the modbus TCP server
            :type port: int
            :param sub_topic: ROS topic name for the subscriber that updates the modbus registers
            :type sub_topic: string
            :param pub_topic: ROS topic name for the publisher that publishes a message, once there is something written to the writeable modbus registers
            :type pub_topic: string
            
        """
        chr = CustomHoldingRegister(ADDRESS_WRITE_START, [17]*100,sub_topic,pub_topic)
        self.store = ModbusSlaveContext(
            di = ModbusSequentialDataBlock(ADDRESS_WRITE_START, [17]*100),
            co = ModbusSequentialDataBlock(ADDRESS_WRITE_START, [17]*100),
            hr = chr, 
            ir = ModbusSequentialDataBlock(ADDRESS_WRITE_START, [17]*100))
        self.context = ModbusServerContext(slaves=self.store, single=True)

        self.identity = ModbusDeviceIdentification()
        self.identity.VendorName  = 'Pymodbus'
        self.identity.ProductCode = 'PM'
        self.identity.VendorUrl   = 'http://github.com/bashwork/pymodbus/'
        self.identity.ProductName = 'Pymodbus Server'
        self.identity.ModelName   = 'Pymodbus Server'
        self.identity.MajorMinorRevision = '1.0'

        self.store.setValues(2,0,[0]*1)
        self.post = Post(self)
        framer = ModbusSocketFramer
        self.server = ModbusTcpServer(self.context, framer, self.identity, address=("0.0.0.0", port))
Beispiel #2
0
class IncomingCall(AbstractWidget):
    def __init__(self,parent):
        super(IncomingCall,self).__init__(parent)
        self.ui = Ui_IncomingCall()
        self.ui.setupUi(self)
        self.post = Post(self)
        self.calling = False
        self.call_active = False
        self.ui.btn_hangup.clicked.connect(self.hangup)
        self.ui.btn_answer.clicked.connect(self.answer)
        self.ui.btn_close.clicked.connect(self.close)
        
    def incomingCall(self):
        if self.calling or self.call_active:
            return
        self.ui.btn_close.setEnabled(False)
        self.ui.btn_hangup.setEnabled(True)
        self.ui.btn_answer.setEnabled(True)        
        self.calling = True
        self.show()
    
    def hangup(self):
        self.parent.sb.hangupCall()
        
    def answer(self):
        self.parent.sb.answerCall()
        
    
    def callAnswered(self):
        self.call_active = True
        self.calling = False
        self.call_start = time.time()
        self.ui.btn_answer.setEnabled(False)
        self.post.showDuration()

    def callStopped(self):
        self.call_active = False
        self.calling = False
        self.ui.btn_close.setEnabled(True)
        self.ui.btn_hangup.setEnabled(False)
        self.ui.btn_answer.setEnabled(False)
    
    def showDuration(self):
        while self.call_active:
            now = time.time()-self.call_start
            minutes = int(now/60)
            seconds = int(now%60)
            self.ui.lbl_duration.setText("%02.d:%02.d"%(minutes,seconds))
            time.sleep(1)
        
    def closeEvent(self,event):
        if self.call_active or self.calling:
            event.ignore()
            self.hangup()
        else:
            event.accept()
Beispiel #3
0
 def __init__(self,parent):
     super(IncomingCall,self).__init__(parent)
     self.ui = Ui_IncomingCall()
     self.ui.setupUi(self)
     self.post = Post(self)
     self.calling = False
     self.call_active = False
     self.ui.btn_hangup.clicked.connect(self.hangup)
     self.ui.btn_answer.clicked.connect(self.answer)
     self.ui.btn_close.clicked.connect(self.close)
Beispiel #4
0
    def __init__(self,
                 host,
                 port=502,
                 rate=50,
                 reset_registers=True,
                 sub_topic="modbus_wrapper/output",
                 pub_topic="modbus_wrapper/input"):
        """
            Use subscribers and publisher to communicate with the modbus server. Check the scripts for example code.
            :param host: Contains the IP adress of the modbus server
            :type host: string
            :param port: The port number, where the modbus server is runnning
            :type port: integer
            :param rate: How often the registers on the modbusserver should be read per second
            :type rate: float
            :param reset_registers: Defines if the holding registers should be reset to 0 after they have been read. Only possible if they are writeable
            :type reset_registers: bool
        """
        try:
            self.client = ModbusTcpClient(host, port)
        except Exception as e:
            rospy.logwarn(
                "Could not get a modbus connection to the host modbus. %s",
                str(e))
            raise e
            return
        self.__rate = rate
        self.__reading_delay = 1 / rate
        self.post = Post(self)

        self.__reset_registers = reset_registers
        self.__reading_register_start = 0
        self.__num_reading_registers = 20
        #         self.input_size = 16
        self.__input = HoldingRegister()
        self.__input.data = [0 for i in xrange(self.__num_reading_registers)]

        self.__writing_registers_start = ADDRESS_WRITE_START
        self.__num_writing_registers = 20
        #         self.output_size = 16
        self.__output = [None for i in range(self.__num_writing_registers)]

        self.__last_output_time = rospy.get_time()
        self.__mutex = Lock()

        self.__sub = rospy.Subscriber(sub_topic,
                                      HoldingRegister,
                                      self.__updateModbusOutput,
                                      queue_size=500)
        self.__pub = rospy.Publisher(pub_topic,
                                     HoldingRegister,
                                     queue_size=500,
                                     latch=True)

        rospy.on_shutdown(self.closeConnection)
Beispiel #5
0
    def __init__(self, app):
        super(ControlMainWindow, self).__init__(None)
        # self.settings = QtCore.QSettings("root_window")
        self.app = app
        self.post = Post(self)
        self.widgets = {}
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        self.ui.statusbar.showMessage("Not connected")
        self.show()

        self.post.initSerialBridge()
        self.initUi()
Beispiel #6
0
    def __init__(self,side,ik=True):
        limb.Limb.__init__(self,side)
        self.side=side
        
        self.DEFAULT_BAXTER_SPEED=0.3
        
        if not side in ["left","right"]:
            raise BaxterException,"Error non existing side: %s, please provide left or right"%side

        self.post=Post(self)
        self.__stop = False
        self.simple=self
        self._moving=False
        self.moving_lock=Lock()
        
        self.ik=ik        
        if self.ik:            
            self.ns = "/ExternalTools/%s/PositionKinematicsNode/IKService"%self.side    
            rospy.loginfo("Waiting for inverse kinematics service on %s..."%self.ns)
            rospy.wait_for_service(self.ns)
            self.iksvc = rospy.ServiceProxy(self.ns, SolvePositionIK)
            rospy.loginfo("Waiting for inverse kinematics service DONE")
        else:
            rospy.loginfo("Skipping inverse kinematics service loading")
        
        #self.simple_limb_srv = rospy.Service("/simple_limb/"+side,LimbPose,self.__cbLimbPose)
        
        self._pub_speed=rospy.Publisher("/robot/limb/%s/set_speed_ratio"%self.side,Float64,queue_size=1)
        while not rospy.is_shutdown() and self._pub_speed.get_num_connections() < 1:
            rospy.sleep(0.1)
Beispiel #7
0
class TFHelper():
    __metaclass__=Singleton
    """
        Helps broadcasting static tf frames
    """
    def __init__(self,frequency=50):
        self.frequency=frequency
        
        
        self.post = Post(self)
        self.mutex = Lock()
        self.tb = tf.TransformBroadcaster()
        self.listener = tf.TransformListener()
        self.transforms={}
        self.thread=None
        self._stop_broadcast=False
        self.last_keys=None
        
        self.medians = {}

    def __del__(self):
        self.stop()
        
        
    def start(self):
        """ Start broadcasting thread,  blocking """
        with self.mutex:            
            if self.thread:
                self.stop()
            self._stop_broadcast=False
            self.thread=self.post.__broadcastLoop()        

    def stop(self):
        """ Stop broadcasting thread,  blocking """
        with self.mutex:
            if self.thread:
                self._stop_broadcast=True
                self.thread.join()
                self.thread=None

    def __broadcastLoop(self): 
        """
            Loop that broadcasts all transformations. Started by (BaxterFrame)
        """       
        with self.mutex:
            self.last_keys=None
        rate=rospy.Rate(self.frequency)
        while not rospy.is_shutdown() and not self._stop_broadcast:
            with self.mutex:
                if not self.last_keys or str(self.last_keys)!=str(self.transforms.keys()):
                    rospy.loginfo("Broadcasting %d frames: %s"%(len(self.transforms.items())  , self.transforms.keys() ) )
                    self.last_keys=str(self.transforms.keys())
                for id,t in self.transforms.items():
                    if not rospy.is_shutdown():
                        try:
                            self.tb.sendTransform(self.pos2list(t.pose.position),self.quat2list(t.pose.orientation),rospy.Time.now(), "/"+id, "/"+t.header.frame_id)
                        except rospy.ROSException,e:
                            rospy.logwarn("Could not publish on TF topic, closing broadcaster: "+str(e))
                            break
            rate.sleep()
    def __init__(self,port=1234):
        """
            Creates a Modbus TCP Server object
            .. note:: The default port for modbus is 502. This modbus server uses port 1234 by default, otherwise superuser rights are required.
            
            .. note:: Use "startServer" to start the listener.
            
            :param port: Port for the modbus TCP server
            :type port: int
                        
        """
#         chr = CustomHoldingRegister(ADDRESS_WRITE_START, [16]*100)
#         cdi = CustomHoldingRegister(ADDRESS_WRITE_START, [16]*100)
        self.onShutdown=False;
        self.state_changed = Signal()
        cco = CustomHoldingRegister(ADDRESS_WRITE_START, [16]*100)
        self.store = ModbusSlaveContext(
            di = ModbusSequentialDataBlock(ADDRESS_WRITE_START, [16]*100),
            co = cco,
            hr = ModbusSequentialDataBlock(ADDRESS_WRITE_START, [16]*100), 
            ir = ModbusSequentialDataBlock(ADDRESS_WRITE_START, [16]*100))
        self.context = ModbusServerContext(slaves=self.store, single=True)

        self.identity = ModbusDeviceIdentification()
        self.identity.VendorName  = 'Pymodbus'
        self.identity.ProductCode = 'PM'
        self.identity.VendorUrl   = 'http://github.com/bashwork/pymodbus/'
        self.identity.ProductName = 'Pymodbus Server'
        self.identity.ModelName   = 'Pymodbus Server'
        self.identity.MajorMinorRevision = '1.0'

        self.store.setValues(2,0,[0]*1)
        self.post = Post(self)
        framer = ModbusSocketFramer
        self.server = ModbusTcpServer(self.context, framer, self.identity, address=("0.0.0.0", port))
    def __init__(self,port=1234,sub_topic="modbus_server/write_to_registers",pub_topic="modbus_server/read_from_registers"):
        """
            Creates a Modbus TCP Server object
            .. note:: The default port for modbus is 502. This modbus server uses port 1234 by default, otherwise superuser rights are required.
            
            .. note:: Use "startServer" to start the listener.
            
            :param port: Port for the modbus TCP server
            :type port: int
            :param sub_topic: ROS topic name for the subscriber that updates the modbus registers
            :type sub_topic: string
            :param pub_topic: ROS topic name for the publisher that publishes a message, once there is something written to the writeable modbus registers
            :type pub_topic: string
            
        """
        chr = CustomHoldingRegister(ADDRESS_WRITE_START, [17]*100,sub_topic,pub_topic)
        self.store = ModbusSlaveContext(
            di = ModbusSequentialDataBlock(ADDRESS_WRITE_START, [17]*100),
            co = ModbusSequentialDataBlock(ADDRESS_WRITE_START, [17]*100),
            hr = chr, 
            ir = ModbusSequentialDataBlock(ADDRESS_WRITE_START, [17]*100))
        self.context = ModbusServerContext(slaves=self.store, single=True)

        self.identity = ModbusDeviceIdentification()
        self.identity.VendorName  = 'Pymodbus'
        self.identity.ProductCode = 'PM'
        self.identity.VendorUrl   = 'http://github.com/bashwork/pymodbus/'
        self.identity.ProductName = 'Pymodbus Server'
        self.identity.ModelName   = 'Pymodbus Server'
        self.identity.MajorMinorRevision = '1.0'

        self.store.setValues(2,0,[0]*1)
        self.post = Post(self)
        framer = ModbusSocketFramer
        self.server = ModbusTcpServer(self.context, framer, self.identity, address=("0.0.0.0", port))
 def __init__(self,parent):
     super(IncomingSMS,self).__init__(parent)
     self.ui = Ui_IncomingSMS()
     self.ui.setupUi(self)
     self.post = Post(self)
     self.calling = False
     self.call_active = False
     self.ui.btn_close.clicked.connect(self.close)
Beispiel #11
0
 def __init__(self, portname, rate=57142, timeout=0.04):
     """
     DO NOT CHANGE THE DEFAULT BAUDRATE HERE: 57142 is the factory setting of Dynamixel motors
     """
     self.portname = portname
     self.rate = rate
     self.timeout = timeout
     self.lock = Lock()
     self.open()
     self.configuration = None
     self.motors = {}
     self.post = Post(self)
Beispiel #12
0
 def __init__(self,frequency=50):
     self.frequency=frequency
     
     
     self.post = Post(self)
     self.mutex = Lock()
     self.tb = tf.TransformBroadcaster()
     self.listener = tf.TransformListener()
     self.transforms={}
     self.thread=None
     self._stop_broadcast=False
     self.last_keys=None
     
     self.medians = {}
Beispiel #13
0
class ControlMainWindow(QtGui.QMainWindow):
    updateFeedbackSignal = Signal(str)

    def __init__(self, app):
        super(ControlMainWindow, self).__init__(None)
        # self.settings = QtCore.QSettings("root_window")
        self.app = app
        self.post = Post(self)
        self.widgets = {}
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        self.ui.statusbar.showMessage("Not connected")
        self.show()

        self.post.initSerialBridge()
        self.initUi()

    def initUi(self):
        self.ui.btn_close.clicked.connect(self.close)

        self.widgets["call"] = Call(self)
        self.ui.btn_call.clicked.connect(self.widgets["call"].show)

        self.widgets["import_csv"] = ImportCSV(self)
        self.ui.btn_import_csv.clicked.connect(self.widgets["import_csv"].show)

        self.widgets["received_sms"] = ReceivedSMS(self)
        self.ui.btn_show_sms.clicked.connect(self.widgets["received_sms"].show)

        self.widgets["sent_sms"] = SentSMS(self)
        self.ui.btn_sent_sms.clicked.connect(self.widgets["sent_sms"].show)

        self.widgets["send_sms"] = SendSMS(self)
        self.ui.btn_send_sms.clicked.connect(self.widgets["send_sms"].show)

        self.widgets["incoming_call"] = IncomingCall(self)
        self.widgets["incoming_sms"] = IncomingSMS(self)
        self.updateFeedbackSignal.connect(self.updateFeedback)
        # self.widgets["import_csv"].show()
        # self.widgets["incomding_sms"].show()

    def initSerialBridge(self):
        self.sb = SerialBridge()
        time.sleep(2.0)
        self.sb.send("AT")
        self.sb.trigger_words += [
            "AT", "RING", "ATA", "ATH", "ATD", "CMT", "NO CARRIER"
        ]
        self.sb.callback = self.cbSim

    def cbSim(self, feedback):
        self.updateFeedbackSignal.emit(feedback)

    @Slot(str)
    def updateFeedback(self, feedback):
        print "got feedback"
        if feedback == "RING":
            self.widgets["incoming_call"].incomingCall()
        if feedback == "ATA":
            self.widgets["incoming_call"].callAnswered()
        if feedback == "ATH":
            self.widgets["incoming_call"].callStopped()
            self.widgets["call"].callStopped()
        if "ATD" in feedback:
            self.widgets["call"].callStarted()
        if "AT" == feedback:
            self.ui.statusbar.showMessage("Connected")
        if "CMT" in feedback:
            self.widgets["incoming_sms"].incomingSMS(feedback)
        # if "CMGR" in feedback:
        # self.widgets["incoming_sms"].messageContent(feedback)
        if "NO CARRIER" in feedback:
            self.widgets["incoming_call"].callStopped()

    def keyPressEvent(self, e):
        """Summary
        
        Args:
            e (TYPE): Description
        
        Returns:
            TYPE: Description
        """
        #closes the window on escape
        if e.key() == QtCore.Qt.Key_Escape:
            self.close()

    def closeEvent(self, event):
        try:
            self.sb.close()
        except:
            pass
        for widget in self.widgets.values():
            widget.close()
        super(ControlMainWindow, self).closeEvent(event)
Beispiel #14
0
class ModbusWrapperServer():
    def __init__(self,
                 port=1234,
                 sub_topic="modbus_server/write_to_registers",
                 pub_topic="modbus_server/read_from_registers"):
        """
            Creates a Modbus TCP Server object
            .. note:: The default port for modbus is 502. This modbus server uses port 1234 by default, otherwise superuser rights are required.
            
            .. note:: Use "startServer" to start the listener.
            
            :param port: Port for the modbus TCP server
            :type port: int
            :param sub_topic: ROS topic name for the subscriber that updates the modbus registers
            :type sub_topic: string
            :param pub_topic: ROS topic name for the publisher that publishes a message, once there is something written to the writeable modbus registers
            :type pub_topic: string
            
        """
        chr = CustomHoldingRegister(ADDRESS_WRITE_START, [17] * 100, sub_topic,
                                    pub_topic)
        self.store = ModbusSlaveContext(
            di=ModbusSequentialDataBlock(ADDRESS_WRITE_START, [17] * 100),
            co=ModbusSequentialDataBlock(ADDRESS_WRITE_START, [17] * 100),
            hr=chr,
            ir=ModbusSequentialDataBlock(ADDRESS_WRITE_START, [17] * 100))
        self.context = ModbusServerContext(slaves=self.store, single=True)

        self.identity = ModbusDeviceIdentification()
        self.identity.VendorName = 'Pymodbus'
        self.identity.ProductCode = 'PM'
        self.identity.VendorUrl = 'http://github.com/bashwork/pymodbus/'
        self.identity.ProductName = 'Pymodbus Server'
        self.identity.ModelName = 'Pymodbus Server'
        self.identity.MajorMinorRevision = '1.0'

        self.store.setValues(2, 0, [0] * 1)
        self.post = Post(self)
        framer = ModbusSocketFramer
        self.server = ModbusTcpServer(self.context,
                                      framer,
                                      self.identity,
                                      address=("0.0.0.0", port))

    def startServer(self):
        """
            Non blocking call to start the server
        """
        self.post.__startServer()
        rospy.loginfo("Modbus server started")

    def __startServer(self):
        self.server.serve_forever()

    def stopServer(self):
        """
            Closes the server
        """
        self.server.server_close()
        self.server.shutdown()

    def waitForCoilOutput(self, address, timeout=2):
        """
            Blocks for the timeout in seconds (or forever) until the specified address becomes true. Adapt this to your needs
            :param address: Address of the register that wanted to be read.
            :type address: int
            :param timeout: The time in seconds until the function should return latest.
            :type: float/int
        """
        now = time.time()
        while True:
            values = self.store.getValues(1, address, 1)
            if values[0] is True:
                return True
            else:
                if timeout <= 0 or now + timeout > time.time():
                    time.sleep(1 / 50)
                else:
                    return False

    def setDigitalInput(self, address, values):
        """
            Writes to the digital input of the modbus server
            :param address: Starting address of the values to write
            :type: int
            :param values: List of values to write
            :type list/boolean/int
        """
        if not values is list:
            values = [values]
        self.store.setValues(2, address, values)
 def __init__(self):
     self.post = Post(self)
     self.conn_down = True
     self.initConnection()
     self.trigger_words = []
     self.callback = None
class ModbusWrapperServer():
    def __init__(self,port=1234):
        """
            Creates a Modbus TCP Server object
            .. note:: The default port for modbus is 502. This modbus server uses port 1234 by default, otherwise superuser rights are required.
            
            .. note:: Use "startServer" to start the listener.
            
            :param port: Port for the modbus TCP server
            :type port: int
                        
        """
#         chr = CustomHoldingRegister(ADDRESS_WRITE_START, [16]*100)
#         cdi = CustomHoldingRegister(ADDRESS_WRITE_START, [16]*100)
        self.onShutdown=False;
        self.state_changed = Signal()
        cco = CustomHoldingRegister(ADDRESS_WRITE_START, [16]*100)
        self.store = ModbusSlaveContext(
            di = ModbusSequentialDataBlock(ADDRESS_WRITE_START, [16]*100),
            co = cco,
            hr = ModbusSequentialDataBlock(ADDRESS_WRITE_START, [16]*100), 
            ir = ModbusSequentialDataBlock(ADDRESS_WRITE_START, [16]*100))
        self.context = ModbusServerContext(slaves=self.store, single=True)

        self.identity = ModbusDeviceIdentification()
        self.identity.VendorName  = 'Pymodbus'
        self.identity.ProductCode = 'PM'
        self.identity.VendorUrl   = 'http://github.com/bashwork/pymodbus/'
        self.identity.ProductName = 'Pymodbus Server'
        self.identity.ModelName   = 'Pymodbus Server'
        self.identity.MajorMinorRevision = '1.0'

        self.store.setValues(2,0,[0]*1)
        self.post = Post(self)
        framer = ModbusSocketFramer
        self.server = ModbusTcpServer(self.context, framer, self.identity, address=("0.0.0.0", port))
        
    
        
    def startServer(self):
        """
            Non blocking call to start the server
        """
        self.post._startServer()
        
    def _startServer(self):
        print "Modbus server started"
        self.server.serve_forever()
        
    def stopServer(self):
        """
            Closes the server
        """
        self.onShutdown = True
        self.server.server_close()
        self.server.shutdown()
        
    def waitForCoilOutput(self,address,timeout=2):
        now = time.time()
        while not self.onShutdown:
            values = self.store.getValues(1,address, 1)
            if values[0] is True:
                return True
            else:
                if timeout > 0 and now + timeout > time.time():
                    time.sleep(1/50)
                else:
                    return False
    
    def stateChangeListener(self,address):
        self.post.__stateChangeListener(address)
    
    def __stateChangeListener(self,address):
        old_state = 0;
        while not self.onShutdown:
            value = self.store.getValues(1,address, 1)[0]
            
            if old_state != value:
                old_state = value
                self.state_changed(address,value)
                #print "state has changed"
            time.sleep(1/50) 
    
    def setDigitalInput(self,address,values):
        if not values is list:
            values = [values]
        self.store.setValues(2,address,values)
        
    def triggerInput(self,address,timeout=0.5):
        self.setDigitalInput(address, True)
        self.post.resetInput(address, timeout)
        
    def resetInput(self,address,timeout):
        time.sleep(timeout)
        self.setDigitalInput(address, False)
class ModbusWrapperServer():
    def __init__(self,port=1234,sub_topic="modbus_server/write_to_registers",pub_topic="modbus_server/read_from_registers"):
        """
            Creates a Modbus TCP Server object
            .. note:: The default port for modbus is 502. This modbus server uses port 1234 by default, otherwise superuser rights are required.
            
            .. note:: Use "startServer" to start the listener.
            
            :param port: Port for the modbus TCP server
            :type port: int
            :param sub_topic: ROS topic name for the subscriber that updates the modbus registers
            :type sub_topic: string
            :param pub_topic: ROS topic name for the publisher that publishes a message, once there is something written to the writeable modbus registers
            :type pub_topic: string
            
        """
        chr = CustomHoldingRegister(ADDRESS_WRITE_START, [17]*100,sub_topic,pub_topic)
        self.store = ModbusSlaveContext(
            di = ModbusSequentialDataBlock(ADDRESS_WRITE_START, [17]*100),
            co = ModbusSequentialDataBlock(ADDRESS_WRITE_START, [17]*100),
            hr = chr, 
            ir = ModbusSequentialDataBlock(ADDRESS_WRITE_START, [17]*100))
        self.context = ModbusServerContext(slaves=self.store, single=True)

        self.identity = ModbusDeviceIdentification()
        self.identity.VendorName  = 'Pymodbus'
        self.identity.ProductCode = 'PM'
        self.identity.VendorUrl   = 'http://github.com/bashwork/pymodbus/'
        self.identity.ProductName = 'Pymodbus Server'
        self.identity.ModelName   = 'Pymodbus Server'
        self.identity.MajorMinorRevision = '1.0'

        self.store.setValues(2,0,[0]*1)
        self.post = Post(self)
        framer = ModbusSocketFramer
        self.server = ModbusTcpServer(self.context, framer, self.identity, address=("0.0.0.0", port))
        
    def startServer(self):
        """
            Non blocking call to start the server
        """
        self.post.__startServer()
        rospy.loginfo("Modbus server started")
        
    def __startServer(self):
        self.server.serve_forever()
        
    def stopServer(self):
        """
            Closes the server
        """
        self.server.server_close()
        self.server.shutdown()
        
    def waitForCoilOutput(self,address,timeout=2):
        """
            Blocks for the timeout in seconds (or forever) until the specified address becomes true. Adapt this to your needs
            :param address: Address of the register that wanted to be read.
            :type address: int
            :param timeout: The time in seconds until the function should return latest.
            :type: float/int
        """
        now = time.time()
        while True:
            values = self.store.getValues(1,address, 1)
            if values[0] is True:
                return True
            else:
                if timeout <=0 or now + timeout > time.time():
                    time.sleep(1/50)
                else:
                    return False
    
    def setDigitalInput(self,address,values):
        """
            Writes to the digital input of the modbus server
            :param address: Starting address of the values to write
            :type: int
            :param values: List of values to write
            :type list/boolean/int
        """
        if not values is list:
            values = [values]
        self.store.setValues(2,address,values)
        
        
class SerialBridge:
    def __init__(self):
        self.post = Post(self)
        self.conn_down = True
        self.initConnection()
        self.trigger_words = []
        self.callback = None

    def openSerialPort(self, i):

        try:
            self.ser = serial.Serial('COM' + str(i), 19200)
            self.ser.flushInput()
            self.ser.flush()
            self.conn_down = False
            return True
        except serial.serialutil.SerialException as e:
            if "could not open port" in str(e):
                return False
            else:
                print "Problem accessing the device", e
                exit(1)

    def initConnection(self):
        for i in xrange(20):
            if self.openSerialPort(i):
                break

        if self.conn_down:
            print "could not establish connection"
            exit(1)
            return
        self.post.readFeedback()

    def readFeedback(self):
        print "reading feedback"
        tmp = ""
        cmt = False
        while not self.conn_down:
            feedback = self.ser.readline()
            feedback = feedback.replace("\n", "")
            feedback = feedback.replace("\r", "")
            if cmt:
                self.callback(tmp + feedback)
                cmt = False
                continue
            print "feedback:", feedback
            if not self.callback is None:
                for word in self.trigger_words:
                    if cmt:
                        break
                    if word in feedback:
                        if word == "CMT":
                            tmp = feedback
                            cmt = True
                            continue
                        self.callback(feedback)

            time.sleep(0.1)
        print "done reading feedback"

    def send(self, data):
        if len(data) > 0:
            # print "send", data
            self.ser.write(data + "\n\r")
            self.ser.flush()

    def sendSMS(self, number, text):
        self.send("SMS:" + str(number) + ";" + str(text))

    def readSMS(index):
        self.send("AT+CMGR=" + str(index))

    def answerCall(self):
        self.send("ATA")

    def hangupCall(self):
        self.send("ATH")

    def callNumber(self, number):
        self.send("ATD" + str(number))

    def getReceivedSMS(self):
        pass

    def getSentSMS(self):
        pass

    def close(self):
        self.conn_down = True
        self.ser.close()
Beispiel #19
0
class ModbusWrapperClient():
    """
        Wrapper that integrates python modbus into standardized ros msgs.
        The wrapper is able to read from and write to a standard modbus tcp/ip server.
    """
    def __init__(self,
                 host,
                 port=502,
                 rate=50,
                 reset_registers=True,
                 sub_topic="modbus_wrapper/output",
                 pub_topic="modbus_wrapper/input"):
        """
            Use subscribers and publisher to communicate with the modbus server. Check the scripts for example code.
            :param host: Contains the IP adress of the modbus server
            :type host: string
            :param port: The port number, where the modbus server is runnning
            :type port: integer
            :param rate: How often the registers on the modbusserver should be read per second
            :type rate: float
            :param reset_registers: Defines if the holding registers should be reset to 0 after they have been read. Only possible if they are writeable
            :type reset_registers: bool
        """
        try:
            self.client = ModbusTcpClient(host, port)
        except Exception as e:
            rospy.logwarn(
                "Could not get a modbus connection to the host modbus. %s",
                str(e))
            raise e
            return
        self.__rate = rate
        self.__reading_delay = 1 / rate
        self.post = Post(self)

        self.__reset_registers = reset_registers
        self.__reading_register_start = 0
        self.__num_reading_registers = 20
        #         self.input_size = 16
        self.__input = HoldingRegister()
        self.__input.data = [0 for i in xrange(self.__num_reading_registers)]

        self.__writing_registers_start = ADDRESS_WRITE_START
        self.__num_writing_registers = 20
        #         self.output_size = 16
        self.__output = [None for i in range(self.__num_writing_registers)]

        self.__last_output_time = rospy.get_time()
        self.__mutex = Lock()

        self.__sub = rospy.Subscriber(sub_topic,
                                      HoldingRegister,
                                      self.__updateModbusOutput,
                                      queue_size=500)
        self.__pub = rospy.Publisher(pub_topic,
                                     HoldingRegister,
                                     queue_size=500,
                                     latch=True)

        rospy.on_shutdown(self.closeConnection)

    def startListening(self):
        """
            Non blocking call for starting the listener for the readable modbus server registers 
        """
        #start reading the modbus
        self.post.__updateModbusInput()

    def stopListening(self):
        """
            Stops the listener loop
        """
        self.stop_listener = True
        while not rospy.is_shutdown() and self.listener_stopped is False:
            rospy.sleep(0.01)

    def setReadingRegisters(self, start, num_registers):
        """
            Sets the start address of the registers which should be read and their number
            :param start: First register that is readable
            :type start: int
            :param num_registers: Amount of readable registers
            :type num_registers: int
        """
        self.__reading_register_start = start
        self.__num_reading_registers = num_registers

    def setWritingRegisters(self, start, num_registers):
        """
            Sets the start address of the registers which are writeable and their number
            :param start: First register that is writeable
            :type start: int
            :param num_registers: Amount of writeable registers
            :type num_registers: int
        """
        self.__writing_registers_start = start
        self.__num_writing_registers = num_registers

    def getReadingRegisters(self):
        """
            :return: Returns the first address of the readable registers and the number of registers
            :rtype: int,int
        """
        return self.__reading_register_start, self.__num_reading_registers

    def getWritingRegisters(self):
        """
            :return: Returns the first address of the writeable registers and the number of registers
            :rtype: int,int
        """
        return self.__writing_registers_start, self.__num_writing_registers

    def __updateModbusInput(self, delay=0):
        """                
            Loop that is listening to the readable modbus registers and publishes it on a topic
            :param delay: The delay time until the loop starts
            :type delay: float 
        """
        rospy.sleep(delay)
        self.listener_stopped = False
        self.stop_listener = False
        update = True
        while not rospy.is_shutdown() and self.stop_listener is False:
            try:
                if not rospy.is_shutdown():
                    tmp = self.readRegisters()
                    if tmp is None:
                        rospy.sleep(2)
                        continue
                    # rospy.logwarn("__updateModbusInput tmp is %s ", str(tmp))
                    # rospy.logwarn("__updateModbusInput self.__input.data is %s ", str(self.__input.data))

                    if tmp != self.__input.data:
                        update = True
                        self.__input.data = tmp
                    else:
                        update = False
            except Exception as e:
                rospy.logwarn("Could not read holding register. %s", str(e))
                raise e
                rospy.sleep(2)

            if update:
                if self.__pub.get_num_connections() > 0:
                    try:
                        self.__pub.publish(self.__input)
                    except Exception as e:
                        rospy.logwarn(
                            "Could not publish message. Exception: %s", str(e))
                        raise e
            rospy.Rate(self.__rate).sleep()
        self.listener_stopped = True

    def __updateModbusOutput(self, msg):
        """
            Callback from the subscriber to update the writeable modbus registers
            :param msg: value of the new registers
            :type msg: std_msgs.Int32MultiArray
        """
        output_changed = False
        for index in xrange(self.__num_writing_registers):
            if self.__output[index] != msg.data[index]:
                output_changed = True
                break
        if not output_changed:
            return
        self.__writeRegisters(self.__writing_registers_start, msg.data)

    def __writeRegisters(self, address, values):
        """
            Writes modbus registers
            :param address: First address of the values to write
            :type address: int
            :param values: Values to write
            :type values: list
        """
        with self.__mutex:
            try:
                if not rospy.is_shutdown():
                    #                 print "writing address",address,"value"
                    self.client.write_registers(address, values)
                    self.output = values
            except Exception as e:
                rospy.logwarn(
                    "Could not write values %s to address %d. Exception %s",
                    str(values), address, str(e))
                raise e

    def readRegisters(self, address=None, num_registers=None):
        """
            Reads modbus registers
            :param address: First address of the registers to read
            :type address: int
            :param num_registers: Amount of registers to read
            :type num_registers: int
        """
        if address is None:
            address = self.__reading_register_start
        if num_registers is None:
            num_registers = self.__num_reading_registers

        tmp = None
        with self.__mutex:
            try:
                tmp = self.client.read_holding_registers(
                    address, num_registers).registers
            except Exception as e:
                rospy.logwarn("Could not read on address %d. Exception: %s",
                              address, str(e))
                raise e

            if self.__reset_registers:
                try:
                    self.client.write_registers(
                        address, [0 for i in xrange(num_registers)])
                except Exception as e:
                    rospy.logwarn(
                        "Could not write to address %d. Exception: %s",
                        address, str(e))
                    raise e

        return tmp

    def setOutput(self, address, value, timeout=0):
        """
            Directly write one register
            :param address: The exact register address to write
            :type address: int
            :param value: What is written in the register
            :type value: int
            :param timeout: If set, the register is set after this time to 0
            :type timeout: float
        """
        if not type(value) is list:
            value = [int(value)]
        self.__writeRegisters(address, value)
        if timeout > 0:
            self.post.__reset(address, timeout)

    def __reset(self, address, timeout):
        """
            Resets a register to 0 after a specific amount of time
            :param address: The register address to reset
            :type address: int
            :param timeout: The delay after the register is reset
            :type timeout: float
        """
        rospy.sleep(timeout)
        self.__writeRegisters(address, [0])

    def closeConnection(self):
        """
            Closes the connection to the modbus
        """
        self.client.close()