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)
Esempio n. 2
0
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)
Esempio n. 3
0
    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
        }
Esempio n. 4
0
 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)
Esempio n. 6
0
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
Esempio n. 8
0
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)
Esempio n. 10
0
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()
Esempio n. 12
0
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 __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)
Esempio n. 18
0
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)
Esempio n. 19
0
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)
Esempio n. 21
0
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()
Esempio n. 24
0
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
        )
Esempio n. 28
0
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'
    ])
Esempio n. 29
0
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]
Esempio n. 31
0
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)
Esempio n. 32
0
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 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
Esempio n. 35
0
 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)
Esempio n. 38
0
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()
Esempio n. 40
0
	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)
Esempio n. 41
0
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
Esempio n. 46
0
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)
Esempio n. 47
0
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 __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()
Esempio n. 50
0
 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'))    
Esempio n. 52
0
    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
Esempio n. 55
0
  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)