class BridgeAdapter: def __init__(self): self._adapter = Adapter(('0.0.0.0', 7878)) self._pose = ThreeDSample('pose') self._adapter.add_data_item(self._pose) avail = Event('avail') self._adapter.add_data_item(avail) avail.set_value('AVAILABLE') def start(self): def handle_turtle_pose(msg, turtlename): self._adapter.begin_gather() self._pose.set_value((msg.x, msg.y, 0.0)) self._adapter.complete_gather() self._adapter.start() rospy.Subscriber('/turtle1/pose', turtlesim.msg.Pose, handle_turtle_pose, 'turtle1')
class BridgeSubscriber(): ## @brief Constructor for a BridgeSubscriber def __init__(self): # Initialize the ROS Bridge subscriber node rospy.init_node('bridge_subscriber') # Read configuration file and extract topics and types self.config = bridge_library.obtain_dataMap() self.msg_parameters = ['url', 'adapter_port'] self.url = self.config[self.msg_parameters[0]] self.adapter_port = self.config[self.msg_parameters[1]] # Setup MTConnect Adapter for robot status data items self.adapter = Adapter((self.url, self.adapter_port)) self.di_dict = { } # XML data item dictionary to store MTConnect Adapter Events # Setup ROS topics as specified in .yaml file self.topic_name_list = [] # List of topic names self.subscribed_list = [] # List of subscribed topics self.lib_manifests = [] # Stores loaded ROS library manifests # Topic type and MTConnect message members from the module import self.topic_type_list = {} self.member_types = {} self.member_names = {} # The ROS message text retainer, used to map ROS values to MTConnect XML tag.text self.msg_text = {} # Create the data sets for the topic types, member names, and member types self.setup_topic_data() # Start the adapter rospy.loginfo('Start the Robot Link adapter') self.adapter.start() # Create class lock self.lock = thread.allocate_lock() # Loop through requested topics and spawn ROS subscribers when topics are available topic_list = self.topic_name_list dwell = 10 while topic_list: published_topics = dict(rospy.get_published_topics()) for topic_name in topic_list: if topic_name in published_topics.keys(): # Create ROS Subscriber idx = topic_list.index(topic_name) del topic_list[idx] self.topic_listener( (topic_name, self.topic_type_list[topic_name], self.member_names[topic_name], self.msg_text[topic_name])) else: rospy.loginfo( 'ROS Topic %s not available, will try to subscribe in %d seconds' % (topic_name, dwell)) time.sleep(dwell) ## @brief This function captures the topic name, type, and member attributes that are required for ## the ROS subscriber. This task is completed for each topic specified in the configuration file. ## ## This function then performs a relative import of the topic via the getattr(import_module) function. ## Data is stored in the following class instance attributes: ## ## self.topic_type_list --> used for module import and msg parameters. ## self.member_types --> member type, not used in msg parameters, future use. ## self.member_names --> used for ROS subscriber msg parameters. def setup_topic_data(self): for topic, topic_type in self.config.items(): if topic not in self.msg_parameters: self.topic_name_list.append(topic) # Only one type per topic type_key = topic_type.keys()[0] self.subscribed_list.append(type_key) # One set of data items per topic type self.data_items = [ data_item for data_item in topic_type[type_key].keys() ] # Add data items to the MTConnect Adapter - must be unique data items bridge_library.add_event( (self.adapter, self.data_items, self.di_dict, False)) # Extract package name and topic type name tokens = topic_type.keys()[0].split('/') package = tokens[0] type_name = tokens[1] # Load package manifest if unique if package not in self.lib_manifests: roslib.load_manifest(package) self.lib_manifests.append(package) # Import module and create topic type class, # i.e. append <class 'mtconnect_msgs.msg._RobotStates.RobotStates'> rospy.loginfo('Class Instance --> ' + package + '.msg.' + type_name) type_handle = getattr(import_module(package + '.msg'), type_name) self.topic_type_list[topic] = type_handle self.msg_text[topic] = type_handle._full_text self.member_types[topic] = type_handle._slot_types self.member_names[topic] = type_handle.__slots__ return ## @brief Callback function that captures the attribute values for a ROS topic. ## The topic data is stored in a list of tuples, where each tuple is a ## (attrib_name, attrib_value) pair. To access the integer value of the attribute ## value, use attrib_value.val. ## ## All data conversions between ROS and MTConnect are stored in the ROS ## subscriber YAML file. A separate function handles the ROS to MTConnect ## conversions for generic robot messages. ## @param data: callback ROS message data from the ROS subscriber ## @param cb_data: tuple containing the following parameters: ## @param topic_name: string defining the name of the ROS topic ## @param type_handle: class instance of the ROS topic message i.e. <class 'mtconnect_msgs.msg._RobotStates.RobotStates'> ## @param member_set: list of strings defining the attribute members for the message class ## @param msg_text: string of the entire message text (may be eliminated in future revisions) def topic_callback(self, data, cb_data): self.lock.acquire() try: (topic_name, type_handle, member_set, msg_text) = cb_data type_name = type_handle._type # Repackage members into a dictionary dout = {val: val for val in member_set} # Iterate through data items and capture message data msg_data = [] msg_constants = {} for dataitem in member_set: # Capture attribute for package/message type --> industrial_msgs/TriState.avail attrib_handle = getattr(type_handle(), dataitem) if 'header' not in dataitem: # Extract string representing message attribute --> 'val' val_key = attrib_handle.__slots__[0] # Capture message data.attrib --> <class message data>.dataitem: TriState.avail token = getattr(data, dataitem) # Capture the integer value for the attribute: avail.val item_value = getattr(token, val_key) # Store published message data for the topic msg_data.append((dataitem, item_value)) # Create a list of strings containing message CONSTANTS msg_constants[dataitem] = [] for attrib in dir(attrib_handle): if attrib.isupper(): if getattr(attrib_handle, attrib) == item_value: msg_constants[dataitem].append(attrib) # Execute the ROS to MTConnet conversion function self.data_item_conversion(topic_name, type_name, msg_data, msg_text, msg_constants) except Exception as e: rospy.logerr('Topic callback failed: %s, releasing lock' % e) finally: self.lock.release() return ## @brief A conversion function that will convert the ROS message value to the ## machine tool value. The conversion dictionary is provided in the configuration file. ## @param topic_name: string defining the name of the ROS topic ## @param type_name: string defining the name of the ROS topic type ## @param msg_data: list of strings defining message attributes ## @param msg_text: string of the entire message text (may be eliminated in future revisions) ## @param constants: list of ROS topic message type CONSTANTS stored as strings 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 ## @brief Main ROS subscriber function. A new thread is created for each callback. ## @param data: tuple containing the following parameters: ## @param topic_name: string defining the name of the ROS topic ## @param type_handle: class instance of the ROS topic message i.e. <class 'mtconnect_msgs.msg._RobotStates.RobotStates'> ## @param member_set: list of strings defining the message attributes ## @param msg_text: string of the entire message text (may be eliminated in future revisions) def topic_listener(self, data): # Unpack arguments topic_name, type_handle, member_set, msg_text = data # Launch the ROS subscriber rospy.Subscriber(topic_name, type_handle, self.topic_callback, data) return
path, file = os.path.split(__file__) sys.path.append(os.path.realpath(path) + '/../src') from data_item import Event, SimpleCondition, Sample from mtconnect_adapter import Adapter if __name__ == "__main__": adapter = Adapter(('localhost', 7878)) e1 = Event('e1') adapter.add_data_item(e1) c1 = SimpleCondition('c1') adapter.add_data_item(c1) s1 = Sample('s1') adapter.add_data_item(s1) adapter.start() while True: adapter.begin_gather() e1.set_value(1) s1.set_value(200.1) adapter.complete_gather() time.sleep(1.0) adapter.begin_gather() c1.add('fault', 'A fault', '123') e1.set_value(2) adapter.complete_gather() time.sleep(1.0) adapter.begin_gather()
class GenericActionClient(object): ## @brief Constructor for a GenericActionClient def __init__(self): # Initialize ROS generic client node rospy.init_node('ActionClient') # Setup MTConnect to ROS Conversion self.config = bridge_library.obtain_dataMap() self.msg_parameters = ['url', 'url_port', 'machine_tool', 'xml_namespace', 'adapter_port', 'service'] self.url = self.config[self.msg_parameters[0]] self.url_port = self.config[self.msg_parameters[1]] self.mtool = self.config[self.msg_parameters[2]] self.ns = dict(m = self.config[self.msg_parameters[3]]) self.port = self.config[self.msg_parameters[4]] self.service = self.config[self.msg_parameters[5]] # Check for url connectivity, dwell until system timeout bridge_library.check_connectivity((1,self.url,self.url_port)) # Setup MTConnect Adapter for robot status data items self.adapter = Adapter((self.url, self.port)) # Create empty lists for actions, message type handles, etc. self.lib_manifests = [] self.type_handle = None self.action_list = {} self.action_goals = {} self.package = None self.xml_goal = None self.di_dict = {} # XML data item dictionary to store MTConnect Adapter Events self.handshake = None # Setup action client data per the config file self.setup_topic_data() # Add data items to the MTConnect Adapter - must be unique data items bridge_library.add_event((self.adapter, self.action_list, self.di_dict, False)) # Start the adapter rospy.loginfo('Start the Robot Link adapter') self.adapter.start() # Create robot Service Server thread for each machine tool action self.action_service = [] for mt_action in self.action_goals.keys(): self.action_service.append(ActionService(mt_action, self.adapter, self.di_dict, self.service_handle, self.service)) # Establish XML connection, read in current XML while True: try: self.conn = HTTPConnection(self.url, self.url_port) response = bridge_library.xml_get_response((self.url, self.url_port, self.port, self.conn, self.mtool + "/current")) body = response.read() break except socket.error as e: rospy.loginfo('HTTP connection error %s' % e) time.sleep(10) # Parse the XML and determine the current sequence and XML Event elements seq, elements = bridge_library.xml_components(body, self.ns, self.action_list) # Start a streaming XML connection response = bridge_library.xml_get_response((self.url, self.url_port, self.port, self.conn, self.mtool + "/sample?interval=1000&count=1000&from=" + seq)) # Create class lock self.lock = thread.allocate_lock() # Create XML polling thread lp = LongPull(response) lp.long_pull(self.xml_callback) # Runs until user interrupts ## @brief This function captures the topic package, type, action goals, and action ## state conversion from ROS to MTConnect as required for the ROS action client. ## This task is completed for each topic specified in the configuration file. ## ## This function then performs a relative import of the topic via the getattr(import_module) ## function. Data is stored in the following class attributes: ## ## self.lib_manifests --> used to track which manifests have been loaded. ## self.type_handle --> used for ROS SimpleActionClient, stores package name with action messages. ## self.action_list --> used for ROS SimpleActionClient, stores CNC action request strings. ## self.action_goals --> data structure for message class instances of the topic type. def setup_topic_data(self): for package, action in self.config.items(): if package not in self.msg_parameters: # Only one ROS package in config by design # Load package manifest if unique if package not in self.lib_manifests: roslib.load_manifest(package) self.lib_manifests.append(package) # Import module rospy.loginfo('Importing --> ' + package + '.msg') self.type_handle = import_module(package + '.msg') self.service_handle = import_module(package + '.srv') # Capture package for action client self.package = package for action_req in action.keys(): # Capture action name and action goal xml tag for action client callback reference goal_tag = self.config[package][action_req].keys()[0] self.action_list[action_req] = {goal_tag : self.config[package][action_req][goal_tag].keys()} # Capture ROS Action parameters self.action_goals[action_req] = None return ## @brief ROS Action client function that compiles and sends the action goal to the ## dedicated ROS action server. The function will block until the server becomes available. ## After the goal result is received from the dedicated ROS action server, the function ## sets the robot data item via the MTConnect adapter. ## ## @param cb_data: tuple containing the following parameters: ## @param name: string containing the data item name for the robot action ## @param goals: dictionary of data_item:goal pairs in str:str or str:float format ## @param handle: Python <module 'mtconnect_msgs.msg'> 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 ## @brief Callback function that launches a ROS action client if the machine ## tool data item tag is 'ACTIVE'. Once the action is completed, it completes ## the handshake signal between the machine tool and the robot via the MTConnect ## adapter. ## ## @param chunk: xml data, read from response.read() 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
from data_item import Event, SimpleCondition, Sample, ThreeDSample from mtconnect_adapter import Adapter import roslib roslib.load_manifest('turtlesim') import rospy import turtlesim.msg adapter = Adapter(('0.0.0.0', 7878)) pose = ThreeDSample('pose') adapter.add_data_item(pose) def handle_turtle_pose(msg, turtlename): adapter.begin_gather() pose.set_value((msg.x, msg.y, 0.0)) adapter.complete_gather() if __name__ == "__main__": avail = Event('avail') adapter.add_data_item(avail) avail.set_value('AVAILABLE') adapter.start() rospy.init_node('mtconnect_adapter') rospy.Subscriber('/turtle1/pose', turtlesim.msg.Pose, handle_turtle_pose, 'turtle1') rospy.spin()
class pocketNCAdapter(object): def __init__(self, host, port): self.host = host self.port = port self.adapter = Adapter((host, port)) self.auto_time = Sample('auto_time') self.adapter.add_data_item(self.auto_time) self.cut_time = Sample('cut_time') self.adapter.add_data_item(self.cut_time) self.total_time = Sample('total_time') self.adapter.add_data_item(self.total_time) self.Xabs = Sample('Xabs') self.adapter.add_data_item(self.Xabs) self.Yabs = Sample('Yabs') self.adapter.add_data_item(self.Yabs) self.Zabs = Sample('Zabs') self.adapter.add_data_item(self.Zabs) self.Aabs = Sample('Aabs') self.adapter.add_data_item(self.Aabs) self.Babs = Sample('Babs') self.adapter.add_data_item(self.Babs) self.Srpm = Sample('Srpm') self.adapter.add_data_item(self.Srpm) self.estop = Event('estop') self.adapter.add_data_item(self.estop) self.power = Event('power') self.adapter.add_data_item(self.power) self._exec = Event('exec') self.adapter.add_data_item(self._exec) self.line = Event('line') self.adapter.add_data_item(self.line) self.program = Event('program') self.adapter.add_data_item(self.program) self.Fovr = Event('Fovr') self.adapter.add_data_item(self.Fovr) self.Sovr = Event('Sovr') self.adapter.add_data_item(self.Sovr) self.Tool_number = Event('Tool_number') self.adapter.add_data_item(self.Tool_number) self.mode = Event('mode') self.adapter.add_data_item(self.mode) self.avail = Event('avail') self.adapter.add_data_item(self.avail) self.adapter.start() self.adapter.begin_gather() self.avail.set_value("AVAILABLE") self.adapter.complete_gather() self.adapter_stream() def data_pull(self): data = linuxcnc.stat() data.poll() return data def adapter_stream(self): at2 = 'initialize' ct2 = 'initialize' ylt2 = 'initialize' ylt1 = datetime.datetime.now() #initialized at1 = datetime.datetime.now() #initialized ct1 = datetime.datetime.now() #initialized ylt = float(0) #initialized at = float(0) #initialized ct = float(0) #initialized while True: try: data = self.data_pull() #time initialization: accumulated time: cut - auto - total if self.power.value() == 'ON': ylt2 = datetime.datetime.now() if ex == 'ACTIVE' and self.Srpm.value() != None and float( self.Srpm.value()) > 0 and estop == 'ARMED': ct2 = datetime.datetime.now() else: ct1 = datetime.datetime.now() if (ex == 'ACTIVE' or ex == 'STOPPED' or ex == 'INTERRUPTED' or float(self.Srpm.value()) > 0) and estop == 'ARMED': at2 = datetime.datetime.now() else: at1 = datetime.datetime.now() else: ylt1 = datetime.datetime.now() if data.state == 1: ex = 'READY' elif data.state == 2: ex = 'ACTIVE' else: ex = '' self.adapter.begin_gather() self._exec.set_value(ex) self.adapter.complete_gather() if data.estop != 1: estp = 'ARMED' else: estp = 'TRIGGERED' self.adapter.begin_gather() self.estop.set_value(estp) self.adapter.complete_gather() self.adapter.begin_gather() self.power.set_value(pwr) self.adapter.complete_gather() self.adapter.begin_gather() xps = str(format(data.actual_position[0], '.4f')) self.Xabs.set_value(xps) yps = str(format(data.actual_position[1], '.4f')) self.Yabs.set_value(yps) zps = str(format(data.actual_position[2], '.4f')) self.Zabs.set_value(zps) abs = str(format(data.actual_position[3], '.4f')) self.Aabs.set_value(abs) bbs = str(format(data.actual_position[4], '.4f')) self.Babs.set_value(bbs) self.adapter.complete_gather() ssp = str(data.spindle_speed) self.adapter.begin_gather() self.Srpm.set_value(ssp) ln = str(data.motion_line) self.adapter.begin_gather() self.line.set_value(ln) pgm = str(data.file) self.adapter.begin_gather() self.program.set_value(pgm) pfo = str(data.feedrate * 100) self.adapter.begin_gather() self.Fovr.set_value(pfo) so = str(data.spindlerate * 100) self.adapter.begin_gather() self.Sovr.set_value(so) tooln = str(data.tool_in_spindle) self.adapter.begin_gather() self.Tool_number.set_value(tooln) self.adapter.complete_gather() if data.task_mode == 1: md = 'MDI' elif data.task_mode == 2: md = 'AUTOMATIC' elif data.task_mode == 3: md = 'MANUAL' else: md = '' self.adapter.begin_gather() self.mode.set_value(md) self.adapter.complete_gather() #time finalization: accumulated time, cut vs auto vs total if ylt2 != 'initialize' and self.power.value( ) == 'ON' and ylt1 != ylt2: #accumulated time:total time if (ylt2 - ylt1).total_seconds() >= 0: ylt += (ylt2 - ylt1).total_seconds() ylt1 = ylt2 if ylt >= 0 and self.total_time.value() != str(int(ylt)): self.adapter.begin_gather() self.total_time.set_value(str(int(ylt))) self.adapter.complete_gather() if ylt < 0: ylt = float(0) #accumulated time:cut time if ct2 != 'initialize' and ex == 'ACTIVE' and self.Srpm.value( ) != None and float(self.Srpm.value( )) > 0 and self.estop.value() == 'ARMED': if (ct2 - ct1).total_seconds() >= 0: ct += (ct2 - ct1).total_seconds() ct1 = ct2 if ct >= 0 and self.cut_time.value() != str(int(ct)): self.adapter.begin_gather() self.cut_time.set_value(str(int(ct))) self.adapter.complete_gather() if ct < 0: ct = float(0) #accumulated time:auto time if (at2 != 'initialize' and ex == 'ACTIVE' or ex == 'INTERRUPTED' or ex == 'STOPPED' or float(self.Srpm.value()) > 0 ) and self.estop.value() == 'ARMED': if (at2 - at1).total_seconds() >= 0: at += (at2 - at1).total_seconds() at1 = at2 if at >= 0 and self.auto_time.value() != str(int(at)): self.adapter.begin_gather() self.auto_time.set_value(str(int(at))) self.adapter.complete_gather() if at < 0: at = float(0) except: if self.power.value() and self.power.value() != 'OFF': self.adapter.begin_gather() self.power.set_value('OFF') self.adapter.complete_gather() ylt2 = 'initialize' ct2 = 'initialize' at2 = 'initialize' ylt1 = datetime.datetime.now() #initialized at1 = datetime.datetime.now() #initialized ct1 = datetime.datetime.now() #initialized
class GenericActionClient(object): ## @brief Constructor for a GenericActionClient def __init__(self): # Initialize ROS generic client node rospy.init_node('ActionClient') # Setup MTConnect to ROS Conversion self.config = bridge_library.obtain_dataMap() self.msg_parameters = [ 'url', 'url_port', 'machine_tool', 'xml_namespace', 'adapter_port', 'service' ] self.url = self.config[self.msg_parameters[0]] self.url_port = self.config[self.msg_parameters[1]] self.mtool = self.config[self.msg_parameters[2]] self.ns = dict(m=self.config[self.msg_parameters[3]]) self.port = self.config[self.msg_parameters[4]] self.service = self.config[self.msg_parameters[5]] # Check for url connectivity, dwell until system timeout bridge_library.check_connectivity((1, self.url, self.url_port)) # Setup MTConnect Adapter for robot status data items self.adapter = Adapter((self.url, self.port)) # Create empty lists for actions, message type handles, etc. self.lib_manifests = [] self.type_handle = None self.action_list = {} self.action_goals = {} self.package = None self.xml_goal = None self.di_dict = { } # XML data item dictionary to store MTConnect Adapter Events self.handshake = None # Setup action client data per the config file self.setup_topic_data() # Add data items to the MTConnect Adapter - must be unique data items bridge_library.add_event( (self.adapter, self.action_list, self.di_dict, False)) # Start the adapter rospy.loginfo('Start the Robot Link adapter') self.adapter.start() # Create robot Service Server thread for each machine tool action self.action_service = [] for mt_action in self.action_goals.keys(): self.action_service.append( ActionService(mt_action, self.adapter, self.di_dict, self.service_handle, self.service)) # Establish XML connection, read in current XML while True: try: self.conn = HTTPConnection(self.url, self.url_port) response = bridge_library.xml_get_response( (self.url, self.url_port, self.port, self.conn, self.mtool + "/current")) body = response.read() break except socket.error as e: rospy.loginfo('HTTP connection error %s' % e) time.sleep(10) # Parse the XML and determine the current sequence and XML Event elements seq, elements = bridge_library.xml_components(body, self.ns, self.action_list) # Start a streaming XML connection response = bridge_library.xml_get_response( (self.url, self.url_port, self.port, self.conn, self.mtool + "/sample?interval=1000&count=1000&from=" + seq)) # Create class lock self.lock = thread.allocate_lock() # Create XML polling thread lp = LongPull(response) lp.long_pull(self.xml_callback) # Runs until user interrupts ## @brief This function captures the topic package, type, action goals, and action ## state conversion from ROS to MTConnect as required for the ROS action client. ## This task is completed for each topic specified in the configuration file. ## ## This function then performs a relative import of the topic via the getattr(import_module) ## function. Data is stored in the following class attributes: ## ## self.lib_manifests --> used to track which manifests have been loaded. ## self.type_handle --> used for ROS SimpleActionClient, stores package name with action messages. ## self.action_list --> used for ROS SimpleActionClient, stores CNC action request strings. ## self.action_goals --> data structure for message class instances of the topic type. def setup_topic_data(self): for package, action in self.config.items(): if package not in self.msg_parameters: # Only one ROS package in config by design # Load package manifest if unique if package not in self.lib_manifests: roslib.load_manifest(package) self.lib_manifests.append(package) # Import module rospy.loginfo('Importing --> ' + package + '.msg') self.type_handle = import_module(package + '.msg') self.service_handle = import_module(package + '.srv') # Capture package for action client self.package = package for action_req in action.keys(): # Capture action name and action goal xml tag for action client callback reference goal_tag = self.config[package][action_req].keys()[0] self.action_list[action_req] = { goal_tag: self.config[package][action_req][goal_tag].keys() } # Capture ROS Action parameters self.action_goals[action_req] = None return ## @brief ROS Action client function that compiles and sends the action goal to the ## dedicated ROS action server. The function will block until the server becomes available. ## After the goal result is received from the dedicated ROS action server, the function ## sets the robot data item via the MTConnect adapter. ## ## @param cb_data: tuple containing the following parameters: ## @param name: string containing the data item name for the robot action ## @param goals: dictionary of data_item:goal pairs in str:str or str:float format ## @param handle: Python <module 'mtconnect_msgs.msg'> 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 ## @brief Callback function that launches a ROS action client if the machine ## tool data item tag is 'ACTIVE'. Once the action is completed, it completes ## the handshake signal between the machine tool and the robot via the MTConnect ## adapter. ## ## @param chunk: xml data, read from response.read() 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
class GenericActionServer(): ## @brief Constructor for a GenericActionServer def __init__(self): # Initialize ROS generic client node rospy.init_node('ActionServer') # Setup MTConnect to ROS Conversion self.config = bridge_library.obtain_dataMap() self.msg_parameters = ['url', 'url_port', 'machine_tool', 'xml_namespace', 'adapter_port'] self.url = self.config[self.msg_parameters[0]] self.url_port = self.config[self.msg_parameters[1]] self.mtool = self.config[self.msg_parameters[2]] self.ns = dict(m = self.config[self.msg_parameters[3]]) self.port = self.config[self.msg_parameters[4]] # Check for url connectivity, dwell until system timeout bridge_library.check_connectivity((1,self.url,self.url_port)) # Setup MTConnect Adapter for robot status data items self.adapter = Adapter((self.url, self.port)) # Create empty lists for actions, message type handles, etc. self.lib_manifests = [] self.type_handle = None self.action_list = {} self.capture_xml = False self.di_dict = {} # XML data item dictionary to store MTConnect Adapter Events # Create a dictionary of _results to return upon action success self._resultDict = {} # Create a dictionary of action servers self._as = {} # Create a dictionary of server names self.server_name = {} # Setup action client data per the config file self.setup_topic_data() # Add data items to the MTConnect Adapter - must be unique data items bridge_library.add_event((self.adapter, self.action_list, self.di_dict, True)) # Start the adapter rospy.loginfo('Start the Robot Link adapter') self.adapter.start() # Establish XML connection, read in current XML while True: try: self.conn = HTTPConnection(self.url, self.url_port) response = bridge_library.xml_get_response((self.url, self.url_port, self.port, self.conn, self.mtool + "/current")) body = response.read() break except socket.error as e: rospy.loginfo('HTTP connection error %s' % e) time.sleep(10) # Parse the XML and determine the current sequence and XML Event elements seq, elements = bridge_library.xml_components(body, self.ns, self.action_list) # Start a streaming XML connection response = bridge_library.xml_get_response((self.url, self.url_port, self.port, self.conn, self.mtool + "/sample?interval=1000&count=1000&from=" + seq)) # Create queue for streaming XML self.XML_queue = Queue() # Create XML polling thread lp = LongPull(response) xml_thread = Thread(target = lp.long_pull, args = (self.xml_callback,)) xml_thread.daemon = True xml_thread.start() ## @brief This function imports the topic package and creates an action server for each ## package specified in the configuration file. ## ## Data is stored in the following class attributes: ## ## self.lib_manifests --> used to track which manifests have been loaded ## self.type_handle --> used for ROS SimpleActionClient, stores package name with action messages ## self.action_list --> used for ROS SimpleActionClient, stores CNC action request strings ## self.server_name --> dictionary of data_item:DataItem pairs in str:str format ## self._resultDict --> dictionary of result class instances, i.e. {'MaterialLoad':mtconnect_msgs.msg._MaterialLoadResult.MaterialLoadResult()} ## self._as --> dictionary of ROS action servers referenced by DataItem def setup_topic_data(self): for package, action in self.config.items(): if package not in self.msg_parameters: # Only one ROS package in config by design # Load package manifest if unique if package not in self.lib_manifests: roslib.load_manifest(package) self.lib_manifests.append(package) # Import module rospy.loginfo('Importing --> ' + package + '.msg') self.type_handle = import_module(package + '.msg') # Iterate through requests and create action result class instances and action servers for request in action: # Capture action name for action client callback reference self.action_list[request] = request # Store the ROS server name in a hash table di_conv = bridge_library.split_event(request) self.server_name[di_conv] = request # Create a dictionary of result class instances request_class = getattr(self.type_handle, request + 'Result') self._resultDict[request] = request_class() # Create instance of the action class action_class = getattr(self.type_handle, request + 'Action') # Create action server for requested action, store in a hash table self._as[request] = actionlib.SimpleActionServer(request + 'Client', action_class, self.execute_cb, False) # Start the action server self._as[request].start() return ## @brief Callback function that monitors the machine tool action sequence for 'ACTIVE', ## 'COMPLETE', and 'READY' data item states. Once the machine tool is completed with the ## requested action and sends the machine tool 'READY' signal, the ROS action server ## returns set_succeeded to the ROS action client that requested the action. ## ## @param goal: from action client, class instance of the data item action goal, i.e. goal = mtconnect_msgs.msg.OpenDoorGoal() 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 ## @brief Processes the xml chunk provided by the LongPull class instance and stores the result ## into the XML_queue since the tags have changed. The queue is used to ensure capture of updated ## tags while the execute_cb function is being executed. ## @param chunk: xml data, read from response.read() def xml_callback(self, chunk): #rospy.loginfo('*******************In PROCESS_XML callback***************') try: if self.capture_xml == True: self.XML_queue.put(chunk) if self.XML_queue.qsize() > 1: rospy.logdebug('STORED XML INTO QUEUE, WAITING ON ROS ACTION SERVER, QUEUE SIZE --> %s' % self.XML_queue.qsize()) except Exception as e: rospy.logerr("Bridge Server: Process XML callback failed: %s, releasing lock" % e) finally: pass #rospy.loginfo('*******************Done with PROCESS_XML callback***************') return
class BridgeSubscriber(): ## @brief Constructor for a BridgeSubscriber def __init__(self): # Initialize the ROS Bridge subscriber node rospy.init_node('bridge_subscriber') # Read configuration file and extract topics and types self.config = bridge_library.obtain_dataMap() self.msg_parameters = ['url', 'adapter_port'] self.url = self.config[self.msg_parameters[0]] self.adapter_port = self.config[self.msg_parameters[1]] # Setup MTConnect Adapter for robot status data items self.adapter = Adapter((self.url, self.adapter_port)) self.di_dict = {} # XML data item dictionary to store MTConnect Adapter Events # Setup ROS topics as specified in .yaml file self.topic_name_list = [] # List of topic names self.subscribed_list = [] # List of subscribed topics self.lib_manifests = [] # Stores loaded ROS library manifests # Topic type and MTConnect message members from the module import self.topic_type_list = {} self.member_types = {} self.member_names = {} # The ROS message text retainer, used to map ROS values to MTConnect XML tag.text self.msg_text = {} # Create the data sets for the topic types, member names, and member types self.setup_topic_data() # Start the adapter rospy.loginfo('Start the Robot Link adapter') self.adapter.start() # Create class lock self.lock = thread.allocate_lock() # Loop through requested topics and spawn ROS subscribers when topics are available topic_list = self.topic_name_list dwell = 10 while topic_list: published_topics = dict(rospy.get_published_topics()) for topic_name in topic_list: if topic_name in published_topics.keys(): # Create ROS Subscriber idx = topic_list.index(topic_name) del topic_list[idx] self.topic_listener((topic_name, self.topic_type_list[topic_name], self.member_names[topic_name], self.msg_text[topic_name])) else: rospy.loginfo('ROS Topic %s not available, will try to subscribe in %d seconds' % (topic_name, dwell)) time.sleep(dwell) ## @brief This function captures the topic name, type, and member attributes that are required for ## the ROS subscriber. This task is completed for each topic specified in the configuration file. ## ## This function then performs a relative import of the topic via the getattr(import_module) function. ## Data is stored in the following class instance attributes: ## ## self.topic_type_list --> used for module import and msg parameters. ## self.member_types --> member type, not used in msg parameters, future use. ## self.member_names --> used for ROS subscriber msg parameters. def setup_topic_data(self): for topic, topic_type in self.config.items(): if topic not in self.msg_parameters: self.topic_name_list.append(topic) # Only one type per topic type_key = topic_type.keys()[0] self.subscribed_list.append(type_key) # One set of data items per topic type self.data_items = [data_item for data_item in topic_type[type_key].keys()] # Add data items to the MTConnect Adapter - must be unique data items bridge_library.add_event((self.adapter, self.data_items, self.di_dict, False)) # Extract package name and topic type name tokens = topic_type.keys()[0].split('/') package = tokens[0] type_name = tokens[1] # Load package manifest if unique if package not in self.lib_manifests: roslib.load_manifest(package) self.lib_manifests.append(package) # Import module and create topic type class, # i.e. append <class 'mtconnect_msgs.msg._RobotStates.RobotStates'> rospy.loginfo('Class Instance --> ' + package + '.msg.' + type_name) type_handle = getattr(import_module(package + '.msg'), type_name) self.topic_type_list[topic] = type_handle self.msg_text[topic] = type_handle._full_text self.member_types[topic] = type_handle._slot_types self.member_names[topic] = type_handle.__slots__ return ## @brief Callback function that captures the attribute values for a ROS topic. ## The topic data is stored in a list of tuples, where each tuple is a ## (attrib_name, attrib_value) pair. To access the integer value of the attribute ## value, use attrib_value.val. ## ## All data conversions between ROS and MTConnect are stored in the ROS ## subscriber YAML file. A separate function handles the ROS to MTConnect ## conversions for generic robot messages. ## @param data: callback ROS message data from the ROS subscriber ## @param cb_data: tuple containing the following parameters: ## @param topic_name: string defining the name of the ROS topic ## @param type_handle: class instance of the ROS topic message i.e. <class 'mtconnect_msgs.msg._RobotStates.RobotStates'> ## @param member_set: list of strings defining the attribute members for the message class ## @param msg_text: string of the entire message text (may be eliminated in future revisions) def topic_callback(self, data, cb_data): self.lock.acquire() try: (topic_name, type_handle, member_set, msg_text) = cb_data type_name = type_handle._type # Repackage members into a dictionary dout = {val:val for val in member_set} # Iterate through data items and capture message data msg_data = [] msg_constants = {} for dataitem in member_set: # Capture attribute for package/message type --> industrial_msgs/TriState.avail attrib_handle = getattr(type_handle(), dataitem) if 'header' not in dataitem: # Extract string representing message attribute --> 'val' val_key = attrib_handle.__slots__[0] # Capture message data.attrib --> <class message data>.dataitem: TriState.avail token = getattr(data, dataitem) # Capture the integer value for the attribute: avail.val item_value = getattr(token, val_key) # Store published message data for the topic msg_data.append((dataitem, item_value)) # Create a list of strings containing message CONSTANTS msg_constants[dataitem] = [] for attrib in dir(attrib_handle): if attrib.isupper(): if getattr(attrib_handle, attrib) == item_value: msg_constants[dataitem].append(attrib) # Execute the ROS to MTConnet conversion function self.data_item_conversion(topic_name, type_name, msg_data, msg_text, msg_constants) except Exception as e: rospy.logerr('Topic callback failed: %s, releasing lock' % e) finally: self.lock.release() return ## @brief A conversion function that will convert the ROS message value to the ## machine tool value. The conversion dictionary is provided in the configuration file. ## @param topic_name: string defining the name of the ROS topic ## @param type_name: string defining the name of the ROS topic type ## @param msg_data: list of strings defining message attributes ## @param msg_text: string of the entire message text (may be eliminated in future revisions) ## @param constants: list of ROS topic message type CONSTANTS stored as strings 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 ## @brief Main ROS subscriber function. A new thread is created for each callback. ## @param data: tuple containing the following parameters: ## @param topic_name: string defining the name of the ROS topic ## @param type_handle: class instance of the ROS topic message i.e. <class 'mtconnect_msgs.msg._RobotStates.RobotStates'> ## @param member_set: list of strings defining the message attributes ## @param msg_text: string of the entire message text (may be eliminated in future revisions) def topic_listener(self, data): # Unpack arguments topic_name, type_handle, member_set, msg_text = data # Launch the ROS subscriber rospy.Subscriber(topic_name, type_handle, self.topic_callback, data) return