def test_check_type(self):
# check_type() currently does not do harder checks like
# type-checking class types. as soon as it does, it will need
# test to validate this.
from genpy.message import check_type, SerializationError
from genpy import Time, Duration
valids = [
('byte', 1),
('byte', -1),
('string', ''),
('string', 'a string of text'),
('int32[]', []),
('int32[]', [1, 2, 3, 4]),
('time', Time()),
('time', Time.from_sec(1.0)),
('time', Time(10000)),
('time', Time(1000, -100)),
('duration', Duration()),
('duration', Duration()),
('duration', Duration(100)),
('duration', Duration(-100, -100)),
]
for t, v in valids:
try:
check_type('n', t, v)
except Exception, e:
traceback.print_exc()
raise Exception("failure type[%s] value[%s]: %s" %
(t, v, str(e)))
def test_check_type(self):
# check_type() currently does not do harder checks like
# type-checking class types. as soon as it does, it will need
# test to validate this.
from genpy.message import check_type, SerializationError
from genpy import Time, Duration
valids = [
('byte', 1),
('byte', -1),
('string', ''),
('string', 'a string of text'),
('int32[]', []),
('int32[]', [1, 2, 3, 4]),
('time', Time()),
('time', Time.from_sec(1.0)),
('time', Time(10000)),
('time', Time(1000, -100)),
('duration', Duration()),
('duration', Duration()),
('duration', Duration(100)),
('duration', Duration(-100, -100)),
]
for t, v in valids:
try:
check_type('n', t, v)
except Exception as e:
traceback.print_exc()
raise Exception('failure type[%s] value[%s]: %s' %
(t, v, str(e)))
invalids = [
('byte', 129),
('byte', -129),
('byte', 'byte'),
('byte', 1.0),
('string', 1),
('uint32', -1),
('int8', 112312),
('int8', -112312),
('uint8', -1),
('uint8', 112312),
('int32', '1'),
('int32', 1.),
('int32[]', 1),
('int32[]', [1., 2.]),
('int32[]', [1, 2.]),
('duration', 1),
('time', 1),
]
for t, v in invalids:
try:
check_type('n', t, v)
self.fail('check_type[%s, %s] should have failed' % (t, v))
except SerializationError:
pass
def test_check_types_valid(self):
'''Test directly a bunch of valid combinations to check_types.
check_type will throw an exception when it fails
'''
genpy.message.check_type('test', 'uint8[]', 'byteDataIsAStringInPy')
genpy.message.check_type('test', 'char[]', 'byteDataIsAStringInPy')
genpy.message.check_type('test', 'uint8[]', [3,4,5])
genpy.message.check_type('test', 'uint8[]', (3,4,5))
genpy.message.check_type('test', 'char[]', [3,4,5])
genpy.message.check_type('test', 'int32[]', [3,4,5])
genpy.message.check_type('test', 'int32', -5)
genpy.message.check_type('test', 'int64', -5)
genpy.message.check_type('test', 'int16', -5)
genpy.message.check_type('test', 'int8', -5)
genpy.message.check_type('test', 'uint32', 5)
genpy.message.check_type('test', 'uint64', 5)
genpy.message.check_type('test', 'uint16', 5)
genpy.message.check_type('test', 'uint8', 5)
genpy.message.check_type('test', 'bool', True)
genpy.message.check_type('test', 'bool', False)
genpy.message.check_type('test', 'bool', 0)
genpy.message.check_type('test', 'bool', 1)
genpy.message.check_type('test', 'string', 'IAmAString')
genpy.message.check_type('test', 'time', Time())
genpy.message.check_type('test', 'duration', Duration(5))
def run(self, args):
# Publish a velocity of zero for a while to stabalize navigator
self.send_feedback('Switching trajectory to constant')
yield self.change_trajectory('constant')
yield self.nh.sleep(0.1)
done_zero = yield self.nh.get_time() + Duration(self.ZERO_TIME)
odom = yield self._odom_sub.get_next_message()
self.msg.header.stamp = odom.header.stamp
self.msg.pose = odom.pose
self.msg.twist.twist = Twist()
self.send_feedback('Sending Zero Velocity for {} seconds'.format(
self.ZERO_TIME))
while (yield self.nh.get_time()) < done_zero:
self.ref_pub.publish(self.msg)
yield self.nh.sleep(0.1)
# Publish user selected velocity until task is canceled or a new task is run
self.send_feedback(
'Publishing constant velocity {}. Cancel task to stop.'.format(
args))
self.msg.twist.twist.linear.x = args[0]
self.msg.twist.twist.linear.y = args[1]
self.msg.twist.twist.angular.z = args[2]
original_odom = yield self._odom_sub.get_last_message()
while True:
odom = yield self._odom_sub.get_next_message()
self.msg.header.stamp = odom.header.stamp
self.msg.pose = odom.pose
if args[0] == 0 and args[
1] == 0: # If only rotation, keep position same
self.msg.pose.pose.position = original_odom.pose.pose.position
elif args[2] == 0: # If only linear, keep orientation same
self.msg.pose.pose.orientation = original_odom.pose.pose.orientation
self.ref_pub.publish(self.msg)
def test_viseme(self):
available_visemes = parse_msg(
run_shell_cmd('rostopic echo -n1 /blender_api/available_visemes',
True))
pub, msg_class = rostopic.create_publisher('/blender_api/queue_viseme',
'blender_api_msgs/Viseme',
True)
timeout = 2
videos = []
for viseme in available_visemes:
if 'old' in viseme: continue
video = '%s/viseme-%s.avi' % (self.output_dir, viseme)
with capture_screen(video, timeout):
pub.publish(
msg_class(viseme, Duration(0, 0), Duration(0, 5 * 1e8),
0.1, 0.8, 1))
add_text_to_video(video)
videos.append(video)
ofile = '%s/viseme.avi' % self.output_dir
concatenate_videos(videos, ofile, True)
pub.unregister()
def cleanup(cls):
'''
When command is canceled, publish zero velocity for a while
so controller doesn't "run of the tracks" and keep going
'''
odom = yield cls._odom_sub.get_next_message()
cls.msg.pose = odom.pose
cls.msg.header.stamp = odom.header.stamp
cls.msg.twist.twist = Twist()
now = yield cls.nh.get_time()
done = now + Duration(cls.CLEANUP_TIME)
while now < done:
cls.msg.header.stamp = now
cls.ref_pub.publish(cls.msg)
now = yield cls.nh.get_time()
yield cls.nh.sleep(0.1)
def test_check_types_valid(self):
"""
Test directly a bunch of valid combinations to check_types.
check_type will throw an exception when it fails
"""
import numpy as np
genpy.message.check_type('test', 'uint8[]', 'byteDataIsAStringInPy')
genpy.message.check_type('test', 'char[]', 'byteDataIsAStringInPy')
genpy.message.check_type('test', 'uint8[]', [3, 4, 5])
genpy.message.check_type('test', 'uint8[]', (3, 4, 5))
genpy.message.check_type('test', 'char[]', [3, 4, 5])
genpy.message.check_type('test', 'int32[]', [3, 4, 5])
genpy.message.check_type('test', 'int32', -5)
genpy.message.check_type('test', 'int64', -5)
genpy.message.check_type('test', 'int16', -5)
genpy.message.check_type('test', 'int8', -5)
genpy.message.check_type('test', 'uint32', 5)
genpy.message.check_type('test', 'uint64', 5)
genpy.message.check_type('test', 'uint16', 5)
genpy.message.check_type('test', 'uint8', 5)
genpy.message.check_type('test', 'bool', True)
genpy.message.check_type('test', 'bool', False)
genpy.message.check_type('test', 'bool', 0)
genpy.message.check_type('test', 'bool', 1)
genpy.message.check_type('test', 'string', 'IAmAString')
genpy.message.check_type('test', 'time', Time())
genpy.message.check_type('test', 'duration', Duration(5))
genpy.message.check_type('test', 'float32', 5)
genpy.message.check_type('test', 'float32', 5.0)
genpy.message.check_type('test', 'float32', np.int16(5))
genpy.message.check_type('test', 'float32', np.float32(5.0))
genpy.message.check_type('test', 'float32', float('inf'))
genpy.message.check_type('test', 'float32', -float('inf'))
genpy.message.check_type('test', 'float32', float('nan'))
genpy.message.check_type('test', 'float32', -float('nan'))
genpy.message.check_type('test', 'float32',
2147483647) # resulting float: 2147483648.0
genpy.message.check_type('test', 'float64', 5.0)
genpy.message.check_type('test', 'float64',
1 + np.finfo(np.float64).eps)
genpy.message.check_type('test', 'float64', np.float64(5.0))
genpy.message.check_type('test', 'float64', np.int32(2147483647))
# smallest representable float larger than 1.0 in builtin float type (64 bits). conversion to float32 loses precision.
genpy.message.check_type('test', 'float32', float(1 + 2**-52))
def test_emotion_state(self):
available_emotions = parse_msg(
run_shell_cmd(
'rostopic echo -n1 /blender_api/available_emotion_states',
True))
pub, msg_class = rostopic.create_publisher(
'/blender_api/set_emotion_state', 'blender_api_msgs/EmotionState',
True)
timeout = 2
videos = []
for emotion in available_emotions:
video = '%s/emotion-%s.avi' % (self.output_dir, emotion)
with capture_screen(video, timeout):
pub.publish(msg_class(emotion, 1, Duration(1, 0)))
add_text_to_video(video)
videos.append(video)
ofile = '%s/emotions.avi' % self.output_dir
concatenate_videos(videos, ofile, True)
pub.unregister()
def test_strify_message(self):
# this is a bit overtuned, but it will catch regressions
from genpy.message import Message, strify_message
class M1(Message):
__slots__ = []
_slot_types = []
def __init__(self): pass
self.assertEquals('', strify_message(M1()))
class M2(Message):
__slots__ = ['str', 'int', 'float', 'bool', 'list']
_slot_types = ['string', 'int32', 'float32', 'bool', 'int32[]']
def __init__(self, str_, int_, float_, bool_, list_):
self.str = str_
self.int = int_
self.float = float_
self.bool = bool_
self.list = list_
self.assertEquals("""str: string
int: 123456789101112
float: 5678.0
bool: True
list: [1, 2, 3]""", strify_message(M2('string', 123456789101112, 5678., True, [1,2,3])))
self.assertEquals("""str: ''
int: -1
float: 0.0
bool: False
list: []""", strify_message(M2('', -1, 0., False, [])))
class M3(Message):
__slots__ = ['m2']
_slot_types=['M1']
def __init__(self, m2):
self.m2 = m2
self.assertEquals("""m2:
str: string
int: -1
float: 0.0
bool: False
list: []""", strify_message(M3(M2('string', -1, 0., False, []))))
# test array of Messages field
class M4(Message):
__slots__ = ['m2s']
_slot_types=['M2[]']
def __init__(self, m2s):
self.m2s = m2s
self.assertEquals("""m2s:
-
str: string
int: 1234
float: 5678.0
bool: True
list: [1, 2, 3]
-
str: string
int: -1
float: 0.0
bool: False
list: []""", strify_message(M4([
M2('string', 1234, 5678., True, [1,2,3]),
M2('string', -1, 0., False, []),
])))
# test Time and Duration
from genpy import Time, Duration
class M5(Message):
__slots__ = ['t', 'd']
_slot_types=['time', 'duration']
def __init__(self, t, d):
self.t = t
self.d = d
self.assertEquals("""t:
secs: 987
nsecs: 654
d:
secs: 123
nsecs: 456""", strify_message(M5(Time(987, 654), Duration(123, 456))))
# test final clause of strify -- str anything that isn't recognized
if sys.hexversion > 0x03000000: # Python3
self.assertEquals("{1}", strify_message(set([1])))
else:
self.assertEquals("set([1])", strify_message(set([1])))
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))
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_sub_str_plot_fields(self):
from rostopic import _str_plot_fields
from std_msgs.msg import String
from test_rostopic.msg import Arrays, Embed, Simple, TVals
from genpy import Time, Duration
from rostopic import create_field_filter
# str plotting requires rospy time, we fix time to a set time
import rospy.rostime
rospy.rostime.set_rostime_initialized(True)
rospy.rostime._set_rostime(Time(0, 1234))
Time(0, 5678)
# prepare test values
simple_v = Simple(1, -2, 3, -4, 'a', 7, 8, 9, 'bar')
simple_d = 'time,field.b,field.int16,field.int32,field.int64,field.c,field.uint16,field.uint32,field.uint64,field.str'
simple_nostr = 'time,field.b,field.int16,field.int32,field.int64,field.c,field.uint16,field.uint32,field.uint64'
arrays_v = Arrays([-1],
chr(2) + chr(3), [3, 4, 5], [6, 7, 8],
['a1', 'b2', 'b3'],
[Time(123, 456), Time(78, 90)])
arrays_d = 'time,field.int8_arr0,field.uint8_arr0,field.uint8_arr1,field.int32_arr0,field.int32_arr1,field.int32_arr2,field.uint32_arr0,field.uint32_arr1,field.uint32_arr2,field.string_arr0,field.string_arr1,field.string_arr2,field.time_arr0,field.time_arr1'
arrays_nostr = 'time,field.int8_arr0,field.uint8_arr0,field.uint8_arr1,field.int32_arr0,field.int32_arr1,field.int32_arr2,field.uint32_arr0,field.uint32_arr1,field.uint32_arr2,field.time_arr0,field.time_arr1'
embed_v = Embed(simple_v, arrays_v)
embed_d = simple_d.replace('field.',
'field.simple.') + ',' + arrays_d.replace(
'field.', 'field.arrays.')[5:]
embed_nostr = simple_nostr.replace(
'field.', 'field.simple.') + ',' + arrays_nostr.replace(
'field.', 'field.arrays.')[5:]
embed_noarr = simple_d.replace('field.', 'field.simple.')
embed_nostr_noarr = simple_nostr.replace('field.', 'field.simple.')
# test over all combinations of field filters
f = create_field_filter(echo_nostr=False, echo_noarr=False)
m = String()
self.assertEquals("time,field.data", _str_plot_fields(m, 'field', f))
m = String('foo')
self.assertEquals('time,field.data', _str_plot_fields(m, 'field', f))
m = TVals(Time(123, 456), Duration(78, 90))
v = _str_plot_fields(m, 'field', f)
self.assertEquals('time,field.t,field.d', v)
m = simple_v
self.assertEquals(simple_d, _str_plot_fields(m, 'field', f))
m = arrays_v
self.assertEquals(arrays_d, _str_plot_fields(m, 'field', f))
m = embed_v
self.assertEquals(embed_d, _str_plot_fields(m, 'field', f))
f = create_field_filter(echo_nostr=True, echo_noarr=False)
m = String()
self.assertEquals("time,", _str_plot_fields(m, 'field', f))
m = String('foo')
self.assertEquals('time,', _str_plot_fields(m, 'field', f))
m = TVals(Time(123, 456), Duration(78, 90))
v = _str_plot_fields(m, 'field', f)
self.assertEquals('time,field.t,field.d', v)
m = simple_v
self.assertEquals(simple_nostr, _str_plot_fields(m, 'field', f))
m = arrays_v
self.assertEquals(arrays_nostr, _str_plot_fields(m, 'field', f))
m = embed_v
self.assertEquals(embed_nostr, _str_plot_fields(m, 'field', f))
f = create_field_filter(echo_nostr=False, echo_noarr=True)
m = String()
self.assertEquals("time,field.data", _str_plot_fields(m, 'field', f))
m = String('foo')
self.assertEquals("time,field.data", _str_plot_fields(m, 'field', f))
m = TVals(Time(123, 456), Duration(78, 90))
v = _str_plot_fields(m, 'field', f)
self.assertEquals('time,field.t,field.d', v)
m = simple_v
self.assertEquals(simple_d, _str_plot_fields(m, 'field', f))
m = arrays_v
self.assertEquals('time,', _str_plot_fields(m, 'field', f))
m = embed_v
self.assertEquals(embed_noarr, _str_plot_fields(m, 'field', f))
f = create_field_filter(echo_nostr=True, echo_noarr=True)
m = String()
self.assertEquals("time,", _str_plot_fields(m, 'field', f))
m = String('foo')
self.assertEquals('time,', _str_plot_fields(m, 'field', f))
m = TVals(Time(123, 456), Duration(78, 90))
v = _str_plot_fields(m, 'field', f)
self.assertEquals('time,field.t,field.d', v)
m = simple_v
self.assertEquals(simple_nostr, _str_plot_fields(m, 'field', f))
m = arrays_v
self.assertEquals('time,', _str_plot_fields(m, 'field', f))
m = embed_v
self.assertEquals(embed_nostr_noarr, _str_plot_fields(m, 'field', f))
def test_strify_message(self):
# strify message is part of roslib, but we want to test with
# rostopic's field filters. It's also the case that
# roslib.messages cannot be unit tested within the ROS stack
# -- part of the reason it needs to be moved elsewhere.
from std_msgs.msg import String
from test_rostopic.msg import Arrays, Embed, Simple, TVals
from genpy import Time, Duration
from roslib.message import strify_message
from rostopic import create_field_filter
simple_v = Simple(1, -2, 3, -4, 'a', 7, 8, 9, 'bar')
simple_d = {
'b': 1,
'int16': -2,
'int32': 3,
'int64': -4,
'c': 'a',
'uint16': 7,
'uint32': 8,
'uint64': 9,
'str': 'bar'
}
simple_nostr = simple_d.copy()
del simple_nostr['str']
arrays_v = Arrays([-1],
chr(2) + chr(3), [3, 4, 5], [6, 7, 8],
['a1', 'b2', 'b3'],
[Time(123, 456), Time(78, 90)])
arrays_d = {
'int8_arr': [-1],
'uint8_arr': [2, 3],
'int32_arr': [3, 4, 5],
'uint32_arr': [6, 7, 8],
'string_arr': ['a1', 'b2', 'b3'],
'time_arr': [{
'secs': 123,
'nsecs': 456
}, {
'secs': 78,
'nsecs': 90
}]
}
arrays_nostr = arrays_d.copy()
del arrays_nostr['string_arr']
embed_v = Embed(simple_v, arrays_v)
embed_d = {'simple': simple_d, 'arrays': arrays_d}
f = create_field_filter(echo_nostr=False, echo_noarr=False)
m = String()
self.assertEquals("data: ''", strify_message(m, field_filter=f))
m = String('foo')
self.assertEquals('data: "foo"', strify_message(m, field_filter=f))
m = TVals(Time(123, 456), Duration(78, 90))
v = yaml.load(strify_message(m, field_filter=f))
self.assertEquals(
{
't': {
'secs': 123,
'nsecs': 456
},
'd': {
'secs': 78,
'nsecs': 90
}
}, v)
m = simple_v
v = yaml.load(strify_message(m, field_filter=f))
self.assertEquals(simple_d, v)
m = arrays_v
v = yaml.load(strify_message(m, field_filter=f))
self.assertEquals(arrays_d, v)
m = embed_v
v = yaml.load(strify_message(m, field_filter=f))
self.assertEquals(embed_d, v)
f = create_field_filter(echo_nostr=True, echo_noarr=False)
m = String()
self.assertEquals('', strify_message(m, field_filter=f))
m = String('foo')
self.assertEquals('', strify_message(m, field_filter=f))
m = TVals(Time(123, 456), Duration(78, 90))
v = yaml.load(strify_message(m, field_filter=f))
self.assertEquals(
{
't': {
'secs': 123,
'nsecs': 456
},
'd': {
'secs': 78,
'nsecs': 90
}
}, v)
m = simple_v
v = yaml.load(strify_message(m, field_filter=f))
self.assertEquals(simple_nostr, v)
m = arrays_v
v = yaml.load(strify_message(m, field_filter=f))
self.assertEquals(arrays_nostr, v)
m = embed_v
v = yaml.load(strify_message(m, field_filter=f))
self.assertEquals({'simple': simple_nostr, 'arrays': arrays_nostr}, v)
f = create_field_filter(echo_nostr=False, echo_noarr=True)
m = String()
self.assertEquals("data: ''", strify_message(m, field_filter=f))
m = String('foo')
self.assertEquals('data: "foo"', strify_message(m, field_filter=f))
m = TVals(Time(123, 456), Duration(78, 90))
v = yaml.load(strify_message(m, field_filter=f))
self.assertEquals(
{
't': {
'secs': 123,
'nsecs': 456
},
'd': {
'secs': 78,
'nsecs': 90
}
}, v)
m = simple_v
v = yaml.load(strify_message(m, field_filter=f))
self.assertEquals(simple_d, v)
m = arrays_v
v = yaml.load(strify_message(m, field_filter=f))
self.assertEquals(None, v)
m = embed_v
v = yaml.load(strify_message(m, field_filter=f))
self.assertEquals({'simple': simple_d, 'arrays': None}, v)
f = create_field_filter(echo_nostr=True, echo_noarr=True)
m = String()
self.assertEquals('', strify_message(m, field_filter=f))
m = String('foo')
self.assertEquals('', strify_message(m, field_filter=f))
m = TVals(Time(123, 456), Duration(78, 90))
v = yaml.load(strify_message(m, field_filter=f))
self.assertEquals(
{
't': {
'secs': 123,
'nsecs': 456
},
'd': {
'secs': 78,
'nsecs': 90
}
}, v)
m = simple_v
v = yaml.load(strify_message(m, field_filter=f))
self.assertEquals(simple_nostr, v)
m = embed_v
v = yaml.load(strify_message(m, field_filter=f))
self.assertEquals({'simple': simple_nostr, 'arrays': None}, v)
def test_str_plot(self):
from rostopic import _str_plot
from std_msgs.msg import String
from test_rostopic.msg import Arrays, Embed, Simple, TVals
from genpy import Time, Duration
from rostopic import create_field_filter
# str plotting requires rospy time, we fix time to a set time
import rospy.rostime
rospy.rostime.set_rostime_initialized(True)
rospy.rostime._set_rostime(Time(0, 1234))
r_time = Time(0, 5678)
# prepare test values
simple_v = Simple(1, -2, 3, -4, 'a', 7, 8, 9, 'bar')
simple_d = '1234,1,-2,3,-4,a,7,8,9,bar'
simple_nostr = '1234,1,-2,3,-4,a,7,8,9'
arrays_v = Arrays([-1],
chr(2) + chr(3), [3, 4, 5], [6, 7, 8],
['a1', 'b2', 'b3'],
[Time(123, 456), Time(78, 90)])
arrays_d = '1234,-1,2,3,3,4,5,6,7,8,a1,b2,b3,123000000456,78000000090'
arrays_nostr = '1234,-1,2,3,3,4,5,6,7,8,123000000456,78000000090'
embed_v = Embed(simple_v, arrays_v)
embed_d = simple_d + ',' + arrays_d[5:]
# test current_time override
m = String('foo')
self.assertEquals('5678,foo',
_str_plot(m, current_time=r_time, field_filter=None))
# test over all combinations of field filters
f = create_field_filter(echo_nostr=False, echo_noarr=False)
m = String()
self.assertEquals("1234,", _str_plot(m, field_filter=f))
m = String('foo')
self.assertEquals('1234,foo', _str_plot(m, field_filter=f))
m = TVals(Time(123, 456), Duration(78, 90))
v = _str_plot(m, field_filter=f)
self.assertEquals('1234,123000000456,78000000090', v)
m = simple_v
self.assertEquals(simple_d, _str_plot(m, field_filter=f))
m = arrays_v
self.assertEquals(arrays_d, _str_plot(m, field_filter=f))
m = embed_v
self.assertEquals(embed_d, _str_plot(m, field_filter=f))
f = create_field_filter(echo_nostr=True, echo_noarr=False)
m = String()
self.assertEquals("1234,", _str_plot(m, field_filter=f))
m = String('foo')
self.assertEquals('1234,', _str_plot(m, field_filter=f))
m = TVals(Time(123, 456), Duration(78, 90))
v = _str_plot(m, field_filter=f)
self.assertEquals('1234,123000000456,78000000090', v)
m = simple_v
self.assertEquals(simple_nostr, _str_plot(m, field_filter=f))
m = arrays_v
self.assertEquals(arrays_nostr, _str_plot(m, field_filter=f))
m = embed_v
self.assertEquals(simple_nostr + arrays_nostr[4:],
_str_plot(m, field_filter=f))
f = create_field_filter(echo_nostr=False, echo_noarr=True)
m = String()
self.assertEquals("1234,", _str_plot(m, field_filter=f))
m = String('foo')
self.assertEquals('1234,foo', _str_plot(m, field_filter=f))
m = TVals(Time(123, 456), Duration(78, 90))
v = _str_plot(m, field_filter=f)
self.assertEquals('1234,123000000456,78000000090', v)
m = simple_v
self.assertEquals(simple_d, _str_plot(m, field_filter=f))
m = arrays_v
self.assertEquals('1234,', _str_plot(m, field_filter=f))
m = embed_v
self.assertEquals(simple_d, _str_plot(m, field_filter=f))
f = create_field_filter(echo_nostr=True, echo_noarr=True)
m = String()
self.assertEquals("1234,", _str_plot(m, field_filter=f))
m = String('foo')
self.assertEquals('1234,', _str_plot(m, field_filter=f))
m = TVals(Time(123, 456), Duration(78, 90))
v = _str_plot(m, field_filter=f)
self.assertEquals('1234,123000000456,78000000090', v)
m = simple_v
self.assertEquals(simple_nostr, _str_plot(m, field_filter=f))
m = arrays_v
self.assertEquals('1234,', _str_plot(m, field_filter=f))
m = embed_v
self.assertEquals(simple_nostr, _str_plot(m, field_filter=f))
def get_simple_plan():
header = Header(0, Time(0, 0), '/world')
joint_names = [
'l_shoulder_pan_joint', 'l_shoulder_lift_joint',
'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint',
'l_wrist_flex_joint', 'l_wrist_roll_joint'
]
points = []
aPoint = JointTrajectoryPoint(
(0.40010140636150293, -0.24941262701698985, 1.0004124485664612,
-0.4997714065764134, 0.5008749075576269, -0.4998209962755613,
1.250362580088737), (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
(-0.7144440176269288, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0), Duration(0, 0))
points.append(aPoint)
aPoint = JointTrajectoryPoint(
(0.3701010413213245, -0.237216433004318, 1.0004124485664612,
-0.4934401455238009, 0.5743228372936882, -0.5563811188230655,
1.1677162171739146),
(-0.17755980716212977, 0.07218425022834517, 0.0, 0.03747212709210088,
0.4347080518162636, -0.33475607510612976, -0.48914979008268733),
(-0.7240219594706212, 0.29434016137178437, 0.0, 0.1527972085370781,
1.7725755648421089, -1.365009082386119, -1.9945684507695596),
Duration(0, 289797186))
points.append(aPoint)
aPoint = JointTrajectoryPoint(
(0.3401006762811461, -0.22502023899164614, 1.0004124485664612,
-0.48710888447118833, 0.6477707670297495, -0.6129412413705697,
1.0850698542590922),
(-0.2880742149704434, 0.11711220883886869, 0.0, 0.06079502882920425,
0.7052732415616743, -0.5431104881451616, -0.7935998807002602),
(-0.6892898489023036, 0.28022034788306643, 0.0, 0.14546736242947506,
1.6875432122960485, -1.2995281314894562, -1.8988868611961462),
Duration(0, 409036620))
points.append(aPoint)
aPoint = JointTrajectoryPoint(
(0.3101003112409677, -0.21282404497897428, 1.0004124485664612,
-0.48077762341857583, 0.7212186967658107, -0.6695013639180738,
1.0024234913442698),
(-0.35255601309591583, 0.14332630720637068, 0.0, 0.07440323314696709,
0.8631398065728579, -0.6646789557013306, -0.9712372555168975),
(-0.6540669674290382, 0.26590101891630397, 0.0, 0.13803394429160473,
1.6013093374719958, -1.2331219231004833, -1.8018532737299),
Duration(0, 501473212))
points.append(aPoint)
aPoint = JointTrajectoryPoint(
(0.28009994620078926, -0.2006278509663024, 1.0004124485664612,
-0.4744463623659633, 0.794666626501872, -0.7260614864655781,
0.9197771284294473),
(-0.406837754916044, 0.16539372717600817, 0.0, 0.08585882301707332,
0.9960342414844434, -0.7670171094313484, -1.120775053743913),
(-0.7097026153852208, 0.28851884891888946, 0.0, 0.14977526179125936,
1.7375184521422526, -1.338012615670257, -1.9551208738351065),
Duration(0, 580305103))
points.append(aPoint)
aPoint = JointTrajectoryPoint(
(0.2500995811606109, -0.18843165695363054, 1.0004124485664612,
-0.46811510131335077, 0.8681145562379333, -0.7826216090130822,
0.8371307655146248),
(-0.4534024449997426, 0.18432389664695084, 0.0, 0.0956858104006955,
1.1100355238311277, -0.8548061937972654, -1.2490535687059658),
(-0.6120130152602035, 0.248804621623673, 0.0, 0.12915890063401106,
1.4983513994641284, -1.153836998230712, -1.6860011436543565),
Duration(0, 649571737))
points.append(aPoint)
aPoint = JointTrajectoryPoint(
(0.22009921612043246, -0.17623546294095868, 1.0004124485664612,
-0.46178384026073827, 0.9415624859739946, -0.8391817315605864,
0.7544844025998024),
(-0.4907755786717509, 0.19951737807675612, 0.0, 0.10357301661684516,
1.2015337203457976, -0.9252663037873301, -1.3520107682128484),
(-0.5583065814085438, 0.22697108439483424, 0.0, 0.11782472345103574,
1.3668654533888738, -1.0525834809427888, -1.5380482298474696),
Duration(0, 712904979))
points.append(aPoint)
aPoint = JointTrajectoryPoint(
(0.19009885108025404, -0.16403926892828682, 1.0004124485664612,
-0.45545257920812576, 1.015010415710056, -0.8957418541080906,
0.67183803968498),
(-0.5261778478447592, 0.2139096343957905, 0.0, 0.11104429263928692,
1.2882067783313866, -0.9920107144037812, -1.4495385410303516),
(-0.6417590184099651, 0.2608974086624451, 0.0, 0.1354364812888343,
1.5711765916358333, -1.2099175686236372, -1.7679467789254342),
Duration(0, 771977037))
points.append(aPoint)
aPoint = JointTrajectoryPoint(
(0.1600984860400756, -0.15184307491561494, 1.0004124485664612,
-0.4491213181555132, 1.0884583454461172, -0.9523019766555949,
0.5891916767701575),
(-0.5444951958339217, 0.22135627478080783, 0.0, 0.11490997599866001,
1.3330519422571325, -1.0265446757614107, -1.5),
(4.030024984049384e-15, -3.5262718610432106e-15, 0.0,
-1.5364470251688274e-14, 3.224019987239507e-14, 4.433027482454322e-14,
0.0), Duration(0, 827074612))
points.append(aPoint)
aPoint = JointTrajectoryPoint(
(0.1300981209998972, -0.13964688090294308, 1.0004124485664612,
-0.4427900571029007, 1.1619062751821785, -1.008862099203099,
0.5065453138553351),
(-0.5444951958339216, 0.22135627478080772, 0.0, 0.1149099759986601,
1.3330519422571334, -1.0265446757614094, -1.5),
(0.0, 0.0, 0.0, 1.813511242822224e-14, 0.0, 0.0, 0.0),
Duration(0, 882172187))
points.append(aPoint)
aPoint = JointTrajectoryPoint(
(0.10009775595971881, -0.12745068689027123, 1.0004124485664612,
-0.43645879605028814, 1.2353542049182398, -1.065422221750603,
0.42389895094051266),
(-0.5444951958339221, 0.22135627478080772, 0.0, 0.1149099759986601,
1.3330519422571334, -1.0265446757614114, -1.5),
(-1.813511242822224e-14, 0.0, 0.0, -1.813511242822224e-14, 0.0,
-7.254044971288896e-14, 0.0), Duration(0, 937269763))
points.append(aPoint)
aPoint = JointTrajectoryPoint(
(0.07009739091954037, -0.11525449287759937, 1.0004124485664612,
-0.43012753499767564, 1.308802134654301, -1.1219823442981074,
0.34125258802569025),
(-0.5444951958339221, 0.22135627478080772, 0.0, 0.1149099759986596,
1.3330519422571334, -1.0265446757614114, -1.5),
(1.813511242822224e-14, 0.0, 0.0, 0.0, 0.0, 7.254044971288896e-14,
0.0), Duration(0, 992367338))
points.append(aPoint)
aPoint = JointTrajectoryPoint(
(0.04009702587936198, -0.10305829886492751, 1.0004124485664612,
-0.42379627394506314, 1.3822500643903624, -1.1785424668456115,
0.25860622511086784),
(-0.5444951958339221, 0.22135627478080797, 0.0, 0.1149099759986601,
1.3330519422571334, -1.0265446757614114, -1.5),
(-1.813511242822224e-14, 9.06755621411112e-15, 0.0,
1.813511242822224e-14, 0.0, -7.254044971288896e-14, 0.0),
Duration(1, 47464913))
points.append(aPoint)
aPoint = JointTrajectoryPoint(
(0.010096660839183536, -0.09086210485225563, 1.0004124485664612,
-0.4174650128924506, 1.4556979941264236, -1.2351025893931158,
0.17595986219604542),
(-0.5444951958339221, 0.22135627478080772, 0.0, 0.1149099759986601,
1.3330519422571334, -1.0265446757614114, -1.5),
(1.813511242822224e-14, -1.813511242822224e-14, 0.0,
-1.813511242822224e-14, 0.0, 7.254044971288896e-14, 0.0),
Duration(1, 102562489))
points.append(aPoint)
aPoint = JointTrajectoryPoint(
(-0.019903704200994854, -0.0786659108395838, 1.0004124485664612,
-0.4111337518398381, 1.529145923862485, -1.2916627119406199,
0.09331349928122301),
(-0.533872188347821, 0.21703765198655037, 0.0, 0.11266810215918183,
1.3070443284704405, -1.0065169659508066, -1.470735258362112),
(0.3779342542440911, -0.15364344675879707, 0.0, -0.07975904363627499,
-0.9252716929742283, 0.7125249211573006, 1.0411503824159534),
Duration(1, 157660064))
points.append(aPoint)
aPoint = JointTrajectoryPoint(
(-0.04990406924117324, -0.06646971682691194, 1.0004124485664612,
-0.4048024907872256, 1.6025938535985462, -1.3482228344881242,
0.010667136366400598),
(-0.5056465694810834, 0.20556295414985992, 0.0, 0.1067113825933525,
1.2379413936059942, -0.9533027980583908, -1.392978046500466),
(0.5926536863783151, -0.24093437968887688, 0.0, -0.12507331818225356,
-1.4509552219327135, 1.1173385752105525, 1.6326691885792808),
Duration(1, 214994822))
points.append(aPoint)
aPoint = JointTrajectoryPoint(
(-0.07990443428135174, -0.054273522814240055, 1.0004124485664612,
-0.398471229734613, 1.6760417833346075, -1.4047829570356285,
-0.07197922654842204),
(-0.4693721826200346, 0.1908161515941057, 0.0, 0.09905605531872848,
1.1491331869784809, -0.8849141713383879, -1.2930477244188552),
(0.5833357813624516, -0.23714632653641227, 0.0, -0.12310687247269494,
-1.4281428050844136, 1.099771393981981, 1.6069998022729344),
Duration(1, 276465442))
points.append(aPoint)
aPoint = JointTrajectoryPoint(
(-0.10990479932153008, -0.0420773288015682, 1.0004124485664612,
-0.3921399686820005, 1.7494897130706688, -1.4613430795831324,
-0.15462558946324423),
(-0.4293568030989694, 0.17454850513460446, 0.0, 0.09061123094649409,
1.0511661529277676, -0.8094725969953693, -1.1828115465042477),
(0.6094176473284412, -0.2477495141012329, 0.0, -0.12861117556862145,
-1.4919973300641747, 1.148943913493855, 1.6788513066540751),
Duration(1, 343029316))
points.append(aPoint)
aPoint = JointTrajectoryPoint(
(-0.13990516436170852, -0.029881134788896313, 1.0004124485664612,
-0.38580870762938796, 1.82293764280673, -1.5179032021306367,
-0.23727195237806686),
(-0.38358943985440314, 0.1559424768135622, 0.0, 0.08095251099415271,
0.939116913683852, -0.7231867244627543, -1.0567295435919777),
(0.6220400789731343, -0.2528809725032963, 0.0, -0.13127500681716633,
-1.5229000031247977, 1.1727411666178345, 1.7136241524237752),
Duration(1, 416557244))
points.append(aPoint)
aPoint = JointTrajectoryPoint(
(-0.16990552940188697, -0.017684940776224456, 1.0004124485664612,
-0.37947744657677546, 1.8963855725427914, -1.574463324678141,
-0.3199183152928893),
(-0.34175135639103427, 0.13893383767353085, 0.0, 0.07212302415314824,
0.8366874728955987, -0.64430877998869, -0.9414720983927828),
(0.39572292144968146, -0.16087515997881496, 0.0, -0.08351315448478334,
-0.9688225223479209, 0.7460621529796324, 1.0901554076439481),
Duration(1, 500085172))
points.append(aPoint)
aPoint = JointTrajectoryPoint(
(-0.1999058944420653, -0.005488746763552627, 1.0004124485664612,
-0.37314618552416295, 1.9698335022788527, -1.631023447225645,
-0.4025646782077117),
(-0.28581742898323226, 0.11619474667771668, 0.0, 0.060318757916974414,
0.6997481001710263, -0.5388557368503288, -0.7873827845592509),
(0.7206317437817781, -0.29296192053269826, 0.0, -0.15208173922455254,
-1.7642755217136517, 1.358617459665321, 1.9852289312069493),
Duration(1, 592582673))
points.append(aPoint)
aPoint = JointTrajectoryPoint(
(-0.22990625948224386, 0.006707447249119258, 1.0004124485664612,
-0.3668149244715504, 2.043281432014914, -1.6875835697731494,
-0.4852110411225341),
(-0.1747157427808418, 0.07102803893117376, 0.0, 0.03687191726051188,
0.42774511517992775, -0.32939411935248397, -0.481314832851524),
(0.6995037352471314, -0.28437264867400674, 0.0, -0.14762289556130062,
-1.712549201575904, 1.3187845192892753, 1.927024537404225),
Duration(1, 713895365))
points.append(aPoint)
aPoint = JointTrajectoryPoint(
(-0.2599066245224222, 0.018903641261791115, 1.0004124485664612,
-0.3604836634189379, 2.1167293617509753, -1.7441436923206535,
-0.5678574040373565), (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
(0.6954105015201669, -0.282708606499642, 0.0, -0.1467590616966102,
-1.7025280054081884, 1.311067486483095, 1.9157483119436316),
Duration(2, 7631682))
points.append(aPoint)
joint_trajectory = JointTrajectory(header, joint_names, points)
newHeader = Header(0, Time(0, 0), '')
multi_dof_joint_trajectory = MultiDOFJointTrajectory(newHeader, [], [])
plan = RobotTrajectory(joint_trajectory, multi_dof_joint_trajectory)
return plan
