Esempio n. 1
0
    def xml_callback(self, chunk):
        #rospy.loginfo('*******************In PROCESS_XML callback***************')
        self.lock.acquire()
        try:
            # Only grab XML elements for machine tool action requests 
            _, elements, self.action_goals = bridge_library.xml_components(chunk, self.ns, self.action_list, get_goal = True, action_goals = self.action_goals)

            if elements:
                # Check for existing handshake, execute it first
                if self.handshake:
                    for e in elements:
                        if self.handshake == e.attrib['name'] and e.text == 'READY':
                            # Send hand shake signal
                            bridge_library.action_cb((self.adapter, self.di_dict, e.attrib['name'], 'READY'))
                            self.handshake = None
                            elements.remove(e)
                    if self.handshake != None:
                        rospy.logerr('DID NOT RECEIVE READY HANDSHAKE FROM MACHINE TOOL') 
                # Process remaining action requests
                for e in elements:
                    # Remove XML namespace string from the element tag for hash tables
                    action_text = re.findall(r'(?<=\})\w+',e.tag)[0]
                    
                    # Check if machine tool is requesting an action, if so, run action client
                    if e.text == 'ACTIVE':
                        self.handshake = self.action_client((action_text, self.action_goals[action_text], self.type_handle))
                        #self.handshake = e.attrib['name']
                    
        except Exception as e:
            rospy.logerr("Generic Action Client: Process XML callback failed: %s, releasing lock" % e)
        finally:
            self.lock.release()
        #rospy.loginfo('*******************Done with PROCESS_XML callback***************')
        return
Esempio n. 2
0
 def robot_state_callback(self, request):
     rospy.loginfo('SERVICE REQUEST --> %s' % request.state_flag)
     response_service_class = getattr(self.service_hndle, self.service_state + 'Response')
     if request.state_flag == -1: # NOT_READY
         try:
             bridge_library.action_cb((self.adapt, self.data_item_dict, self.as_name, 'NOT_READY'))
             return response_service_class(True)
         except:
             return response_service_class(False)
     elif request.state_flag == 0: # READY
         try:
             bridge_library.action_cb((self.adapt, self.data_item_dict, self.as_name, 'READY'))
             return response_service_class(True)
         except:
             return response_service_class(False)
Esempio n. 3
0
    def xml_callback(self, chunk):
        #rospy.loginfo('*******************In PROCESS_XML callback***************')
        self.lock.acquire()
        try:
            # Only grab XML elements for machine tool action requests
            _, elements, self.action_goals = bridge_library.xml_components(
                chunk,
                self.ns,
                self.action_list,
                get_goal=True,
                action_goals=self.action_goals)

            if elements:
                # Check for existing handshake, execute it first
                if self.handshake:
                    for e in elements:
                        if self.handshake == e.attrib[
                                'name'] and e.text == 'READY':
                            # Send hand shake signal
                            bridge_library.action_cb(
                                (self.adapter, self.di_dict, e.attrib['name'],
                                 'READY'))
                            self.handshake = None
                            elements.remove(e)
                    if self.handshake != None:
                        rospy.logerr(
                            'DID NOT RECEIVE READY HANDSHAKE FROM MACHINE TOOL'
                        )
                # Process remaining action requests
                for e in elements:
                    # Remove XML namespace string from the element tag for hash tables
                    action_text = re.findall(r'(?<=\})\w+', e.tag)[0]

                    # Check if machine tool is requesting an action, if so, run action client
                    if e.text == 'ACTIVE':
                        self.handshake = self.action_client(
                            (action_text, self.action_goals[action_text],
                             self.type_handle))
                        #self.handshake = e.attrib['name']

        except Exception as e:
            rospy.logerr(
                "Generic Action Client: Process XML callback failed: %s, releasing lock"
                % e)
        finally:
            self.lock.release()
        #rospy.loginfo('*******************Done with PROCESS_XML callback***************')
        return
