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
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 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
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
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
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
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
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