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