Esempio n. 4
0
 def robot_state_callback(self, request):
     rospy.loginfo('SERVICE REQUEST --> %s' % request.state_flag)
     response_service_class = getattr(self.service_hndle,
                                      self.service_state + 'Response')
     if request.state_flag == -1:  # NOT_READY
         try:
             bridge_library.action_cb((self.adapt, self.data_item_dict,
                                       self.as_name, 'NOT_READY'))
             return response_service_class(True)
         except:
             return response_service_class(False)
     elif request.state_flag == 0:  # READY
         try:
             bridge_library.action_cb(
                 (self.adapt, self.data_item_dict, self.as_name, 'READY'))
             return response_service_class(True)
         except:
             return response_service_class(False)
    def data_item_conversion(self, topic_name, type_name, msg_data, msg_text, constants):
        # Set the Robot XML data item via the MTConnect Adapter
        for member_data in msg_data:
            
            # Iterate through list of ROS message CONSTANTS and set the MTConnect Adapter value
            for const_val in constants[member_data[0]]:
                if const_val in self.config[topic_name][type_name][member_data[0]].keys():
                    # If ROS to MTConnect mapping dictionary contains the constant, set the adapter value 
                    adapter_val = self.config[topic_name][type_name][member_data[0]][const_val]
                    break
                else:
                    adapter_val = None
            
            if adapter_val is None:
                rospy.logerr('ROS to MTConnect Mapping failed')

            # Set the Robot XML data item
            bridge_library.action_cb((self.adapter, self.di_dict, member_data[0], adapter_val))
        return   
Esempio n. 6
0
    def data_item_conversion(self, topic_name, type_name, msg_data, msg_text,
                             constants):
        # Set the Robot XML data item via the MTConnect Adapter
        for member_data in msg_data:

            # Iterate through list of ROS message CONSTANTS and set the MTConnect Adapter value
            for const_val in constants[member_data[0]]:
                if const_val in self.config[topic_name][type_name][
                        member_data[0]].keys():
                    # If ROS to MTConnect mapping dictionary contains the constant, set the adapter value
                    adapter_val = self.config[topic_name][type_name][
                        member_data[0]][const_val]
                    break
                else:
                    adapter_val = None

            if adapter_val is None:
                rospy.logerr('ROS to MTConnect Mapping failed')

            # Set the Robot XML data item
            bridge_library.action_cb(
                (self.adapter, self.di_dict, member_data[0], adapter_val))
        return
Esempio n. 7
0
    def action_client(self, cb_data):
        # Execute ROS Action
        rospy.init_node('ActionClient')
        
        # Unpack action client data
        name, goals, handle = cb_data
        rospy.loginfo('Launched %s Action CLient' % name)
        
        # Creates the SimpleActionClient, passing the type of the action (MaterialLoadAction) to the constructor.
        # i.e. name = 'MaterialLoad', action_type = mtconnect_msgs.msg.MaterialLoadAction
        action_type = getattr(handle, name + 'Action')
        client = actionlib.SimpleActionClient(name + 'Client', action_type)
        
        # Waits until the action server has started up and started listening for goals.
        rospy.loginfo('Waiting for %s Dedicated Action Server' % name)
        client.wait_for_server()
        rospy.loginfo('%s Dedicated Action Server Activated' % name)

        # Creates a MaterialLoad goal to send to the action server.
        # i.e. goal = mtconnect_msgs.msg.MaterialLoad()
        goal_handle = getattr(handle, name + 'Goal')
        goal = goal_handle()
        
        # Check to make sure the action goal has been set
        if self.action_goals[name] == None:
            # Ping the current url for the goal Event
            goal_tag = self.action_list[name].keys()[0]
            response = bridge_library.xml_get_response((self.url, self.url_port, self.port, self.conn, 
                                                        self.mtool + "/current?path=//DataItem[@type='" + goal_tag.upper() +"']"))
            body = response.read()
            root = ElementTree.fromstring(body)

            # Set the action goals
            self.action_goals = bridge_library.set_goal(name, self.action_list[name], root, self.ns, self.action_goals)
            
        # Check goal source for required attributes
        for attrib, attrib_type, action_goal in zip(goal_handle.__slots__, goal_handle._slot_types, self.action_goals[name]):
            if bridge_library.type_check(attrib_type, action_goal) == False:
                rospy.logerr('INCOMPATIBLE GOAL TYPES ROS MSG: %s --> XML: %s' % (attrib_type, type(action_goal)))
                sys.exit()
        
        # Set the goal attributes from XML data
        try:
            for attrib, xml_goal in zip(goal_handle.__slots__, self.action_goals[name]):
                setattr(goal, attrib, xml_goal)
        except Exception as e:
            rospy.logerr("ROS Action %s Client failed: %s" % (name, e))
        
        # Sends the goal to the action server.
        rospy.loginfo('Sending the goal')
        name_conv = bridge_library.split_event(name)
        client.send_goal(goal, done_cb = None, active_cb = bridge_library.action_cb((self.adapter, self.di_dict, name_conv, 'ACTIVE')))
        
        
        # Waits for the server to finish performing the action.
        rospy.loginfo('Waiting for result')
        client.wait_for_result()
        
        # Obtain result state
        result = client.get_state()
        
        # Prints out the result of the executing action
        rospy.loginfo('Returning the result --> %s' % result)
        
        # Set the Robot XML data item
        data_item = bridge_library.split_event(name)
        
        # Submit converted result to host via MTConnect adapter
        if result == 3: # SUCCEEDED from actionlib_msgs.msg.GoalStatus
            rospy.loginfo('Sending COMPLETE flag')
            bridge_library.action_cb((self.adapter, self.di_dict, data_item, 'COMPLETE'))
            return name
        elif result == 4: # ABORTED from actionlib_msgs.msg.GoalStatus
            rospy.loginfo('%s ABORTED, terminating action request' % data_item)
            bridge_library.action_cb((self.adapter, self.di_dict, data_item, 'FAIL'))
            return None
