def test_fill_message_args_embed_time(self):
from genpy import Time
from genpy.message import fill_message_args
from genpy.msg import TestFillEmbedTime
# test fill_message_args with embeds and time vals
# time t
# duration d
# std_msgs/String str_msg
# std_msgs/String[] str_msg_array
# int32 i32
m = TestFillEmbedTime()
fill_message_args(m, [{}])
self.assertEquals(m.t, Time())
self.assertEquals(m.d, Duration())
self.assertEquals(m.str_msg.data, "")
self.assertEquals(m.str_msg_array, [])
self.assertEquals(m.i32, 0)
# list tests
# - these should be equivalent
equiv = [
[[10, 20], [30, 40], ["foo"], [["bar"], ["baz"]], 32],
[{"secs": 10, "nsecs": 20}, {"secs": 30, "nsecs": 40}, ["foo"], [["bar"], ["baz"]], 32],
[[10, 20], [30, 40], {"data": "foo"}, [["bar"], ["baz"]], 32],
[[10, 20], [30, 40], ["foo"], [{"data": "bar"}, {"data": "baz"}], 32],
[
{
"t": [10, 20],
"d": [30, 40],
"str_msg": {"data": "foo"},
"str_msg_array": [{"data": "bar"}, {"data": "baz"}],
"i32": 32,
}
],
[
{
"t": {"secs": 10, "nsecs": 20},
"d": [30, 40],
"str_msg": {"data": "foo"},
"str_msg_array": [{"data": "bar"}, {"data": "baz"}],
"i32": 32,
}
],
]
for test in equiv:
m = TestFillEmbedTime()
try:
fill_message_args(m, test)
except Exception, e:
self.fail("failed to fill with : %s\n%s" % (str(test), traceback.format_exc()))
self.assertEquals(m.t, Time(10, 20))
self.assertEquals(m.d, Duration(30, 40))
self.assertEquals(m.str_msg.data, "foo")
self.assertEquals(len(m.str_msg_array), 2, m.str_msg_array)
self.assertEquals(m.str_msg_array[0].data, "bar")
self.assertEquals(m.str_msg_array[1].data, "baz")
self.assertEquals(m.i32, 32)
def test_serialize_exception_msg(self):
from genpy.message import fill_message_args
from genpy.msg import TestStringFloat
try:
from cStringIO import StringIO
except ImportError:
from io import BytesIO as StringIO
buff = StringIO()
m = TestStringFloat()
fill_message_args(m, ['foo', 1.0])
self.assertEqual(m.data, 'foo')
self.assertEqual(m.float, 1.0)
# should serialize fine with float
m.serialize(buff)
# setting a string instead should fail with correct message
m.float = '1.0'
try:
m.serialize(buff)
assert False, 'This should have raised a genpy.SerializationError'
except genpy.SerializationError as e:
self.assertEqual(
str(e),
"<class 'struct.error'>: 'required argument is not a float' when writing '1.0'"
)
except Exception:
assert False, 'This should have raised a genpy.SerializationError instead'
def check_msg(self, msg_str, expects):
pub, msg_class = rostopic.create_publisher(
'/blender_api/get_pau', 'pau2motors/pau', True)
msg = msg_class()
fill_message_args(msg, yaml.load(msg_str))
pub.publish(msg)
msg = wait_for_message('/joint_states_combined', JointState, 2)
for key in ['position', 'name']:
self.assertListEqual(list(getattr(msg, key)), expects[key])
pub.unregister()
def quaternion_mult(q1, q2):
w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z
x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y
y = q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z
z = q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x
q = Quaternion()
fill_message_args(q, [x, y, z, w])
return q
def check_msg(self, msg_str, expects):
pub, msg_class = rostopic.create_publisher('/blender_api/get_pau',
'pau2motors/pau', True)
msg = msg_class()
fill_message_args(msg, yaml.load(msg_str))
pub.publish(msg)
msg = wait_for_message('/joint_states_combined', JointState, 2)
for key in ['position', 'name']:
self.assertListEqual(list(getattr(msg, key)), expects[key])
pub.unregister()
def roundtrip(m):
yaml_text = strify_message(m)
print(yaml_text)
loaded = yaml.safe_load(yaml_text)
print("loaded", loaded)
new_inst = m.__class__()
if loaded is not None:
fill_message_args(new_inst, [loaded])
else:
fill_message_args(new_inst, [])
return new_inst
def roundtrip(m, escape_strings=False):
yaml_text = strify_message(m, escape_strings=escape_strings)
print(yaml_text)
loaded = yaml.load(yaml_text)
print("loaded", loaded)
new_inst = m.__class__()
if loaded is not None:
fill_message_args(new_inst, [loaded])
else:
fill_message_args(new_inst, [])
return new_inst
def roundtrip(m):
yaml_text = strify_message(m)
print(yaml_text)
loaded = yaml.load(yaml_text)
print("loaded", loaded)
new_inst = m.__class__()
if loaded is not None:
fill_message_args(new_inst, [loaded])
else:
fill_message_args(new_inst, [])
return new_inst
def from_dict_to_ROS(dict_msg, ros_message_type, srv=False):
"""
Converts from a dict representation of a ROS message to a
ROS message instance.
"""
msg_class = get_ROS_class(ros_message_type, srv=srv)
msg_instance = msg_class()
# Workaround
if len(dict_msg) == 1:
dict_msg = [dict_msg]
fill_message_args(msg_instance, dict_msg)
return msg_instance
def matrix_to_posestamped(mat):
m00 = mat[0, 0]
m01 = mat[0, 1]
m02 = mat[0, 2]
m10 = mat[1, 0]
m11 = mat[1, 1]
m12 = mat[1, 2]
m20 = mat[2, 0]
m21 = mat[2, 1]
m22 = mat[2, 2]
tr = m00 + m11 + m22
#qw = 0; qx = 0; qy = 0; qz = 0
if (tr > 0):
S = sqrt(tr + 1.0) * 2 # S=4 * qw
qw = 0.25 * S
qx = (m21 - m12) / S
qy = (m02 - m20) / S
qz = (m10 - m01) / S
elif ((m00 > m11) and (m00 > m22)):
S = sqrt(1.0 + m00 - m11 - m22) * 2 # S=4 * qx
qw = (m21 - m12) / S
qx = 0.25 * S
qy = (m01 + m10) / S
qz = (m02 + m20) / S
elif (m11 > m22):
S = sqrt(1.0 + m11 - m00 - m22) * 2 # S=4 * qy
qw = (m02 - m20) / S
qx = (m01 + m10) / S
qy = 0.25 * S
qz = (m12 + m21) / S
else:
S = sqrt(1.0 + m22 - m00 - m11) * 2 # S=4 * qz
qw = (m10 - m01) / S
qx = (m02 + m20) / S
qy = (m12 + m21) / S
qz = 0.25 * S
pose = PoseStamped()
args = [{
'pose': {
'position': [mat[0, 3], mat[1, 3], mat[2, 3]],
'orientation': [qx, qy, qz, qw]
}
}]
fill_message_args(pose, args)
print(pose)
return pose
def load_poses_cb(self, feedback):
filename = self.get_open_filename('*.yaml')
if filename:
with open(filename, 'r') as file:
content = yaml.load(file.read())
msg = NamedPoseStampedList()
fill_message_args(msg, [content])
active_name = None
for np in msg.poses:
self.create_path_marker(np)
active_name = np.name
if active_name:
self.activate_marker(active_name)
self.server.applyChanges()
def move_handler(self, req):
X =req.x
Y =req.y
Z =req.z
yaw = req.yaw * pi / 180
world2drone = posestamped_to_transform(self.current_pose)
translation = np.array([[X],[Y],[Z],[1]])
new_pos = np.matmul(world2drone,translation)
q = perform_yaw(self.current_pose, yaw)
args = [{'pose': {'position': [new_pos[0],new_pos[1],new_pos[2]],
'orientation': [q.x, q.y, q.z, q.w]} }]
fill_message_args(self.next_pose, args)
def test_face_tf(self):
pub, msg_class = rostopic.create_publisher('/camera/face_locations',
'pi_face_tracker/Faces',
True)
msg = msg_class()
msg_str = """faces:
- id: 1
point:
x: 0.76837966452
y: -0.132697071241
z: 0.0561996095957
attention: 0.990000009537
- id: 2
point:
x: 0.807249662369
y: -0.00428759906469
z: 0.035407830253
attention: 0.990000009537"""
fill_message_args(msg, [yaml.load(msg_str)])
pub.publish(msg)
msg = wait_for_message('/tf', tfMessage, 5)
expect_str = """
transforms:
-
header:
seq: 0
stamp:
nsecs: 752310037
frame_id: camera
child_frame_id: face_base1
transform:
translation:
x: 0.76837966452
y: -0.132697071241
z: 0.0561996095957
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
"""
expect = tfMessage()
fill_message_args(expect, [yaml.load(expect_str)])
#self.assertEqual(msg.transforms[0].child_frame_id,
# expect.transforms[0].child_frame_id)
self.assertEqual(msg.transforms[0].transform,
expect.transforms[0].transform)
pub.unregister()
def test_fill_message_args_embed_time(self):
from genpy import Time, Duration
from genpy.message import fill_message_args
from genpy.msg import TestFillEmbedTime
# test fill_message_args with embeds and time vals
# time t
# duration d
# std_msgs/String str_msg
# std_msgs/String[] str_msg_array
# int32 i32
tests = [
]
m = TestFillEmbedTime()
fill_message_args(m, [{}])
self.assertEquals(m.t, Time())
self.assertEquals(m.d, Duration())
self.assertEquals(m.str_msg.data, '')
self.assertEquals(m.str_msg_array, [])
self.assertEquals(m.i32, 0)
# list tests
# - these should be equivalent
equiv = [
[[10, 20], [30, 40], ['foo'], [['bar'], ['baz']], 32],
[{'secs': 10, 'nsecs': 20}, {'secs': 30, 'nsecs': 40}, ['foo'], [['bar'], ['baz']], 32],
[[10, 20], [30, 40], {'data': 'foo'}, [['bar'], ['baz']], 32],
[[10, 20], [30, 40], ['foo'], [{'data': 'bar'}, {'data': 'baz'}], 32],
[{'t': [10, 20], 'd': [30, 40], 'str_msg': {'data': 'foo'}, 'str_msg_array': [{'data': 'bar'}, {'data': 'baz'}], 'i32': 32}],
[{'t': {'secs': 10, 'nsecs': 20}, 'd': [30, 40], 'str_msg': {'data': 'foo'}, 'str_msg_array': [{'data': 'bar'}, {'data': 'baz'}], 'i32': 32}],
]
for test in equiv:
m = TestFillEmbedTime()
try:
fill_message_args(m, test)
except Exception, e:
self.fail("failed to fill with : %s\n%s"%(str(test), traceback.format_exc()))
self.assertEquals(m.t, Time(10, 20))
self.assertEquals(m.d, Duration(30, 40))
self.assertEquals(m.str_msg.data, 'foo')
self.assertEquals(len(m.str_msg_array), 2, m.str_msg_array)
self.assertEquals(m.str_msg_array[0].data, 'bar')
self.assertEquals(m.str_msg_array[1].data, 'baz')
self.assertEquals(m.i32, 32)
def yaml_to_msgs(filename):
"""
Given a filename of a YAML file with a list
of ExecuteCommandGoals return the messages filled.
"""
f = open(filename, 'r')
yaml_doc = yaml.load(f.read())
msgs = []
for msg_dict in yaml_doc:
if not type(msg_dict) == dict:
raise ValueError("yaml file " + filename +
" does not contain a list of dictionaries")
m = ExecuteCommandGoal()
fill_message_args(m, msg_dict)
msgs.append(m)
return msgs
def test_face_tf(self):
pub, msg_class = rostopic.create_publisher(
'/camera/face_locations', 'pi_face_tracker/Faces', True)
msg = msg_class()
msg_str = """faces:
- id: 1
point:
x: 0.76837966452
y: -0.132697071241
z: 0.0561996095957
attention: 0.990000009537
- id: 2
point:
x: 0.807249662369
y: -0.00428759906469
z: 0.035407830253
attention: 0.990000009537"""
fill_message_args(msg, [yaml.load(msg_str)])
pub.publish(msg)
msg = wait_for_message('/tf', tfMessage, 5)
expect_str = """
transforms:
-
header:
seq: 0
stamp:
nsecs: 752310037
frame_id: camera
child_frame_id: face_base1
transform:
translation:
x: 0.76837966452
y: -0.132697071241
z: 0.0561996095957
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
"""
expect = tfMessage()
fill_message_args(expect, [yaml.load(expect_str)])
#self.assertEqual(msg.transforms[0].child_frame_id,
# expect.transforms[0].child_frame_id)
self.assertEqual(msg.transforms[0].transform,
expect.transforms[0].transform)
pub.unregister()
def from_dict_to_ROS(dict_msg, ros_message_type, srv=False):
"""
Converts from a dict representation of a ROS message to a
ROS message instance.
"""
msg_class = get_ROS_class(ros_message_type, srv=srv)
msg_instance = msg_class()
# Workaround
if len(dict_msg) == 1:
dict_msg = [dict_msg]
fill_message_args(msg_instance, dict_msg)
# mfc: if msg contains a frame_id with an unicode frame_id we will have an exception...
if hasattr(msg_instance, 'header'):
msg_instance.header.frame_id = msg_instance.header.frame_id.encode(
"ascii")
return msg_instance
def euler_angle_2_quaternion(R, P, Y):
cr = cos(R * 0.5)
sr = sin(R * 0.5)
cp = cos(P * 0.5)
sp = sin(P * 0.5)
cy = cos(Y * 0.5)
sy = sin(Y * 0.5)
w = cy * cr * cp + sy * sr * sp
x = cy * sr * cp - sy * cr * sp
y = cy * cr * sp + sy * sr * cp
z = sy * cr * cp - cy * sr * sp
q = Quaternion()
fill_message_args(q, [x, y, z, w])
return q
def perform_yaw(ps, yaw):
q = euler_angle_2_quaternion(0, 0, yaw)
delta_pose = PoseStamped()
args = [{
'pose': {
'position': [0, 0, 2],
'orientation': [q.x, q.y, q.z, q.w]
}
}]
fill_message_args(delta_pose, args)
pose = PoseStamped()
q = quaternion_mult(ps.pose.orientation, delta_pose.pose.orientation)
# args = [{'pose': {'position': [ps.pose.position.x, ps.pose.position.y, ps.pose.position.z], 'orientation': [q.x, q.y, q.z, q.w]}}]
# fill_message_args(pose, args)
return q
def main():
rospy.init_node('test_ros_trajectory_forwarder')
client = actionlib.SimpleActionClient('follow_joint_trajectory',
FollowJointTrajectoryAction)
client.wait_for_server()
print("client connected")
rospack = rospkg.RosPack()
arm_robots_path = pathlib.Path(rospack.get_path('arm_robots'))
yaml_filename = arm_robots_path / "src/arm_robots/tests/test_trajectory1.yaml"
with yaml_filename.open('r') as f:
msg_data = yaml.safe_load(f)
msg = FollowJointTrajectoryGoal()
fill_message_args(msg, msg_data)
res = client.send_goal_and_wait(msg)
print(res)
def connect(self):
yaw = 0 * pi / 180
roll = 0 * pi / 180
pitch = 0 * pi / 180
q = euler_angle_2_quaternion(roll, pitch, yaw)
print("Quaternion: {}".format(q))
self.next_pose = PoseStamped()
args = [{'pose': {'position': [0, 0, 2], 'orientation': [q.x, q.y, q.z, q.w]}}]
fill_message_args(self.next_pose, args)
while not rospy.is_shutdown() and not self.current_state.connected:
self.rate.sleep()
# you must publish set points to the drone before you can change the mode
for i in range(1, 10):
self.pub.publish(self.next_pose)
self.rate.sleep()
self.set_mode_client(custom_mode="OFFBOARD")
self.arming_client(value=True)
def __init__(self, gps_ref, map_origin_frame):
rospy.init_node("satellite_map_gps_publisher", anonymous=True)
self.pub = rospy.Publisher("/sat_map_gps", NavSatFix, queue_size=10)
self.msg = NavSatFix()
fill_message_args(self.msg, next(file_yaml_arg(gps_ref)()))
self.llaRef = (self.msg.latitude, self.msg.longitude,
self.msg.altitude)
self.map_origin_frame = map_origin_frame
self.tfBuffer = tf2_ros.Buffer()
self.tfListener = tf2_ros.TransformListener(self.tfBuffer)
self.tfBroadcaster = tf2_ros.StaticTransformBroadcaster()
map_origin_tf = self.tfBuffer.lookup_transform("world",
map_origin_frame,
rospy.Time(0),
rospy.Duration(5.0))
map_origin_tf.transform.translation = Vector3()
map_origin_tf.transform.rotation.x = -map_origin_tf.transform.rotation.x
map_origin_tf.transform.rotation.y = -map_origin_tf.transform.rotation.y
map_origin_tf.transform.rotation.z = -map_origin_tf.transform.rotation.z
map_origin_tf.header.frame_id = map_origin_frame
map_origin_tf.header.stamp = rospy.Time.now()
map_origin_tf.child_frame_id = "sat_map_origin"
self.tfBroadcaster.sendTransform(map_origin_tf)
def test_fill_message_args_fixed_size_3785(self):
# mainly regression tests for #3785: filling fixed-sized arrays
from genpy.message import fill_message_args
from genpy import Message, MessageException
from genpy.msg import TestPrimitiveArray
class M1(Message):
__slots__ = ['int']
_slot_types=['int32']
def __init__(self, int_=None):
if int_ is None:
int_ = 0
self.int = int_
msg = M1()
fill_message_args(msg, [{'int': 16}])
assert msg == M1(16)
fill_message_args(msg, [17])
assert msg == M1(17)
msg = M1()
fill_message_args(msg, [{}])
assert msg == M1()
msg = TestPrimitiveArray()
fill_message_args(msg, [{}])
assert msg == TestPrimitiveArray()
msg = TestPrimitiveArray()
# - test fill with non-fixed size
fill_message_args(msg, [{'ints': [1, 2, 3, 4], 'strings': ['a', 'b', 'c']}])
assert msg == TestPrimitiveArray(ints=[1, 2, 3, 4], strings=['a', 'b', 'c'])
# - test fill with fixed size
msg = TestPrimitiveArray()
fill_message_args(msg, [{'fixed_ints': [1, 2, 3, 4], 'fixed_strings': ['a', 'b', 'c', 'd']}])
assert msg == TestPrimitiveArray(fixed_ints=[1, 2, 3, 4], fixed_strings=['a', 'b', 'c', 'd'])
# - underflow
try:
fill_message_args(msg, [{'fixed_ints': [1, 2, 3], 'fixed_strings': ['a', 'b', 'c', 'd']}])
assert False, "should have failed"
except MessageException: pass
try:
fill_message_args(msg, [{'fixed_ints': [1, 2, 3, 4], 'fixed_strings': ['a', 'b', 'c']}])
assert False, "should have failed"
except MessageException: pass
# - overflow
try:
fill_message_args(msg, [{'fixed_ints': [1, 2, 3, 4, 5], 'fixed_strings': ['a', 'b', 'c', 'd']}])
assert False, "should have failed"
except MessageException: pass
try:
fill_message_args(msg, [{'fixed_ints': [1, 2, 3, 4], 'fixed_strings': ['a', 'b', 'c', 'd', 'e']}])
assert False, "should have failed"
except MessageException: pass
# test fixed-size message arrays
from genpy.msg import TestMsgArray, TestString
msg = TestMsgArray()
fill_message_args(msg, [{}])
assert msg == TestMsgArray()
msg = TestMsgArray()
assert len(msg.strings) == 0
assert len(msg.fixed_strings) == 1
# - test fill with non-fixed size
fill_message_args(msg, [{'strings': [['a'], ['b']]}])
assert msg == TestMsgArray(strings=[TestString('a'), TestString('b')])
# - test fill with fixed size
msg = TestMsgArray()
fill_message_args(msg, [{'fixed_strings': [['a'],]}])
assert msg == TestMsgArray(fixed_strings=[TestString('a')])
# overflow
msg = TestMsgArray()
try:
fill_message_args(msg, [{'fixed_strings': [['a'], ['b']]}])
assert False, "should have failed"
except MessageException: pass
# underflow
msg = TestMsgArray()
try:
fill_message_args(msg, [{'fixed_strings': []}])
assert False, "should have failed"
except MessageException: pass
def test_fill_message_args_simple(self):
from genpy.message import fill_message_args
from genpy.msg import TestFillSimple
#int32 i32
#string str
#int32[] i32_array
#bool b
for v in [[], {}]:
try:
fill_message_args(object(), v)
self.fail("should have raised ValueError")
except ValueError: pass
try:
m = TestFillSimple()
# call underlying routine as the branch is not reachable from above
genpy.message._fill_message_args(m, 1, {}, '')
self.fail("should have raised ValueError for bad msg_args")
except ValueError: pass
simple_tests = [
[1, 'foo', [], True],
[1, 'foo', [1, 2, 3, 4], False],
]
for test in simple_tests:
m = TestFillSimple()
fill_message_args(m, test)
self.assertEquals(m.i32, test[0])
self.assertEquals(m.str, test[1])
self.assertEquals(m.i32_array, test[2])
self.assertEquals(m.b, test[3])
# test with dictionaries
m = TestFillSimple()
fill_message_args(m, [{}])
self.assertEquals(m.i32, 0)
self.assertEquals(m.str, '')
self.assertEquals(m.i32_array, [])
self.assertEquals(m.b, False)
m = TestFillSimple()
fill_message_args(m, [{'i32': 10}])
self.assertEquals(m.i32, 10)
self.assertEquals(m.str, '')
self.assertEquals(m.i32_array, [])
self.assertEquals(m.b, False)
m = TestFillSimple()
fill_message_args(m, [{'str': 'hello', 'i32_array': [1, 2, 3]}])
self.assertEquals(m.i32, 0)
self.assertEquals(m.str, 'hello')
self.assertEquals(m.i32_array, [1, 2, 3])
self.assertEquals(m.b, False)
# fill_message_args currently does not type check
bad = [
# extra key
[{'bad': 1, 'str': 'hello', 'i32_array': [1, 2, 3]}],
# underfill
[1, 'foo', [1, 2, 3]],
# overfill
[1, 'foo', [1, 2, 3], True, 1],
# non-list value for list field
[1, 'foo', 1, True],
]
for b in bad:
failed = True
try:
m = TestFillSimple()
fill_message_args(m, b)
except genpy.MessageException:
failed = False
self.failIf(failed, "fill_message_args should have failed: %s"%str(b))
def test_fill_message_args_embed_time(self):
from genpy import Time
from genpy.message import fill_message_args
from genpy.msg import TestFillEmbedTime
# test fill_message_args with embeds and time vals
# time t
# duration d
# std_msgs/String str_msg
# std_msgs/String[] str_msg_array
# int32 i32
m = TestFillEmbedTime()
fill_message_args(m, [{}])
self.assertEquals(m.t, Time())
self.assertEquals(m.d, Duration())
self.assertEquals(m.str_msg.data, '')
self.assertEquals(m.str_msg_array, [])
self.assertEquals(m.i32, 0)
# list tests
# - these should be equivalent
equiv = [
[[10, 20], [30, 40], ['foo'], [['bar'], ['baz']], 32],
[{'secs': 10, 'nsecs': 20}, {'secs': 30, 'nsecs': 40}, ['foo'], [['bar'], ['baz']], 32],
[[10, 20], [30, 40], {'data': 'foo'}, [['bar'], ['baz']], 32],
[[10, 20], [30, 40], ['foo'], [{'data': 'bar'}, {'data': 'baz'}], 32],
[{'t': [10, 20], 'd': [30, 40], 'str_msg': {'data': 'foo'}, 'str_msg_array': [{'data': 'bar'}, {'data': 'baz'}], 'i32': 32}],
[{'t': {'secs': 10, 'nsecs': 20}, 'd': [30, 40], 'str_msg': {'data': 'foo'}, 'str_msg_array': [{'data': 'bar'}, {'data': 'baz'}], 'i32': 32}],
]
for test in equiv:
m = TestFillEmbedTime()
try:
fill_message_args(m, test)
except Exception as e:
self.fail("failed to fill with : %s\n%s"%(str(test), traceback.format_exc()))
self.assertEquals(m.t, Time(10, 20))
self.assertEquals(m.d, Duration(30, 40))
self.assertEquals(m.str_msg.data, 'foo')
self.assertEquals(len(m.str_msg_array), 2, m.str_msg_array)
self.assertEquals(m.str_msg_array[0].data, 'bar')
self.assertEquals(m.str_msg_array[1].data, 'baz')
self.assertEquals(m.i32, 32)
# test creation of Time/Duration from single number representation, which is necessary for
# yaml single-number support
# - cannot include in tests above as conversion from integer is lossy
m = TestFillEmbedTime()
fill_message_args(m, [10000000020, 30000000040, ['foo'], [['bar'], ['baz']], 32])
self.assertEquals(10, m.t.secs)
self.assert_(abs(20 - m.t.nsecs) < 2)
self.assertEquals(30, m.d.secs)
self.assert_(abs(40 - m.d.nsecs) < 2)
self.assertEquals(len(m.str_msg_array), 2, m.str_msg_array)
self.assertEquals(m.str_msg_array[0].data, 'bar')
self.assertEquals(m.str_msg_array[1].data, 'baz')
self.assertEquals(m.i32, 32)
bad = [
# underfill in sub-args
[[10, 20], [30, 40], ['foo'], [['bar'], ['baz']]],
[[10], [30, 40], ['foo'], [['bar'], ['baz']], 32],
[[10, 20], [30], ['foo'], [['bar'], ['baz']], 32],
[[10, 20], [30, 40], [], [['bar'], ['baz']], 32],
[[10, 20], [30, 40], ['foo'], [['bar'], []], 32],
# overfill
[[10, 20], [30, 40], ['foo'], [['bar'], ['baz']], 32, 64],
[[10, 20, 30], [30, 40], ['foo'], [['bar'], ['baz']], 32],
[[10, 20], [30, 40, 50], ['foo'], [['bar'], ['baz']], 32],
[[10, 20], [30, 40], ['foo', 'bar'], [['bar'], ['baz']], 32],
[[10, 20], [30, 40], ['foo'], [['bar', 'baz'], ['baz']], 32],
[[10, 20], [30, 40], ['foo'], [['bar'], ['baz', 'car']], 32],
# invalid fields
[{'secs': 10, 'nsecs': 20, 'foo': 1}, {'secs': 30, 'nsecs': 40}, ['foo'], [['bar'], ['baz']], 32],
[{'secs': 10, 'nsecs': 20}, {'secs': 30, 'nsecs': 40, 'foo': 1}, ['foo'], [['bar'], ['baz']], 32],
[[10, 20], [30, 40], {'data': 'foo', 'fata': 1}, [['bar'], ['baz']], 32],
[[10, 20], [30, 40], ['foo'], [{'data': 'bar'}, {'beta': 'baz'}], 32],
[{'t': [10, 20], 'd': [30, 40], 'str_msg': {'data': 'foo'}, 'str_msg_array': [{'data': 'bar'}, {'data': 'baz'}], 'i32': 32, 'i64': 64}],
]
for b in bad:
failed = True
try:
m = TestFillEmbedTime()
fill_message_args(m, b)
except genpy.MessageException:
failed = False
self.failIf(failed, "fill_message_args should have failed: %s"%str(b))
class MessageTest(unittest.TestCase):
def xtest_check_types_Header(self):
# #2128: test that check_types works with a Header
# #message. This is a weird case because Header has an aliased
# #type and is the only ROS type for which that is true
from test_roslib_comm.msg import HeaderTest
x = HeaderTest()
x._check_types()
def test_Message_check_types(self):
# test on a generated message
# - use UInt16MultiArray because it has an embedded MultiArrayLayout
from std_msgs.msg import String, UInt16MultiArray, MultiArrayLayout, MultiArrayDimension
from genpy import SerializationError
# not checking overflow in this test
correct = [
String(),
String('foo'),
String(''),
String(data='data'),
UInt16MultiArray(),
UInt16MultiArray(MultiArrayLayout(), []),
UInt16MultiArray(MultiArrayLayout(data_offset=1), [1, 2, 3]),
UInt16MultiArray(layout=MultiArrayLayout(data_offset=1)),
UInt16MultiArray(layout=MultiArrayLayout(dim=[])),
UInt16MultiArray(layout=MultiArrayLayout(
dim=[MultiArrayDimension()])),
UInt16MultiArray(data=[1, 2, 3]),
]
for t in correct:
t._check_types()
for t in correct:
try:
t._check_types(exc=Exception())
self.fail("should have raised wrapped exc")
except SerializationError:
pass
wrong = [
String(1),
String(data=1),
UInt16MultiArray(1, []),
UInt16MultiArray(MultiArrayLayout(), 1),
UInt16MultiArray(String(), []),
UInt16MultiArray(layout=MultiArrayLayout(dim=[1])),
UInt16MultiArray(layout=MultiArrayLayout(data_offset='')),
]
for t in wrong:
try:
t._check_types()
self.fail("should have raised")
except SerializationError:
pass
def test_fill_message_args_fixed_size_3785(self):
# mainly regression tests for #3785: filling fixed-sized arrays
from genpy.message import fill_message_args
from genpy import Message, MessageException
from genpy.msg import TestPrimitiveArray
class M1(Message):
__slots__ = ['int']
_slot_types = ['int32']
def __init__(self, int_=None):
if int_ is None:
int_ = 0
self.int = int_
msg = M1()
fill_message_args(msg, [{'int': 16}])
assert msg == M1(16)
fill_message_args(msg, [17])
assert msg == M1(17)
msg = M1()
fill_message_args(msg, [{}])
assert msg == M1()
from genpy.msg import TestPrimitiveArray
msg = TestPrimitiveArray()
fill_message_args(msg, [{}])
assert msg == TestPrimitiveArray()
msg = TestPrimitiveArray()
# - test fill with non-fixed size
fill_message_args(msg, [{
'ints': [1, 2, 3, 4],
'strings': ['a', 'b', 'c']
}])
assert msg == TestPrimitiveArray(ints=[1, 2, 3, 4],
strings=['a', 'b', 'c'])
# - test fill with fixed size
msg = TestPrimitiveArray()
fill_message_args(msg, [{
'fixed_ints': [1, 2, 3, 4],
'fixed_strings': ['a', 'b', 'c', 'd']
}])
assert msg == TestPrimitiveArray(fixed_ints=[1, 2, 3, 4],
fixed_strings=['a', 'b', 'c', 'd'])
# - underflow
try:
fill_message_args(msg, [{
'fixed_ints': [1, 2, 3],
'fixed_strings': ['a', 'b', 'c', 'd']
}])
assert False, "should have failed"
except MessageException:
pass
try:
fill_message_args(msg, [{
'fixed_ints': [1, 2, 3, 4],
'fixed_strings': ['a', 'b', 'c']
}])
assert False, "should have failed"
except MessageException:
pass
# - overflow
try:
fill_message_args(msg, [{
'fixed_ints': [1, 2, 3, 4, 5],
'fixed_strings': ['a', 'b', 'c', 'd']
}])
assert False, "should have failed"
except MessageException:
pass
try:
fill_message_args(msg, [{
'fixed_ints': [1, 2, 3, 4],
'fixed_strings': ['a', 'b', 'c', 'd', 'e']
}])
assert False, "should have failed"
except MessageException:
pass
# test fixed-size message arrays
from genpy.msg import TestMsgArray, TestString
msg = TestMsgArray()
fill_message_args(msg, [{}])
assert msg == TestMsgArray()
msg = TestMsgArray()
assert len(msg.strings) == 0
assert len(msg.fixed_strings) == 1
# - test fill with non-fixed size
fill_message_args(msg, [{'strings': [['a'], ['b']]}])
assert msg == TestMsgArray(strings=[TestString('a'), TestString('b')])
# - test fill with fixed size
msg = TestMsgArray()
fill_message_args(msg, [{
'fixed_strings': [
['a'],
]
}])
assert msg == TestMsgArray(fixed_strings=[TestString('a')])
# overflow
msg = TestMsgArray()
try:
fill_message_args(msg, [{'fixed_strings': [['a'], ['b']]}])
assert False, "should have failed"
except MessageException:
pass
# underflow
msg = TestMsgArray()
try:
fill_message_args(msg, [{'fixed_strings': []}])
assert False, "should have failed"
except MessageException:
pass
def test_fill_message_args_embed_time(self):
from genpy import Time, Duration
from genpy.message import fill_message_args
from genpy.msg import TestFillEmbedTime
# test fill_message_args with embeds and time vals
# time t
# duration d
# std_msgs/String str_msg
# std_msgs/String[] str_msg_array
# int32 i32
tests = []
m = TestFillEmbedTime()
fill_message_args(m, [{}])
self.assertEquals(m.t, Time())
self.assertEquals(m.d, Duration())
self.assertEquals(m.str_msg.data, '')
self.assertEquals(m.str_msg_array, [])
self.assertEquals(m.i32, 0)
# list tests
# - these should be equivalent
equiv = [
[[10, 20], [30, 40], ['foo'], [['bar'], ['baz']], 32],
[{
'secs': 10,
'nsecs': 20
}, {
'secs': 30,
'nsecs': 40
}, ['foo'], [['bar'], ['baz']], 32],
[[10, 20], [30, 40], {
'data': 'foo'
}, [['bar'], ['baz']], 32],
[[10, 20], [30, 40], ['foo'], [{
'data': 'bar'
}, {
'data': 'baz'
}], 32],
[{
't': [10, 20],
'd': [30, 40],
'str_msg': {
'data': 'foo'
},
'str_msg_array': [{
'data': 'bar'
}, {
'data': 'baz'
}],
'i32': 32
}],
[{
't': {
'secs': 10,
'nsecs': 20
},
'd': [30, 40],
'str_msg': {
'data': 'foo'
},
'str_msg_array': [{
'data': 'bar'
}, {
'data': 'baz'
}],
'i32': 32
}],
]
for test in equiv:
m = TestFillEmbedTime()
try:
fill_message_args(m, test)
except Exception, e:
self.fail("failed to fill with : %s\n%s" %
(str(test), traceback.format_exc()))
self.assertEquals(m.t, Time(10, 20))
self.assertEquals(m.d, Duration(30, 40))
self.assertEquals(m.str_msg.data, 'foo')
self.assertEquals(len(m.str_msg_array), 2, m.str_msg_array)
self.assertEquals(m.str_msg_array[0].data, 'bar')
self.assertEquals(m.str_msg_array[1].data, 'baz')
self.assertEquals(m.i32, 32)
# test creation of Time/Duration from single number representation, which is necessary for
# yaml single-number support
# - cannot include in tests above as conversion from integer is lossy
m = TestFillEmbedTime()
fill_message_args(
m, [10000000020, 30000000040, ['foo'], [['bar'], ['baz']], 32])
self.assertEquals(10, m.t.secs)
self.assert_(abs(20 - m.t.nsecs) < 2)
self.assertEquals(30, m.d.secs)
self.assert_(abs(40 - m.d.nsecs) < 2)
self.assertEquals(len(m.str_msg_array), 2, m.str_msg_array)
self.assertEquals(m.str_msg_array[0].data, 'bar')
self.assertEquals(m.str_msg_array[1].data, 'baz')
self.assertEquals(m.i32, 32)
bad = [
# underfill in sub-args
[[10, 20], [30, 40], ['foo'], [['bar'], ['baz']]],
[[10], [30, 40], ['foo'], [['bar'], ['baz']], 32],
[[10, 20], [30], ['foo'], [['bar'], ['baz']], 32],
[[10, 20], [30, 40], [], [['bar'], ['baz']], 32],
[[10, 20], [30, 40], ['foo'], [['bar'], []], 32],
# overfill
[[10, 20], [30, 40], ['foo'], [['bar'], ['baz']], 32, 64],
[[10, 20, 30], [30, 40], ['foo'], [['bar'], ['baz']], 32],
[[10, 20], [30, 40, 50], ['foo'], [['bar'], ['baz']], 32],
[[10, 20], [30, 40], ['foo', 'bar'], [['bar'], ['baz']], 32],
[[10, 20], [30, 40], ['foo'], [['bar', 'baz'], ['baz']], 32],
[[10, 20], [30, 40], ['foo'], [['bar'], ['baz', 'car']], 32],
# invalid fields
[{
'secs': 10,
'nsecs': 20,
'foo': 1
}, {
'secs': 30,
'nsecs': 40
}, ['foo'], [['bar'], ['baz']], 32],
[{
'secs': 10,
'nsecs': 20
}, {
'secs': 30,
'nsecs': 40,
'foo': 1
}, ['foo'], [['bar'], ['baz']], 32],
[[10, 20], [30, 40], {
'data': 'foo',
'fata': 1
}, [['bar'], ['baz']], 32],
[[10, 20], [30, 40], ['foo'], [{
'data': 'bar'
}, {
'beta': 'baz'
}], 32],
[{
't': [10, 20],
'd': [30, 40],
'str_msg': {
'data': 'foo'
},
'str_msg_array': [{
'data': 'bar'
}, {
'data': 'baz'
}],
'i32': 32,
'i64': 64
}],
]
for b in bad:
failed = True
try:
m = TestFillEmbedTime()
fill_message_args(m, b)
except genpy.MessageException:
failed = False
self.failIf(failed,
"fill_message_args should have failed: %s" % str(b))
def test_fill_message_args_embed_time(self):
from genpy import Time, Duration
from genpy.message import fill_message_args
from genpy.msg import TestFillEmbedTime
# test fill_message_args with embeds and time vals
# time t
# duration d
# std_msgs/String str_msg
# std_msgs/String[] str_msg_array
# int32 i32
tests = []
m = TestFillEmbedTime()
fill_message_args(m, [{}])
self.assertEquals(m.t, Time())
self.assertEquals(m.d, Duration())
self.assertEquals(m.str_msg.data, '')
self.assertEquals(m.str_msg_array, [])
self.assertEquals(m.i32, 0)
# list tests
# - these should be equivalent
equiv = [
[[10, 20], [30, 40], ['foo'], [['bar'], ['baz']], 32],
[{
'secs': 10,
'nsecs': 20
}, {
'secs': 30,
'nsecs': 40
}, ['foo'], [['bar'], ['baz']], 32],
[[10, 20], [30, 40], {
'data': 'foo'
}, [['bar'], ['baz']], 32],
[[10, 20], [30, 40], ['foo'], [{
'data': 'bar'
}, {
'data': 'baz'
}], 32],
[{
't': [10, 20],
'd': [30, 40],
'str_msg': {
'data': 'foo'
},
'str_msg_array': [{
'data': 'bar'
}, {
'data': 'baz'
}],
'i32': 32
}],
[{
't': {
'secs': 10,
'nsecs': 20
},
'd': [30, 40],
'str_msg': {
'data': 'foo'
},
'str_msg_array': [{
'data': 'bar'
}, {
'data': 'baz'
}],
'i32': 32
}],
]
for test in equiv:
m = TestFillEmbedTime()
try:
fill_message_args(m, test)
except Exception, e:
self.fail("failed to fill with : %s\n%s" %
(str(test), traceback.format_exc()))
self.assertEquals(m.t, Time(10, 20))
self.assertEquals(m.d, Duration(30, 40))
self.assertEquals(m.str_msg.data, 'foo')
self.assertEquals(len(m.str_msg_array), 2, m.str_msg_array)
self.assertEquals(m.str_msg_array[0].data, 'bar')
self.assertEquals(m.str_msg_array[1].data, 'baz')
self.assertEquals(m.i32, 32)
def test_fill_message_args_fixed_size_3785(self):
# mainly regression tests for #3785: filling fixed-sized arrays
from genpy.message import fill_message_args
from genpy import Message, MessageException
from genpy.msg import TestPrimitiveArray
class M1(Message):
__slots__ = ["int"]
_slot_types = ["int32"]
def __init__(self, int_=None):
if int_ is None:
int_ = 0
self.int = int_
msg = M1()
fill_message_args(msg, [{"int": 16}])
assert msg == M1(16)
fill_message_args(msg, [17])
assert msg == M1(17)
msg = M1()
fill_message_args(msg, [{}])
assert msg == M1()
msg = TestPrimitiveArray()
fill_message_args(msg, [{}])
assert msg == TestPrimitiveArray()
msg = TestPrimitiveArray()
# - test fill with non-fixed size
fill_message_args(msg, [{"ints": [1, 2, 3, 4], "strings": ["a", "b", "c"]}])
assert msg == TestPrimitiveArray(ints=[1, 2, 3, 4], strings=["a", "b", "c"])
# - test fill with fixed size
msg = TestPrimitiveArray()
fill_message_args(msg, [{"fixed_ints": [1, 2, 3, 4], "fixed_strings": ["a", "b", "c", "d"]}])
assert msg == TestPrimitiveArray(fixed_ints=[1, 2, 3, 4], fixed_strings=["a", "b", "c", "d"])
# - underflow
try:
fill_message_args(msg, [{"fixed_ints": [1, 2, 3], "fixed_strings": ["a", "b", "c", "d"]}])
assert False, "should have failed"
except MessageException:
pass
try:
fill_message_args(msg, [{"fixed_ints": [1, 2, 3, 4], "fixed_strings": ["a", "b", "c"]}])
assert False, "should have failed"
except MessageException:
pass
# - overflow
try:
fill_message_args(msg, [{"fixed_ints": [1, 2, 3, 4, 5], "fixed_strings": ["a", "b", "c", "d"]}])
assert False, "should have failed"
except MessageException:
pass
try:
fill_message_args(msg, [{"fixed_ints": [1, 2, 3, 4], "fixed_strings": ["a", "b", "c", "d", "e"]}])
assert False, "should have failed"
except MessageException:
pass
# test fixed-size message arrays
from genpy.msg import TestMsgArray, TestString
msg = TestMsgArray()
fill_message_args(msg, [{}])
assert msg == TestMsgArray()
msg = TestMsgArray()
assert len(msg.strings) == 0
assert len(msg.fixed_strings) == 1
# - test fill with non-fixed size
fill_message_args(msg, [{"strings": [["a"], ["b"]]}])
assert msg == TestMsgArray(strings=[TestString("a"), TestString("b")])
# - test fill with fixed size
msg = TestMsgArray()
fill_message_args(msg, [{"fixed_strings": [["a"]]}])
assert msg == TestMsgArray(fixed_strings=[TestString("a")])
# overflow
msg = TestMsgArray()
try:
fill_message_args(msg, [{"fixed_strings": [["a"], ["b"]]}])
assert False, "should have failed"
except MessageException:
pass
# underflow
msg = TestMsgArray()
try:
fill_message_args(msg, [{"fixed_strings": []}])
assert False, "should have failed"
except MessageException:
pass
