def checkForSubscribers(self):
try:
subCheck = re.search("Subscribers:.*", rostopic.get_info_text(self._pub.name)).group(0).split(": ")[1]
subMagCheck = re.search("Subscribers:.*", rostopic.get_info_text(self._pubMag.name)).group(0).split(": ")[1]
if not self._haveRightToPublish and (subCheck == "" or subMagCheck == ""):
self._output.write(PublishRequest(IMU, 0, True).dataTosend())
self._haveRightToPublish = True
elif self._haveRightToPublish and (subCheck == "None" and subMagCheck == "None"):
self._output.write(PublishRequest(IMU, 0, False).dataTosend())
self._haveRightToPublish = False
except:
pass
def checkForSubscribers(self):
try:
subCheck = re.search('Subscribers:.*', rostopic.get_info_text(self._pub.name)).group(0).split(': ')[1]
subTfCheck = re.search('Subscribers:.*', rostopic.get_info_text('/tf')).group(0).split(': ')[1]
if not self._haveRightToPublish and (subCheck == '' or subTfCheck == ''):
self._output.write(PublishRequest(DiffDriverCL, 0, True).dataTosend())
self._haveRightToPublish = True
elif self._haveRightToPublish and (subCheck == 'None' and subTfCheck == 'None'):
self._output.write(PublishRequest(DiffDriverCL, 0, False).dataTosend())
self._haveRightToPublish = False
self._firstTimePub = True
except:
pass
def show_table(self):
t = Table(names=('Node', 'Topic', 'Alive'), dtype=('S50', 'S50', 'S5'))
i = 0
alive = False
node = ""
topic = ""
# t.add_row(('', self.mon_clk_sub[0], self.mon_clk_sub[1]))
stringr = str(self.mon_clk_sub)
stringrr = stringr.split("'")
if len(stringrr) >= 2:
print('mon clk sub ', stringrr[1], stringrr[3])
for pck in self.get_avail_topics():
node = str(pck[1])
topic = str(pck[0])
info = rostopic.get_info_text(topic)
if info != None:
alive = True
# print(info)
# l = rosmsg.list_types('rospy', mode='.msg')
# print(l)
# var = os.system("rosmsg info " + node + "\n")
# message = rospy.wait_for_message(topic)
# print(message)
t.add_row(
(node, topic,
alive)) # Adds all the topics to the table for monitoring
i += 1
print(t)
def checkForSubscribers(self):
try:
subCheck = re.search('Subscribers:.*', rostopic.get_info_text(self._pub.name)).group(0).split(': ')[1]
if not self._haveRightToPublish and subCheck == '':
self._output.write(PublishRequest(Button, self._switchId, True).dataTosend())
self._haveRightToPublish = True
elif self._haveRightToPublish and subCheck == 'None':
self._output.write(PublishRequest(Button, self._switchId, False).dataTosend())
self._haveRightToPublish = False
except: pass
def checkForSubscribers(self):
try:
subCheck = re.search('Subscribers:.*', rostopic.get_info_text(self._pub.name)).group(0).split(': ')[1]
if not self._haveRightToPublish and subCheck == '':
self._output.write(PublishRequest(7, 0, True).dataTosend())
self._haveRightToPublish = True
elif self._haveRightToPublish and subCheck == 'None':
self._output.write(PublishRequest(7, 0, False).dataTosend())
self._haveRightToPublish = False
except: pass
def checkForSubscribers(self):
try:
subCheck = re.search('Subscribers:.*',
rostopic.get_info_text(
self._pub.name)).group(0).split(': ')[1]
subTfCheck = re.search(
'Subscribers:.*',
rostopic.get_info_text('/tf')).group(0).split(': ')[1]
if not self._haveRightToPublish and (subCheck == ''
or subTfCheck == ''):
self._output.write(
PublishRequest(DiffDriverCL, 0, True).dataTosend())
self._haveRightToPublish = True
elif self._haveRightToPublish and (subCheck == 'None'
and subTfCheck == 'None'):
self._output.write(
PublishRequest(DiffDriverCL, 0, False).dataTosend())
self._haveRightToPublish = False
self._firstTimePub = True
except:
pass
def __init__(self):
self.action_clients = []
self.joint_names = []
while not rospy.has_param('~state_topics'):
logging.loginfo('waiting for param ' + '~state_topics')
rospy.sleep(1)
while not rospy.has_param('~client_topics'):
logging.loginfo('waiting for param ' + '~client_topics')
rospy.sleep(1)
self.state_topics = rospy.get_param('~state_topics', [])
self.client_topics = rospy.get_param('~client_topics', [])
self.number_of_clients = len(self.state_topics)
if self.number_of_clients != len(self.client_topics):
logging.logerr('number of state and action topics do not match')
exit()
if self.number_of_clients == 0:
logging.logerr(
'the state_topic or client_topic parameter is empty')
exit()
self.client_type = []
for i in range(self.number_of_clients):
waiting_for_topic = True
while waiting_for_topic:
try:
rospy.wait_for_message(self.state_topics[i],
AnyMsg,
timeout=10)
waiting_for_topic = False
type = rostopic.get_info_text(self.client_topics[i] +
'/goal').split('\n')[0][6:]
self.client_type.append(type)
except rostopic.ROSTopicException as e:
logging.logerr(
'Exception: {} .Maybe unknown topic \'{}/goal\' \nmissing / in front of topic name?'
.format(e, self.client_topics[i]))
exit()
except rospy.ROSException as e:
if e.message == 'rospy shutdown':
exit()
logging.loginfo('waiting for state topic {}'.format(
self.state_topics[i]))
self.state_type = []
for i in range(self.number_of_clients):
try:
type = rostopic.get_info_text(
self.state_topics[i]).split('\n')[0][6:]
self.state_type.append(type)
except rostopic.ROSTopicException:
logging.logerr(
'unknown topic \'{}/goal\' \nmissing / in front of topic name?'
.format(self.state_topics[i]))
exit()
for i in range(self.number_of_clients):
if self.client_type[
i] == 'control_msgs/FollowJointTrajectoryActionGoal':
self.action_clients.append(
actionlib.SimpleActionClient(
self.client_topics[i],
control_msgs.msg.FollowJointTrajectoryAction))
elif self.client_type[
i] == 'pr2_controllers_msgs/JointTrajectoryActionGoal':
self.action_clients.append(
actionlib.SimpleActionClient(
self.client_topics[i],
pr2_controllers_msgs.msg.JointTrajectoryAction))
else:
logging.logerr(
'wrong client topic type:' + self.client_type[i] +
'\nmust be either control_msgs/FollowJointTrajectoryActionGoal or pr2_controllers_msgs/JointTrajectoryActionGoal'
)
exit()
self.joint_names.append(
rospy.wait_for_message(
self.state_topics[i], control_msgs.msg.
JointTrajectoryControllerState).joint_names)
logging.loginfo('connected to {}'.format(self.client_topics[i]))
self.current_controller_state = control_msgs.msg.JointTrajectoryControllerState(
)
total_number_joints = 0
for joint_name_list in self.joint_names:
total_number_joints += len(joint_name_list)
self.current_controller_state.joint_names.extend(joint_name_list)
self.current_controller_state.desired.positions = [
0 for i in range(total_number_joints)
]
self.current_controller_state.desired.accelerations = [
0 for i in range(total_number_joints)
]
self.current_controller_state.desired.effort = [
0 for i in range(total_number_joints)
]
self.current_controller_state.desired.velocities = [
0 for i in range(total_number_joints)
]
self.current_controller_state.actual = copy.deepcopy(
self.current_controller_state.desired)
self.current_controller_state.error = copy.deepcopy(
self.current_controller_state.desired)
self.state_pub = rospy.Publisher(
'/whole_body_controller/state',
control_msgs.msg.JointTrajectoryControllerState,
queue_size=10)
for i in range(self.number_of_clients):
if self.state_type[
i] == 'control_msgs/JointTrajectoryControllerState':
rospy.Subscriber(
self.state_topics[i],
control_msgs.msg.JointTrajectoryControllerState,
self.state_cb_update)
elif self.state_type[
i] == 'pr2_controllers_msgs/JointTrajectoryControllerState':
rospy.Subscriber(
self.state_topics[i],
pr2_controllers_msgs.msg.JointTrajectoryControllerState,
self.state_cb_update)
else:
logging.logerr(
'wrong state topic type ' + self.state_type[i] +
'\nmust be either control_msgs/JointTrajectoryControllerState or pr2_controllers_msgs/JointTrajectoryControllerState'
)
exit()
rospy.Subscriber(self.state_topics[0],
control_msgs.msg.JointTrajectoryControllerState,
self.state_cb_publish)
self._as = actionlib.SimpleActionServer(
'/whole_body_controller/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction,
execute_cb=self.callback,
auto_start=False)
self._as.register_preempt_callback(self.preempt_cb)
self._as.start()
logging.loginfo(u'Joint Trajector Splitter: running')
rospy.spin()
def arch():
try:
rospy.init_node('handler', anonymous=True)
currentServices = []
loggerCount = 0
last_topics = []
last_nodes = []
last_publish = {}
last_publish1 = {}
currentTopics = []
loggerCount = 0
global currentReported
global serviceExpeptions
global noServiceIntermediator
global oldnoServiceIntermediator
service_dict = {}
old_service_dict = {}
length = 0
updated = False
oldReported = list()
while not rospy.is_shutdown():
new_topics = rospy.get_published_topics()
new_nodes = rosnode.get_node_names()
new_publish = {}
new_publish1 = {}
inc_topics = [
item for item in new_topics if item not in last_topics
]
inc_nodes = [item for item in new_nodes if item not in last_nodes]
inc_service_dict = {}
inc_publish = {}
inc_publish1 = {}
inc_service_dict = {}
inc_reported = {}
for topic, typ in new_topics:
info = rostopic.get_info_text(topic)
subscribers = parse_info_sub(info)
publishers = parse_info_pub(info)
new_publish[topic] = set(subscribers)
new_publish1[topic] = set(publishers)
if (topic not in last_publish.keys()
) or set(subscribers) != last_publish[topic]:
inc_publish[topic] = set(subscribers)
if (topic not in last_publish1.keys()
) or set(publishers) != last_publish1[topic]:
inc_publish1[topic] = set(publishers)
try:
for newService, Provider in get_current_services():
if not newService in currentServices:
currentServices.append(newService)
newServiceHandler(newService)
service_dict[newService] = [
rosservice.get_service_type(newService),
rosservice.get_service_args(newService)
]
if (newService not in old_service_dict.keys()
) or service_dict[newService] != old_service_dict[
newService]:
inc_service_dict[newService] = [
rosservice.get_service_type(newService),
rosservice.get_service_args(newService)
]
except Exception as e:
print e
pass
if old_service_dict != service_dict:
old_service_dict = dict(service_dict)
updated = True
inc_reported = [
item for item in currentReported if item not in oldReported
]
oldReported = list(currentReported)
if len(inc_reported) != 0:
updated = True
if noServiceIntermediator != oldnoServiceIntermediator:
oldnoServiceIntermediator = list(noServiceIntermediator)
print
print
print 'Services calls not being reported for these services: ' + str(
noServiceIntermediator)
if inc_topics or inc_nodes or inc_publish or inc_publish1 != inc_service_dict or inc_reported:
y = {}
y["topics"] = send_topics(inc_topics)
y["nodes"] = send_nodes(inc_nodes)
y["pub"] = sendPub(inc_publish)
y["sub"] = sendSub(inc_publish1)
y["service"] = inc_service_dict
y["calls"] = inc_reported
last_nodes = list(new_nodes)
last_topics = list(new_topics)
last_publish = dict(new_publish)
last_publish1 = dict(new_publish1)
updated = False
requests.get(URL, data=json.dumps(y))
except Exception as e:
print e
pass
def get_info_text(selection_url):
if selection_url is None:
return ''
if selection_url.startswith('node:'):
try:
node_name = selection_url[5:]
master = roslib.scriptutil.get_master()
node_api = rosnode.get_api_uri(master, node_name)
return rosnode.get_node_info_description(node_name) + rosnode.get_node_connection_info_description(node_api)
except rosnode.ROSNodeException, e:
return "ERROR: %s"%str(e)
elif selection_url.startswith('topic:'):
try:
return rostopic.get_info_text(selection_url[6:])
except rostopic.ROSTopicException, e:
return "ERROR: %s"%str(e)
class DotUpdate(threading.Thread):
"""Thread to control update of dot file"""
def __init__(self, graph, viewer, output_file=None):
threading.Thread.__init__(self, name="DotUpdate")
self.viewer = viewer
self.graph = graph
self.output_file = output_file
self.selection_url = None
self.selection_update = False
def select_callback(self, target, event):
if selection_url is None:
return ''
if selection_url.startswith('node:'):
try:
node_name = selection_url[5:]
master = rosgraph.Master('/rxgraph')
node_api = rosnode.get_api_uri(master, node_name)
return rosnode.get_node_info_description(
node_name) + rosnode.get_node_connection_info_description(
node_api, master)
except rosnode.ROSNodeException, e:
return "ERROR: %s" % str(e)
elif selection_url.startswith('topic:'):
try:
return rostopic.get_info_text(selection_url[6:])
except rostopic.ROSTopicException, e:
return "ERROR: %s" % str(e)
class DotUpdate(threading.Thread):
"""Thread to control update of dot file"""
def __init__(self, graph, viewer, output_file=None):
threading.Thread.__init__(self, name="DotUpdate")
self.viewer = viewer
self.graph = graph
self.output_file = output_file
self.selection_url = None
self.selection_update = False
def select_callback(self, target, event):
def arch(filters):
try:
rospy.init_node('acme_data_collector')
filters.ignore_node.append('acme_data_collector') # Add this node to the set of things to ignore
currentServices = []
loggerCount = 0
last_topics = []
last_nodes = []
last_publish = {}
last_publish1 = {}
currentTopics = []
loggerCount = 0
global currentReported
global serviceExpeptions
global noServiceIntermediator
global oldnoServiceIntermediator
# append list to add / to every node
filters.ignore_node = map((lambda x : '/' + x), filters.ignore_node)
service_dict = {}
old_service_dict = {}
length = 0
updated = False
oldReported = list()
shortcircuit = False
rate = 1.0/float(filters.rate) #The Hz for the rate
r = rospy.Rate(rate)
while not rospy.is_shutdown() and not shortcircuit:
shortcircuit = filters.once
pubs, subs, srvs = get_current_state(filters.ignore_node, filters.ignore_service, filters.ignore_topic)
pubs = [pub[0] for pub in pubs]
subs = [sub[0] for sub in subs]
new_topics = [topic for topic in rospy.get_published_topics() if topic[0] in pubs or topic[0] in subs]
new_nodes = rosnode.get_node_names()
new_publish = {}
new_publish1 = {}
inc_topics = [item for item in new_topics if item not in last_topics and item not in filters.ignore_topic]
inc_nodes = [item for item in new_nodes if item not in last_nodes and item not in filters.ignore_node]
inc_service_dict = {}
inc_publish = {}
inc_publish1 = {}
inc_service_dict = {}
inc_reported = {}
for topic, typ in new_topics:
info = rostopic.get_info_text(topic)
subscribers = parse_info_sub(info, filters.ignore_topic, filters.ignore_node)
publishers = parse_info_pub(info, filters.ignore_topic, filters.ignore_node)
new_publish[topic] = set(subscribers)
new_publish1[topic] = set(publishers)
if (topic not in last_publish.keys()) or set(subscribers) != last_publish[topic]:
inc_publish[topic] = set(subscribers)
if (topic not in last_publish1.keys()) or set(publishers) != last_publish1[topic]:
inc_publish1[topic] = set(publishers)
try:
for newService, Provider in srvs:
if not newService in currentServices and not newService in filters.ignore_service:
currentServices.append(newService)
newServiceHandler(newService)
service_dict[newService] = [rosservice.get_service_type(newService), rosservice.get_service_args(newService)]
if (newService not in old_service_dict.keys()) or service_dict[newService] != old_service_dict[newService]:
inc_service_dict[newService] = [rosservice.get_service_type(newService), rosservice.get_service_args(newService)]
except Exception as e:
print e
pass
if old_service_dict != service_dict:
old_service_dict = dict(service_dict)
updated = True
inc_reported = [item for item in currentReported if item not in oldReported]
oldReported = list(currentReported)
if len(inc_reported) != 0:
updated = True
if noServiceIntermediator != oldnoServiceIntermediator:
oldnoServiceIntermediator = list(noServiceIntermediator)
print
print
print 'Services calls not being reported for these services: ' + str(noServiceIntermediator)
if inc_topics or inc_nodes or inc_publish or inc_publish1 != inc_service_dict or inc_reported:
y = {}
y["topics"] = send_topics(inc_topics)
y["nodes"] = send_nodes(inc_nodes)
y["pub"] = sendPub(inc_publish)
y["sub"] = sendSub(inc_publish1)
y["service"] = inc_service_dict
y["calls"] = inc_reported
last_nodes = list(new_nodes)
last_topics = list(new_topics)
last_publish = dict(new_publish)
last_publish1 = dict(new_publish1)
updated = False
if filters.file is not None:
output = open(filters.file, 'wb')
output.write(json.dumps(y, indent=4))
output.close()
else:
requests.get(URL, data=json.dumps(y))
r.sleep()
except Exception as e:
print e
pass
def arch():
try:
rospy.init_node('handler', anonymous=True)
currentServices = []
loggerCount = 0
last_topics = []
last_nodes = []
last_publish = {}
last_publish1 = {}
currentTopics = []
loggerCount = 0
global currentReported
global serviceExpeptions
global noServiceIntermediator
global oldnoServiceIntermediator
service_dict = {}
old_service_dict = {}
length = 0
updated = False
oldReported = list()
while not rospy.is_shutdown():
new_topics = rospy.get_published_topics()
new_nodes = rosnode.get_node_names()
new_publish = {}
new_publish1 = {}
inc_topics = [item for item in new_topics if item not in last_topics]
inc_nodes = [item for item in new_nodes if item not in last_nodes]
inc_service_dict = {}
inc_publish = {}
inc_publish1 = {}
inc_service_dict = {}
inc_reported = {}
for topic, typ in new_topics:
info = rostopic.get_info_text(topic)
subscribers = parse_info_sub(info)
publishers = parse_info_pub(info)
new_publish[topic] = set(subscribers)
new_publish1[topic] = set(publishers)
if (topic not in last_publish.keys()) or set(subscribers) != last_publish[topic]:
inc_publish[topic] = set(subscribers)
if (topic not in last_publish1.keys()) or set(publishers) != last_publish1[topic]:
inc_publish1[topic] = set(publishers)
try:
for newService, Provider in get_current_services():
if not newService in currentServices:
currentServices.append(newService)
newServiceHandler(newService)
service_dict[newService] = [rosservice.get_service_type(newService), rosservice.get_service_args(newService)]
if (newService not in old_service_dict.keys()) or service_dict[newService] != old_service_dict[newService]:
inc_service_dict[newService] = [rosservice.get_service_type(newService), rosservice.get_service_args(newService)]
except Exception as e:
print e
pass
if old_service_dict != service_dict:
old_service_dict = dict(service_dict)
updated = True
inc_reported = [item for item in currentReported if item not in oldReported]
oldReported = list(currentReported)
if len(inc_reported) != 0:
updated = True
if noServiceIntermediator != oldnoServiceIntermediator:
oldnoServiceIntermediator = list(noServiceIntermediator)
print
print
print 'Services calls not being reported for these services: ' + str(noServiceIntermediator)
if inc_topics or inc_nodes or inc_publish or inc_publish1 != inc_service_dict or inc_reported:
y = {}
y["topics"] = send_topics(inc_topics)
y["nodes"] = send_nodes(inc_nodes)
y["pub"] = sendPub(inc_publish)
y["sub"] = sendSub(inc_publish1)
y["service"] = inc_service_dict
y["calls"] = inc_reported
last_nodes = list(new_nodes)
last_topics = list(new_topics)
last_publish = dict(new_publish)
last_publish1 = dict(new_publish1)
updated = False
requests.get(URL, data=json.dumps(y))
except Exception as e:
print e
pass