Esempio n. 8
0
    def action_client(self, cb_data):
        # Execute ROS Action
        rospy.init_node('ActionClient')

        # Unpack action client data
        name, goals, handle = cb_data
        rospy.loginfo('Launched %s Action CLient' % name)

        # Creates the SimpleActionClient, passing the type of the action (MaterialLoadAction) to the constructor.
        # i.e. name = 'MaterialLoad', action_type = mtconnect_msgs.msg.MaterialLoadAction
        action_type = getattr(handle, name + 'Action')
        client = actionlib.SimpleActionClient(name + 'Client', action_type)

        # Waits until the action server has started up and started listening for goals.
        rospy.loginfo('Waiting for %s Dedicated Action Server' % name)
        client.wait_for_server()
        rospy.loginfo('%s Dedicated Action Server Activated' % name)

        # Creates a MaterialLoad goal to send to the action server.
        # i.e. goal = mtconnect_msgs.msg.MaterialLoad()
        goal_handle = getattr(handle, name + 'Goal')
        goal = goal_handle()

        # Check to make sure the action goal has been set
        if self.action_goals[name] == None:
            # Ping the current url for the goal Event
            goal_tag = self.action_list[name].keys()[0]
            response = bridge_library.xml_get_response(
                (self.url, self.url_port, self.port, self.conn, self.mtool +
                 "/current?path=//DataItem[@type='" + goal_tag.upper() + "']"))
            body = response.read()
            root = ElementTree.fromstring(body)

            # Set the action goals
            self.action_goals = bridge_library.set_goal(
                name, self.action_list[name], root, self.ns, self.action_goals)

        # Check goal source for required attributes
        for attrib, attrib_type, action_goal in zip(goal_handle.__slots__,
                                                    goal_handle._slot_types,
                                                    self.action_goals[name]):
            if bridge_library.type_check(attrib_type, action_goal) == False:
                rospy.logerr(
                    'INCOMPATIBLE GOAL TYPES ROS MSG: %s --> XML: %s' %
                    (attrib_type, type(action_goal)))
                sys.exit()

        # Set the goal attributes from XML data
        try:
            for attrib, xml_goal in zip(goal_handle.__slots__,
                                        self.action_goals[name]):
                setattr(goal, attrib, xml_goal)
        except Exception as e:
            rospy.logerr("ROS Action %s Client failed: %s" % (name, e))

        # Sends the goal to the action server.
        rospy.loginfo('Sending the goal')
        name_conv = bridge_library.split_event(name)
        client.send_goal(goal,
                         done_cb=None,
                         active_cb=bridge_library.action_cb(
                             (self.adapter, self.di_dict, name_conv,
                              'ACTIVE')))

        # Waits for the server to finish performing the action.
        rospy.loginfo('Waiting for result')
        client.wait_for_result()

        # Obtain result state
        result = client.get_state()

        # Prints out the result of the executing action
        rospy.loginfo('Returning the result --> %s' % result)

        # Set the Robot XML data item
        data_item = bridge_library.split_event(name)

        # Submit converted result to host via MTConnect adapter
        if result == 3:  # SUCCEEDED from actionlib_msgs.msg.GoalStatus
            rospy.loginfo('Sending COMPLETE flag')
            bridge_library.action_cb(
                (self.adapter, self.di_dict, data_item, 'COMPLETE'))
            return name
        elif result == 4:  # ABORTED from actionlib_msgs.msg.GoalStatus
            rospy.loginfo('%s ABORTED, terminating action request' % data_item)
            bridge_library.action_cb(
                (self.adapter, self.di_dict, data_item, 'FAIL'))
            return None
