def msgFromKukaCb(self, msg):
try:
fdata = String()
edata = String()
edata.data = 's'
tid = ET.fromstring(msg.data).find("ReturnTaskID").text
prg = ET.fromstring(msg.data).find("ReturnTaskProgress").text
fdata.data = tid + "," + prg
if (self.ignore is None):
self.feedbackPublisher.publish(fdata)
if (float(prg) == 50):
self.effectorPublisher.publish(edata)
else:
if (self.ignoreCounter == 0):
self.feedbackPublisher.publish(fdata)
if (float(prg) == 50):
self.effectorPublisher.publish(edata)
self.ignoreCounter += 1
self.ignoreCounter %= self.ignore
except:
util.logErr("Parsing Error! This message will be ignored.")
#eof
#eoc
def msgFromKukaCb(self, msg):
try:
data = A6()
val = ET.fromstring(msg.data).find("ASPos")
data.A1 = float(val.get('A1'))
data.A2 = float(val.get('A2'))
data.A3 = float(val.get('A3'))
data.A4 = float(val.get('A4'))
data.A5 = float(val.get('A5'))
data.A6 = float(val.get('A6'))
self.valuePublisher.publish(data)
if (self.ignore is not None):
if (self.ignoreCounter == 0):
self.valueWithDelayPublisher.publish(data)
self.ignoreCounter += 1
self.ignoreCounter %= self.ignore
except:
util.logErr("Parsing Error! This message will be ignored.")
#eof
#eoc
def writeToSerialCb(self, msg):
if(self.serial is not None and self.serial.isOpen()):
try:
self.serial.write(msg.data)
except:
util.logErr("Error in writing to serial port")
else:
util.logErr("Serial port is not ready")
def main(argv=None):
rospy.init_node('progress_controller', anonymous=True)
util.logBeginningInfo("progress_controller")
ignore, feedbackTopic, effectorTopic = getArgs()
if (feedbackTopic is not None and effectorTopic is not None):
Controller(feedbackTopic, effectorTopic, ignore)
rospy.spin()
else:
util.logErr("feedbackTopic has to be specified...")
def sendToKuka(self):
if (self.msgTemplateString is not None and self.kukaIp is not None
and self.kukaPort is not None):
msgStr = self.regex.sub(self.lastIPOC, self.msgTemplateString)
self.msgToKukaSock.sendto(msgStr, (self.kukaIp, self.kukaPort))
self.msgToKukaPublisher.publish(msgStr)
self.setStatus(rsiStatus.RSI_RUNNING)
else:
util.logErr(
"First message template or first handshake message has not arrived yet! Can not send data/start rsi"
)
self.setStatus(rsiStatus.WAITING_FOR_TEMPLATE)
def msgTemplateCb(self, msg):
try:
if (ET.fromstring(msg.data).find("IPOC") is None):
util.logErr(
"Missing IPOC! IPOC tag has to be included in message template. Can not set message template by this message..."
)
util.logErr(msg.data)
return
firstMsg = False
if (self.msgTemplateString is None):
firstMsg = True
self.msgTemplateString = msg.data
if (firstMsg):
util.logWarn("First message template recieved!")
self.setStatus(rsiStatus.WAITING_FOR_HANDSHAKE)
except:
util.logErr(
"Invalid xml structure! Can not set message template by this message..."
)
util.logErr(msg.data)
def readlineThread(self):
while(True):
try:
if(self.serial is not None):
if(not self.serial.isOpen()):
util.logErr("Port is close. Set the serial port to None!")
self.serial = None
elif(self.serial.inWaiting()>0):
data = self.serial.readline()
self.readlineFormSerialPublisher.publish(data)
else:
sleep(2)
except IOError:
util.logErr("Port is close. Set the serial port to None!")
self.serial = None
except:
util.logErr("Error is readline function")
sleep(2)
#eof