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 BridgeAdapter:
def __init__(self):
self._adapter = Adapter(('0.0.0.0', 7878))
self._pose = ThreeDSample('pose')
self._adapter.add_data_item(self._pose)
avail = Event('avail')
self._adapter.add_data_item(avail)
avail.set_value('AVAILABLE')
def start(self):
def handle_turtle_pose(msg, turtlename):
self._adapter.begin_gather()
self._pose.set_value((msg.x, msg.y, 0.0))
self._adapter.complete_gather()
self._adapter.start()
rospy.Subscriber('/turtle1/pose',
turtlesim.msg.Pose,
handle_turtle_pose,
'turtle1')
class BridgeSubscriber():
## @brief Constructor for a BridgeSubscriber
def __init__(self):
# Initialize the ROS Bridge subscriber node
rospy.init_node('bridge_subscriber')
# Read configuration file and extract topics and types
self.config = bridge_library.obtain_dataMap()
self.msg_parameters = ['url', 'adapter_port']
self.url = self.config[self.msg_parameters[0]]
self.adapter_port = self.config[self.msg_parameters[1]]
# Setup MTConnect Adapter for robot status data items
self.adapter = Adapter((self.url, self.adapter_port))
self.di_dict = {
} # XML data item dictionary to store MTConnect Adapter Events
# Setup ROS topics as specified in .yaml file
self.topic_name_list = [] # List of topic names
self.subscribed_list = [] # List of subscribed topics
self.lib_manifests = [] # Stores loaded ROS library manifests
# Topic type and MTConnect message members from the module import
self.topic_type_list = {}
self.member_types = {}
self.member_names = {}
# The ROS message text retainer, used to map ROS values to MTConnect XML tag.text
self.msg_text = {}
# Create the data sets for the topic types, member names, and member types
self.setup_topic_data()
# Start the adapter
rospy.loginfo('Start the Robot Link adapter')
self.adapter.start()
# Create class lock
self.lock = thread.allocate_lock()
# Loop through requested topics and spawn ROS subscribers when topics are available
topic_list = self.topic_name_list
dwell = 10
while topic_list:
published_topics = dict(rospy.get_published_topics())
for topic_name in topic_list:
if topic_name in published_topics.keys():
# Create ROS Subscriber
idx = topic_list.index(topic_name)
del topic_list[idx]
self.topic_listener(
(topic_name, self.topic_type_list[topic_name],
self.member_names[topic_name],
self.msg_text[topic_name]))
else:
rospy.loginfo(
'ROS Topic %s not available, will try to subscribe in %d seconds'
% (topic_name, dwell))
time.sleep(dwell)
## @brief This function captures the topic name, type, and member attributes that are required for
## the ROS subscriber. This task is completed for each topic specified in the configuration file.
##
## This function then performs a relative import of the topic via the getattr(import_module) function.
## Data is stored in the following class instance attributes:
##
## self.topic_type_list --> used for module import and msg parameters.
## self.member_types --> member type, not used in msg parameters, future use.
## self.member_names --> used for ROS subscriber msg parameters.
def setup_topic_data(self):
for topic, topic_type in self.config.items():
if topic not in self.msg_parameters:
self.topic_name_list.append(topic)
# Only one type per topic
type_key = topic_type.keys()[0]
self.subscribed_list.append(type_key)
# One set of data items per topic type
self.data_items = [
data_item for data_item in topic_type[type_key].keys()
]
# Add data items to the MTConnect Adapter - must be unique data items
bridge_library.add_event(
(self.adapter, self.data_items, self.di_dict, False))
# Extract package name and topic type name
tokens = topic_type.keys()[0].split('/')
package = tokens[0]
type_name = tokens[1]
# Load package manifest if unique
if package not in self.lib_manifests:
roslib.load_manifest(package)
self.lib_manifests.append(package)
# Import module and create topic type class,
# i.e. append <class 'mtconnect_msgs.msg._RobotStates.RobotStates'>
rospy.loginfo('Class Instance --> ' + package + '.msg.' +
type_name)
type_handle = getattr(import_module(package + '.msg'),
type_name)
self.topic_type_list[topic] = type_handle
self.msg_text[topic] = type_handle._full_text
self.member_types[topic] = type_handle._slot_types
self.member_names[topic] = type_handle.__slots__
return
## @brief Callback function that captures the attribute values for a ROS topic.
## The topic data is stored in a list of tuples, where each tuple is a
## (attrib_name, attrib_value) pair. To access the integer value of the attribute
## value, use attrib_value.val.
##
## All data conversions between ROS and MTConnect are stored in the ROS
## subscriber YAML file. A separate function handles the ROS to MTConnect
## conversions for generic robot messages.
## @param data: callback ROS message data from the ROS subscriber
## @param cb_data: tuple containing the following parameters:
## @param topic_name: string defining the name of the ROS topic
## @param type_handle: class instance of the ROS topic message i.e. <class 'mtconnect_msgs.msg._RobotStates.RobotStates'>
## @param member_set: list of strings defining the attribute members for the message class
## @param msg_text: string of the entire message text (may be eliminated in future revisions)
def topic_callback(self, data, cb_data):
self.lock.acquire()
try:
(topic_name, type_handle, member_set, msg_text) = cb_data
type_name = type_handle._type
# Repackage members into a dictionary
dout = {val: val for val in member_set}
# Iterate through data items and capture message data
msg_data = []
msg_constants = {}
for dataitem in member_set:
# Capture attribute for package/message type --> industrial_msgs/TriState.avail
attrib_handle = getattr(type_handle(), dataitem)
if 'header' not in dataitem:
# Extract string representing message attribute --> 'val'
val_key = attrib_handle.__slots__[0]
# Capture message data.attrib --> <class message data>.dataitem: TriState.avail
token = getattr(data, dataitem)
# Capture the integer value for the attribute: avail.val
item_value = getattr(token, val_key)
# Store published message data for the topic
msg_data.append((dataitem, item_value))
# Create a list of strings containing message CONSTANTS
msg_constants[dataitem] = []
for attrib in dir(attrib_handle):
if attrib.isupper():
if getattr(attrib_handle, attrib) == item_value:
msg_constants[dataitem].append(attrib)
# Execute the ROS to MTConnet conversion function
self.data_item_conversion(topic_name, type_name, msg_data,
msg_text, msg_constants)
except Exception as e:
rospy.logerr('Topic callback failed: %s, releasing lock' % e)
finally:
self.lock.release()
return
## @brief A conversion function that will convert the ROS message value to the
## machine tool value. The conversion dictionary is provided in the configuration file.
## @param topic_name: string defining the name of the ROS topic
## @param type_name: string defining the name of the ROS topic type
## @param msg_data: list of strings defining message attributes
## @param msg_text: string of the entire message text (may be eliminated in future revisions)
## @param constants: list of ROS topic message type CONSTANTS stored as strings
def data_item_conversion(self, topic_name, type_name, msg_data, msg_text,
constants):
# Set the Robot XML data item via the MTConnect Adapter
for member_data in msg_data:
# Iterate through list of ROS message CONSTANTS and set the MTConnect Adapter value
for const_val in constants[member_data[0]]:
if const_val in self.config[topic_name][type_name][
member_data[0]].keys():
# If ROS to MTConnect mapping dictionary contains the constant, set the adapter value
adapter_val = self.config[topic_name][type_name][
member_data[0]][const_val]
break
else:
adapter_val = None
if adapter_val is None:
rospy.logerr('ROS to MTConnect Mapping failed')
# Set the Robot XML data item
bridge_library.action_cb(
(self.adapter, self.di_dict, member_data[0], adapter_val))
return
## @brief Main ROS subscriber function. A new thread is created for each callback.
## @param data: tuple containing the following parameters:
## @param topic_name: string defining the name of the ROS topic
## @param type_handle: class instance of the ROS topic message i.e. <class 'mtconnect_msgs.msg._RobotStates.RobotStates'>
## @param member_set: list of strings defining the message attributes
## @param msg_text: string of the entire message text (may be eliminated in future revisions)
def topic_listener(self, data):
# Unpack arguments
topic_name, type_handle, member_set, msg_text = data
# Launch the ROS subscriber
rospy.Subscriber(topic_name, type_handle, self.topic_callback, data)
return
path, file = os.path.split(__file__)
sys.path.append(os.path.realpath(path) + '/../src')
from data_item import Event, SimpleCondition, Sample
from mtconnect_adapter import Adapter
if __name__ == "__main__":
adapter = Adapter(('localhost', 7878))
e1 = Event('e1')
adapter.add_data_item(e1)
c1 = SimpleCondition('c1')
adapter.add_data_item(c1)
s1 = Sample('s1')
adapter.add_data_item(s1)
adapter.start()
while True:
adapter.begin_gather()
e1.set_value(1)
s1.set_value(200.1)
adapter.complete_gather()
time.sleep(1.0)
adapter.begin_gather()
c1.add('fault', 'A fault', '123')
e1.set_value(2)
adapter.complete_gather()
time.sleep(1.0)
adapter.begin_gather()
class GenericActionClient(object):
## @brief Constructor for a GenericActionClient
def __init__(self):
# Initialize ROS generic client node
rospy.init_node('ActionClient')
# Setup MTConnect to ROS Conversion
self.config = bridge_library.obtain_dataMap()
self.msg_parameters = ['url', 'url_port', 'machine_tool', 'xml_namespace', 'adapter_port', 'service']
self.url = self.config[self.msg_parameters[0]]
self.url_port = self.config[self.msg_parameters[1]]
self.mtool = self.config[self.msg_parameters[2]]
self.ns = dict(m = self.config[self.msg_parameters[3]])
self.port = self.config[self.msg_parameters[4]]
self.service = self.config[self.msg_parameters[5]]
# Check for url connectivity, dwell until system timeout
bridge_library.check_connectivity((1,self.url,self.url_port))
# Setup MTConnect Adapter for robot status data items
self.adapter = Adapter((self.url, self.port))
# Create empty lists for actions, message type handles, etc.
self.lib_manifests = []
self.type_handle = None
self.action_list = {}
self.action_goals = {}
self.package = None
self.xml_goal = None
self.di_dict = {} # XML data item dictionary to store MTConnect Adapter Events
self.handshake = None
# Setup action client data per the config file
self.setup_topic_data()
# Add data items to the MTConnect Adapter - must be unique data items
bridge_library.add_event((self.adapter, self.action_list, self.di_dict, False))
# Start the adapter
rospy.loginfo('Start the Robot Link adapter')
self.adapter.start()
# Create robot Service Server thread for each machine tool action
self.action_service = []
for mt_action in self.action_goals.keys():
self.action_service.append(ActionService(mt_action, self.adapter, self.di_dict, self.service_handle, self.service))
# Establish XML connection, read in current XML
while True:
try:
self.conn = HTTPConnection(self.url, self.url_port)
response = bridge_library.xml_get_response((self.url, self.url_port, self.port, self.conn, self.mtool + "/current"))
body = response.read()
break
except socket.error as e:
rospy.loginfo('HTTP connection error %s' % e)
time.sleep(10)
# Parse the XML and determine the current sequence and XML Event elements
seq, elements = bridge_library.xml_components(body, self.ns, self.action_list)
# Start a streaming XML connection
response = bridge_library.xml_get_response((self.url, self.url_port, self.port, self.conn, self.mtool + "/sample?interval=1000&count=1000&from=" + seq))
# Create class lock
self.lock = thread.allocate_lock()
# Create XML polling thread
lp = LongPull(response)
lp.long_pull(self.xml_callback) # Runs until user interrupts
## @brief This function captures the topic package, type, action goals, and action
## state conversion from ROS to MTConnect as required for the ROS action client.
## This task is completed for each topic specified in the configuration file.
##
## This function then performs a relative import of the topic via the getattr(import_module)
## function. Data is stored in the following class attributes:
##
## self.lib_manifests --> used to track which manifests have been loaded.
## self.type_handle --> used for ROS SimpleActionClient, stores package name with action messages.
## self.action_list --> used for ROS SimpleActionClient, stores CNC action request strings.
## self.action_goals --> data structure for message class instances of the topic type.
def setup_topic_data(self):
for package, action in self.config.items():
if package not in self.msg_parameters: # Only one ROS package in config by design
# Load package manifest if unique
if package not in self.lib_manifests:
roslib.load_manifest(package)
self.lib_manifests.append(package)
# Import module
rospy.loginfo('Importing --> ' + package + '.msg')
self.type_handle = import_module(package + '.msg')
self.service_handle = import_module(package + '.srv')
# Capture package for action client
self.package = package
for action_req in action.keys():
# Capture action name and action goal xml tag for action client callback reference
goal_tag = self.config[package][action_req].keys()[0]
self.action_list[action_req] = {goal_tag : self.config[package][action_req][goal_tag].keys()}
# Capture ROS Action parameters
self.action_goals[action_req] = None
return
## @brief ROS Action client function that compiles and sends the action goal to the
## dedicated ROS action server. The function will block until the server becomes available.
## After the goal result is received from the dedicated ROS action server, the function
## sets the robot data item via the MTConnect adapter.
##
## @param cb_data: tuple containing the following parameters:
## @param name: string containing the data item name for the robot action
## @param goals: dictionary of data_item:goal pairs in str:str or str:float format
## @param handle: Python <module 'mtconnect_msgs.msg'>
def action_client(self, cb_data):
# Execute ROS Action
rospy.init_node('ActionClient')
# Unpack action client data
name, goals, handle = cb_data
rospy.loginfo('Launched %s Action CLient' % name)
# Creates the SimpleActionClient, passing the type of the action (MaterialLoadAction) to the constructor.
# i.e. name = 'MaterialLoad', action_type = mtconnect_msgs.msg.MaterialLoadAction
action_type = getattr(handle, name + 'Action')
client = actionlib.SimpleActionClient(name + 'Client', action_type)
# Waits until the action server has started up and started listening for goals.
rospy.loginfo('Waiting for %s Dedicated Action Server' % name)
client.wait_for_server()
rospy.loginfo('%s Dedicated Action Server Activated' % name)
# Creates a MaterialLoad goal to send to the action server.
# i.e. goal = mtconnect_msgs.msg.MaterialLoad()
goal_handle = getattr(handle, name + 'Goal')
goal = goal_handle()
# Check to make sure the action goal has been set
if self.action_goals[name] == None:
# Ping the current url for the goal Event
goal_tag = self.action_list[name].keys()[0]
response = bridge_library.xml_get_response((self.url, self.url_port, self.port, self.conn,
self.mtool + "/current?path=//DataItem[@type='" + goal_tag.upper() +"']"))
body = response.read()
root = ElementTree.fromstring(body)
# Set the action goals
self.action_goals = bridge_library.set_goal(name, self.action_list[name], root, self.ns, self.action_goals)
# Check goal source for required attributes
for attrib, attrib_type, action_goal in zip(goal_handle.__slots__, goal_handle._slot_types, self.action_goals[name]):
if bridge_library.type_check(attrib_type, action_goal) == False:
rospy.logerr('INCOMPATIBLE GOAL TYPES ROS MSG: %s --> XML: %s' % (attrib_type, type(action_goal)))
sys.exit()
# Set the goal attributes from XML data
try:
for attrib, xml_goal in zip(goal_handle.__slots__, self.action_goals[name]):
setattr(goal, attrib, xml_goal)
except Exception as e:
rospy.logerr("ROS Action %s Client failed: %s" % (name, e))
# Sends the goal to the action server.
rospy.loginfo('Sending the goal')
name_conv = bridge_library.split_event(name)
client.send_goal(goal, done_cb = None, active_cb = bridge_library.action_cb((self.adapter, self.di_dict, name_conv, 'ACTIVE')))
# Waits for the server to finish performing the action.
rospy.loginfo('Waiting for result')
client.wait_for_result()
# Obtain result state
result = client.get_state()
# Prints out the result of the executing action
rospy.loginfo('Returning the result --> %s' % result)
# Set the Robot XML data item
data_item = bridge_library.split_event(name)
# Submit converted result to host via MTConnect adapter
if result == 3: # SUCCEEDED from actionlib_msgs.msg.GoalStatus
rospy.loginfo('Sending COMPLETE flag')
bridge_library.action_cb((self.adapter, self.di_dict, data_item, 'COMPLETE'))
return name
elif result == 4: # ABORTED from actionlib_msgs.msg.GoalStatus
rospy.loginfo('%s ABORTED, terminating action request' % data_item)
bridge_library.action_cb((self.adapter, self.di_dict, data_item, 'FAIL'))
return None
## @brief Callback function that launches a ROS action client if the machine
## tool data item tag is 'ACTIVE'. Once the action is completed, it completes
## the handshake signal between the machine tool and the robot via the MTConnect
## adapter.
##
## @param chunk: xml data, read from response.read()
def xml_callback(self, chunk):
#rospy.loginfo('*******************In PROCESS_XML callback***************')
self.lock.acquire()
try:
# Only grab XML elements for machine tool action requests
_, elements, self.action_goals = bridge_library.xml_components(chunk, self.ns, self.action_list, get_goal = True, action_goals = self.action_goals)
if elements:
# Check for existing handshake, execute it first
if self.handshake:
for e in elements:
if self.handshake == e.attrib['name'] and e.text == 'READY':
# Send hand shake signal
bridge_library.action_cb((self.adapter, self.di_dict, e.attrib['name'], 'READY'))
self.handshake = None
elements.remove(e)
if self.handshake != None:
rospy.logerr('DID NOT RECEIVE READY HANDSHAKE FROM MACHINE TOOL')
# Process remaining action requests
for e in elements:
# Remove XML namespace string from the element tag for hash tables
action_text = re.findall(r'(?<=\})\w+',e.tag)[0]
# Check if machine tool is requesting an action, if so, run action client
if e.text == 'ACTIVE':
self.handshake = self.action_client((action_text, self.action_goals[action_text], self.type_handle))
#self.handshake = e.attrib['name']
except Exception as e:
rospy.logerr("Generic Action Client: Process XML callback failed: %s, releasing lock" % e)
finally:
self.lock.release()
#rospy.loginfo('*******************Done with PROCESS_XML callback***************')
return
from data_item import Event, SimpleCondition, Sample, ThreeDSample
from mtconnect_adapter import Adapter
import roslib
roslib.load_manifest('turtlesim')
import rospy
import turtlesim.msg
adapter = Adapter(('0.0.0.0', 7878))
pose = ThreeDSample('pose')
adapter.add_data_item(pose)
def handle_turtle_pose(msg, turtlename):
adapter.begin_gather()
pose.set_value((msg.x, msg.y, 0.0))
adapter.complete_gather()
if __name__ == "__main__":
avail = Event('avail')
adapter.add_data_item(avail)
avail.set_value('AVAILABLE')
adapter.start()
rospy.init_node('mtconnect_adapter')
rospy.Subscriber('/turtle1/pose',
turtlesim.msg.Pose,
handle_turtle_pose,
'turtle1')
rospy.spin()
class pocketNCAdapter(object):
def __init__(self, host, port):
self.host = host
self.port = port
self.adapter = Adapter((host, port))
self.auto_time = Sample('auto_time')
self.adapter.add_data_item(self.auto_time)
self.cut_time = Sample('cut_time')
self.adapter.add_data_item(self.cut_time)
self.total_time = Sample('total_time')
self.adapter.add_data_item(self.total_time)
self.Xabs = Sample('Xabs')
self.adapter.add_data_item(self.Xabs)
self.Yabs = Sample('Yabs')
self.adapter.add_data_item(self.Yabs)
self.Zabs = Sample('Zabs')
self.adapter.add_data_item(self.Zabs)
self.Aabs = Sample('Aabs')
self.adapter.add_data_item(self.Aabs)
self.Babs = Sample('Babs')
self.adapter.add_data_item(self.Babs)
self.Srpm = Sample('Srpm')
self.adapter.add_data_item(self.Srpm)
self.estop = Event('estop')
self.adapter.add_data_item(self.estop)
self.power = Event('power')
self.adapter.add_data_item(self.power)
self._exec = Event('exec')
self.adapter.add_data_item(self._exec)
self.line = Event('line')
self.adapter.add_data_item(self.line)
self.program = Event('program')
self.adapter.add_data_item(self.program)
self.Fovr = Event('Fovr')
self.adapter.add_data_item(self.Fovr)
self.Sovr = Event('Sovr')
self.adapter.add_data_item(self.Sovr)
self.Tool_number = Event('Tool_number')
self.adapter.add_data_item(self.Tool_number)
self.mode = Event('mode')
self.adapter.add_data_item(self.mode)
self.avail = Event('avail')
self.adapter.add_data_item(self.avail)
self.adapter.start()
self.adapter.begin_gather()
self.avail.set_value("AVAILABLE")
self.adapter.complete_gather()
self.adapter_stream()
def data_pull(self):
data = linuxcnc.stat()
data.poll()
return data
def adapter_stream(self):
at2 = 'initialize'
ct2 = 'initialize'
ylt2 = 'initialize'
ylt1 = datetime.datetime.now() #initialized
at1 = datetime.datetime.now() #initialized
ct1 = datetime.datetime.now() #initialized
ylt = float(0) #initialized
at = float(0) #initialized
ct = float(0) #initialized
while True:
try:
data = self.data_pull()
#time initialization: accumulated time: cut - auto - total
if self.power.value() == 'ON':
ylt2 = datetime.datetime.now()
if ex == 'ACTIVE' and self.Srpm.value() != None and float(
self.Srpm.value()) > 0 and estop == 'ARMED':
ct2 = datetime.datetime.now()
else:
ct1 = datetime.datetime.now()
if (ex == 'ACTIVE' or ex == 'STOPPED'
or ex == 'INTERRUPTED' or
float(self.Srpm.value()) > 0) and estop == 'ARMED':
at2 = datetime.datetime.now()
else:
at1 = datetime.datetime.now()
else:
ylt1 = datetime.datetime.now()
if data.state == 1:
ex = 'READY'
elif data.state == 2:
ex = 'ACTIVE'
else:
ex = ''
self.adapter.begin_gather()
self._exec.set_value(ex)
self.adapter.complete_gather()
if data.estop != 1:
estp = 'ARMED'
else:
estp = 'TRIGGERED'
self.adapter.begin_gather()
self.estop.set_value(estp)
self.adapter.complete_gather()
self.adapter.begin_gather()
self.power.set_value(pwr)
self.adapter.complete_gather()
self.adapter.begin_gather()
xps = str(format(data.actual_position[0], '.4f'))
self.Xabs.set_value(xps)
yps = str(format(data.actual_position[1], '.4f'))
self.Yabs.set_value(yps)
zps = str(format(data.actual_position[2], '.4f'))
self.Zabs.set_value(zps)
abs = str(format(data.actual_position[3], '.4f'))
self.Aabs.set_value(abs)
bbs = str(format(data.actual_position[4], '.4f'))
self.Babs.set_value(bbs)
self.adapter.complete_gather()
ssp = str(data.spindle_speed)
self.adapter.begin_gather()
self.Srpm.set_value(ssp)
ln = str(data.motion_line)
self.adapter.begin_gather()
self.line.set_value(ln)
pgm = str(data.file)
self.adapter.begin_gather()
self.program.set_value(pgm)
pfo = str(data.feedrate * 100)
self.adapter.begin_gather()
self.Fovr.set_value(pfo)
so = str(data.spindlerate * 100)
self.adapter.begin_gather()
self.Sovr.set_value(so)
tooln = str(data.tool_in_spindle)
self.adapter.begin_gather()
self.Tool_number.set_value(tooln)
self.adapter.complete_gather()
if data.task_mode == 1:
md = 'MDI'
elif data.task_mode == 2:
md = 'AUTOMATIC'
elif data.task_mode == 3:
md = 'MANUAL'
else:
md = ''
self.adapter.begin_gather()
self.mode.set_value(md)
self.adapter.complete_gather()
#time finalization: accumulated time, cut vs auto vs total
if ylt2 != 'initialize' and self.power.value(
) == 'ON' and ylt1 != ylt2:
#accumulated time:total time
if (ylt2 - ylt1).total_seconds() >= 0:
ylt += (ylt2 - ylt1).total_seconds()
ylt1 = ylt2
if ylt >= 0 and self.total_time.value() != str(int(ylt)):
self.adapter.begin_gather()
self.total_time.set_value(str(int(ylt)))
self.adapter.complete_gather()
if ylt < 0:
ylt = float(0)
#accumulated time:cut time
if ct2 != 'initialize' and ex == 'ACTIVE' and self.Srpm.value(
) != None and float(self.Srpm.value(
)) > 0 and self.estop.value() == 'ARMED':
if (ct2 - ct1).total_seconds() >= 0:
ct += (ct2 - ct1).total_seconds()
ct1 = ct2
if ct >= 0 and self.cut_time.value() != str(int(ct)):
self.adapter.begin_gather()
self.cut_time.set_value(str(int(ct)))
self.adapter.complete_gather()
if ct < 0:
ct = float(0)
#accumulated time:auto time
if (at2 != 'initialize' and ex == 'ACTIVE'
or ex == 'INTERRUPTED' or ex == 'STOPPED'
or float(self.Srpm.value()) > 0
) and self.estop.value() == 'ARMED':
if (at2 - at1).total_seconds() >= 0:
at += (at2 - at1).total_seconds()
at1 = at2
if at >= 0 and self.auto_time.value() != str(int(at)):
self.adapter.begin_gather()
self.auto_time.set_value(str(int(at)))
self.adapter.complete_gather()
if at < 0:
at = float(0)
except:
if self.power.value() and self.power.value() != 'OFF':
self.adapter.begin_gather()
self.power.set_value('OFF')
self.adapter.complete_gather()
ylt2 = 'initialize'
ct2 = 'initialize'
at2 = 'initialize'
ylt1 = datetime.datetime.now() #initialized
at1 = datetime.datetime.now() #initialized
ct1 = datetime.datetime.now() #initialized
class GenericActionClient(object):
## @brief Constructor for a GenericActionClient
def __init__(self):
# Initialize ROS generic client node
rospy.init_node('ActionClient')
# Setup MTConnect to ROS Conversion
self.config = bridge_library.obtain_dataMap()
self.msg_parameters = [
'url', 'url_port', 'machine_tool', 'xml_namespace', 'adapter_port',
'service'
]
self.url = self.config[self.msg_parameters[0]]
self.url_port = self.config[self.msg_parameters[1]]
self.mtool = self.config[self.msg_parameters[2]]
self.ns = dict(m=self.config[self.msg_parameters[3]])
self.port = self.config[self.msg_parameters[4]]
self.service = self.config[self.msg_parameters[5]]
# Check for url connectivity, dwell until system timeout
bridge_library.check_connectivity((1, self.url, self.url_port))
# Setup MTConnect Adapter for robot status data items
self.adapter = Adapter((self.url, self.port))
# Create empty lists for actions, message type handles, etc.
self.lib_manifests = []
self.type_handle = None
self.action_list = {}
self.action_goals = {}
self.package = None
self.xml_goal = None
self.di_dict = {
} # XML data item dictionary to store MTConnect Adapter Events
self.handshake = None
# Setup action client data per the config file
self.setup_topic_data()
# Add data items to the MTConnect Adapter - must be unique data items
bridge_library.add_event(
(self.adapter, self.action_list, self.di_dict, False))
# Start the adapter
rospy.loginfo('Start the Robot Link adapter')
self.adapter.start()
# Create robot Service Server thread for each machine tool action
self.action_service = []
for mt_action in self.action_goals.keys():
self.action_service.append(
ActionService(mt_action, self.adapter, self.di_dict,
self.service_handle, self.service))
# Establish XML connection, read in current XML
while True:
try:
self.conn = HTTPConnection(self.url, self.url_port)
response = bridge_library.xml_get_response(
(self.url, self.url_port, self.port, self.conn,
self.mtool + "/current"))
body = response.read()
break
except socket.error as e:
rospy.loginfo('HTTP connection error %s' % e)
time.sleep(10)
# Parse the XML and determine the current sequence and XML Event elements
seq, elements = bridge_library.xml_components(body, self.ns,
self.action_list)
# Start a streaming XML connection
response = bridge_library.xml_get_response(
(self.url, self.url_port, self.port, self.conn,
self.mtool + "/sample?interval=1000&count=1000&from=" + seq))
# Create class lock
self.lock = thread.allocate_lock()
# Create XML polling thread
lp = LongPull(response)
lp.long_pull(self.xml_callback) # Runs until user interrupts
## @brief This function captures the topic package, type, action goals, and action
## state conversion from ROS to MTConnect as required for the ROS action client.
## This task is completed for each topic specified in the configuration file.
##
## This function then performs a relative import of the topic via the getattr(import_module)
## function. Data is stored in the following class attributes:
##
## self.lib_manifests --> used to track which manifests have been loaded.
## self.type_handle --> used for ROS SimpleActionClient, stores package name with action messages.
## self.action_list --> used for ROS SimpleActionClient, stores CNC action request strings.
## self.action_goals --> data structure for message class instances of the topic type.
def setup_topic_data(self):
for package, action in self.config.items():
if package not in self.msg_parameters: # Only one ROS package in config by design
# Load package manifest if unique
if package not in self.lib_manifests:
roslib.load_manifest(package)
self.lib_manifests.append(package)
# Import module
rospy.loginfo('Importing --> ' + package + '.msg')
self.type_handle = import_module(package + '.msg')
self.service_handle = import_module(package + '.srv')
# Capture package for action client
self.package = package
for action_req in action.keys():
# Capture action name and action goal xml tag for action client callback reference
goal_tag = self.config[package][action_req].keys()[0]
self.action_list[action_req] = {
goal_tag:
self.config[package][action_req][goal_tag].keys()
}
# Capture ROS Action parameters
self.action_goals[action_req] = None
return
## @brief ROS Action client function that compiles and sends the action goal to the
## dedicated ROS action server. The function will block until the server becomes available.
## After the goal result is received from the dedicated ROS action server, the function
## sets the robot data item via the MTConnect adapter.
##
## @param cb_data: tuple containing the following parameters:
## @param name: string containing the data item name for the robot action
## @param goals: dictionary of data_item:goal pairs in str:str or str:float format
## @param handle: Python <module 'mtconnect_msgs.msg'>
def action_client(self, cb_data):
# Execute ROS Action
rospy.init_node('ActionClient')
# Unpack action client data
name, goals, handle = cb_data
rospy.loginfo('Launched %s Action CLient' % name)
# Creates the SimpleActionClient, passing the type of the action (MaterialLoadAction) to the constructor.
# i.e. name = 'MaterialLoad', action_type = mtconnect_msgs.msg.MaterialLoadAction
action_type = getattr(handle, name + 'Action')
client = actionlib.SimpleActionClient(name + 'Client', action_type)
# Waits until the action server has started up and started listening for goals.
rospy.loginfo('Waiting for %s Dedicated Action Server' % name)
client.wait_for_server()
rospy.loginfo('%s Dedicated Action Server Activated' % name)
# Creates a MaterialLoad goal to send to the action server.
# i.e. goal = mtconnect_msgs.msg.MaterialLoad()
goal_handle = getattr(handle, name + 'Goal')
goal = goal_handle()
# Check to make sure the action goal has been set
if self.action_goals[name] == None:
# Ping the current url for the goal Event
goal_tag = self.action_list[name].keys()[0]
response = bridge_library.xml_get_response(
(self.url, self.url_port, self.port, self.conn, self.mtool +
"/current?path=//DataItem[@type='" + goal_tag.upper() + "']"))
body = response.read()
root = ElementTree.fromstring(body)
# Set the action goals
self.action_goals = bridge_library.set_goal(
name, self.action_list[name], root, self.ns, self.action_goals)
# Check goal source for required attributes
for attrib, attrib_type, action_goal in zip(goal_handle.__slots__,
goal_handle._slot_types,
self.action_goals[name]):
if bridge_library.type_check(attrib_type, action_goal) == False:
rospy.logerr(
'INCOMPATIBLE GOAL TYPES ROS MSG: %s --> XML: %s' %
(attrib_type, type(action_goal)))
sys.exit()
# Set the goal attributes from XML data
try:
for attrib, xml_goal in zip(goal_handle.__slots__,
self.action_goals[name]):
setattr(goal, attrib, xml_goal)
except Exception as e:
rospy.logerr("ROS Action %s Client failed: %s" % (name, e))
# Sends the goal to the action server.
rospy.loginfo('Sending the goal')
name_conv = bridge_library.split_event(name)
client.send_goal(goal,
done_cb=None,
active_cb=bridge_library.action_cb(
(self.adapter, self.di_dict, name_conv,
'ACTIVE')))
# Waits for the server to finish performing the action.
rospy.loginfo('Waiting for result')
client.wait_for_result()
# Obtain result state
result = client.get_state()
# Prints out the result of the executing action
rospy.loginfo('Returning the result --> %s' % result)
# Set the Robot XML data item
data_item = bridge_library.split_event(name)
# Submit converted result to host via MTConnect adapter
if result == 3: # SUCCEEDED from actionlib_msgs.msg.GoalStatus
rospy.loginfo('Sending COMPLETE flag')
bridge_library.action_cb(
(self.adapter, self.di_dict, data_item, 'COMPLETE'))
return name
elif result == 4: # ABORTED from actionlib_msgs.msg.GoalStatus
rospy.loginfo('%s ABORTED, terminating action request' % data_item)
bridge_library.action_cb(
(self.adapter, self.di_dict, data_item, 'FAIL'))
return None
## @brief Callback function that launches a ROS action client if the machine
## tool data item tag is 'ACTIVE'. Once the action is completed, it completes
## the handshake signal between the machine tool and the robot via the MTConnect
## adapter.
##
## @param chunk: xml data, read from response.read()
def xml_callback(self, chunk):
#rospy.loginfo('*******************In PROCESS_XML callback***************')
self.lock.acquire()
try:
# Only grab XML elements for machine tool action requests
_, elements, self.action_goals = bridge_library.xml_components(
chunk,
self.ns,
self.action_list,
get_goal=True,
action_goals=self.action_goals)
if elements:
# Check for existing handshake, execute it first
if self.handshake:
for e in elements:
if self.handshake == e.attrib[
'name'] and e.text == 'READY':
# Send hand shake signal
bridge_library.action_cb(
(self.adapter, self.di_dict, e.attrib['name'],
'READY'))
self.handshake = None
elements.remove(e)
if self.handshake != None:
rospy.logerr(
'DID NOT RECEIVE READY HANDSHAKE FROM MACHINE TOOL'
)
# Process remaining action requests
for e in elements:
# Remove XML namespace string from the element tag for hash tables
action_text = re.findall(r'(?<=\})\w+', e.tag)[0]
# Check if machine tool is requesting an action, if so, run action client
if e.text == 'ACTIVE':
self.handshake = self.action_client(
(action_text, self.action_goals[action_text],
self.type_handle))
#self.handshake = e.attrib['name']
except Exception as e:
rospy.logerr(
"Generic Action Client: Process XML callback failed: %s, releasing lock"
% e)
finally:
self.lock.release()
#rospy.loginfo('*******************Done with PROCESS_XML callback***************')
return
class GenericActionServer():
## @brief Constructor for a GenericActionServer
def __init__(self):
# Initialize ROS generic client node
rospy.init_node('ActionServer')
# Setup MTConnect to ROS Conversion
self.config = bridge_library.obtain_dataMap()
self.msg_parameters = ['url', 'url_port', 'machine_tool', 'xml_namespace', 'adapter_port']
self.url = self.config[self.msg_parameters[0]]
self.url_port = self.config[self.msg_parameters[1]]
self.mtool = self.config[self.msg_parameters[2]]
self.ns = dict(m = self.config[self.msg_parameters[3]])
self.port = self.config[self.msg_parameters[4]]
# Check for url connectivity, dwell until system timeout
bridge_library.check_connectivity((1,self.url,self.url_port))
# Setup MTConnect Adapter for robot status data items
self.adapter = Adapter((self.url, self.port))
# Create empty lists for actions, message type handles, etc.
self.lib_manifests = []
self.type_handle = None
self.action_list = {}
self.capture_xml = False
self.di_dict = {} # XML data item dictionary to store MTConnect Adapter Events
# Create a dictionary of _results to return upon action success
self._resultDict = {}
# Create a dictionary of action servers
self._as = {}
# Create a dictionary of server names
self.server_name = {}
# Setup action client data per the config file
self.setup_topic_data()
# Add data items to the MTConnect Adapter - must be unique data items
bridge_library.add_event((self.adapter, self.action_list, self.di_dict, True))
# Start the adapter
rospy.loginfo('Start the Robot Link adapter')
self.adapter.start()
# Establish XML connection, read in current XML
while True:
try:
self.conn = HTTPConnection(self.url, self.url_port)
response = bridge_library.xml_get_response((self.url, self.url_port, self.port, self.conn, self.mtool + "/current"))
body = response.read()
break
except socket.error as e:
rospy.loginfo('HTTP connection error %s' % e)
time.sleep(10)
# Parse the XML and determine the current sequence and XML Event elements
seq, elements = bridge_library.xml_components(body, self.ns, self.action_list)
# Start a streaming XML connection
response = bridge_library.xml_get_response((self.url, self.url_port, self.port, self.conn, self.mtool + "/sample?interval=1000&count=1000&from=" + seq))
# Create queue for streaming XML
self.XML_queue = Queue()
# Create XML polling thread
lp = LongPull(response)
xml_thread = Thread(target = lp.long_pull, args = (self.xml_callback,))
xml_thread.daemon = True
xml_thread.start()
## @brief This function imports the topic package and creates an action server for each
## package specified in the configuration file.
##
## Data is stored in the following class attributes:
##
## self.lib_manifests --> used to track which manifests have been loaded
## self.type_handle --> used for ROS SimpleActionClient, stores package name with action messages
## self.action_list --> used for ROS SimpleActionClient, stores CNC action request strings
## self.server_name --> dictionary of data_item:DataItem pairs in str:str format
## self._resultDict --> dictionary of result class instances, i.e. {'MaterialLoad':mtconnect_msgs.msg._MaterialLoadResult.MaterialLoadResult()}
## self._as --> dictionary of ROS action servers referenced by DataItem
def setup_topic_data(self):
for package, action in self.config.items():
if package not in self.msg_parameters: # Only one ROS package in config by design
# Load package manifest if unique
if package not in self.lib_manifests:
roslib.load_manifest(package)
self.lib_manifests.append(package)
# Import module
rospy.loginfo('Importing --> ' + package + '.msg')
self.type_handle = import_module(package + '.msg')
# Iterate through requests and create action result class instances and action servers
for request in action:
# Capture action name for action client callback reference
self.action_list[request] = request
# Store the ROS server name in a hash table
di_conv = bridge_library.split_event(request)
self.server_name[di_conv] = request
# Create a dictionary of result class instances
request_class = getattr(self.type_handle, request + 'Result')
self._resultDict[request] = request_class()
# Create instance of the action class
action_class = getattr(self.type_handle, request + 'Action')
# Create action server for requested action, store in a hash table
self._as[request] = actionlib.SimpleActionServer(request + 'Client', action_class, self.execute_cb, False)
# Start the action server
self._as[request].start()
return
## @brief Callback function that monitors the machine tool action sequence for 'ACTIVE',
## 'COMPLETE', and 'READY' data item states. Once the machine tool is completed with the
## requested action and sends the machine tool 'READY' signal, the ROS action server
## returns set_succeeded to the ROS action client that requested the action.
##
## @param goal: from action client, class instance of the data item action goal, i.e. goal = mtconnect_msgs.msg.OpenDoorGoal()
def execute_cb(self, goal):
# If goal is OpenDoor, use the goal defined in the message as your key, access it as goal.__slots__[0]
action = goal.__slots__[0]
# Empty function -- assumes action was successful
rospy.loginfo('In %s Callback -- determining action request result.' % self.server_name[action])
# Check to make sure machine tool is READY, if not, ABORT the robot action request
# Change to XML Tag format
tokens = re.findall(r'([A-Z][a-z]*)', self.server_name[action])
tokenlist = [val.upper() for val in tokens]
di_tag = tokenlist[0] + '_' + tokenlist[1]
# Check current status of XML Tag
check_response = bridge_library.xml_get_response((self.url, self.url_port, self.port, self.conn, self.mtool + "/current?path=//DataItem[@type='" + di_tag + "']"))
body = check_response.read()
_, element = bridge_library.xml_components(body, self.ns, {self.server_name[action]:self.server_name[action]})
# Complete the action request
if element[0].text == 'READY': # Execute the action request
# Submit goal value to MTConnect via agent (i.e. goal.open_door = 'ACTIVE')
bridge_library.action_cb((self.adapter, self.di_dict, action, 'ACTIVE'))
robot_hold = 0 # Used to make sure you only change robot state once after 'COMPLETE' received from CNC
# Start capturing XML
self.capture_xml = True
# While loop to poll CNC XML until READY is received for Robot action request
dwell = True
while dwell == True:
try:
# Obtain XML chunk
if not self.XML_queue.empty():
chunk = self.XML_queue.get()
# Parse the XML and determine the current sequence and XML Event elements
root = ElementTree.fromstring(chunk)
element_list = root.findall('.//m:' + self.server_name[action], namespaces = self.ns)
if len(element_list) > 1:
rospy.logdebug('XML --> %s' % chunk)
if element_list:
# Must iterate -- multiple elements possible for a single tag
for element in element_list:
if element.text == 'ACTIVE':
# Set accepted back to action client
pass
# While polling monitor CNC response for COMPLETE, submit READY handshake
elif element.text == 'COMPLETE' and robot_hold == 0:
bridge_library.action_cb((self.adapter, self.di_dict, action, 'READY'))
robot_hold = 1
elif element.text == 'READY' and robot_hold == 1:
dwell = False
self.capture_xml = False
# When response is READY, set server result and communicate as below:
# Extract action attribute
result_attribute = self._resultDict[self.server_name[action]].__slots__[0]
# Set the attribute per the ROS to MTConnect conversion
setattr(self._resultDict[self.server_name[action]], result_attribute, 'READY')
# Indicate a successful action
self._as[self.server_name[action]].set_succeeded(self._resultDict[self.server_name[action]])
rospy.loginfo('In %s Callback -- action succeeded.' % self.server_name[action])
elif element.text == 'FAIL':
bridge_library.action_cb((self.adapter, self.di_dict, action, 'FAIL'))
dwell = False
self.capture_xml = False
# When response is FAIL, set server result and communicate as below:
# Extract action attribute. For CNC Actions, only one result --> i.e. OpenDoorResult.__slots__[0] = 'door_ready'
result_attribute = self._resultDict[self.server_name[action]].__slots__[0]
# Set the attribute per the ROS to MTConnect conversion
setattr(self._resultDict[self.server_name[action]], result_attribute, 'FAIL')
# Indicate an aborted action
self._as[self.server_name[action]].set_aborted(self._resultDict[self.server_name[action]])
rospy.loginfo('In %s Callback -- action aborted by CNC.' % self.server_name[action])
# Release the queue
self.XML_queue.task_done()
except rospy.ROSInterruptException:
rospy.loginfo('program interrupted before completion')
else: # Abort the action request, machine tool not ready or faulted
# Extract action attribute. For CNC Actions, only one result --> i.e. OpenDoorResult.__slots__[0] = 'door_ready'
result_attribute = self._resultDict[self.server_name[action]].__slots__[0]
# Set the attribute per the ROS to MTConnect conversion
setattr(self._resultDict[self.server_name[action]], result_attribute, 'NOT_READY')
# Indicate an aborted action
self._as[self.server_name[action]].set_aborted(self._resultDict[self.server_name[action]])
rospy.loginfo('In %s Callback -- action aborted, machine tool NOT_READY, FAULTED, or UNAVAILABLE.' % self.server_name[action])
return
## @brief Processes the xml chunk provided by the LongPull class instance and stores the result
## into the XML_queue since the tags have changed. The queue is used to ensure capture of updated
## tags while the execute_cb function is being executed.
## @param chunk: xml data, read from response.read()
def xml_callback(self, chunk):
#rospy.loginfo('*******************In PROCESS_XML callback***************')
try:
if self.capture_xml == True:
self.XML_queue.put(chunk)
if self.XML_queue.qsize() > 1:
rospy.logdebug('STORED XML INTO QUEUE, WAITING ON ROS ACTION SERVER, QUEUE SIZE --> %s' % self.XML_queue.qsize())
except Exception as e:
rospy.logerr("Bridge Server: Process XML callback failed: %s, releasing lock" % e)
finally:
pass
#rospy.loginfo('*******************Done with PROCESS_XML callback***************')
return
class BridgeSubscriber():
## @brief Constructor for a BridgeSubscriber
def __init__(self):
# Initialize the ROS Bridge subscriber node
rospy.init_node('bridge_subscriber')
# Read configuration file and extract topics and types
self.config = bridge_library.obtain_dataMap()
self.msg_parameters = ['url', 'adapter_port']
self.url = self.config[self.msg_parameters[0]]
self.adapter_port = self.config[self.msg_parameters[1]]
# Setup MTConnect Adapter for robot status data items
self.adapter = Adapter((self.url, self.adapter_port))
self.di_dict = {} # XML data item dictionary to store MTConnect Adapter Events
# Setup ROS topics as specified in .yaml file
self.topic_name_list = [] # List of topic names
self.subscribed_list = [] # List of subscribed topics
self.lib_manifests = [] # Stores loaded ROS library manifests
# Topic type and MTConnect message members from the module import
self.topic_type_list = {}
self.member_types = {}
self.member_names = {}
# The ROS message text retainer, used to map ROS values to MTConnect XML tag.text
self.msg_text = {}
# Create the data sets for the topic types, member names, and member types
self.setup_topic_data()
# Start the adapter
rospy.loginfo('Start the Robot Link adapter')
self.adapter.start()
# Create class lock
self.lock = thread.allocate_lock()
# Loop through requested topics and spawn ROS subscribers when topics are available
topic_list = self.topic_name_list
dwell = 10
while topic_list:
published_topics = dict(rospy.get_published_topics())
for topic_name in topic_list:
if topic_name in published_topics.keys():
# Create ROS Subscriber
idx = topic_list.index(topic_name)
del topic_list[idx]
self.topic_listener((topic_name, self.topic_type_list[topic_name], self.member_names[topic_name], self.msg_text[topic_name]))
else:
rospy.loginfo('ROS Topic %s not available, will try to subscribe in %d seconds' % (topic_name, dwell))
time.sleep(dwell)
## @brief This function captures the topic name, type, and member attributes that are required for
## the ROS subscriber. This task is completed for each topic specified in the configuration file.
##
## This function then performs a relative import of the topic via the getattr(import_module) function.
## Data is stored in the following class instance attributes:
##
## self.topic_type_list --> used for module import and msg parameters.
## self.member_types --> member type, not used in msg parameters, future use.
## self.member_names --> used for ROS subscriber msg parameters.
def setup_topic_data(self):
for topic, topic_type in self.config.items():
if topic not in self.msg_parameters:
self.topic_name_list.append(topic)
# Only one type per topic
type_key = topic_type.keys()[0]
self.subscribed_list.append(type_key)
# One set of data items per topic type
self.data_items = [data_item for data_item in topic_type[type_key].keys()]
# Add data items to the MTConnect Adapter - must be unique data items
bridge_library.add_event((self.adapter, self.data_items, self.di_dict, False))
# Extract package name and topic type name
tokens = topic_type.keys()[0].split('/')
package = tokens[0]
type_name = tokens[1]
# Load package manifest if unique
if package not in self.lib_manifests:
roslib.load_manifest(package)
self.lib_manifests.append(package)
# Import module and create topic type class,
# i.e. append <class 'mtconnect_msgs.msg._RobotStates.RobotStates'>
rospy.loginfo('Class Instance --> ' + package + '.msg.' + type_name)
type_handle = getattr(import_module(package + '.msg'), type_name)
self.topic_type_list[topic] = type_handle
self.msg_text[topic] = type_handle._full_text
self.member_types[topic] = type_handle._slot_types
self.member_names[topic] = type_handle.__slots__
return
## @brief Callback function that captures the attribute values for a ROS topic.
## The topic data is stored in a list of tuples, where each tuple is a
## (attrib_name, attrib_value) pair. To access the integer value of the attribute
## value, use attrib_value.val.
##
## All data conversions between ROS and MTConnect are stored in the ROS
## subscriber YAML file. A separate function handles the ROS to MTConnect
## conversions for generic robot messages.
## @param data: callback ROS message data from the ROS subscriber
## @param cb_data: tuple containing the following parameters:
## @param topic_name: string defining the name of the ROS topic
## @param type_handle: class instance of the ROS topic message i.e. <class 'mtconnect_msgs.msg._RobotStates.RobotStates'>
## @param member_set: list of strings defining the attribute members for the message class
## @param msg_text: string of the entire message text (may be eliminated in future revisions)
def topic_callback(self, data, cb_data):
self.lock.acquire()
try:
(topic_name, type_handle, member_set, msg_text) = cb_data
type_name = type_handle._type
# Repackage members into a dictionary
dout = {val:val for val in member_set}
# Iterate through data items and capture message data
msg_data = []
msg_constants = {}
for dataitem in member_set:
# Capture attribute for package/message type --> industrial_msgs/TriState.avail
attrib_handle = getattr(type_handle(), dataitem)
if 'header' not in dataitem:
# Extract string representing message attribute --> 'val'
val_key = attrib_handle.__slots__[0]
# Capture message data.attrib --> <class message data>.dataitem: TriState.avail
token = getattr(data, dataitem)
# Capture the integer value for the attribute: avail.val
item_value = getattr(token, val_key)
# Store published message data for the topic
msg_data.append((dataitem, item_value))
# Create a list of strings containing message CONSTANTS
msg_constants[dataitem] = []
for attrib in dir(attrib_handle):
if attrib.isupper():
if getattr(attrib_handle, attrib) == item_value:
msg_constants[dataitem].append(attrib)
# Execute the ROS to MTConnet conversion function
self.data_item_conversion(topic_name, type_name, msg_data, msg_text, msg_constants)
except Exception as e:
rospy.logerr('Topic callback failed: %s, releasing lock' % e)
finally:
self.lock.release()
return
## @brief A conversion function that will convert the ROS message value to the
## machine tool value. The conversion dictionary is provided in the configuration file.
## @param topic_name: string defining the name of the ROS topic
## @param type_name: string defining the name of the ROS topic type
## @param msg_data: list of strings defining message attributes
## @param msg_text: string of the entire message text (may be eliminated in future revisions)
## @param constants: list of ROS topic message type CONSTANTS stored as strings
def data_item_conversion(self, topic_name, type_name, msg_data, msg_text, constants):
# Set the Robot XML data item via the MTConnect Adapter
for member_data in msg_data:
# Iterate through list of ROS message CONSTANTS and set the MTConnect Adapter value
for const_val in constants[member_data[0]]:
if const_val in self.config[topic_name][type_name][member_data[0]].keys():
# If ROS to MTConnect mapping dictionary contains the constant, set the adapter value
adapter_val = self.config[topic_name][type_name][member_data[0]][const_val]
break
else:
adapter_val = None
if adapter_val is None:
rospy.logerr('ROS to MTConnect Mapping failed')
# Set the Robot XML data item
bridge_library.action_cb((self.adapter, self.di_dict, member_data[0], adapter_val))
return
## @brief Main ROS subscriber function. A new thread is created for each callback.
## @param data: tuple containing the following parameters:
## @param topic_name: string defining the name of the ROS topic
## @param type_handle: class instance of the ROS topic message i.e. <class 'mtconnect_msgs.msg._RobotStates.RobotStates'>
## @param member_set: list of strings defining the message attributes
## @param msg_text: string of the entire message text (may be eliminated in future revisions)
def topic_listener(self, data):
# Unpack arguments
topic_name, type_handle, member_set, msg_text = data
# Launch the ROS subscriber
rospy.Subscriber(topic_name, type_handle, self.topic_callback, data)
return
