def getOrderList(max_orders):
ret = []
r = requests.get("http://192.168.10.100/orders/", stream = True)
dom = parseString(r.text)
n = dom.getElementsByTagName('order');
number_of_orders = max_orders
if number_of_orders > n.length:
number_of_orders = n.length
for i in range(0, number_of_orders):
line = dom.getElementsByTagName('order')[i].toxml()
requestString = line.replace('<order>','').replace('</order>','')
r = requests.get(requestString, stream = True)
orderXml = parseString(r.text)
orderData = order()
xmlRed = orderXml.getElementsByTagName('red')[0].toxml()
xmlBlue = orderXml.getElementsByTagName('blue')[0].toxml()
xmlYellow = orderXml.getElementsByTagName('yellow')[0].toxml()
xmlTime = orderXml.getElementsByTagName('time')[0].toxml()
xmlStatus = orderXml.getElementsByTagName('status')[0].toxml()
orderData.order_id = int(requestString.replace('http://192.168.10.100/orders/ord_',''))
if xmlStatus.replace('<status>','').replace('</status>','') == "ready":
orderData.status = orderData.STATUS_READY
elif xmlStatus.replace('<status>','').replace('</status>','') == "taken":
orderData.status = orderData.STATUS_TAKEN
else:
orderData.status = orderData.STATUS_UNKNOWN
orderData.time = xmlTime.replace('<time>','').replace('</time>','')
orderData.red_bricks = int(xmlRed.replace('<red>','').replace('</red>',''))
orderData.blue_bricks = int(xmlBlue.replace('<blue>','').replace('</blue>',''))
orderData.yellow_bricks = int(xmlYellow.replace('<yellow>','').replace('</yellow>',''))
ret.append(orderData)
return ret
def updateOrderList(self, max_orders):
orderUrl = serverAddress + "/orders/" + str(self.__NoOfOrders)
[status_code, response] = getRequest(orderUrl)
# Check if request was succesful
if status_code != 200:
print "Updating orderlist failed, error code: %d" % status_code
return
dom = parseString(response)
n = dom.getElementsByTagName('order');
number_of_orders = max_orders
if number_of_orders > n.length:
number_of_orders = n.length
# create list of order urls
OrderUrls = []
for i in range(0, number_of_orders):
OrderUrls.append(dom.getElementsByTagName('order')[i].toxml().replace('<order>','').replace('</order>',''))
self.__OrderUrls = OrderUrls
# Remove taken orders
OrdersToRemove = []
for i in range(len(self.__OrderList)):
isNotPresent = 1
for j in range(len(self.__OrderUrls)):
if self.__OrderList[i].order_id == int(self.__OrderUrls[j].replace(serverAddress + '/orders/ord_','')):
isNotPresent = 0
break
if isNotPresent:
OrdersToRemove.append(self.__OrderList[i])
for i in range(len(OrdersToRemove)):
self.__OrderList.remove(OrdersToRemove[i])
# Add new orders
for i in range(len(self.__OrderUrls)):
isPresent = 0
for j in range(len(self.__OrderList)):
if int(self.__OrderUrls[i].replace(serverAddress + '/orders/ord_','')) == self.__OrderList[j].order_id:
isPresent = 1
break
if not isPresent:
[status_code, response] = getRequest(self.__OrderUrls[i])
if status_code != 200:
print "Error getting order, error_code: %d" % status_code
else:
r = requests.get(self.__OrderUrls[i], stream = True)
orderXml = parseString(response)
orderData = order()
orderData.order_id = int(self.__OrderUrls[i].replace(serverAddress + '/orders/ord_',''))
orderData.red_bricks = int(orderXml.getElementsByTagName('red')[0].toxml().replace('<red>','').replace('</red>',''))
orderData.blue_bricks = int(orderXml.getElementsByTagName('blue')[0].toxml().replace('<blue>','').replace('</blue>',''))
orderData.yellow_bricks = int(orderXml.getElementsByTagName('yellow')[0].toxml().replace('<yellow>','').replace('</yellow>',''))
orderData.time = orderXml.getElementsByTagName('time')[0].toxml().replace('<time>','').replace('</time>','')
xmlStatus = orderXml.getElementsByTagName('status')[0].toxml()
if xmlStatus.replace('<status>','').replace('</status>','') == "ready":
orderData.status = orderData.STATUS_READY
elif xmlStatus.replace('<status>','').replace('</status>','') == "taken":
orderData.status = orderData.STATUS_TAKEN
else:
orderData.status = orderData.STATUS_UNKNOWN
self.__OrderList.append(orderData)
# Copy order list to public orderlist
dataLock.acquire()
self.OrderList = self.__OrderList
dataLock.release()
return
def orderHandler():
rospy.init_node('OrderHandler')
print "OrderHandler started"
# Init
global loop_rate
loop_rate = rospy.get_param("~loop_rate", 10)
global state
state.state = state.PML_IDLE
global next_state
next_state.state = state.PML_IDLE
global last_state
last_state.state = state.PML_IDLE
# Subscriber for external state change
changeStateTopic = rospy.get_param("~change_state_sub_topic", "/bpOrderHandler/change_state")
rospy.Subscriber(changeStateTopic, system_state, changeStateCallback)
# Subscriber for detected legobricks
brickTopic = rospy.get_param("~brick_sub_topic", "/bpBrickDetector/bricks")
rospy.Subscriber(brickTopic, brick, brickCallback)
# Publisher for the robot motion planner
robotplannerPubTopic = rospy.get_param("~robot_planner_pub_topic", "/bpRobotMotionController/brick_command_topic")
global robot_publisher
robot_publisher = rospy.Publisher(robotplannerPubTopic, robot_pick)
# Subscriber for the robot planner messages
robotplannerSubTopic = rospy.get_param("~robot_planner_sub_topic", "/bpRobotMotionController/brick_response_topic")
rospy.Subscriber(robotplannerSubTopic, robot_pick, robotCallback)
# Add dynamic reconfiguration server
dyn_srv = Server(offset_paramsConfig, dynamic_reconfiguration_callback)
global robot_queue
robot_queue = 0
# Add publisher for the OEE data
counter = 0 # counter for slowing down the calculations
oeeDataPubTopic = rospy.get_param("~oee_data_publisher_topic", "/oee")
oeeDataPublisher = rospy.Publisher(oeeDataPubTopic, oee_data)
# timers for the OEE calculations
TotalRuntime = 0
ProductionTime = 0
idealOrderTime = 45
# Emergency stop subscriber
emergencyStopSubTopic = rospy.get_param("~emergency_stop_topic", "/emergency_stop")
rospy.Subscriber(emergencyStopSubTopic, Bool, emergencyStopCallback)
# System state publisher for the GUI
systemStatePubTopic = rospy.get_param("~system_state_publisher_topic", "/bpOrderHandler/system_state")
systemStatePublisher = rospy.Publisher(systemStatePubTopic, system_state)
# Log publisher
logPubTopic = rospy.get_param("~log_publisher_topic", "/log")
global logPublisher
logPublisher = rospy.Publisher(logPubTopic, String)
# Order data publisher
orderDataPubTopic = rospy.get_param("~order_data_publisher_topic", "/bpOrderHandler/order_status")
global orderDataPublisher
orderDataPublisher = rospy.Publisher(orderDataPubTopic, order)
global ordersDone
global totalOrders
global currentOrders
global robot_queue
# End of Init
# PML statemachine loop
while not rospy.is_shutdown():
TotalRuntime += 1.0/loop_rate
if hasStateChanged():
rospy.loginfo("System state changed from %d to %d" % (state.state, next_state.state))
last_state.state = state.state
state.state = next_state.state
if state.state == state.PML_IDLE:
logState(last_state, state)
elif state.state == state.PML_EXECUTE:
logState(last_state, state)
execute(False)
ProductionTime += 1.0/loop_rate
elif state.state == state.PML_COMPLETE:
rospy.loginfo("PML_COMPLETE")
elif state.state == state.PML_HELD:
rospy.loginfo("PML_HELD")
elif state.state == state.PML_SUSPENDED:
rospy.loginfo("PML_SUSPENDED")
elif state.state == state.PML_ABORTED:
logState(last_state, state)
elif state.state == state.PML_STOPPED:
logState(last_state, state)
# Transition states
elif state.state == state.PML_START: #Start the system from Idle state ending in execute
if (last_state.state != state.PML_IDLE):
logPublisher.publish("System can only be started from IDLE.. Try resetting system")
state.state = last_state.state
next_state.state = last_state.state
else:
next_state.state = state.PML_EXECUTE
plcService = rospy.ServiceProxy('/plc_controller/plc_command', plc_command)
request = plc_commandRequest()
request.command_number = request.START_BELT
response = plcService(request)
rospy.loginfo("PML_START")
elif state.state == state.PML_STOP: #Controlled stop - finish orders
rospy.loginfo("PML_STOP")
if (last_state.state != state.PML_EXECUTE and last_state.state != state.PML_STOP):
logPublisher.publish("System can only be stopped from EXECUTE..")
state.state = last_state.state
next_state.state = last_state.state
else:
if last_state.state != state.PML_STOP:
logPublisher.publish("System is stopping - current orders are finished first..")
execute(True)
if (currentOrders[0] == 0 and currentOrders[1] == 0 and currentOrders[2] == 0):
next_state.state = state.PML_STOPPED
plcService = rospy.ServiceProxy('/plc_controller/plc_command', plc_command)
request = plc_commandRequest()
request.command_number = request.STOP_BELT
response = plcService(request)
elif state.state == state.PML_RESET: #Reset after ESTOP and ABORTED
rospy.loginfo("PML_RESET")
if emergencyStopStatus == False:
if last_state.state == state.PML_STOPPED:
next_state.state = state.PML_IDLE
elif last_state.state == state.PML_ABORTED:
next_state.state = state.PML_IDLE
else:
if last_state != state.PML_RESET:
logPublisher.publish("System can only be resetted from STOPPED and ABORTED..")
state.state = last_state.state
next_state.state = last_state.state
elif last_state.state != state.PML_RESET:
logPublisher.publish("Emergency stop is engaged - release before reset!")
elif state.state == state.PML_ESTOP: #STOP the system and end in aborted
currentOrders[0] = 0
currentOrders[1] = 0
currentOrders[2] = 0
robot_queue = 0
rospy.loginfo("PML_ESTOP")
logPublisher.publish("ESTOP..! Release Emergency stop, reengage robot arm power, resume robot program, press reset and start to start system")
plcService = rospy.ServiceProxy('/plc_controller/plc_command', plc_command)
request = plc_commandRequest()
request.command_number = request.STOP_BELT
response = plcService(request)
next_state.state = state.PML_ABORTED
# Calculate and publish data for GUI
counter += 1
if (counter > loop_rate): #Runs once a second
counter = 0
# OEE data
oeeData = oee_data()
oeeData.availability = (ProductionTime / TotalRuntime)
oeeData.performance = ((idealOrderTime * ordersDone) / (ProductionTime + 0.00001))
oeeData.quality = ordersDone / (totalOrders + 0.00001)
oeeData.oee = oeeData.availability*oeeData.performance*oeeData.quality
oeeDataPublisher.publish(oeeData)
#System state
systemStatePublisher.publish(state)
#Order data
for i in range(len(currentOrders)):
if currentOrders[i] != 0:
tmpOrder = order()
tmpOrder.red_bricks = currentOrders[i].red_bricks
tmpOrder.yellow_bricks = currentOrders[i].yellow_bricks
tmpOrder.blue_bricks = currentOrders[i].blue_bricks
tmpOrder.order_id = currentOrders[i].order_id
tmpOrder.order_tray = currentOrders[i].order_tray
tmpOrder.header.stamp = currentOrders[i].header.stamp
tmpOrder.time = str(int(180 - (rospy.Time.now().to_sec() - currentOrders[i].header.stamp.to_sec())))
orderDataPublisher.publish(tmpOrder)
else:
tmpOrder = order()
tmpOrder.red_bricks = 0
tmpOrder.yellow_bricks = 0
tmpOrder.blue_bricks = 0
tmpOrder.order_id = 0
tmpOrder.order_tray = i+1
tmpOrder.header.stamp = rospy.Time.now()
tmpOrder.time = str(0)
orderDataPublisher.publish(tmpOrder)
last_state.state = state.state
rospy.sleep(1.0/loop_rate)
rospy.spin()
print "OrderHandler shutting down..."
