useful_functions.py 2.49 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
ros_time_types = ["time", "duration"]
ros_primitive_types = [
    "bool",
    "byte",
    "char",
    "int",
    "uint",
    "float",
    "double",
    "int8",
    "int16",
    "int32",
    "int64",
    "uint8",
    "uint16",
    "uint32",
    "uint64",
    "float32",
    "float64",
    "string",
]


def convert_ros_message_to_dictionary(message):
    """
    Takes in a ROS message and returns a Python dictionary.

    Example:
        ros_message = std_msgs.msg.String(data="Hello, Robot")
        dict_message = convert_ros_message_to_dictionary(ros_message)
    """
    dictionary = {}
    message_fields = message._fields_and_field_types  # _get_message_fields(message)
    for field_name, field_type in message_fields.items():
        field_value = getattr(message, field_name)
        dictionary[field_name] = _convert_from_ros_type(field_type, field_value)

    return dictionary


def _convert_from_ros_type(field_type, field_value):
    if field_type in ros_primitive_types:
        field_value = field_value
    elif field_type in ros_time_types:
        field_value = _convert_from_ros_time(field_type, field_value)
    elif _is_ros_binary_type(field_type):
        field_value = _convert_from_ros_binary(field_type, field_value)
    elif _is_field_type_a_primitive_array(field_type):
        field_value = list(field_value)
    elif _is_field_type_an_array(field_type):
        field_value = _convert_from_ros_array(field_type, field_value)
    else:
        field_value = convert_ros_message_to_dictionary(field_value)
    return field_value


def _convert_from_ros_time(field_type, field_value):
    field_value = {"secs": field_value.secs, "nsecs": field_value.nsecs}
    return field_value


def _convert_from_ros_binary(field_type, field_value):
    field_value = base64.standard_b64encode(field_value).decode("utf-8")
    return field_value


def _convert_from_ros_array(field_type, field_value):
    # use index to raise ValueError if '[' not present
    list_type = field_type[: field_type.index("[")]
    return [_convert_from_ros_type(list_type, value) for value in field_value]


def _is_field_type_an_array(field_type):
    return field_type.find("[") >= 0


def _is_field_type_a_primitive_array(field_type):
    bracket_index = field_type.find("[")
    if bracket_index < 0:
        return False
    else:
        list_type = field_type[:bracket_index]
        return list_type in ros_primitive_types


def _is_ros_binary_type(field_type):
    return field_type.startswith("uint8[") or field_type.startswith("char[")