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))
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()
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 __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 __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 __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)
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)
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)
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 = {}
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)
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()
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()
