Example #1
0
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
Example #2
0
 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
Example #3
0
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..."