def generate_type_info_msg(self): """ Basic connection details are provided by get system state from the master, which is a one shot call to give you information about every connection possible. it does not however provide type info information and the only way of retrieving that from the master is making one xmlrpc call to the master for every single connection. This gets expensive, so generating this information is usually delayed until we need it and done via this method. """ if self.type_info is None: if self.type == PUBLISHER or self.type == SUBSCRIBER: try: self.type_info = rostopic.get_topic_type( self.name)[0] # message type self.type_msg = self.type_info except rostopic.ROSTopicIOException as topic_exc: rospy.logwarn(topic_exc) elif self.type == SERVICE: try: self.type_info = rosservice.get_service_uri(self.name) self.type_msg = rosservice.get_service_type(self.name) except rosservice.ROSServiceIOException as service_exc: rospy.logwarn(service_exc) elif self.type == ACTION_SERVER or self.type == ACTION_CLIENT: try: goal_topic = self.name + '/goal' goal_topic_type = rostopic.get_topic_type(goal_topic) self.type_info = re.sub( 'ActionGoal$', '', goal_topic_type[0]) # Base type for action self.type_msg = self.type_info except rostopic.ROSTopicIOException as topic_exc: rospy.logwarn(topic_exc.msg) return self # chaining
def add_trigger(topic, expr, select, index, wait=False): global topics if topic in topics: topic_type = topics[topic] topic_class = roslib.message.get_message_class(topic_type) else: topic_type, _, _ = rostopic.get_topic_type(topic) topic_class, _, _ = rostopic.get_topic_class(topic) if topic_type is None: if wait is False: rospy.loginfo('%s is not published yet', topic) return None elif wait is True: rate = rospy.Rate(1) while not rospy.is_shutdown() and topic_type is None: topic_type, _, _ = rostopic.get_topic_type(topic) topic_class, _, _ = rostopic.get_topic_class(topic) rospy.loginfo('waiting topic %s' % topic) rate.sleep() else: raise ValueError('wait should be bool') if (topic_class is None): rospy.loginfo('%s is not builded yet', topic_type) return None try: cb_obj = gen_callback(expr, select, index) rospy.loginfo("start subscribe (topic=%s type=%s)", topic, topic_type) sub = rospy.Subscriber(topic, topic_class, cb_obj) except Exception, e: rospy.loginfo(str(e)) return None
def __init__(self, sub="/bvt_P900/RT/image"): self.sub = sub self.image_sub = rospy.Subscriber(sub, Image, self.image_callback) self.bridge = CvBridge() self.previous_features = None self.current_features = None data_type = rostopic.get_topic_type('/nav/nav_sts', blocking=False)[0] data_class = roslib.message.get_message_class(data_type) self.nav_sub = rospy.Subscriber('/nav/nav_sts', data_class, self.nav_callback) self.nav_y = None self.nav_x = None self.nav_yaw = None data_type = rostopic.get_topic_type('/bvt_P900/RT/parameter', blocking=False)[0] data_class = roslib.message.get_message_class(data_type) self.nav_sub = rospy.Subscriber('/bvt_P900/RT/parameter', data_class, self.parameter_callback) self.max_fov = None self.height = None self.width = None self.origin_row = None self.origin_col = None self.max_range = None self.pcl_pub = rospy.Publisher("/my_pcl_topic", PointCloud) self.image_pub = rospy.Publisher(str(sub) + "_marked", Image)
def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self._start_time = rospy.get_time() if not self._connected: (msg_path, msg_topic, fn) = rostopic.get_topic_type( self._control_manager_diagnostics_topic) if msg_topic == self._control_manager_diagnostics_topic: msg_type = self._get_msg_from_path(msg_path) self._sub_cont_diag = ProxySubscriberCached( {self._control_manager_diagnostics_topic: msg_type}) self._connected = True Logger.loginfo( '[ServiceRotateRelativeToGoToState]: Successfully subscribed to previously unavailable topic %s' % self._control_manager_diagnostics_topic) else: Logger.logwarn( '[ServiceRotateRelativeToGoToState]: Topic %s still not available, giving up.' % self._control_manager_diagnostics_topic) if not self._connected_odom: (msg_path_odom, msg_topic_odom, fn_odom) = rostopic.get_topic_type(self._odometry_topic) if msg_topic_odom == self._odometry_topic: msg_type_odom = self._get_msg_from_path(msg_path_odom) self._sub_odom = ProxySubscriberCached( {self._odometry_topic: msg_type_odom}) self._connected_odom = True Logger.loginfo( '[ServiceRotateRelativeToGoToState]: Successfully subscribed to topic %s' % self._odometry_topic) else: Logger.logwarn( '[ServiceRotateRelativeToGoToState]: Topic %s still not available, giving up.' % self._odometry_topic) if self._connected and self._sub_cont_diag.has_msg( self._control_manager_diagnostics_topic): Logger.loginfo( '[ServiceRotateRelativeToGoToState]: Waiting for msg from topic %s' % self._control_manager_diagnostics_topic) self._sub_cont_diag.remove_last_msg( self._control_manager_diagnostics_topic) if self._connected_odom and self._sub_odom.has_msg( self._odometry_topic): Logger.loginfo( '[ServiceRotateRelativeToGoToState]: Waiting for msg from topic %s' % self._odometry_topic) self._sub_odom.remove_last_msg(self._odometry_topic) self._failed = False
def get_goal_type(action_name): try: type, name, fn = rostopic.get_topic_type("{}/goal".format(action_name)) return type except rospy.ROSException as e: try: type, name, fn = rostopic.get_topic_type("{}/Goal".format(action_name), e) return type except rospy.ROSException as e2: rospy.logerr("Couldnt find feedback type for action {}".format(action_name), e2) return None
def get_goal_type(action_name): try: type, name, fn = rostopic.get_topic_type(action_name + "/goal") return type except rospy.ROSException as e: try: type, name, fn = rostopic.get_topic_type(action_name + "/Goal", e) return type except rospy.ROSException as e2: rospy.logerr( "Couldnt find feedback type for action " + action_name, e2) return None
def generate_advertisement_connection_details(self, connection_type, name, node): ''' Creates all the extra details to create a connection object from an advertisement rule. This is a bit different to the previous one - we just need the type and single node uri that everything originates from (don't need to generate all the pub/sub connections themselves. Probably flips could be merged into this sometime, but it'd be a bit gnarly. @param connection_type : the connection type (one of gateway_msgs.msg.ConnectionType) @type string @param name : the name of the connection @type string @param node : the master node name it comes from @param string @return the utils.Connection object complete with type_info and xmlrpc_uri @type utils.Connection ''' # Very important here to check for the results of xmlrpc_uri and especially topic_type # https://github.com/robotics-in-concert/rocon_multimaster/issues/173 # In the watcher thread, we get the local connection index (whereby the arguments of this function # come from) via master.get_connection_state. That means there is a small amount of time from # getting the topic name, to checking for hte xmlrpc_uri and especially topic_type here in which # the topic could have disappeared. When this happens, it returns None. connection = None xmlrpc_uri = self.lookupNode(node) if xmlrpc_uri is None: return connection if connection_type == rocon_python_comms.PUBLISHER or connection_type == rocon_python_comms.SUBSCRIBER: type_info = rostopic.get_topic_type(name)[0] # message type if type_info is not None: connection = utils.Connection( gateway_msgs.Rule(connection_type, name, node), type_info, type_info, xmlrpc_uri) elif connection_type == rocon_python_comms.SERVICE: type_info = rosservice.get_service_uri(name) type_msg = rosservice.get_service_type(name) if type_info is not None: connection = utils.Connection( gateway_msgs.Rule(connection_type, name, node), type_msg, type_info, xmlrpc_uri) elif connection_type == rocon_python_comms.ACTION_SERVER or connection_type == rocon_python_comms.ACTION_CLIENT: goal_topic = name + '/goal' goal_topic_type = rostopic.get_topic_type(goal_topic) type_info = re.sub('ActionGoal$', '', goal_topic_type[0]) # Base type for action if type_info is not None: connection = utils.Connection( gateway_msgs.Rule(connection_type, name, node), type_info, type_info, xmlrpc_uri) return connection
def topic_wait_type(self, topic_name, retries=5, retrysleep=1): resolved_topic_name = rospy.resolve_name(topic_name) topic_type, _, _ = rostopic.get_topic_type(resolved_topic_name) retry = 0 while not topic_type and retry < 5: print 'Topic {topic} not found. Retrying...'.format( topic=resolved_topic_name) rospy.rostime.wallsleep(retrysleep) retry += 1 topic_type, _, _ = rostopic.get_topic_type(resolved_topic_name) if retry >= retries: self.fail( "Topic {0} not found ! Failing.".format(resolved_topic_name)) return topic_type
def test_get_topic_type(self): import rostopic self.assertEquals((None, None, None), rostopic.get_topic_type('/fake', blocking=False)) t, n, f = rostopic.get_topic_type('/rosout', blocking=False) self.assertEquals('rosgraph_msgs/Log', t) self.assertEquals('/rosout', n) self.assert_(f is None) t, n, f = rostopic.get_topic_type('/rosout/name', blocking=False) self.assertEquals('rosgraph_msgs/Log', t) self.assertEquals('/rosout', n) self.failIf(f is None) from rosgraph_msgs.msg import Log self.assertEquals("bob", f(Log(name="bob")))
def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self.ref_frame = userdata.ref_frame self._topic = userdata.camera_topic self._camera_frame = userdata.camera_frame self._connected = False self._failed = False self._start_time = rospy.get_rostime() # Subscribe to the topic for the logical camera (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic) if msg_topic == self._topic: msg_type = self._get_msg_from_path(msg_path) self._sub = ProxySubscriberCached({self._topic: msg_type}) self._connected = True else: Logger.logwarn( 'Topic %s for state %s not yet available.\nFound: %s\nWill try again when entering the state...' % (self._topic, self.name, str(msg_topic))) # Get transform between camera and robot_base try: self._transform = self._tf_buffer.lookup_transform( self.ref_frame, self._camera_frame, rospy.Time(0), rospy.Duration(1.0)) except Exception as e: Logger.logwarn('Could not transform pose: ' + str(e)) self._failed = True
def execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. for i in [0, 1, 2, 3, 4, 5]: self._topic = "/ariac/logical_camera_" + str(i) self._start_time = rospy.Time.now() (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic) if msg_topic == self._topic: msg_type = self._get_msg_from_path(msg_path) self._sub = ProxySubscriberCached({self._topic: msg_type}) elapsed = rospy.get_rostime() - self._start_time while (elapsed.to_sec() < self._wait): elapsed = rospy.get_rostime() - self._start_time if self._sub.has_msg(self._topic): message = self._sub.get_last_msg(self._topic) for model in message.models: if model.type == userdata.part_type: userdata.bin = "bingr" + str(i) + "PreGrasp" userdata.camera_topic = "/ariac/logical_camera_" + str( i) userdata.camera_frame = "logical_camera_" + str( i) + "_frame" Logger.loginfo("bingr" + str(i) + "PreGrasp") userdata.ref_frame = "torso_main" return 'continue' Logger.loginfo("part_type not found") return 'failed'
def wait_topic_ready(topic_name, url): remote_topic_type = "" while remote_topic_type == "": remote_topic_type = get_remote_topic_type(topic_name, url) #print remote_topic_type+" remote_topic_type" if (remote_topic_type == ""): rospy.loginfo( "Failed to get the remote type of topic %s. Retrying...", topic_name) time.sleep(1) local_topic_type = (None, None, None) while local_topic_type[0] == None: local_topic_type = get_topic_type(topic_name) #print str(local_topic_type)+" local_topic_type" if (local_topic_type[0] == None): rospy.loginfo( "Failed to get the local type of topic %s. Retrying...", topic_name) time.sleep(1) if remote_topic_type == local_topic_type[0]: #print str(local_topic_type)+" equal" return local_topic_type[0] else: return None
def __init__(self): self.is_ready = None self.pub = rospy.Publisher('notification', Notification) rospy.init_node('sensor_notifier_node') sensor_topic = rospy.get_param('~sensor_topic', None) if sensor_topic == None: rospy.logerr('param sensor_topic not defined') self.notification_text = rospy.get_param('~notification_text', u'sensor activated') self.notification_level = rospy.get_param('~notification_level', 2) self.notification_target = rospy.get_param('~notification_target', 'voice,log') self.notification_value = rospy.get_param('~notification_value', '1') self.criteria = rospy.get_param('~criteria', 'eq') self.interval = rospy.get_param('~interval', 30) self.notif_time = rospy.Time() # subscribe to sensor topic data_type = rostopic.get_topic_type(sensor_topic, blocking=False)[0] if data_type: data_class = roslib.message.get_message_class(data_type) rospy.Subscriber(sensor_topic, data_class, self.callback) rospy.loginfo("start listening to %s, notification: %s %s", sensor_topic, self.criteria, str(self.notification_value)) self.is_ready = 1 else: rospy.logerr("error getting type for topic " + sensor_topic)
def execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. # One of the outcomes declared above. for i in [1,2,3,4,5,6]: self._topic = "/ariac/logical_camera_stock"+str(i) self._start_time = rospy.Time.now() (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic) if msg_topic == self._topic: msg_type = self._get_msg_from_path(msg_path) self._sub = ProxySubscriberCached({self._topic: msg_type}) elapsed = rospy.get_rostime() - self._start_time; while (elapsed.to_sec() < self._wait): elapsed = rospy.get_rostime() - self._start_time; if self._sub.has_msg(self._topic): message = self._sub.get_last_msg(self._topic) for model in message.models: if model.type == userdata.part_type: if userdata.arm_id == "Right_Arm": userdata.gantry_pos = "Gantry_Part"+str(i)+"_R" elif userdata.arm_id == "Left_Arm": userdata.gantry_pos = "Gantry_Part"+str(i)+"_L" userdata.camera_topic = "/ariac/logical_camera_stock"+str(i) userdata.camera_frame = "logical_camera_stock"+str(i)+"_frame" Logger.loginfo("Gantry_Part"+str(i)) if i < 5: return 'bin' else: return 'shelf' Logger.loginfo("part_type not found") return 'not_found'
def __init__(self, service_topic, control_manager_diagnostics_topic): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(ServiceGoToState, self).__init__(input_keys=['goal'], outcomes=['successed', 'failed']) # Store state parameter for later use. self._service_topic = rospy.resolve_name(service_topic) self._control_manager_diagnostics_topic = rospy.resolve_name( control_manager_diagnostics_topic) # Create proxy service client self._srv = ProxyServiceCaller( {self._service_topic: mrs_msgs.srv.Vec4}) (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._control_manager_diagnostics_topic) if msg_topic == self._control_manager_diagnostics_topic: msg_type = self._get_msg_from_path(msg_path) self._sub_cont_diag = ProxySubscriberCached( {self._control_manager_diagnostics_topic: msg_type}) self._connected = True Logger.loginfo( '[ServiceGoToState]: Successfully subscribed to topic %s' % self._control_manager_diagnostics_topic) self._failed = False
def subscribe(self, topic): if topic is None: rospy.logerr("Got topic None, not subscribing") return if self.subscriber is not None: self.subscriber.unregister() self.last_img = None type_name, topic_name, _ = get_topic_type(topic) if type_name == 'sensor_msgs/Image': self.subscriber = rospy.Subscriber(topic, Image, self.image_cb, queue_size=1) self.bridge = CvBridge() elif type_name == 'sensor_msgs/CompressedImage': self.subscriber = rospy.Subscriber(topic, CompressedImage, self.image_cb, queue_size=1) else: rospy.logerr('Topic is not of image type') return while not rospy.is_shutdown() and self.last_img is None: rospy.sleep(0.1) if type_name == 'sensor_msgs/Image': w = self.last_img.width h = self.last_img.height elif type_name == 'sensor_msgs/CompressedImage': w, h = self.get_compressed_image_size(self.last_img) self.img_w = w self.img_h = h self.last_topic = topic
def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. Logger.loginfo('[IsRealsenseConnectedState] entering state') self.last_time = rospy.get_time() if not self._connected: (msg_path, msg_topic, fn) = rostopic.get_topic_type(self.realsense_camera_info_topic) if msg_topic == self.realsense_camera_info_topic: msg_type = self._get_msg_from_path(msg_path) self.realsense_subs_ = ProxySubscriberCached( {self.realsense_camera_info_topic: msg_type}) self._connected = True Logger.loginfo( '[IsRealsenseConnectedState]: Successfully subscribed to previously unavailable topic %s' % self.realsense_camera_info_topic) else: Logger.logwarn( '[IsRealsenseConnectedState]: Topic %s still not available, giving up.' % self.realsense_camera_info_topic) if self._connected and self.realsense_subs_.has_msg( self.realsense_camera_info_topic): Logger.loginfo( '[IsRealsenseConnectedState]: Waiting for msg from topic %s' % self.realsense_camera_info_topic) self.realsense_subs_.remove_last_msg( self.realsense_camera_info_topic)
def _handle_add_topic_clicked(self): selected_topic = str(self.topics_dropdown.currentText()) if selected_topic in self._exported_topics: return topic_type = rostopic.get_topic_type(selected_topic)[0] publisher_info = { 'topic_name': str(selected_topic), 'type_name': str(topic_type), 'enabled': bool(True), } publisher_info['publisher_id'] = self._id_counter self._id_counter += 1 publisher_info['counter'] = 0 publisher_info['enabled'] = publisher_info.get('enabled', False) publisher_info['expressions'] = publisher_info.get('expressions', {}) publisher_info['message_instance'] = self._create_message_instance(publisher_info['type_name']) if publisher_info['message_instance'] is None: return self.publisher_tree_widget.model().add_publisher(publisher_info) self._exported_topics.append(selected_topic) self._exported_publisher_info.append(publisher_info)
def addRobots(robots, regex, entries, pubsub): ''' This adds the Entry in the complex robots dictionary We iterate over each entry and initialize the robots-dict appropiately. Then It is simply added. robots: The dictionary robots[ROBOT_ID]["topics"][TOPIC_NAME] = {msg, type} regex: The Regex we try to match in each entry entries:The String Entries. Each element is in the following structure "/ROBOT_ID/TOPIC_NAME" pubsub: A String. Either "publisher" or "subscriber" ''' for entry in entries: matches = re.search(regex, entry) if matches is not None: # We found a Match. Now add it to robots robotID = matches.group(1) topic = matches.group(2) if robotID not in robots: # Init robot Dict robots[robotID] = {} robots[robotID]["topics"] = {} if topic not in robots[robotID]["topics"]: # Init topic Dict robots[robotID]["topics"][topic] = {} # Get Message Type and convert it to Python-Specific Import topic_type, _, _ = rostopic.get_topic_type(entry) msg_type = topic_type.replace("/", ".msg.") # Set it in robots robots[robotID]["topics"][topic]["type"] = pubsub robots[robotID]["topics"][topic]["msg"] = msg_type
def __init__(self, topic): self.bridge = CvBridge() self.last_frame = None # Creating a window for later use self.w_name = 'HSV color range chooser' cv2.namedWindow('result') # Creating track bar cv2.createTrackbar('lh', 'result', 0, 179, nothing) cv2.createTrackbar('ls', 'result', 0, 255, nothing) cv2.createTrackbar('lv', 'result', 0, 255, nothing) cv2.createTrackbar('hh', 'result', 0, 179, nothing) cv2.createTrackbar('hs', 'result', 0, 255, nothing) cv2.createTrackbar('hv', 'result', 0, 255, nothing) cv2.setTrackbarPos('hh', 'result', 179) cv2.setTrackbarPos('hs', 'result', 255) cv2.setTrackbarPos('hv', 'result', 255) type_name, topic_name, _ = get_topic_type(topic) if type_name == 'sensor_msgs/Image': self.subscriber = rospy.Subscriber(topic, Image, self.img_cb, queue_size=1) elif type_name == 'sensor_msgs/CompressedImage': self.subscriber = rospy.Subscriber(topic, CompressedImage, self.img_cb, queue_size=1) rospy.loginfo("Subscribing to: " + self.subscriber.resolved_name)
def __init__(self, battery_status_topic, num_battery_cells): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(IsBatteryBellowValueState, self).__init__(input_keys=['cell_voltage_target'], output_keys=['current_cell_voltage'], outcomes=['is_bellow', 'is_ok'] ) self._topic = rospy.resolve_name(battery_status_topic) self._num_battery_cells = num_battery_cells # Store state parameter for later use. (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic) #Logger.loginfo('[IsBatteryBellowValueState]: topic %s msg_topic %s'%(self._topic,msg_topic)) if msg_topic == self._topic: msg_type = self._get_msg_from_path(msg_path) self._sub_battery = ProxySubscriberCached({self._topic: msg_type}) self._connected = True Logger.loginfo('[IsBatteryBellowValueState]: Successfully subscribed to topic %s' % self._topic) #brick_subs_ = rospy.Subscriber(self._topic, sensor_msgs.msg.BatteryState, self.battery_status_callback) self.moving_average_hist_v_param = 0.1 self.volage_per_cell = 4.2 self.voltage = num_battery_cells * self.volage_per_cell # The constructor is called when building the state machine, not when actually starting the behavior. # Thus, we cannot save the starting time now and will do so later. self._start_time = None
def __check_topic(self, topic_name): topic_type, real_topic, msg_eval = rostopic.get_topic_type(topic_name) # (None, None, None) if real_topic: self._print_info("Checking topic '%s': OK" % topic_name) else: self.ALL_OK = False self._print_fatal("Checking topic '%s': FAILED" % topic_name)
def action_check(self, params, userdata): rospy.loginfo("Checking <<actions>>") result_summary = {} self.result = "success" for item in params.values()[0]: action_type = item['action_type'] action_name = item['action_name'] rospy.loginfo("Now checking <<%s>>", action_name) rospy.loginfo("Of type <<%s>>", action_type) #************ # Description: Get topic type and uses that for dynamically importing the required modules imp_action = rostopic.get_topic_type("/"+action_name+"/goal", blocking=True) imp_action = imp_action[0].split("/")[0] imp_action += ".msg" rospy.loginfo("Trying to import " + action_type + " from" +imp_action) mod = __import__(imp_action, fromlist=[action_type]) # from move_base_msg.msg cls = getattr(mod, action_type) # import MoveBaseAction #**************************** ac_client = actionlib.SimpleActionClient(action_name, cls) result = ac_client.wait_for_server(rospy.Duration(5)) result_summary[action_name] = result # this make a summary for the results for all the actions checked if result == False: self.result = "failed" rospy.loginfo("Result of the Actions Check") rospy.loginfo(result_summary) rospy.loginfo("Finished Checking <<actions>>")
def goal_cb(self, msg): goal=msg print "Got Message" if goal.marker_topic[0]=="/": #check if there is a '/' in front of the topic name so rostopic doesn't error marker_type, real_name, fn=rostopic.get_topic_type(goal.marker_topic) else: marker_type, real_name, fn=rostopic.get_topic_type("/"+goal.marker_topic) print (marker_type) self.marker_repub=rospy.Publisher('ar_tag_pose', AlvarMarkers) if marker_type == "ar_pose/ARMarker" and not self.defined: print"Republisher started for AR Pose markers" #Publish new goal message self.send_new_goal(goal) rospy.Subscriber(real_name, ARMarker, self.ar_pose_switch) self.defined=True
def _get_topic_data_map(self, topic_data_map, topic_nm): result = False if (topic_nm in topic_data_map): result = True self.topic_items = topic_data_map[topic_nm] else: try: topic_type_info = rostopic.get_topic_type(topic_nm) if (topic_type_info[0] is not None): topic_msg = rosmsg.get_msg_text(topic_type_info[0]) self.topic_items = self._parse_topic_items( self.topic, topic_msg) topic_data_map[topic_nm] = { "topic_msg": topic_type_info[0], "items": self.topic_items } result = True except NameError as ne: rospy.logerr("topic info get failed. %s", self.topic) print(ne) return result
def __init__(self, plan_keeper_mapped_arena_topic): ''' Constructor ''' super(WaitForArenaMappedObjectsState, self).__init__(outcomes=['successed', 'failed'], output_keys=['mapped_objects']) self._topic = rospy.resolve_name(plan_keeper_mapped_arena_topic) #self._sub = rospy.Subscriber(self._topic, brick_mapping.msg.MappedArenaObjectsStamped, self.mapped_area_callback) #self._connected = True # (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic) #Logger.loginfo('[IsBatteryBellowValueState]: topic %s msg_topic %s'%(self._topic,msg_topic)) if msg_topic == self._topic: msg_type = self._get_msg_from_path(msg_path) self._sub = ProxySubscriberCached({self._topic: msg_type}) self._connected = True Logger.loginfo( '[WaitForArenaMappedObjectsState]: subscribing topic %s ' % (self._topic)) self.received = False self.mapped_objects = None
def topic_scan(config): g = rosgraph.impl.graph.Graph() g.update() topics = map(str.strip, g.nt_nodes) subs = collections.defaultdict(list) pubs = collections.defaultdict(list) for edge in g.nt_all_edges: src = edge.start.strip() dest = edge.end.strip() if src in topics: subs[src] = dest if dest in topics: pubs[dest] = src for topic in topics: type_name = rostopic.get_topic_type(topic)[0] if type_name == 'sensor_msgs/PointCloud2' or type_name == 'sensor_msgs/LaserScan': config.observation_sources[topic] = {'type': type_name} elif type_name == 'nav_msgs/OccupancyGrid': base = topic.split('/')[-1] if base == 'costmap': if 'local' in topic: config.maps[topic] = 'Local Costmap' else: config.maps[topic] = 'Global Costmap' else: config.maps[topic] = 'Map' elif type_name == 'nav_msgs/Path': config.paths[topic] = 'Path' elif type_name == 'geometry_msgs/PoseStamped': if topic in subs: config.goal = topic
def __init__(self, ref_frame, camera_topic, camera_frame): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(DetectPartCameraState, self).__init__(outcomes=['continue', 'failed'], output_keys=['pose']) self.ref_frame = ref_frame self._topic = camera_topic self._camera_frame = camera_frame self._connected = False self._failed = False # tf to transfor the object pose self._tf_buffer = tf2_ros.Buffer( rospy.Duration(10.0)) #tf buffer length self._tf_listener = tf2_ros.TransformListener(self._tf_buffer) # Subscribe to the topic for the logical camera (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic) if msg_topic == self._topic: msg_type = self._get_msg_from_path(msg_path) self._sub = ProxySubscriberCached({self._topic: msg_type}) self._connected = True else: Logger.logwarn( 'Topic %s for state %s not yet available.\nFound: %s\nWill try again when entering the state...' % (self._topic, self.name, str(msg_topic)))
def topic_scan(config): g = rosgraph.impl.graph.Graph() g.update() topics = map(str.strip, g.nt_nodes) subs = collections.defaultdict(list) pubs = collections.defaultdict(list) for edge in g.nt_all_edges: src = edge.start.strip() dest = edge.end.strip() if src in topics: subs[src] = dest if dest in topics: pubs[dest] = src for topic in topics: type_name = rostopic.get_topic_type(topic)[0] if type_name=='sensor_msgs/PointCloud2' or type_name=='sensor_msgs/LaserScan': config.observation_sources[topic] = {'type': type_name} elif type_name=='nav_msgs/OccupancyGrid': base = topic.split('/')[-1] if base == 'costmap': if 'local' in topic: config.maps[topic] = 'Local Costmap' else: config.maps[topic] = 'Global Costmap' else: config.maps[topic] = 'Map' elif type_name=='nav_msgs/Path': config.paths[topic] = 'Path' elif type_name=='geometry_msgs/PoseStamped': if topic in subs: config.goal = topic
def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. #Logger.logerr('[ServiceFollowTrajectory]: enter') self._start_time = rospy.get_time() if not self._connected: (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._control_manager_diagnostics_topic) if msg_topic == self._control_manager_diagnostics_topic: msg_type = self._get_msg_from_path(msg_path) self._sub_cont_diag = ProxySubscriberCached({self._control_manager_diagnostics_topic: msg_type}) self._connected = True Logger.loginfo('[ServiceFollowTrajectory]: Successfully subscribed to previously unavailable topic %s' % self._control_manager_diagnostics_topic) else: Logger.logwarn('[ServiceFollowTrajectory]: Topic %s still not available, giving up.' % self._control_manager_diagnostics_topic) if self._connected and self._sub_cont_diag.has_msg(self._control_manager_diagnostics_topic): Logger.loginfo('[ServiceFollowTrajectory]: Waiting for msg from topic %s' % self._control_manager_diagnostics_topic) self._sub_cont_diag.remove_last_msg(self._control_manager_diagnostics_topic) self._failed = False # Check if the ProxyServiceCaller has been registered if not self._srv_follow.is_available(self._service_topic_follow): Logger.logerr('[ServiceFollowTrajectory]: Topic \'{}\' not yet registered!'.format(self._service_topic_follow)) self._failed = True return if not self._srv_set_trajectory.is_available(self._service_topic_set_trajectory): Logger.logerr('[ServiceFollowTrajectory]: Topic \'{}\' not yet registered!'.format(self._service_topic_set_trajectory)) self._failed = True return try: service_request_set_trajectory = mrs_msgs.srv.TrajectoryReferenceSrvRequest()#TrackerTrajectorySrvRequest() trajectory_msg = mrs_msgs.msg.TrajectoryReference() trajectory_msg.header = std_msgs.msg.Header() trajectory_msg.header.stamp = rospy.Time.now() trajectory_msg.header.seq = 0 trajectory_msg.header.frame_id = userdata.frame_id trajectory_msg.use_heading = True trajectory_msg.fly_now = True trajectory_msg.loop = False trajectory_msg.points = userdata.scanning_trajectory service_request_set_trajectory.trajectory = trajectory_msg service_result_set_trajectory = self._srv_set_trajectory.call(self._service_topic_set_trajectory,service_request_set_trajectory) Logger.logwarn('[ServiceFollowTrajectory]: Called \'{}\' in ServiceFollowTrajectory set trejectory'.format(self._service_topic_set_trajectory)) if not service_result_set_trajectory.success: self._failed = True Logger.logwarn('[ServiceFollowTrajectory]: Calling \'{}\' was not successful'.format(self._service_topic_set_trajectory)) else: Logger.loginfo('[ServiceFollowTrajectory]: Calling \'{}\' was successful'.format(self._service_topic_set_trajectory)) except Exception as e: Logger.logerr('Failed to call \'{}\' service request: {}'.format(self._service_topic_follow, str(e))) self._failed = True
def update_topicinfo(self,topics): new_topic_type = [] for topic in topics: topic_type, _, _ = rostopic.get_topic_type(topic) if topic_type: new_topic_type += [topic_type] self.topicinfo[topic] = topic_type self.update_idl(new_topic_type)
def __init__(self, set_yaw_service_topic, control_manager_diagnostics_topic, odometry_topic): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(ServiceRotateRelativeToGoToState, self).__init__(input_keys=[ 'goal_tracker_point', 'frame_id', 'yaw_relative_to_goal' ], output_keys=['desired_yaw'], outcomes=['successed', 'failed']) # Store state parameter for later use. self._set_yaw_service_topic = rospy.resolve_name(set_yaw_service_topic) self._control_manager_diagnostics_topic = rospy.resolve_name( control_manager_diagnostics_topic) self._odometry_topic = rospy.resolve_name(odometry_topic) # Create proxy service client self._srv = ProxyServiceCaller( {self._set_yaw_service_topic: mrs_msgs.srv.Vec1}) (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._control_manager_diagnostics_topic) if msg_topic == self._control_manager_diagnostics_topic: msg_type = self._get_msg_from_path(msg_path) self._sub_cont_diag = ProxySubscriberCached( {self._control_manager_diagnostics_topic: msg_type}) self._connected = True Logger.loginfo( '[ServiceRotateRelativeToGoToState]: Successfully subscribed to topic %s' % self._control_manager_diagnostics_topic) (msg_path_odom, msg_topic_odom, fn_odom) = rostopic.get_topic_type(self._odometry_topic) if msg_topic_odom == self._odometry_topic: msg_type_odom = self._get_msg_from_path(msg_path_odom) self._sub_odom = ProxySubscriberCached( {self._odometry_topic: msg_type_odom}) self._connected_odom = True Logger.loginfo( '[ServiceRotateRelativeToGoToState]: Successfully subscribed to topic %s' % self._odometry_topic) self._desired_yaw = 0 self._sended = False self._failed = False
def check_topic(userdata, topic_name): """ Check if is some node publishing on a topic """ topic_name = str(topic_name) topic_type, real_topic, msg_eval = get_topic_type(topic_name) # (None, None, None) if real_topic: rospy.loginfo("Checking topic '%s': OK" % topic_name) return succeeded rospy.loginfo("Checking topic '%s': FAILED" % topic_name) return aborted
def get_topic_type(self, topic_name, clb=None): """Returns a list of currently alive ROS Topics. Similar `rostopic type <topic_type>`. Args: topic_name (str): The Topic name. clb (function, optional): Callback function that is called upon response is received. """ topic_type = rostopic.get_topic_type(topic_name) return topic_type
def generate_type_info(self): ''' Basic connection details are provided by get system state from the master, which is a one shot call to give you information about every connection possible. it does not however provide type info information and the only way of retrieving that from the master is making one xmlrpc call to the master for every single connection. This gets expensive, so generating this information is usually delayed until we need it and done via this method. ''' if self.type_info is None: if self.type == PUBLISHER or self.type == SUBSCRIBER: self.type_info = rostopic.get_topic_type(self.name)[0] # message type elif self.type == SERVICE: self.type_info = rosservice.get_service_uri(self.name) elif self.type == ACTION_SERVER or self.type == ACTION_CLIENT: goal_topic = self.name + '/goal' goal_topic_type = rostopic.get_topic_type(goal_topic) self.type_info = re.sub('ActionGoal$', '', goal_topic_type[0]) # Base type for action
def _type(topic): topic = '/' + topic try: _topic_type, _, _ = rt.get_topic_type(topic) if _topic_type: return response_ok({'topic': topic, 'message_type': _topic_type}) else: return response_error("Topic '{:s}' not found".format(topic)) except Exception as e: return response_error(str(e))
def listener(self,fd,topicName): fo = os.fdopen(int(fd), "w", 0) self.data.append(fo) rosmsg = get_topic_type(topicName)[0] rosmsg = rosmsg.split("/") module = import_module(rosmsg[0]+".msg") rospy.init_node('listener', anonymous=True) rospy.Subscriber(topicName, getattr(module,rosmsg[1]), self.callback) rospy.spin()
def test_lg_media_topic_presence(self): """ Check lg_media topic is there. """ #master = rosgraph.masterapi.Master("caller") #topics = master.getTopicTypes() #assert ["/media_service/left_one", "lg_media/AdhocMedias"] in topics topic_type, real_topic, msg_eval = rostopic.get_topic_type(TOPIC_NAME, blocking=False) assert topic_type is not None, "Topic not found: {}".format(TOPIC_NAME) assert topic_type == "%s/AdhocMedias" % ROS_NODE_NAME
def main(): #rospy.init_node('goal_sender') argv = sys.argv #argv = rospy.myargv(argv) from optparse import OptionParser parser = OptionParser(usage="usage: %prog list [/namespace]", prog=NAME) parser.add_option("-f", dest="filename", default=None, help="Name of yaml file name of options") parser.add_option("-w", dest="timeout", default=3, type="float", help="time to wait for server and response") parser.add_option("-t", dest="topicname", default=None) parser.add_option("-r", dest="responsetopic", default=None) parser.add_option("--topic-type", dest="topictype", default=None) (options, args) = parser.parse_args(argv) if not options.filename or not options.topicname: rospy.logfatal("No yaml file name given") sys.exit(-1) topictype, topicname, fn = rostopic.get_topic_type(options.topicname, False) if not topictype: rospy.logerror("Topic not currently subscribed %s"%topicname) if not options.topictype and not topictype: exit(-1) if options.topictype and topictype and options.topictype is not topictype: rospy.logerror("Topic type does not agree: topic %s, given type: %s, detected type: %s" % (topicname, options.topictype, topictype)) exit(-1) if options.topictype: topictype = options.topictype pub, msg_class = rostopic.create_publisher(options.topicname, topictype, False) rostopic.stdin_publish(pub, msg_class, 1, True, options.filename, True) if options.responsetopic: subtype, responsetopic, fn = rostopic.get_topic_class(options.responsetopic, False) msg = rospy.wait_for_message(options.responsetopic, subtype, options.timeout) print(msg)
def _init_type(self): """ Lazy-initialize type based on graph state. """ if self._ns == '/': return if self._type is None: self._type_name = rostopic.get_topic_type(self._ns, blocking=False)[0] if self._type_name: self._type = roslib.message.get_message_class(self._type_name) if self._type is None: pkg, base_type = roslib.names.package_resource_name(self._type_name) print >> sys.stderr, "\ncannot retrieve type [%s].\nPlease type 'rosmake %s'"%(self._type_name, pkg)
def on_enter(self, userdata): if not self._connected: (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic) if msg_topic == self._topic: msg_type = self._get_msg_from_path(msg_path) self._sub = ProxySubscriberCached({self._topic: msg_type}) self._connected = True Logger.loginfo('Successfully subscribed to previously unavailable topic %s' % self._topic) else: Logger.logwarn('Topic %s still not available, giving up.' % self._topic) if self._connected and self._clear and self._sub.has_msg(self._topic): self._sub.remove_last_msg(self._topic)
def get_goal_type(action): """Gets the corresponding ROS action goal type. Args: action: ROS action name. Returns: Goal message type. None if not found. """ msg_type = rostopic.get_topic_type("{}/goal".format(action))[0] # Replace 'ActionGoal' with 'Goal'. return msg_type[:-10] + "Goal"
def get_result_type(action): """Gets the corresponding ROS action result type. Args: action: ROS action name. Returns: Result message type. None if not found. """ msg_type = rostopic.get_topic_type("{}/result".format(action))[0] # Replace 'ActionResult' with 'Result'. return msg_type[:-12] + "Result"
def get_feedback_type(action): """Gets the corresponding ROS action feedback type. Args: action: ROS action name. Returns: Feedback message type. None not found. """ msg_type = rostopic.get_topic_type("{}/feedback".format(action))[0] # Replace 'ActionFeedback' with 'Feedback'. return msg_type[:-14] + "Feedback"
def process_topics(self, tfs): the_type = rostopic.get_topic_type(tfs, blocking=True)[0] the_type = the_type.split("/") the_type[0] += ".msg" mod = __import__(the_type[0], fromlist=[the_type[1]]) cls = getattr( mod , the_type[1] ) msg = rospy.wait_for_message(tfs, cls) return msg
def add_topic(self, topic_name, ws_name=None, topic_type=None, allow_pub=True, allow_sub=True): resolved_topic_name = rospy.resolve_name(topic_name) if topic_type is None: topic_type, _, _ = rostopic.get_topic_type(resolved_topic_name) if not topic_type: print 'Unknown topic %s' % topic_name return False if ws_name is None: ws_name = topic_name if ws_name.startswith('/'): ws_name = ws_name[1:] self.topics[ws_name] = Topic(topic_name, topic_type, allow_pub=allow_pub, allow_sub=allow_sub) return True
def add_action(self, action_name, ws_name=None, action_type=None): if action_type is None: resolved_topic_name = rospy.resolve_name(action_name + '/result') topic_type, _, _ = rostopic.get_topic_type(resolved_topic_name) if not topic_type: print 'Unknown action %s' % action_name return False action_type = topic_type[:-len('ActionResult')] if ws_name is None: ws_name = action_name if ws_name.startswith('/'): ws_name = ws_name[1:] self.actions[ws_name] = Action(action_name, action_type) return True
def __init__(self, topic, msg_type=None, latched_client_id=None, queue_size=100): """ Register a publisher on the specified topic. Keyword arguments: topic -- the name of the topic to register the publisher to msg_type -- (optional) the type to register the publisher as. If not provided, an attempt will be made to infer the topic type latch -- (optional) if a client requested this publisher to be latched, provide the client_id of that client here Throws: TopicNotEstablishedException -- if no msg_type was specified by the caller and the topic is not yet established, so a topic type cannot be inferred TypeConflictException -- if the msg_type was specified by the caller and the topic is established, and the established type is different to the user-specified msg_type """ # First check to see if the topic is already established topic_type = get_topic_type(topic)[0] # If it's not established and no type was specified, exception if msg_type is None and topic_type is None: raise TopicNotEstablishedException(topic) # Use the established topic type if none was specified if msg_type is None: msg_type = topic_type # Load the message class, propagating any exceptions from bad msg types msg_class = ros_loader.get_message_class(msg_type) # Make sure the specified msg type and established msg type are same if topic_type is not None and topic_type != msg_class._type: raise TypeConflictException(topic, topic_type, msg_class._type) # Create the publisher and associated member variables self.clients = {} self.latched_client_id = latched_client_id self.topic = topic self.msg_class = msg_class self.publisher = Publisher(topic, msg_class, latch=(latched_client_id!=None), queue_size=queue_size) self.listener = PublisherConsistencyListener() self.listener.attach(self.publisher)
def _init_action(self): if self._ns == '/': return if self._type is None: #TODO: shift away from feedback, use status instead feedback_topic = roslib.names.ns_join(self._ns, 'feedback') feedback_type_name = rostopic.get_topic_type(feedback_topic, blocking=False)[0] if feedback_type_name and feedback_type_name.endswith('Feedback'): self._type_name = feedback_type_name[:-len('Feedback')] if not self._type_name.endswith('Action'): # abort init self._type_name = None else: # rosh's loader has extra toys self._type = rosh.impl.msg.get_message_class(self._type_name) if self._type is not None: # only load if the previous load succeeded self._goal = rosh.impl.msg.get_message_class(self._type_name[:-len('Action')]+'Goal')
def _generate_tool_tip(self, url): if url is not None and ':' in url: item_type, item_path = url.split(':', 1) if item_type == 'node': tool_tip = 'Node:\n %s' % (item_path) service_names = rosservice.get_service_list(node=item_path) if service_names: tool_tip += '\nServices:' for service_name in service_names: try: service_type = rosservice.get_service_type(service_name) tool_tip += '\n %s [%s]' % (service_name, service_type) except rosservice.ROSServiceIOException, e: tool_tip += '\n %s' % (e) return tool_tip elif item_type == 'topic': topic_type, topic_name, _ = rostopic.get_topic_type(item_path) return 'Topic:\n %s\nType:\n %s' % (topic_name, topic_type)
def get_action_class(action): """Gets the corresponding ROS action class. Args: action: ROS action name. Returns: Action message class. None if not found. """ goal_msg_type = rostopic.get_topic_type("{}/goal".format(action))[0] # Verify goal message was found. if not goal_msg_type: return None # Goal message name is the same as the action message name + 'Goal'. action_msg_type = goal_msg_type[:-4] return roslib.message.get_message_class(action_msg_type)
def add_trigger(topic, expr, select, index): topic_type, _, _ = rostopic.get_topic_type(topic) topic_class, _, _ = rostopic.get_topic_class(topic) if(topic_type == None): rospy.loginfo('%s is not published yet', topic) return None if(topic_class == None): rospy.loginfo('%s is not builded yet', topic_type) return None try: cb_obj = gen_callback(expr, select, index) rospy.loginfo("start subscribe (topic=%s type=%s)", topic, topic_type); sub = rospy.Subscriber(topic,topic_class,cb_obj) except Exception, e: rospy.loginfo(str(e)) return None
def __init__(self, topic, blocking = True, clear = False): ''' Constructor ''' super(SubscriberState, self).__init__(outcomes=['received', 'unavailable'], output_keys=['message']) self._topic = topic self._blocking = blocking self._clear = clear self._connected = False (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic) if msg_topic == self._topic: msg_type = self._get_msg_from_path(msg_path) self._sub = ProxySubscriberCached({self._topic: msg_type}) self._connected = True else: Logger.logwarn('Topic %s for state %s not yet available.\nFound: %s\nWill try again when entering the state...' % (self._topic, self.name, str(msg_topic)))
def __init__(self, topic_name, bagfile, continuous=False): # this gets the topics type using the roslib the_type = rostopic.get_topic_type(topic_name, blocking=True)[0] the_type = the_type.split("/") the_type[0] += ".msg" # this imports the necessary modules and instantiates the necessary object mod = __import__(the_type[0], fromlist=[the_type[1]]) cls = getattr( mod , the_type[1] ) self.msg = None self.topic_name = topic_name self.topic_type = cls self.continuous = continuous self.bagfile = bagfile # this creates the subscriber for the specific topic rospy.Subscriber(self.topic_name, self.topic_type, self.callback) global_lock.locked = False
def __init__(self, topic, msg_type=None): """ Register a subscriber on the specified topic. Keyword arguments: topic -- the name of the topic to register the subscriber on msg_type -- (optional) the type to register the subscriber as. If not provided, an attempt will be made to infer the topic type Throws: TopicNotEstablishedException -- if no msg_type was specified by the caller and the topic is not yet established, so a topic type cannot be inferred TypeConflictException -- if the msg_type was specified by the caller and the topic is established, and the established type is different to the user-specified msg_type """ # First check to see if the topic is already established topic_type = get_topic_type(topic)[0] # If it's not established and no type was specified, exception if msg_type is None and topic_type is None: raise TopicNotEstablishedException(topic) # Use the established topic type if none was specified if msg_type is None: msg_type = topic_type # Load the message class, propagating any exceptions from bad msg types msg_class = ros_loader.get_message_class(msg_type) # Make sure the specified msg type and established msg type are same if topic_type is not None and topic_type != msg_class._type: raise TypeConflictException(topic, topic_type, msg_class._type) # Create the subscriber and associated member variables self.subscriptions = {} self.lock = Lock() self.topic = topic self.msg_class = msg_class self.subscriber = Subscriber(topic, msg_class, self.callback)
def get_field_type(topic_name): """ Get the Python type of a specific field in the given registered topic. If the field is an array, the type of the array's values are returned and the is_array flag is set to True. This is a static type check, so it works for unpublished topics and with empty arrays. :param topic_name: name of field of a registered topic, ``str``, i.e. '/rosout/file' :returns: field_type, is_array """ # get topic_type and message_evaluator topic_type, real_topic_name, _ = get_topic_type(topic_name) if topic_type is None: #qDebug('topic_helpers.get_field_type(%s): get_topic_type failed' % (topic_name)) return None, False message_class = roslib.message.get_message_class(topic_type) if message_class is None: qDebug('topic_helpers.get_field_type(%s): get_message_class(%s) failed' % (topic_name, topic_type)) return None, False slot_path = topic_name[len(real_topic_name):] return get_slot_type(message_class, slot_path)
def process_topics(self, tfs): # process the topics # this gets the topics type using the roslib the_type = rostopic.get_topic_type(tfs, blocking=True)[0] the_type = the_type.split("/") the_type[0] += ".msg" # this imports the necessary modules and instantiates the necessary object mod = __import__(the_type[0], fromlist=[the_type[1]]) cls = getattr(mod, the_type[1]) timeout = 3.0 try: if tfs == "/joint_states": msg = self.ja.jointsMsg # rospy.loginfo("joint Message") # rospy.loginfo(msg) elif tfs == "/tf": # self.tfa.process_tfs() transf = self.tfa.transforms self.tf_write(transf) msg = Empty() else: msg = rospy.wait_for_message(tfs, cls, timeout) except rospy.ROSException: # an empty message is created due to an exception for preventing breaks # on the recording process rospy.logwarn("skipping topic: " + str(tfs)) msg = Empty() return msg