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()
