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