def topic_publisher(self, cb_data):
# Unpack function arguments
topic_name, topic_type, pub, msg, data_items = cb_data
msg.header.stamp = rospy.Time.now()
for di_name, value in self.di_changed.items():
# Convert tag text from MTConnect to ROS and set attribute value
if di_name in data_items:
if value is not None:
# Check for conversion key error
conv_keys = self.config[topic_name][topic_type][di_name].keys()
if value not in conv_keys:
rospy.logerr("CONVERSION ERROR IN TOPIC CONFIG FILE: XML is '%s' --> CONFIG is %s" % (value, conv_keys))
os._exit(0)
# Convert from MTConnect to ROS format from di_changed
ros_tag = self.config[topic_name][topic_type][di_name][self.di_changed[di_name]]
else:
# Convert from MTConnect to ROS format via di_current
ros_tag = self.config[topic_name][topic_type][di_name][self.di_current[di_name]]
# Obtain the state value via operator.attrgetter object: 'door_state.CLOSED'
name = bridge_library.split_event(di_name)
state_object = operator.attrgetter(name + '.' + ros_tag)
state_value = state_object(msg)
# From the msg class, obtain the message attribute using data item name: attrib = msg.door_state
attrib = getattr(msg, name)
# Set the attribute to the converted ROS value: setattr(msg, 'door_state.val', 1) --> msg.door_state.val = 1
setattr(attrib, attrib.__slots__[0], state_value)
pub.publish(msg)
return
def action_service_server(self):
self.as_name = bridge_library.split_event(self.mt_action)
action_service_class = getattr(self.service_hndle, self.service_state)
s = rospy.Service(self.mt_action + '/' + 'set_mtconnect_state',
action_service_class, self.robot_state_callback)
rospy.spin()
return
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
def process_xml(self, xml):
nextSeq, elements = bridge_library.xml_components(xml, self.ns, self.di_current)
# Create local data item dictionary to track event changes
local_di = self.init_di_dict()
# Loop through XML elements, update the di_changed and di_current
di_list = [di_val for di_list in self.data_items for di_val in di_list]
if elements:
rospy.logdebug('***********XML -->*************\n%s' % xml)
for e in elements:
for d_item in di_list:
if e.attrib['name'] == bridge_library.split_event(d_item):
local_di[d_item] = e.text
self.di_current[d_item] = e.text # Update di_current with new state
break
return nextSeq, local_di
def action_service_server(self):
self.as_name = bridge_library.split_event(self.mt_action)
action_service_class = getattr(self.service_hndle, self.service_state)
s = rospy.Service(self.mt_action + '/' + 'set_mtconnect_state', action_service_class, self.robot_state_callback)
rospy.spin()
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 __init__(self):
# Initialize the ROS publisher node
rospy.init_node('bridge_publisher')
# Setup MTConnect to ROS Conversion
self.config = bridge_library.obtain_dataMap()
self.msg_parameters = ['url', 'url_port', 'machine_tool', 'xml_namespace']
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]])
# Check for url connectivity, dwell until system timeout
bridge_library.check_connectivity((1,self.url,self.url_port))
# Create the data lists for the topic names, types, manifests, data items, ROS publishers, and ROS messages
self.topic_name_list = []
self.topic_type_list = []
self.lib_manifests = []
self.data_items = []
self.pub = []
self.msg = []
self.data_hold = None
# Setup topic, data items, pub, etc. via configuration file
self.setup_topic_data()
# Hash table of persistent CNC event states, only updates when state changes
self.di_current = self.init_di_dict()
# Hash table to store changed CNC event states
self.di_changed = None
# 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, None, 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.di_current)
# Use XML to establish current data item state dictionary
for di in self.di_current.keys():
name = bridge_library.split_event(di)
for e in elements:
rospy.loginfo('Element %s --> %s' % (e.tag, e.text))
if name == e.attrib['name']:
self.di_current[di] = e.text
# Confirm that data items in config file are present in XML
for di_set in self.data_items:
if not set(di_set).issubset(set(self.di_current.keys())):
rospy.logerr('ERROR: INCORRECT EVENTS IN TOPIC CONFIG OR XML AGENT FILE')
sys.exit()
# Start a streaming XML connection
response = bridge_library.xml_get_response((self.url, self.url_port, None, self.conn, self.mtool + "/sample?interval=1000&count=1000&from=" + seq))
# Create queue for streaming XML
self.XML_queue = Queue()
# Create a ROS publishing thread
self.ros_publisher()
# Streams data from the agent...
lp = LongPull(response)
lp.long_pull(self.xml_callback) # Runs until user interrupts
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
