DFKI-NI / rospy_message_converter

Converts between Python dictionaries and JSON to ROS messages.
BSD 3-Clause "New" or "Revised" License
226 stars 101 forks source link

latest version can't handle numpy.float64 #39

Closed ichumuh closed 4 years ago

ichumuh commented 4 years ago

Hi!

This is a very useful package, but I'm having trouble, since I've updated to:

$ dpkg -l | grep message-convert
ii  ros-melodic-rospy-message-converter          0.5.2-1bionic.20200801.235102                    amd64        Converts between Python dictionaries and JSON to rospy messages.

It seems like it can't handle numpy types anymore. Here is an example:

import numpy as np

from geometry_msgs.msg import PoseStamped
from rospy_message_converter.message_converter import convert_ros_message_to_dictionary, \
    convert_dictionary_to_ros_message

pose = PoseStamped()
pose.pose.position.x = np.float64(1)

d = convert_ros_message_to_dictionary(pose)
msg = convert_dictionary_to_ros_message('geometry_msgs/PoseStamped', d)

Running it, gives me this error:

Traceback (most recent call last):
  File "/usr/lib/python2.7/dist-packages/IPython/core/interactiveshell.py", line 2882, in run_code
    exec(code_obj, self.user_global_ns, self.user_ns)
  File "<ipython-input-2-bb41facf3913>", line 1, in <module>
    runfile('/home/stelter/giskard_ws/src/giskardpy/scripts/muh4.py', wdir='/home/stelter/giskard_ws/src/giskardpy/scripts')
  File "/home/stelter/pycharm-2018.3.1/plugins/python/helpers/pydev/_pydev_bundle/pydev_umd.py", line 197, in runfile
    pydev_imports.execfile(filename, global_vars, local_vars)  # execute the script
  File "/home/stelter/giskard_ws/src/giskardpy/scripts/muh4.py", line 11, in <module>
    msg = convert_dictionary_to_ros_message('geometry_msgs/PoseStamped', d)
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy_message_converter/message_converter.py", line 115, in convert_dictionary_to_ros_message
    field_value = _convert_to_ros_type(field_type, field_value)
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy_message_converter/message_converter.py", line 146, in _convert_to_ros_type
    field_value = convert_dictionary_to_ros_message(field_type, field_value)
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy_message_converter/message_converter.py", line 115, in convert_dictionary_to_ros_message
    field_value = _convert_to_ros_type(field_type, field_value)
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy_message_converter/message_converter.py", line 146, in _convert_to_ros_type
    field_value = convert_dictionary_to_ros_message(field_type, field_value)
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy_message_converter/message_converter.py", line 115, in convert_dictionary_to_ros_message
    field_value = _convert_to_ros_type(field_type, field_value)
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy_message_converter/message_converter.py", line 139, in _convert_to_ros_type
    raise TypeError("Wrong type: '{0}' must be {1}".format(field_value, field_type))
TypeError: Wrong type: '1.0' must be float64

That is odd, cause this 1.0 is of type float64. Can you please look into it? It does work with version

0.5.1-1bionic.20200530.021813 
mintar commented 4 years ago

