def __init__(self): message_class_str = rospy.get_param("~message", "jsk_network_tools/FC2OCSLargeData") try: self.message_class = get_message_class(message_class_str) except: raise Exception("invalid topic type: %s"%message_class_str) self.lock = Lock() self.diagnostic_updater = diagnostic_updater.Updater() self.diagnostic_updater.setHardwareID("none") self.diagnostic_updater.add("HighspeedReceiver", self.diagnosticCallback) self.latch = rospy.get_param("~latch", True) self.pesimistic = rospy.get_param("~pesimistic", False) self.receive_port = rospy.get_param("~receive_port", 16484) self.receive_ip = rospy.get_param("~receive_ip", "localhost") self.topic_prefix = rospy.get_param("~topic_prefix", "/from_fc") if not self.topic_prefix.startswith("/"): self.topic_prefix = "/" + self.topic_prefix if self.topic_prefix == "/": self.topic_prefix = "" self.publishers = publishersFromMessage(self.message_class, self.topic_prefix, latch=self.latch) self.socket_server = socket(AF_INET, SOCK_DGRAM) self.socket_server.bind((self.receive_ip, self.receive_port)) self.packet_size = rospy.get_param("~packet_size", 1000) #2Hz self.packets = [] self.launched_time = rospy.Time.now() self.last_received_time = rospy.Time(0) self.last_received_time_pub = rospy.Publisher("~last_received_time", Time) self.diagnostic_timer = rospy.Timer(rospy.Duration(1.0 / 10), self.diagnosticTimerCallback)
def get_msg_info(yaml_info, topics, parse_header=True): ''' Get info from all of the messages about what they contain and will be added to the dataframe ''' topic_info = yaml_info['topics'] msgs = {} classes = {} for topic in topics: base_key = get_key_name(topic) msg_paths = [] msg_types = {} for info in topic_info: if info['topic'] == topic: msg_class = get_message_class(info['type']) if msg_class is None: warnings.warn( 'Could not find types for ' + topic + ' skpping ') else: (msg_paths, msg_types) = get_base_fields(msg_class(), "", parse_header) msgs[topic] = msg_paths classes[topic] = msg_types return (msgs, classes)
def test_valid_variables(self): ENVIRONMENTAL_VARIABLES = frozenset( VariableInfo.from_dict(d) for d in rospy.get_param("/var_types/environment_variables").itervalues()) RECIPE_VARIABLES = frozenset( VariableInfo.from_dict(d) for d in rospy.get_param("/var_types/recipe_variables").itervalues()) VALID_VARIABLES = ENVIRONMENTAL_VARIABLES.union(RECIPE_VARIABLES) print(VALID_VARIABLES) assert len(VALID_VARIABLES) == 18 # This builds a dictionary of publisher instances using a # "dictionary comprehension" (syntactic sugar for building dictionaries). # The constant has to be declared here because get_message_class # needs to be called after the node is initialized. PUBLISHERS = { variable.name: rospy.Publisher( "{}/desired".format(variable.name), get_message_class(variable.type), queue_size=10) for variable in VALID_VARIABLES }
def __init__(self): message_class_str = rospy.get_param("~message", "jsk_network_tools/FC2OCSLargeData") try: self.message_class = get_message_class(message_class_str) except: raise Exception("invalid topic type: %s"%message_class_str) self.lock = Lock() self.diagnostic_updater = diagnostic_updater.Updater() self.diagnostic_updater.setHardwareID("none") self.diagnostic_updater.add("HighspeedReceiver", self.diagnosticCallback) self.pesimistic = rospy.get_param("~pesimistic", False) self.receive_port = rospy.get_param("~receive_port", 16484) self.receive_ip = rospy.get_param("~receive_ip", "localhost") self.topic_prefix = rospy.get_param("~topic_prefix", "/from_fc") if not self.topic_prefix.startswith("/"): self.topic_prefix = "/" + self.topic_prefix if self.topic_prefix == "/": self.topic_prefix = "" self.publishers = publishersFromMessage(self.message_class, self.topic_prefix) self.socket_server = socket(AF_INET, SOCK_DGRAM) self.socket_server.bind((self.receive_ip, self.receive_port)) self.packet_size = rospy.get_param("~packet_size", 1000) #2Hz self.packets = [] self.launched_time = rospy.Time.now() self.last_received_time = rospy.Time(0) self.last_received_time_pub = rospy.Publisher("~last_received_time", Time) self.diagnostic_timer = rospy.Timer(rospy.Duration(1.0 / 10), self.diagnosticTimerCallback)
def get_msg_info(yaml_info, topics, parse_header=True): ''' Get info from all of the messages about what they contain and will be added to the dataframe ''' topic_info = yaml_info['topics'] msgs = {} classes = {} for topic in topics: base_key = get_key_name(topic) msg_paths = [] msg_types = {} for info in topic_info: if info['topic'] == topic: msg_class = get_message_class(info['type']) if msg_class is None: warnings.warn('Could not find types for ' + topic + ' skpping ') else: (msg_paths, msg_types) = get_base_fields(msg_class(), "", parse_header) msgs[topic] = msg_paths classes[topic] = msg_types return (msgs, classes)
def post_topic_message(topic_name): """ POST /api/<version>/topic/<topic_name> POST a message to a ROS topic by name. Requires a JSON payload of shape: ``[x, y, z]``. The values of the JSON array must match the argument types of the topic's type constructor. """ topic_name = "/" + topic_name # Get the list of ROS topics in the system. # Returns a list of [topic, type_string] pairs. topic_list = rostopic_master.getTopicTypes() # Find topic in list (this is Python's approach to find). try: topic_match = next(x for x in topic_list if x[0] == topic_name) except StopIteration: # If we can't find the topic, return early (400 bad request). return error("Topic does not exist") json = request.get_json(silent=True) args = json if json else [] try: # Get the message type constructor for the topic's type string. MsgType = get_message_class(topic_match[1]) pub = rospy.Publisher(topic_name, MsgType, queue_size=10) # Unpack JSON list and pass to publish. # pub.publish() will pass the list of arguments to the message # type constructor. pub.publish(*args) except Exception, e: return error("Wrong arguments for topic type constructor")
def find_fields(msg_srv_class, field_name_pattern=None, field_type_pattern=None): fields = [] if field_name_pattern is None: field_name_pattern = ".*" if field_type_pattern is None: field_type_pattern = ".*" name_regex = re.compile(field_name_pattern) type_regex = re.compile(field_type_pattern) # ROS msg/srv creates python definitions via __slots__ if hasattr(msg_srv_class, '__slots__'): for (field_name, field_type) in zip(msg_srv_class.__slots__, msg_srv_class._slot_types): # If this field is a frame ID, add it to the output list if name_regex.match(field_name) is not None and type_regex.match(field_type) is not None: fields.append(field_name) elif not is_primitive_msg(field_type): # If this field is another ROS message type, look inside it for more frame IDs if is_msg_array(field_type): # If this field is a message array, field_name += "[]" field_type = field_type.rstrip("[]") child_fields = find_fields(get_message_class(field_type), field_name_pattern, field_type_pattern) for child_field in child_fields: fields.append("{0}.{1}".format(field_name, child_field)) return fields
def get_slot_type_field_names(msg, slot_type, field_name=None, found=None): if field_name is None: field_name = '' if found is None: found = [] if msg is None: return [] for slot, slot_t in zip(msg.__slots__, msg._slot_types): deeper_field_name = field_name + '/' + slot if slot_t == slot_type: found.append(deeper_field_name) elif slot_t == slot_type + '[]': # supports array of type field like string[] deeper_field_name += '[]' found.append(deeper_field_name) try: if slot_t.endswith('[]'): # supports array of ros message like std_msgs/Header[] deeper_field_name += '[]' slot_t = slot_t.rstrip('[]') msg_impl = get_message_class(slot_t) except ValueError: continue found = get_slot_type_field_names(msg_impl, slot_type, deeper_field_name, found) return found
def __init__(self): message_class_str = rospy.get_param("~message", "jsk_network_tools/FC2OCS") try: self.receive_message = get_message_class(message_class_str) except: raise Exception("invalid topic type: %s" % message_class_str) self.lock = Lock() self.launched_time = rospy.Time.now() self.diagnostic_updater = diagnostic_updater.Updater() self.diagnostic_updater.setHardwareID("none") self.diagnostic_updater.add("LowspeedReceiver", self.diagnosticCallback) self.received_num = 0 self.receive_port = rospy.get_param("~receive_port", 1024) self.receive_ip = rospy.get_param("~receive_ip", "127.0.0.1") self.receive_buffer = rospy.get_param("~receive_buffer_size", 250) self.socket_server = socket(AF_INET, SOCK_DGRAM) self.socket_server.bind((self.receive_ip, self.receive_port)) self.receive_format = msgToStructFormat(self.receive_message()) self.pub = rospy.Publisher("~output", self.receive_message) self.last_received_time = rospy.Time(0) self.last_received_time_pub = rospy.Publisher("~last_received_time", Time) self.last_publish_output_time = rospy.Time(0) self.last_publish_output_time_pub = rospy.Publisher( "~last_publish_output_time", Time) self.diagnostic_timer = rospy.Timer(rospy.Duration(1.0 / 10), self.diagnosticTimerCallback)
def create_persistence_objects(server, max_update_interval, min_update_interval): env_var_db = server[ENVIRONMENTAL_DATA_POINT] module_db = server[FIRMWARE_MODULE] module_type_db = server[FIRMWARE_MODULE_TYPE] modules = { module_id: FirmwareModule(module_db[module_id]) for module_id in module_db if not module_id.startswith('_') } module_types = { type_id: FirmwareModuleType(module_type_db[type_id]) for type_id in module_type_db if not type_id.startswith("_") } modules = synthesize_firmware_module_info(modules, module_types) valid_vars = list(EnvVar.items.keys()) for module_id, module_info in modules.items(): for output_name, output_info in module_info["outputs"].items(): if not output_info["variable"] in valid_vars: rospy.logwarn( "Encountered a module output that references a " 'non-existant variable: Output "%s" of module "%s"', output_name, module_id) continue topic = "/sensors/{}/{}/filtered".format(module_id, output_name) topic_type = get_message_class(output_info["type"]) TopicPersistence(topic=topic, topic_type=topic_type, environment=module_info["environment"], variable=output_info["variable"], is_desired=False, db=env_var_db, max_update_interval=max_update_interval, min_update_interval=min_update_interval)
def __init__(self): message_class_str = rospy.get_param("~message", "jsk_network_tools/FC2OCSLargeData") try: self.message_class = get_message_class(message_class_str) except: raise Exception("invalid topic type: %s"%message_class_str) self.lock = Lock() self.diagnostic_updater = diagnostic_updater.Updater() self.diagnostic_updater.setHardwareID("none") self.diagnostic_updater.add("HighspeedReceiver", self.diagnosticCallback) self.latch = rospy.get_param("~latch", True) self.pesimistic = rospy.get_param("~pesimistic", False) self.fragment_packets_torelance = rospy.get_param("~fragment_packets_torelance", 20) self.timestamp_overwrite_topics = rospy.get_param("~timestamp_overwrite_topics", []) self.publish_only_if_updated_topics = rospy.get_param("~publish_only_if_updated_topics", []) self.prev_seq_ids = {} self.receive_port = rospy.get_param("~receive_port", 16484) self.receive_ip = rospy.get_param("~receive_ip", "localhost") self.topic_prefix = rospy.get_param("~topic_prefix", "/from_fc") if not self.topic_prefix.startswith("/"): self.topic_prefix = "/" + self.topic_prefix if self.topic_prefix == "/": self.topic_prefix = "" self.publishers = publishersFromMessage(self.message_class, self.topic_prefix, latch=self.latch) self.packet_size = rospy.get_param("~packet_size", 1400) #2Hz self.launched_time = rospy.Time.now() self.last_received_time = rospy.Time(0) self.last_received_time_pub = rospy.Publisher("~last_received_time", Time) self.last_published_seq_id = -1 self.diagnostic_timer = rospy.Timer(rospy.Duration(1.0 / 10), self.diagnosticTimerCallback) self.packets_queue = Queue()
def __init__(self): message_class_str = rospy.get_param("~message", "jsk_network_tools/FC2OCS") try: self.send_message = get_message_class(message_class_str) except: raise Exception("invalid topic type: %s" % message_class_str) self.lock = Lock() self.launched_time = rospy.Time.now() self.diagnostic_updater = diagnostic_updater.Updater() self.diagnostic_updater.setHardwareID("none") self.diagnostic_updater.add("LowspeedStreamer", self.diagnosticCallback) self.send_num = 0 self.last_send_time = rospy.Time(0) self.last_input_received_time = rospy.Time(0) self.last_send_time_pub = rospy.Publisher("~last_send_time", Time) self.last_input_received_time_pub = rospy.Publisher( "~last_input_received_time", Time) self.to_port = rospy.get_param("~to_port", 1024) self.to_ip = rospy.get_param("~to_ip", "127.0.0.1") self.send_rate = rospy.get_param("~send_rate", 1) self.event_driven = rospy.get_param("~event_driven", False) self.latest_message = None self.socket_client = socket(AF_INET, SOCK_DGRAM) self.send_format = msgToStructFormat(self.send_message()) self.sub = rospy.Subscriber("~input", self.send_message, self.messageCallback) if not self.event_driven: self.send_timer = rospy.Timer(rospy.Duration(1 / self.send_rate), self.sendTimerCallback) self.diagnostic_timer = rospy.Timer(rospy.Duration(1.0 / 10), self.diagnosticTimerCallback)
def __init__(self): message_class_str = rospy.get_param("~message", "jsk_network_tools/FC2OCS") try: self.send_message = get_message_class(message_class_str) except: raise Exception("invalid topic type: %s"%message_class_str) self.lock = Lock() self.launched_time = rospy.Time.now() self.diagnostic_updater = diagnostic_updater.Updater() self.diagnostic_updater.setHardwareID("none") self.diagnostic_updater.add("LowspeedStreamer", self.diagnosticCallback) self.send_num = 0 self.last_send_time = rospy.Time(0) self.last_input_received_time = rospy.Time(0) self.last_send_time_pub = rospy.Publisher("~last_send_time", Time) self.last_input_received_time_pub = rospy.Publisher( "~last_input_received_time", Time) self.to_port = rospy.get_param("~to_port", 1024) self.to_ip = rospy.get_param("~to_ip", "127.0.0.1") self.send_rate = rospy.get_param("~send_rate", 1) self.event_driven = rospy.get_param("~event_driven", False) self.latest_message = None self.socket_client = socket(AF_INET, SOCK_DGRAM) self.send_format = msgToStructFormat(self.send_message()) self.sub = rospy.Subscriber("~input", self.send_message, self.messageCallback) if not self.event_driven: self.send_timer = rospy.Timer(rospy.Duration(1 / self.send_rate), self.sendTimerCallback) self.diagnostic_timer = rospy.Timer(rospy.Duration(1.0 / 10), self.diagnosticTimerCallback)
def __init__(self): rospy.init_node('image_denoising', anonymous=True) prefix = rospy.get_param('~prefix', "/denoised") self.noise = rospy.get_param('~noise') rate = 30 r = rospy.Rate(rate) self.topics_selected = rospy.get_param('/topics', '') self.topics_ = dict({}) self._master = rosgraph.Master(rospy.get_name()) self.all_topics_info = self._master.getTopicTypes() if len(self.topics_selected) == 0: rospy.loginfo("No topic selected, please add camera rgb or depth topics") if VERBOSE : print "subscribed to /camera/rgb/image_raw" self.bridge = CvBridge() for topic in self.topics_selected: msg_name = [ty for tp, ty in self.all_topics_info if tp == topic][0] self.pub_ = rospy.Publisher(prefix+topic,Image,queue_size=1) sub_ = rospy.Subscriber(topic, get_message_class(msg_name), callback = self.callback, callback_args = topic) #msg_ = Image #self.topics_[topic] = [sub_, pub_, msg_] rospy.loginfo("Topics with noise: std = " + str(self.noise))
def __init__(self): message_class_str = rospy.get_param("~message", "jsk_network_tools/FC2OCSLargeData") try: self.message_class = get_message_class(message_class_str) except: raise Exception("invalid topic type: %s"%message_class_str) self.lock = Lock() self.launched_time = rospy.Time.now() self.packet_interval = None self.diagnostic_updater = diagnostic_updater.Updater() self.diagnostic_updater.setHardwareID("none") # register function to publish diagnostic self.diagnostic_updater.add("HighspeedStreamer", self.diagnosticCallback) self.send_port = rospy.get_param("~to_port", 16484) self.send_ip = rospy.get_param("~to_ip", "localhost") self.last_send_time = rospy.Time(0) self.last_input_received_time = rospy.Time(0) self.last_send_time_pub = rospy.Publisher("~last_send_time", Time) self.last_input_received_time_pub = rospy.Publisher( "~last_input_received_time", Time) self.send_num = 0 #self.packet_interval = rospy.get_param("~packet_interval", 0.001) self.bandwidth = rospy.get_param("~bandwidth", 280 * 1000 * 1000) self.rate = rospy.get_param("~send_rate", 2) #2Hz self.socket_client = socket(AF_INET, SOCK_DGRAM) self.packet_size = rospy.get_param("~packet_size", 1000) #2Hz subscriber_info = subscribersFromMessage(self.message_class()) self.messages = {} self.subscribe(subscriber_info) self.counter = 0 self.send_timer = rospy.Timer(rospy.Duration(1.0 / self.rate), self.sendTimerCallback) self.diagnostic_timer = rospy.Timer(rospy.Duration(1.0 / 10), self.diagnosticTimerCallback)
def __init__(self): message_class_str = rospy.get_param("~message", "jsk_network_tools/FC2OCSLargeData") try: self.message_class = get_message_class(message_class_str) except: raise Exception("invalid topic type: %s"%message_class_str) self.lock = Lock() self.dynamic_reconfigure = Server(SilverhammerHighspeedStreamerConfig, self.dynamicReconfigureCallback) self.launched_time = rospy.Time.now() self.packet_interval = None self.diagnostic_updater = diagnostic_updater.Updater() self.diagnostic_updater.setHardwareID("none") # register function to publish diagnostic self.diagnostic_updater.add("HighspeedStreamer", self.diagnosticCallback) self.send_port = rospy.get_param("~to_port", 16484) self.send_ip = rospy.get_param("~to_ip", "localhost") self.last_send_time = rospy.Time(0) self.last_input_received_time = rospy.Time(0) self.last_send_time_pub = rospy.Publisher("~last_send_time", Time) self.last_input_received_time_pub = rospy.Publisher( "~last_input_received_time", Time) self.send_num = 0 self.rate = rospy.get_param("~send_rate", 2) #2Hz self.socket_client = socket(AF_INET, SOCK_DGRAM) self.packet_size = rospy.get_param("~packet_size", 1400) # for MTU:=1500 subscriber_info = subscribersFromMessage(self.message_class()) self.messages = {} self.subscribe(subscriber_info) self.counter = 0 self.send_timer = rospy.Timer(rospy.Duration(1.0 / self.rate), self.sendTimerCallback) self.diagnostic_timer = rospy.Timer(rospy.Duration(1.0 / 10), self.diagnosticTimerCallback)
def images(cam): """Extract images from input stream to jpg files. Args: cam: Input stream of raw rosbag messages. Returns: File instances for images of input stream. """ # Set output stream title and pull first message yield marv.set_header(title=cam.topic) # Fetch and process first 20 image messages name_template = '%s-{}.jpg' % cam.topic.replace('/', ':')[1:] while True: idx, msg = yield marv.pull(cam, enumerate=True) if msg is None or idx >= 20: break # Deserialize raw ros message pytype = get_message_class(cam.msg_type) rosmsg = pytype() rosmsg.deserialize(msg.data) # Write image to jpeg and push it to output stream img = imgmsg_to_cv2(rosmsg, "rgb8") name = name_template.format(idx) imgfile = yield marv.make_file(name) cv2.imwrite(imgfile.path, img) yield marv.push(imgfile)
def image(cam): """Extract first image of input stream to jpg file. Args: cam: Input stream of raw rosbag messages. Returns: File instance for first image of input stream. """ # Set output stream title and pull first message yield marv.set_header(title=cam.topic) msg = yield marv.pull(cam) if msg is None: return # Deserialize raw ros message pytype = get_message_class(cam.msg_type) rosmsg = pytype() rosmsg.deserialize(msg.data) # Write image to jpeg and push it to output stream name = '{}.jpg'.format(cam.topic.replace('/', ':')[1:]) imgfile = yield marv.make_file(name) img = imgmsg_to_cv2(rosmsg, "rgb8") cv2.imwrite(imgfile.path, img, (cv2.IMWRITE_JPEG_QUALITY, 60)) yield marv.push(imgfile)
def __init__(self): message_class_str = rospy.get_param("~message", "jsk_network_tools/FC2OCS") try: self.receive_message = get_message_class(message_class_str) except: raise Exception("invalid topic type: %s" % message_class_str) self.lock = Lock() self.launched_time = rospy.Time.now() self.diagnostic_updater = diagnostic_updater.Updater() self.diagnostic_updater.setHardwareID("none") self.diagnostic_updater.add("LowspeedReceiver", self.diagnosticCallback) self.received_num = 0 self.receive_port = rospy.get_param("~receive_port", 1024) self.receive_ip = rospy.get_param("~receive_ip", "127.0.0.1") self.receive_buffer = rospy.get_param("~receive_buffer_size", 250) self.socket_server = socket(AF_INET, SOCK_DGRAM) self.socket_server.settimeout(None) self.socket_server.bind((self.receive_ip, self.receive_port)) self.receive_format = msgToStructFormat(self.receive_message()) self.pub = rospy.Publisher("~output", self.receive_message) self.last_received_time = rospy.Time(0) self.last_received_time_pub = rospy.Publisher("~last_received_time", Time) self.last_publish_output_time = rospy.Time(0) self.last_publish_output_time_pub = rospy.Publisher("~last_publish_output_time", Time) self.diagnostic_timer = rospy.Timer(rospy.Duration(1.0 / 10), self.diagnosticTimerCallback)
def add_generic_parser(msg_typename, parsers): """ Adds a generic parser for msg_typename in the parsers dictionnary. @param parsers: The parsers dictionnary is expected to have entries of type Parser_desc. @attention: Any msg_typename of subfield ((recursively) which isn't in parsers will get a generic parser and it'll be added in parsers. So that custom parsers will be used for subfields only if they are defined in parsers *before* generating custom parsers. @return: the entry added in parsers for msg_typename """ # First check if the parser is already defined if msg_typename in parsers: return parsers[msg_typename] try: c = get_message_class(msg_typename) except ValueError: if msg_typename in basic_types_parsers: # Nothing is given for this basic type, we use the generic one bpd = basic_types_parsers[msg_typename] parsers[msg_typename] = bpd return bpd else: raise ValueError( "Trying to parse message type {} which is not in the environment." " You probably need to source the correct ROS setup.bash". format(msg_typename)) fdtypes = [] fparsers = [] for (field, ros_type) in zip(c.__slots__, c._slot_types): (rbtype, is_array, array_size) = parse_type(ros_type) fparser_desc = add_generic_parser(rbtype, parsers) if is_array: if array_size is None: # dynamic arrays, no support for it now raise ValueError( "Message field {}.{} is of dynamic size, please provide a custom parser." .format(msg_typename, field)) else: # Fixed size array fdtypes.append((field, fparser_desc.dtype, array_size)) fparsers.append( (field, _generic_fixed_size_array_parser(fparser_desc, array_size))) else: fparsers.append((field, fparser_desc.parser)) fdtypes.append((field, fparser_desc.dtype)) # TODO do actual raw parsing ? # TODO backport this in the genpy functions to have a fully numpy deserialization method. def parser(msg): return tuple(p(getattr(msg, f)) for (f, p) in fparsers) msg_dtype = np.dtype(fdtypes) parsers[msg_typename] = Parser_desc(parser, msg_dtype) return Parser_desc(parser, msg_dtype)
def connect_all_topics(module_db, module_type_db): modules = { module_id: FirmwareModule(module_db[module_id]) for module_id in module_db if not module_id.startswith('_') } module_types = { type_id: FirmwareModuleType(module_type_db[type_id]) for type_id in module_type_db if not type_id.startswith("_") } modules = synthesize_firmware_module_info(modules, module_types) for module_id, module_info in modules.items(): for input_name, input_info in module_info["inputs"].items(): if not "actuators" in input_info["categories"]: continue src_topic = "/environments/{}/{}/commanded".format( module_info["environment"], input_info["variable"] ) dest_topic = "/actuators/{}/{}".format(module_id, input_name) dest_topic_type = get_message_class(input_info["type"]) src_topic_type = Float64 connect_topics( src_topic, dest_topic, src_topic_type, dest_topic_type, multiplier=input_info.get("multiplier", 1), deadband=input_info.get("deadband", 0) ) for output_name, output_info in module_info["outputs"].items(): if not "sensors" in output_info["categories"]: continue src_topic = "/sensors/{}/{}/raw".format(module_id, output_name) dest_topic = "/environments/{}/{}/raw".format( module_info["environment"], output_info["variable"] ) src_topic_type = get_message_class(output_info["type"]) dest_topic_type = Float64 connect_topics( src_topic, dest_topic, src_topic_type, dest_topic_type ) src_info_topic = "/sensors/{}/{}/info".format( module_id, output_name ) dest_info_topic = "/environments/{}/{}/info".format( module_info["environment"], output_info["variable"] ) connect_sensor_info_topics(src_info_topic, dest_info_topic)
def talker(): # avoid that whole "make a ROS package" issue # make sure ROS_PACKAGE_PATH contains amrl_msgs RobofleetStatus = get_message_class("amrl_msgs/RobofleetStatus") if RobofleetStatus is None: raise Exception("Ensure that amrl_msgs is on ROS_PACKAGE_PATH") Localization2DMsg = get_message_class("amrl_msgs/Localization2DMsg") Odometry = get_message_class("nav_msgs/Odometry") status_pub = rospy.Publisher("status", RobofleetStatus, queue_size=1) odom_pub = rospy.Publisher("odometry/raw", Odometry, queue_size=1) loc_pub = rospy.Publisher("localization", Localization2DMsg, queue_size=1) rospy.init_node("test_publisher", anonymous=True) rate = rospy.Rate(15) while not rospy.is_shutdown(): t = rospy.get_time() + seed rf_status = RobofleetStatus() rf_status.battery_level = math.sin(t / 5) * 0.5 + 0.5 rf_status.is_ok = True rf_status.location = "cyberspace" rf_status.status = "Testing" odom = Odometry() odom.pose.pose.position.x = math.sin(t / 10) * 5 + 5 odom.pose.pose.position.y = math.sin(t / 10 + 0.5) * 5 + 5 odom.pose.pose.position.z = math.sin(t / 10 + 1) * 5 + 5 odom.twist.twist.linear.x = 1 odom.twist.twist.linear.y = 2 odom.twist.twist.linear.z = 3 loc = Localization2DMsg() loc.map = "UT_Campus" loc.pose.x = math.sin(t / 50) * 100 + 100 loc.pose.y = math.cos(t / 50) * -100 - 100 loc.pose.theta = t / 10 rospy.loginfo("publishing") status_pub.publish(rf_status) odom_pub.publish(odom) loc_pub.publish(loc) rate.sleep()
def test_get_slot_type_field_names(): # test for type as slot_type msg = get_message_class('jsk_rviz_plugins/OverlayText') field_names = get_slot_type_field_names(msg, slot_type='string') assert_equal(field_names, ['/font', '/text']) # test for msg as slot_type field_names = get_slot_type_field_names(msg, slot_type='std_msgs/ColorRGBA') assert_equal(field_names, ['/bg_color', '/fg_color']) # test for type array msg = get_message_class('jsk_recognition_msgs/Histogram') field_names = get_slot_type_field_names(msg, slot_type='float64[]') assert_equal(field_names, ['/histogram']) # test for msg array msg = get_message_class('diagnostic_msgs/DiagnosticArray') field_names = get_slot_type_field_names(msg, slot_type='string') assert_equal(field_names, ['/header/frame_id', '/status[]/name', '/status[]/message', '/status[]/hardware_id', '/status[]/values[]/key', '/status[]/values[]/value'])
def init_subscribers(self, recipe_phases): ALL_SUBSCRIBER_LIST = { variable.name: rospy.Subscriber( "{}/desired".format(variable.name), get_message_class(variable.type), self.callback) for variable in VALID_VARIABLES } VARIABLES = list(recipe_phases[0]['step'].keys()) # Select the variables in the 1st phase subs = [ALL_SUBSCRIBER_LIST[variable] for variable in VARIABLES]
def callback(data, args): #Assume that the first argument represents the name of the type of the message topic = args[0] msg_name = args[1] #Retrieve the class associated with that message name msg_class = get_message_class(msg_name) #Transform the message data into an instance of the message class msg = msg_class().deserialize(data._buff) value = msg.data appendDataPoint(topic, value)
def create_persistence_objects( server, environment_id, ): env_var_db = server[ENVIRONMENTAL_DATA_POINT] for variable in VALID_VARIABLES: topic = "{}/desired".format(variable.name) TopicPersistence( db=env_var_db, topic=topic, topic_type=get_message_class(variable.type), environment=environment_id, variable=variable.name, is_desired=True )
def test_get_slot_type_field_names(): # test for type as slot_type msg = get_message_class('jsk_rviz_plugins/OverlayText') field_names = get_slot_type_field_names(msg, slot_type='string') assert_equal(field_names, ['/font', '/text']) # test for msg as slot_type field_names = get_slot_type_field_names(msg, slot_type='std_msgs/ColorRGBA') assert_equal(field_names, ['/bg_color', '/fg_color']) # test for type array msg = get_message_class('jsk_recognition_msgs/Histogram') field_names = get_slot_type_field_names(msg, slot_type='float64[]') assert_equal(field_names, ['/histogram']) # test for msg array msg = get_message_class('diagnostic_msgs/DiagnosticArray') field_names = get_slot_type_field_names(msg, slot_type='string') assert_equal(field_names, [ '/header/frame_id', '/status[]/name', '/status[]/message', '/status[]/hardware_id', '/status[]/values[]/key', '/status[]/values[]/value' ])
def update_subscribers(SUBSCRIBERS_LIST): for topic in rospy.get_published_topics(): topic_name = topic[0] if topic_name in SUBSCRIBERS_LIST or 'robot' not in topic_name: continue msg_type = roslib_message.get_message_class(topic[1]) rospy.Subscriber(topic_name, msg_type, emit_topicupdate, callback_args=topic_name) print "Sub to %s" % topic_name SUBSCRIBERS_LIST.append(topic_name)
def init_subscribers(self, recipe_phases): ALL_SUBSCRIBER_LIST = { variable.name: rospy.Subscriber("{}/desired".format(variable.name), get_message_class(variable.type), self.callback) for variable in VALID_VARIABLES } VARIABLES = list( recipe_phases[0] ['step'].keys()) # Select the variables in the 1st phase subs = [ALL_SUBSCRIBER_LIST[variable] for variable in VARIABLES]
def connect_all_topics(firmware_module, firmware_module_type): modules = { record["_id"]: FirmwareModule(record) for record in firmware_module if not record["_id"].startswith('_') } module_types = { record["_id"]: FirmwareModuleType(record) for record in firmware_module_type if not record["_id"].startswith("_") } modules = synthesize_firmware_module_info(modules, module_types) for module_id, module_info in modules.items(): for input_name, input_info in module_info["inputs"].items(): if not "actuators" in input_info["categories"]: continue src_topic = "/environments/{}/{}/commanded".format( module_info["environment"], input_info["variable"]) dest_topic = "/actuators/{}/{}".format(module_id, input_name) dest_topic_type = get_message_class(input_info["type"]) src_topic_type = Float64 connect_topics(src_topic, dest_topic, src_topic_type, dest_topic_type, multiplier=input_info.get("multiplier", 1), deadband=input_info.get("deadband", 0)) for output_name, output_info in module_info["outputs"].items(): if not "sensors" in output_info["categories"]: continue src_topic = "/sensors/{}/{}/raw".format(module_id, output_name) dest_topic = "/environments/{}/{}/raw".format( module_info["environment"], output_info["variable"]) src_topic_type = get_message_class(output_info["type"]) dest_topic_type = Float64 connect_topics(src_topic, dest_topic, src_topic_type, dest_topic_type) src_info_topic = "/sensors/{}/{}/info".format( module_id, output_name) dest_info_topic = "/environments/{}/{}/info".format( module_info["environment"], output_info["variable"]) connect_sensor_info_topics(src_info_topic, dest_info_topic)
def create_persistence_objects( server, environment_id, ): env_var_db = server[ENVIRONMENTAL_DATA_POINT] for variable in VALID_VARIABLES: topic = "{}/desired".format(variable.name) TopicPersistence(db=env_var_db, topic=topic, topic_type=get_message_class(variable.type), environment=environment_id, variable=variable.name, is_desired=True)
def checkSize(class_str): try: message_class = get_message_class(class_str) format = msgToStructFormat(message_class) print class_str print " binary format: ", format print " size: ", calcsize(format), "bytes" print " ", 8 * calcsize(format), "bits" print " size w/ UDP header: ", calcsize(format) + 36, "bytes" print " ", 8 * (calcsize(format) + 36), "bits" except Exception, e: print "cannot serialize %s" % class_str print " error: ", e.message return
def __init__(self): message_class_str = rospy.get_param("~message", "jsk_network_tools/FC2OCS") try: self.receive_message = get_message_class(message_class_str) except: raise Exception("invalid topic type: %s" % message_class_str) self.receive_port = rospy.get_param("~receive_port", 1024) self.receive_ip = rospy.get_param("~receive_ip", "127.0.0.1") self.receive_buffer = rospy.get_param("~receive_buffer_size", 250) self.socket_server = socket(AF_INET, SOCK_DGRAM) self.socket_server.bind((self.receive_ip, self.receive_port)) self.receive_format = msgToStructFormat(self.receive_message()) self.pub = rospy.Publisher("~output", self.receive_message)
def refresh_topics(self): ''' Refresh topic list in the combobox ''' topic_list = rospy.get_published_topics() if topic_list is None: return self.topic_combox.clear() self.topic_name_class_map = {} for (name, type) in topic_list: if type in [ 'trajectory_msgs/JointTrajectory', 'control_msgs/FollowJointTrajectoryActionGoal' ]: self.topic_name_class_map[name] = get_message_class(type) self.topic_combox.addItem(name)
def __init__(self): message_class_str = rospy.get_param("~message", "jsk_network_tools/FC2OCS") try: self.receive_message = get_message_class(message_class_str) except: raise Exception("invalid topic type: %s"%message_class_str) self.receive_port = rospy.get_param("~receive_port", 1024) self.receive_ip = rospy.get_param("~receive_ip", "127.0.0.1") self.receive_buffer = rospy.get_param("~receive_buffer_size", 250) self.socket_server = socket(AF_INET, SOCK_DGRAM) self.socket_server.bind((self.receive_ip, self.receive_port)) self.receive_format = msgToStructFormat(self.receive_message()) self.pub = rospy.Publisher("~output", self.receive_message)
def connect_all_topics(module_db, module_type_db): modules = { module_id: FirmwareModule(module_db[module_id]) for module_id in module_db if not module_id.startswith('_') } module_types = { type_id: FirmwareModuleType(module_type_db[type_id]) for type_id in module_type_db if not type_id.startswith("_") } modules = synthesize_firmware_module_info(modules, module_types) for module_id, module_info in modules.items(): for input_name, input_info in module_info["inputs"].items(): if not "actuators" in input_info["categories"]: continue src_topic = "/{}/commanded/{}".format( module_info["environment"], input_info["variable"] ) dest_topic = "/actuators/{}/{}".format(module_id, input_name) dest_topic_type = get_message_class(input_info["type"]) src_topic_type = Float64 connect_topics( src_topic, dest_topic, src_topic_type, dest_topic_type, multiplier=input_info.get("multiplier", 1), deadband=input_info.get("deadband", 0) ) for output_name, output_info in module_info["outputs"].items(): if not "sensors" in output_info["categories"]: continue src_topic = "/sensors/{}/{}/filtered".format(module_id, output_name) dest_topic = "/{}/measured/{}".format( module_info["environment"], output_info["variable"] ) src_topic_type = get_message_class(output_info["type"]) dest_topic_type = Float64 connect_topics( src_topic, dest_topic, src_topic_type, dest_topic_type )
def __init__(self): rospy.init_node('downsampling_publish', anonymous=True) prefix = rospy.get_param('~prefix', "/downsampling") rate = rospy.get_param('~rate', 1) r = rospy.Rate(rate) self.topics_selected = rospy.get_param('/topics', '') self.topics_ = dict({}) self._master = rosgraph.Master(rospy.get_name()) self.all_topics_info = self._master.getTopicTypes() if len(self.topics_selected) == 0: for topic in self.all_topics_info: self.topics_selected.append(topic[0]) for topic in self.topics_selected: msg_name = [ty for tp, ty in self.all_topics_info if tp == topic][0] sub_ = rospy.Subscriber(topic, get_message_class(msg_name), callback=self.callback, callback_args=topic) pub_ = rospy.Publisher(prefix + topic, get_message_class(msg_name), queue_size=1) msg_ = get_message_class(msg_name) self.topics_[topic] = [sub_, pub_, msg_] rospy.loginfo("Starting Downsamplig Publisher at " + str(rate) + " Hz") rospy.loginfo("Topics: " + str(self.topics_selected)) while not rospy.is_shutdown(): for topic in self.topics_selected: self.topics_[topic][1].publish(self.topics_[topic][2]) r.sleep()
def __init__(self, topic_in, topic_out, typename): self.srv = rospy.Service(topic_out + '/switch_set_state', SetState, self.state_cb) self.srv = rospy.Service(topic_out + '/set_divider_rate', SetDividerRate, self.divider_cb) self.active = rospy.get_param('~active', False) #By default the switch is of self.divider_rate = rospy.get_param('~divider_rate', 1) #By default the divider rate does not change the normal rate self.counter = 0 topic_type = get_message_class(typename) if topic_type == None: print "Error: " + typename + " is not a valid message type." exit(1) self.sub = rospy.Subscriber(topic_in, topic_type, self.recv_cb) self.pub = rospy.Publisher(topic_out, topic_type)
def filter_all_topics(module_db, module_type_db): modules = { module_id: FirmwareModule(module_db[module_id]) for module_id in module_db if not module_id.startswith('_') } module_types = { type_id: FirmwareModuleType(module_type_db[type_id]) for type_id in module_type_db if not type_id.startswith("_") } modules = synthesize_firmware_module_info(modules, module_types) for module_id, module_info in modules.items(): for output_name, output_info in module_info["outputs"].items(): src_topic = "/sensors/{}/{}/raw".format(module_id, output_name) dest_topic = "/sensors/{}/{}/filtered".format( module_id, output_name) topic_type = get_message_class(output_info["type"]) filter_topic(src_topic, dest_topic, topic_type)
def __init__(self): message_class_str = rospy.get_param("~message", "jsk_network_tools/FC2OCS") try: self.send_message = get_message_class(message_class_str) except: raise Exception("invalid topic type: %s"%message_class_str) self.lock = Lock() self.to_port = rospy.get_param("~to_port", 1024) self.to_ip = rospy.get_param("~to_ip", "127.0.0.1") self.send_rate = rospy.get_param("~send_rate", 1) self.latest_message = None self.socket_client = socket(AF_INET, SOCK_DGRAM) self.send_format = msgToStructFormat(self.send_message()) self.sub = rospy.Subscriber("~input", self.send_message, self.messageCallback) self.send_timer = rospy.Timer(rospy.Duration(1 / self.send_rate), self.sendTimerCallback)
def __init__(self): message_class_str = rospy.get_param( "~message", "jsk_network_tools/FC2OCSLargeData") try: self.message_class = get_message_class(message_class_str) except: raise Exception("invalid topic type: %s" % message_class_str) self.lock = Lock() self.send_port = rospy.get_param("~to_port", 16484) self.send_ip = rospy.get_param("~to_ip", "localhost") self.rate = rospy.get_param("~send_rate", 2) #2Hz self.socket_client = socket(AF_INET, SOCK_DGRAM) self.packet_size = rospy.get_param("~packet_size", 1000) #2Hz subscriber_info = subscribersFromMessage(self.message_class()) self.messages = {} self.subscribe(subscriber_info) self.counter = 0 self.send_timer = rospy.Timer(rospy.Duration(1.0 / self.rate), self.sendTimerCallback)
def __init__(self): message_class_str = rospy.get_param("~message", "jsk_network_tools/FC2OCSLargeData") try: self.message_class = get_message_class(message_class_str) except: raise Exception("invalid topic type: %s"%message_class_str) self.lock = Lock() self.send_port = rospy.get_param("~to_port", 16484) self.send_ip = rospy.get_param("~to_ip", "localhost") self.rate = rospy.get_param("~send_rate", 2) #2Hz self.socket_client = socket(AF_INET, SOCK_DGRAM) self.packet_size = rospy.get_param("~packet_size", 1000) #2Hz subscriber_info = subscribersFromMessage(self.message_class()) self.messages = {} self.subscribe(subscriber_info) self.counter = 0 self.send_timer = rospy.Timer(rospy.Duration(1.0 / self.rate), self.sendTimerCallback)
def checkSize(class_str): try: message_class = get_message_class(class_str) format = msgToStructFormat(message_class) ros_msg = message_class() ros_buffer = StringIO() rospy.msg.serialize_message(ros_buffer, 0, ros_msg) print class_str print " binary format: ", format print " size: ", calcsize(format), "bytes" print " ", 8 * calcsize(format), "bits" print " size w/ UDP header: ", calcsize(format) + 36, "bytes" print " ", 8 * (calcsize(format) + 36), "bits" print " ROS size: ", ros_buffer.len, "bytes" print " ", ros_buffer.len * 8, "bits" except Exception, e: print "cannot serialize %s" % class_str print " error: ", e.message return
def fetch_bag(ctx, lz4, url): """fetch custom filtered bag from bagbunker example URLS: http://bagbunker.int.bosppaa.com/marv/api/messages/<md5> http://bagbunker.int.bosppaa.com/marv/api/messages/<md5>?topic=/foo http://bagbunker.int.bosppaa.com/marv/api/messages/<md5>?topic=/foo&topic=/bar http://bagbunker.int.bosppaa.com/marv/api/messages/<md5>?topic=/foo&msg_type=std_msgs/String (topic1 OR topic2) AND (msg_type1 OR msg_type2) """ resp = requests.get(url, stream=True, headers={'Accept': 'application/x-ros-bag-msgs'}) if resp.status_code != 200: raise Exception(resp) msc = MessageStreamClient(resp.iter_content(chunk_size=512)) missing = [] for topic, msg_type in msc.topics.items(): if get_message_class(msg_type) is None: missing.append(msg_type) continue if missing: missing.sort() click.echo("Missing message types:\n" + '\n '.join(missing)) ctx.exit(2) path = '{}.bag'.format(msc.name) if os.path.exists(path): click.echo('Will not overwrite existing bag: ' + path) ctx.exit(-1) bag = rosbag.Bag(path, 'w', compression='lz4' if lz4 else 'none') for topic, secs, nsecs, raw_msg in msc.messages: time = genpy.Time(secs, nsecs) # This will need the correct message classes available bag.write(topic, raw_msg, time, raw=True) click.echo('Finished writing bag %s' % path)
def __init__(self): message_class_str = rospy.get_param("~message", "jsk_network_tools/FC2OCSLargeData") try: self.message_class = get_message_class(message_class_str) except: raise Exception("invalid topic type: %s"%message_class_str) self.lock = Lock() self.receive_port = rospy.get_param("~receive_port", 16484) self.receive_ip = rospy.get_param("~receive_ip", "localhost") self.topic_prefix = rospy.get_param("~topic_prefix", "/from_fc") if not self.topic_prefix.startswith("/"): self.topic_prefix = "/" + self.topic_prefix if self.topic_prefix == "/": self.topic_prefix = "" self.publishers = publishersFromMessage(self.message_class, self.topic_prefix) self.socket_server = socket(AF_INET, SOCK_DGRAM) self.socket_server.bind((self.receive_ip, self.receive_port)) self.packet_size = rospy.get_param("~packet_size", 1000) #2Hz self.packets = []
def __init__(self, topic, buffer_size, buffer_size_type, throttle_rate, namespace): self.buffer = Buffer(buffer_size, buffer_size_type, throttle_rate) self.topic = topic self.namespace = namespace master_topics_graph = rosgraph.Master('/rostopic') topics = master_topics_graph.getTopicTypes() self.msg_type = None for (t, t_type) in topics: if t == topic: self.msg_type = roslib_message.get_message_class(t_type) break if self.msg_type is None: print "Topic: %s wasn't found in the current topics." % topic return self.subscriber = rospy.Subscriber(topic, self.msg_type, self.subscriber_callback) self.service = rospy.Service(self.namespace + topic, BufferSrv, self.request_handler) rospy.Timer(rospy.Duration(10), self.loop) rospy.spin()
def _cb(self, msg): if self._unknown_topic_type: anymsg = msg # get message class from input topic topic_types = self._master.getTopicTypes() msg_name = [ty for tp, ty in topic_types if tp == self._sub.name][0] msg_class = get_message_class(msg_name) self._unknown_topic_type = False # construct publisher self._pub = rospy.Publisher('~output', msg_class, queue_size=10) # reconstruct subscriber self._sub.unregister() self._sub = rospy.Subscriber('~input', msg_class, self._cb) # AnyMsg -> input message msg = msg_class().deserialize(anymsg._buff) # created delayed msg msg.header.stamp += rospy.Duration(self._delay) while msg.header.stamp > rospy.Time.now(): rospy.sleep(0.001) self._pub.publish(msg)
def test_get_message_class(self): from roslib.message import get_message_class try: self.assertEquals(None, get_message_class('String')) self.fail("should have thrown ValueError") except ValueError: pass # non-existent package self.assertEquals(None, get_message_class('fake/Fake')) # non-existent message self.assertEquals(None, get_message_class('roslib/Fake')) # package with no messages self.assertEquals(None, get_message_class('genmsg_cpp/Fake')) import rosgraph_msgs.msg import std_msgs.msg self.assertEquals(std_msgs.msg.Header, get_message_class('Header')) self.assertEquals(std_msgs.msg.Header, get_message_class('std_msgs/Header')) self.assertEquals(rosgraph_msgs.msg.Log, get_message_class('rosgraph_msgs/Log'))
rospy.init_node('bbmsg') resp = requests.get(url, stream=True, headers={'Accept': 'application/x-ros-bag-msgs'}) if resp.status_code != 200: raise Exception(resp) msc = MessageStreamClient(resp.iter_content(chunk_size=512)) topics = msc.topics if '/clock' in topics: raise ValueError('/clock must not be streamed from server') pubs = {} missing = [] for topic, msg_type in topics.items(): pytype = get_message_class(msg_type) if pytype is None: missing.append(msg_type) continue pubs[topic] = rospy.Publisher(topic, pytype, queue_size=1) if missing: missing.sort() click.echo("Missing message types:\n" + '\n '.join(missing)) ctx.exit(2) clock_pub = rospy.Publisher('/clock', Clock, queue_size=1) clock_msg = Clock() for topic, secs, nsecs, raw_msg in msc.messages: time = genpy.Time(secs, nsecs) clock_msg.clock = time
def __init__(self, topic, msg_type, show_only_rate=False, masteruri=None, use_ssh=False, parent=None): ''' Creates an input dialog. @param topic: the name of the topic @type topic: C{str} @param msg_type: the type of the topic @type msg_type: C{str} @raise Exception: if no topic class was found for the given type ''' QDialog.__init__(self, parent=parent) self._masteruri = masteruri masteruri_str = '' if masteruri is None else '[%s]' % masteruri echo_dialog_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'EchoDialog.ui') loadUi(echo_dialog_file, self) self.setObjectName(' - '.join(['EchoDialog', topic, masteruri_str])) self.setAttribute(Qt.WA_DeleteOnClose, True) self.setWindowFlags(Qt.Window) self.setWindowTitle('%s %s %s' % ('Echo --- ' if not show_only_rate else 'Hz --- ', topic, masteruri_str)) self.resize(900, 512) self.topic = topic self.show_only_rate = show_only_rate self.lock = threading.RLock() self.last_printed_count = 0 self.msg_t0 = -1. self.msg_tn = 0 self.times = [] self.bytes = [] self.message_count = 0 self._state_message = '' self._state_size_message = '' self._scrapped_msgs = 0 self._scrapped_msgs_sl = 0 self._last_received_ts = 0 self.chars_limit = self.MESSAGE_CHARS_LIMIT self.receiving_hz = self.MESSAGE_HZ_LIMIT self.line_limit = self.MESSAGE_LINE_LIMIT self.max_displayed_msgs = self.MAX_DISPLAY_MSGS self.digits_after_in_array = 2 self.enabled_message_filter = True self.field_filter_fn = None self._latched = False self._msgs = [] self.filterFrame.setVisible(False) self.topicControlButton.clicked.connect(self.on_topic_control_btn_clicked) self.clearButton.clicked.connect(self.on_clear_btn_clicked) if show_only_rate: self.filterButton.setVisible(False) else: self.filterButton.clicked.connect(self.on_filter_clicked) self.showStringsCheckBox.toggled.connect(self.on_no_str_checkbox_toggled) self.maxLenStringComboBox.activated[str].connect(self.combobox_reduce_ch_activated) self.showArraysCheckBox.toggled.connect(self.on_no_arr_checkbox_toggled) self.maxDigitsComboBox.activated[str].connect(self.combobox_reduce_digits_activated) self.enableMsgFilterCheckBox.toggled.connect(self.on_enable_msg_filter_checkbox_toggled) self.maxLenComboBox.activated[str].connect(self.on_combobox_chars_activated) self.maxHzComboBox.activated[str].connect(self.on_combobox_hz_activated) self.displayCountComboBox.activated[str].connect(self.on_combobox_count_activated) self.combobox_reduce_ch_activated(self.MESSAGE_LINE_LIMIT) self.on_combobox_chars_activated(self.MESSAGE_CHARS_LIMIT) self.on_combobox_hz_activated(self.MESSAGE_HZ_LIMIT) self.on_combobox_count_activated(self.MAX_DISPLAY_MSGS) self.filterButton.setFocus() self.display.setReadOnly(True) self.display.document().setMaximumBlockCount(500) self._blocks_in_msg = None self.display.setOpenLinks(False) self.display.anchorClicked.connect(self._on_display_anchorClicked) # subscribe to the topic errmsg = '' try: self.__msg_class = message.get_message_class(msg_type) if not self.__msg_class: errmsg = "Cannot load message class for [%s]. Did you build messages?" % msg_type except Exception as e: self.__msg_class = None errmsg = "Cannot load message class for [%s]. Did you build messagest?\nError: %s" % (msg_type, utf8(e)) # variables for Subscriber self.msg_signal.connect(self._append_message) self.sub = None # vairables for SSH connection self.ssh_output_file = None self.ssh_error_file = None self.ssh_input_file = None self.text_signal.connect(self._append_text) self.text_hz_signal.connect(self._append_text_hz) self._current_msg = '' self._current_errmsg = '' self.text_error_signal.connect(self._append_error_text) # decide, which connection to open if use_ssh: self.__msg_class = None self._on_display_anchorClicked(QUrl(self._masteruri)) elif self.__msg_class is None: errtxt = '<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">\n%s</pre>' % (errmsg) self.display.setText('<a href="%s">open using SSH</a>' % (masteruri)) self.display.append(errtxt) else: self.sub = rospy.Subscriber(self.topic, self.__msg_class, self._msg_handle) self.print_hz_timer = QTimer() self.print_hz_timer.timeout.connect(self._on_calc_hz) self.print_hz_timer.start(1000) self._start_time = time.time()
RECIPE_START = VariableInfo.from_dict( rospy.get_param('/var_types/recipe_variables/recipe_start')) RECIPE_END = VariableInfo.from_dict( rospy.get_param('/var_types/recipe_variables/recipe_end')) # This builds a dictionary of publisher instances using a # "dictionary comprehension" (syntactic sugar for building dictionaries). # The constant has to be declared here because get_message_class # needs to be called after the node is initialized. PUBLISHERS = { variable.name: rospy.Publisher( "{}/desired".format(variable.name), get_message_class(variable.type), queue_size=10) for variable in VALID_VARIABLES } RECIPE_INTERPRETERS = { "simple": interpret_simple_recipe, "flexformat": interpret_flexformat_recipe } #------------------------------------------------------------------------------ class RecipeRunningError(Exception): """Thrown when trying to set a recipe, but recipe is already running.""" pass
def __init__(self, topic, msg_type, show_only_rate=False, masteruri=None, use_ssh=False, parent=None): ''' Creates an input dialog. @param topic: the name of the topic @type topic: C{str} @param msg_type: the type of the topic @type msg_type: C{str} @raise Exception: if no topic class was found for the given type ''' QtGui.QDialog.__init__(self, parent=parent) self._masteruri = masteruri masteruri_str = '' if masteruri is None else '[%s]'%masteruri self.setObjectName(' - '.join(['EchoDialog', topic, masteruri_str])) self.setAttribute(QtCore.Qt.WA_DeleteOnClose, True) self.setWindowFlags(QtCore.Qt.Window) self.setWindowTitle('%s %s %s'%('Echo --- ' if not show_only_rate else 'Hz --- ', topic, masteruri_str)) self.resize(728,512) self.verticalLayout = QtGui.QVBoxLayout(self) self.verticalLayout.setObjectName("verticalLayout") self.verticalLayout.setContentsMargins(1, 1, 1, 1) self.mIcon = QtGui.QIcon(":/icons/crystal_clear_prop_run_echo.png") self.setWindowIcon(self.mIcon) self.topic = topic self.show_only_rate = show_only_rate self.lock = threading.RLock() self.last_printed_count = 0 self.msg_t0 = -1. self.msg_tn = 0 self.times =[] self.message_count = 0 self._rate_message = '' self._scrapped_msgs = 0 self._scrapped_msgs_sl = 0 self._last_received_ts = 0 self.receiving_hz = self.MESSAGE_HZ_LIMIT self.line_limit = self.MESSAGE_LINE_LIMIT self.field_filter_fn = None options = QtGui.QWidget(self) if not show_only_rate: hLayout = QtGui.QHBoxLayout(options) hLayout.setContentsMargins(1, 1, 1, 1) self.no_str_checkbox = no_str_checkbox = QtGui.QCheckBox('Hide strings') no_str_checkbox.toggled.connect(self.on_no_str_checkbox_toggled) hLayout.addWidget(no_str_checkbox) self.no_arr_checkbox = no_arr_checkbox = QtGui.QCheckBox('Hide arrays') no_arr_checkbox.toggled.connect(self.on_no_arr_checkbox_toggled) hLayout.addWidget(no_arr_checkbox) self.combobox_reduce_ch = QtGui.QComboBox(self) self.combobox_reduce_ch.addItems([str(self.MESSAGE_LINE_LIMIT), '0', '80', '256', '1024']) self.combobox_reduce_ch.activated[str].connect(self.combobox_reduce_ch_activated) self.combobox_reduce_ch.setEditable(True) self.combobox_reduce_ch.setToolTip("Set maximum line width. 0 disables the limit.") hLayout.addWidget(self.combobox_reduce_ch) # reduce_ch_label = QtGui.QLabel('ch', self) # hLayout.addWidget(reduce_ch_label) # add spacer spacerItem = QtGui.QSpacerItem(515, 20, QtGui.QSizePolicy.Expanding, QtGui.QSizePolicy.Minimum) hLayout.addItem(spacerItem) # add combobox for displaying frequency of messages self.combobox_displ_hz = QtGui.QComboBox(self) self.combobox_displ_hz.addItems([str(self.MESSAGE_HZ_LIMIT), '0', '0.1', '1', '50', '100', '1000']) self.combobox_displ_hz.activated[str].connect(self.on_combobox_hz_activated) self.combobox_displ_hz.setEditable(True) hLayout.addWidget(self.combobox_displ_hz) displ_hz_label = QtGui.QLabel('Hz', self) hLayout.addWidget(displ_hz_label) # add combobox for count of displayed messages self.combobox_msgs_count = QtGui.QComboBox(self) self.combobox_msgs_count.addItems([str(self.MAX_DISPLAY_MSGS), '0', '50', '100']) self.combobox_msgs_count.activated[str].connect(self.on_combobox_count_activated) self.combobox_msgs_count.setEditable(True) self.combobox_msgs_count.setToolTip("Set maximum displayed message count. 0 disables the limit.") hLayout.addWidget(self.combobox_msgs_count) displ_count_label = QtGui.QLabel('#', self) hLayout.addWidget(displ_count_label) # add topic control button for unsubscribe and subscribe self.topic_control_button = QtGui.QToolButton(self) self.topic_control_button.setText('stop') self.topic_control_button.setIcon(QtGui.QIcon(':/icons/deleket_deviantart_stop.png')) self.topic_control_button.clicked.connect(self.on_topic_control_btn_clicked) hLayout.addWidget(self.topic_control_button) # add clear button clearButton = QtGui.QToolButton(self) clearButton.setText('clear') clearButton.clicked.connect(self.on_clear_btn_clicked) hLayout.addWidget(clearButton) self.verticalLayout.addWidget(options) self.display = QtGui.QTextBrowser(self) self.display.setReadOnly(True) self.verticalLayout.addWidget(self.display); self.display.document().setMaximumBlockCount(500) self.max_displayed_msgs = self.MAX_DISPLAY_MSGS self._blocks_in_msg = None self.display.setOpenLinks(False) self.display.anchorClicked.connect(self._on_display_anchorClicked) self.status_label = QtGui.QLabel('0 messages', self) self.verticalLayout.addWidget(self.status_label) # subscribe to the topic errmsg = '' try: self.__msg_class = message.get_message_class(msg_type) if not self.__msg_class: errmsg = "Cannot load message class for [%s]. Did you build messages?"%msg_type # raise Exception("Cannot load message class for [%s]. Did you build messages?"%msg_type) except Exception as e: self.__msg_class = None errmsg = "Cannot load message class for [%s]. Did you build messagest?\nError: %s"%(msg_type, e) # raise Exception("Cannot load message class for [%s]. Did you build messagest?\nError: %s"%(msg_type, e)) # variables for Subscriber self.msg_signal.connect(self._append_message) self.sub = None # vairables for SSH connection self.ssh_output_file = None self.ssh_error_file = None self.ssh_input_file = None self.text_signal.connect(self._append_text) self.text_hz_signal.connect(self._append_text_hz) self._current_msg = '' self._current_errmsg = '' self.text_error_signal.connect(self._append_error_text) # decide, which connection to open if use_ssh: self.__msg_class = None self._on_display_anchorClicked(QtCore.QUrl(self._masteruri)) elif self.__msg_class is None: errtxt = '<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">\n%s</pre>'%(errmsg) self.display.setText('<a href="%s">open using SSH</a>'%(masteruri)) self.display.append(errtxt) else: self.sub = rospy.Subscriber(self.topic, self.__msg_class, self._msg_handle) self.print_hz_timer = QtCore.QTimer() self.print_hz_timer.timeout.connect(self._on_calc_hz) self.print_hz_timer.start(1000)