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 __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)
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')
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')
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)
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 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
def create_server():
fname = os.tmpnam()
adapter = Adapter(fname, 10000, socket.AF_UNIX)
return (fname, adapter)
limitations under the License."""
import sys, os, time
path, file = os.path.split(__file__)
sys.path.append(os.path.realpath(path) + '/../src')
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()
limitations under the License."""
import sys, os, time
path, file = os.path.split(__file__)
sys.path.append(os.path.realpath(path) + '/../src')
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',
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()
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 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
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
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
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()
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
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License."""
import sys, os, time
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)
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
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
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License."""
import sys, os, time
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)
