def statusInterpreter(status):
# Variable Initialization
gripperStatus = 0
gripperMovement = 0
# Gripper Reset
if (status.gACT == 0) and (status.gIMC == 0):
gripperStatus = 0
# Gripper Activating
if (status.gACT == 1) and (status.gIMC == 1) and (status.gSTA == 0):
gripperStatus = 1
# Gripper Activated
if (status.gACT == 1) and (status.gIMC == 3):
gripperStatus = 2
# Gripper Open
if (status.gPOA < 110) and (status.gPOB < 110) and (status.gPOC < 110):
handClosed = False
# Gripper Closed
if (status.gPOA >= 110) and (status.gPOB >= 110) and (status.gPOC >= 110):
handClosed = True
# Gripper Moving
if (status.gDTA == 0) and (status.gDTA == 0) and (status.gDTA == 0):
handMoving = True
# Gripper Stopped
if (status.gDTA == 3) and (status.gDTA == 3) and (status.gDTA == 3):
handMoving = False
# Hand Moving and Opening
if handMoving and not handClosed:
gripperMovement = 0
# Hand Moving and Closing
if handMoving and handClosed:
gripperMovement = 1
# Hand Stoppend and Open
if not handMoving and not handClosed:
gripperMovement = 2
# Hand Stopped and Closed
if not handMoving and handClosed:
gripperMovement = 3
output = HoldingRegister()
output.data = [gripperStatus, gripperMovement]
# rospy.loginfo("Updating Robot Registers")
pub_robot.publish(output)
def setValues(self, address, value):
"""
Sets the requested values to the holding registers
and publishes they new values on a rostopic
:param address: The starting address
:type address: int
:param values: The new values to be set
:type values: list[int]
"""
ModbusSequentialDataBlock.setValues(self, address, value)
if address >= self.reading_start:
msg = HoldingRegister()
msg.data = value
self.pub.publish(msg)
def setValues(self, address, value):
"""
Sets the requested values to the holding registers
and publishes they new values on a rostopic
:param address: The starting address
:type address: int
:param values: The new values to be set
:type values: list[int]
"""
# print "address",address,"value",value
ModbusSequentialDataBlock.setValues(self,address, value)
if address >= self.reading_start:
msg = HoldingRegister()
msg.data = value
self.pub.publish(msg)
rospy.loginfo("Modbus server registers have been updated: %s",
str(msg.data))
sub = rospy.Subscriber("modbus_wrapper/input",
HoldingRegister,
showUpdatedRegisters,
queue_size=500)
#################
#################
# Example 3
# writing to modbus registers using a standard ros publisher
pub = rospy.Publisher("modbus_wrapper/output",
HoldingRegister,
queue_size=500)
output = HoldingRegister()
output.data = xrange(20, 40)
output2 = HoldingRegister()
output2.data = xrange(40, 20, -1)
rospy.loginfo("Sending arrays to the modbus server")
while not rospy.is_shutdown():
rospy.sleep(1)
pub.publish(output)
rospy.sleep(1)
pub.publish(output2)
#################
# Stops the listener on the modbus
modclient.stopListening()
rospy.loginfo("Modbus register have been written: %s", str(msg.data))
rospy.sleep(2)
sub = rospy.Subscriber("modbus_server/read_from_registers",
HoldingRegister,
callback,
queue_size=500)
###############
###############
# Example 4
# Publisher to write first 20 modbus registers from any node
pub = rospy.Publisher("modbus_server/write_to_registers",
HoldingRegister,
queue_size=500)
rospy.sleep(1)
msg = HoldingRegister()
msg.data = range(20)
msg2 = HoldingRegister()
msg2.data = range(20, 0, -1)
while not rospy.is_shutdown():
pub.publish(msg)
rospy.sleep(1)
pub.publish(msg2)
rospy.sleep(1)
################
rospy.spin()
mws.stopServer()
###############
# Example 3
# Listen for the writeable modbus registers in any node
def callback(msg):
rospy.loginfo("Modbus register have been written: %s",str(msg.data))
rospy.sleep(2)
sub = rospy.Subscriber("modbus_server/read_from_registers",HoldingRegister,callback,queue_size=500)
###############
###############
# Example 4
# Publisher to write first 20 modbus registers from any node
pub = rospy.Publisher("modbus_server/write_to_registers",HoldingRegister,queue_size=500)
rospy.sleep(1)
msg = HoldingRegister()
msg.data = range(20)
msg2 = HoldingRegister()
msg2.data = range(20,0,-1)
while not rospy.is_shutdown():
pub.publish(msg)
rospy.sleep(1)
pub.publish(msg2)
rospy.sleep(1)
################
rospy.spin()
mws.stopServer()
#################
# Example 2
# Create a listener that show us a message if anything on the readable modbus registers change
rospy.loginfo("All done. Listening to inputs... Terminate by Ctrl+c")
def showUpdatedRegisters(msg):
rospy.loginfo("Modbus server registers have been updated: %s",str(msg.data))
sub = rospy.Subscriber("modbus_wrapper/input",HoldingRegister,showUpdatedRegisters,queue_size=500)
#################
#################
# Example 3
# writing to modbus registers using a standard ros publisher
pub = rospy.Publisher("modbus_wrapper/output",HoldingRegister,queue_size=500)
output = HoldingRegister()
output.data = [1 for x in xrange(0,6)]
output2 = HoldingRegister()
output2.data = [0 for x in xrange(0,6)]
rospy.loginfo("Sending arrays to the modbus server")
for i in xrange(5):
rospy.sleep(1)
pub.publish(output)
rospy.sleep(1)
pub.publish(output2)
#################
rospy.loginfo("Outputs tests all done, just listening to inputs. Stop listening by pressing Ctrl+c")
rospy.spin()
# Stops the listener on the modbus
modclient.stopListening()
#################
# Example 2
# Create a listener that show us a message if anything on the readable modbus registers change
rospy.loginfo("All done. Listening to inputs... Terminate by Ctrl+c")
def showUpdatedRegisters(msg):
rospy.loginfo("Modbus server registers have been updated: %s",str(msg.data))
sub = rospy.Subscriber("modbus_wrapper/input",HoldingRegister,showUpdatedRegisters,queue_size=500)
#################
#################
# Example 3
# writing to modbus registers using a standard ros publisher
pub = rospy.Publisher("modbus_wrapper/output",HoldingRegister,queue_size=500)
output = HoldingRegister()
output.data = xrange(20,40)
output2 = HoldingRegister()
output2.data = xrange(40,20,-1)
rospy.loginfo("Sending arrays to the modbus server")
while not rospy.is_shutdown():
rospy.sleep(1)
pub.publish(output)
rospy.sleep(1)
pub.publish(output2)
#################
# Stops the listener on the modbus
modclient.stopListening()
sub = rospy.Subscriber("modbus_wrapper/input",
HoldingRegister,
showUpdatedRegisters,
queue_size=500)
rospy.spin()
#################
#################
# Example 3
# writing to modbus registers using a standard ros publisher
pub = rospy.Publisher("modbus_wrapper/output",
HoldingRegister,
queue_size=500)
output = HoldingRegister()
output.data = [1 for x in range(0, 6)]
output2 = HoldingRegister()
output2.data = [0 for x in range(0, 6)]
rospy.loginfo("Sending arrays to the modbus server")
for i in xrange(5):
rospy.sleep(1)
pub.publish(output)
rospy.sleep(1)
pub.publish(output2)
#################
rospy.loginfo(
"Outputs tests all done, just listening to inputs. Stop listening by pressing Ctrl+c"
)
# Stops the listener on the modbus