Esempio n. 9
0
 def execute_cb(self, goal):
     # If goal is OpenDoor, use the goal defined in the message as your key, access it as goal.__slots__[0]
     action = goal.__slots__[0]
     
     # Empty function -- assumes action was successful
     rospy.loginfo('In %s Callback -- determining action request result.' % self.server_name[action])
     
     # Check to make sure machine tool is READY, if not, ABORT the robot action request
     
     # Change to XML Tag format
     tokens = re.findall(r'([A-Z][a-z]*)', self.server_name[action])
     tokenlist = [val.upper() for val in tokens]
     di_tag = tokenlist[0] + '_' + tokenlist[1]
     
     # Check current status of XML Tag
     check_response = bridge_library.xml_get_response((self.url, self.url_port, self.port, self.conn, self.mtool + "/current?path=//DataItem[@type='" + di_tag + "']"))
     body = check_response.read()
     _, element = bridge_library.xml_components(body, self.ns, {self.server_name[action]:self.server_name[action]})
     
     # Complete the action request
     if element[0].text == 'READY': # Execute the action request
         # Submit goal value to MTConnect via agent (i.e. goal.open_door = 'ACTIVE')
         bridge_library.action_cb((self.adapter, self.di_dict, action, 'ACTIVE'))
         
         robot_hold = 0 # Used to make sure you only change robot state once after 'COMPLETE' received from CNC
         
         # Start capturing XML
         self.capture_xml = True
         
         # While loop to poll CNC XML until READY is received for Robot action request
         dwell = True
         while dwell == True:
             try:
                 # Obtain XML chunk
                 if not self.XML_queue.empty():
                     chunk = self.XML_queue.get()
                     
                     # Parse the XML and determine the current sequence and XML Event elements
                     root = ElementTree.fromstring(chunk)
                     element_list = root.findall('.//m:' + self.server_name[action], namespaces = self.ns)
                     if len(element_list) > 1:
                         rospy.logdebug('XML --> %s' % chunk)
                     if element_list:
                         # Must iterate -- multiple elements possible for a single tag
                         for element in element_list:
                             if element.text == 'ACTIVE':
                                 # Set accepted back to action client
                                 pass
                             
                             # While polling monitor CNC response for COMPLETE, submit READY handshake
                             elif element.text == 'COMPLETE' and robot_hold == 0:
                                 bridge_library.action_cb((self.adapter, self.di_dict, action, 'READY'))
                                 robot_hold = 1
                             
                             elif element.text == 'READY' and robot_hold == 1:
                                 dwell = False
                                 self.capture_xml = False
                                 
                                 # When response is READY, set server result and communicate as below:
                                 # Extract action attribute
                                 result_attribute = self._resultDict[self.server_name[action]].__slots__[0]
                                 
                                 # Set the attribute per the ROS to MTConnect conversion
                                 setattr(self._resultDict[self.server_name[action]], result_attribute, 'READY')
                                 
                                 # Indicate a successful action
                                 self._as[self.server_name[action]].set_succeeded(self._resultDict[self.server_name[action]])
                                 rospy.loginfo('In %s Callback -- action succeeded.' % self.server_name[action])
                             
                             elif element.text == 'FAIL':
                                 bridge_library.action_cb((self.adapter, self.di_dict, action, 'FAIL'))
                                 dwell = False
                                 self.capture_xml = False
                                 
                                 # When response is FAIL, set server result and communicate as below:
                                 # Extract action attribute.  For CNC Actions, only one result --> i.e. OpenDoorResult.__slots__[0] = 'door_ready'
                                 result_attribute = self._resultDict[self.server_name[action]].__slots__[0]
                                 
                                 # Set the attribute per the ROS to MTConnect conversion
                                 setattr(self._resultDict[self.server_name[action]], result_attribute, 'FAIL')
                                 
                                 # Indicate an aborted action
                                 self._as[self.server_name[action]].set_aborted(self._resultDict[self.server_name[action]])
                                 rospy.loginfo('In %s Callback -- action aborted by CNC.' % self.server_name[action])
                     
                     # Release the queue
                     self.XML_queue.task_done()
             except rospy.ROSInterruptException:
                 rospy.loginfo('program interrupted before completion')
     else: # Abort the action request, machine tool not ready or faulted
         # Extract action attribute.  For CNC Actions, only one result --> i.e. OpenDoorResult.__slots__[0] = 'door_ready'
         result_attribute = self._resultDict[self.server_name[action]].__slots__[0]
         
         # Set the attribute per the ROS to MTConnect conversion
         setattr(self._resultDict[self.server_name[action]], result_attribute, 'NOT_READY')
         
         # Indicate an aborted action
         self._as[self.server_name[action]].set_aborted(self._resultDict[self.server_name[action]])
         rospy.loginfo('In %s Callback -- action aborted, machine tool NOT_READY, FAULTED, or UNAVAILABLE.' % self.server_name[action])
     return