Thank you for your very useful bug report! The type checking was indeed added in version 0.5.2 (in #37). The error message was very confusing, so I have just pushed a commit that prints a better error message.

Old error message (on your code):

TypeError: Wrong type: '1.0' must be float64

New error message (on your code):

TypeError: Field 'x' has wrong type <type 'numpy.float64'> (valid types: [<type 'int'>, <type 'float'>])

This explains the problem here better: You've passed in a numpy.float64, but when a PoseStamped message is deserialized (such as when you receive it from another ROS node), it will never contain a numpy.float64, but always a float. In that sense, numpy.float64 is the "wrong" type for that field.

However, you can put any numeric python type (including numpy types) into any numeric ROS message field, and it will serialize just fine (e.g., when you publish it). You might just lose some precision (e.g. when converting from float to int). If we want to allow that again, we could do it like in the diff below.

I'm a bit undecided if that's really what we should be doing. On the one hand, this test allows any messages that won't produce a serialization error. On the other hand, it feels wrong to allow a np.float64 for a byte field.

@alecarnevale : Since you implemented the type check in the first place, what is your use case for it? Any opinions?

diff --git i/src/rospy_message_converter/message_converter.py w/src/rospy_message_converter/message_converter.py
index 5a6b787..7ac991b 100644
--- i/src/rospy_message_converter/message_converter.py
+++ w/src/rospy_message_converter/message_converter.py
@@ -36,31 +36,36 @@ import rospy
 import base64
 import sys
 import copy
+import numpy as np
+
 python3 = (sys.hexversion > 0x03000000)

 if python3:
     python_string_types = [str]
-    python_long_int_types = [int]
+    python_numeric_types = [int, float]
 else:
     python_string_types = [str, unicode]
-    python_long_int_types = [int, long]
+    python_numeric_types = [int, long, float]
+
 python_list_types = [list, tuple]

+numpy_numeric_types = [np.int8, np.int16, np.int32, np.int64, np.uint8, np.uint16, np.uint32, np.uint64, np.float32, np.float64]
+
 ros_to_python_type_map = {
     'bool'    : [bool],
-    'byte'    : [int],
-    'char'    : [int],
-    'float32' : [int, float],
-    'float64' : [int, float],
-    'int16'   : [int],
-    'int32'   : [int],
-    'int64'   : [int],
-    'int8'    : [int],
+    'byte'    : python_numeric_types + numpy_numeric_types,
+    'char'    : python_numeric_types + numpy_numeric_types,
+    'float32' : python_numeric_types + numpy_numeric_types,
+    'float64' : python_numeric_types + numpy_numeric_types,
+    'int16'   : python_numeric_types + numpy_numeric_types,
+    'int32'   : python_numeric_types + numpy_numeric_types,
+    'int64'   : python_numeric_types + numpy_numeric_types,
+    'int8'    : python_numeric_types + numpy_numeric_types,
     'string'  : python_string_types,
-    'uint16'  : [int],
-    'uint32'  : [int],
-    'uint64'  : python_long_int_types,
-    'uint8'   : [int]
+    'uint16'  : python_numeric_types + numpy_numeric_types,
+    'uint32'  : python_numeric_types + numpy_numeric_types,
+    'uint64'  : python_numeric_types + numpy_numeric_types,
+    'uint8'   : python_numeric_types + numpy_numeric_types
 }
mintar commented 4 years ago

The issue is even a bit more nuanced than that. Serialization will only work if the field value is within the interval representable by the datatype. E.g., filling a uint8 field with a python int with value "23" will work, but if you put in "10000" you'll get a serialization error. This means that even the current code doesn't guarantee that there will be no serialization errors. You'd have to use something like genpy.message.check_type, and that's not fast enough to do for every message.

I've thrown together a quick test script to show this:

#!/usr/bin/env python
import sys
import numpy as np

from std_msgs.msg import Byte, Char, Float32, Float64, Int8, Int16, Int32, Int64, UInt8, UInt16, UInt32, UInt64

MESSAGE_TYPES = [Byte, Char, Float32, Float64, Int8, Int16, Int32, Int64, UInt8, UInt16, UInt32, UInt64]
NUMPY_NUMERIC_TYPES = [np.int8, np.int16, np.int32, np.int64, np.uint8, np.uint16, np.uint32, np.uint64, np.float32, np.float64]
PYTHON2_NUMERIC_TYPES = [int, long, float]

# "long" doesn't have any max or min value
MAX_VALUES = [sys.maxint, 10 * sys.maxint, sys.float_info.max] \
             + [np.iinfo(t).max for t in [np.int8, np.int16, np.int32, np.int64, np.uint8, np.uint16, np.uint32, np.uint64]] \
             + [np.finfo(t).max for t in [np.float32, np.float64]]
MIN_VALUES = [-sys.maxint, 10 * -sys.maxint, sys.float_info.min] \
             + [np.iinfo(t).min for t in [np.int8, np.int16, np.int32, np.int64, np.uint8, np.uint16, np.uint32, np.uint64]] \
             + [np.finfo(t).min for t in [np.float32, np.float64]]

def serialize_deserialize(message):
    """
    Serialize and then deserialize a message. This simulates sending a message
    between ROS nodes and makes sure that the ROS messages being tested are
    actually serializable, and are in the same format as they would be received
    over the network. In rospy, it is possible to assign an illegal data type
    to a message field (for example, `message = String(data=42)`), but trying
    to publish this message will throw `SerializationError: field data must be
    of type str`. This method will expose such bugs.
    """
    from io import BytesIO
    buff = BytesIO()
    message.serialize(buff)
    result = message.__class__()   # create new instance of same class as message
    result.deserialize(buff.getvalue())
    return result

for ros_type in MESSAGE_TYPES:
    valid_types = []

    for numeric_type, max_value, min_value in zip(PYTHON2_NUMERIC_TYPES + NUMPY_NUMERIC_TYPES, MAX_VALUES, MIN_VALUES):
        try:
            success = True
            for value in [max_value, min_value]:
                msg = ros_type(data=numeric_type(value))
                msg2 = serialize_deserialize(msg)
                if msg2.data != value:
                    success = False
                    break
            if success:
                valid_types.append(numeric_type)
        except:
            pass

    print '{}: ['.format(ros_type.__name__), ', '.join(['{}.{}'.format(t.__module__, t.__name__) for t in valid_types]), ']'

The (lightly reformatted) output is:

Byte:    [ np.int8 ]
Char:    [ np.uint8 ]
Float32: [ np.int8, np.int16, np.uint8, np.uint16, np.float32 ]
Float64: [ float, np.int8, np.int16, np.int32, np.uint8, np.uint16, np.uint32, np.float32, np.float64 ]
Int8:    [ np.int8 ]
Int16:   [ np.int8, np.int16, np.uint8 ]
Int32:   [ np.int8, np.int16, np.int32, np.uint8, np.uint16 ]
Int64:   [ int, np.int8, np.int16, np.int32, np.int64, np.uint8, np.uint16, np.uint32 ]
UInt8:   [ np.uint8 ]
UInt16:  [ np.uint8, np.uint16 ]
UInt32:  [ np.uint8, np.uint16, np.uint32 ]
UInt64:  [ np.uint8, np.uint16, np.uint32, np.uint64 ]

You can see that the python builtins (e.g., int, float) don't even show up for most types. That's because it's not always safe to assign any int to a ROS "int8" field, for example.

For this reason, I'll probably merge the diff from my previous comment. @alecarnevale , any objections?

ichumuh commented 4 years ago

If I interpret this correctly, the fix would only work for numpy's types. What was the reasoning to add this check in the first place? With the way python's typing system works, it feels the most fitting to me, if we could just kinda assign anything to everything. Even a np.float64 to a bool (since True and False are defined as 1 and 0 in python) or the other way around. If this gives a serialization error, if the value is out of range its fine, to me.

mintar commented 4 years ago

What was the reasoning to add this check in the first place?

I don't know what @alecarnevale 's use case was (he's the one that submitted the PR with the check). However, I believe it's a valuable contribution because it avoids a lot of subtle bugs.

I've thought a lot about this and I'm not sure what the most "pythonic" thing to do would be. On the one hand, you're right that python allows conversion from almost anything to anything. On the other hand, numpy also forces you to use stuff like np.astype to explicitly convert between numpy types.

I've added a more restrictive version of my fix above as PR #41. Unless there are any objections, I'm going to merge it tomorrow.

Regarding your concerns: Would it be okay for you to add a boolean parameter check_types to the convert_ros_message_to_dictionary function that would allow you to disable the type check altogether? Similar to the check_missing_fields parameter of convert_dictionary_to_ros_message.