Beispiel #1
0
    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)))
Beispiel #2
0
    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
Beispiel #3
0
    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))
Beispiel #4
0
    def test_strify_message(self):
        from genpy.message import Message, strify_message, fill_message_args
        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

        # The following tests have not been ported to genpy yet

        # test array of Messages field. We can't use M4 or M5 because fill_message_args has to instantiate the embedded type
        from test_roslib_comm.msg import ArrayOfMsgs
        from std_msgs.msg import String, Time, MultiArrayLayout, MultiArrayDimension
        dims1 = [MultiArrayDimension(*args) for args in [('', 0, 0), ('x', 1, 2), ('y of z', 3, 4)]]
        dims2 = [MultiArrayDimension('hello world', 91280, 1983274)]
        times = [Time(genpy.Time(*args)) for args in [(0,), (12345, 6789), (1, 1)]]
        val = ArrayOfMsgs([String(''), String('foo'), String('bar of soap')],
                          times,
                          [MultiArrayLayout(dims1, 0), MultiArrayLayout(dims2, 12354)],
                          )
        self.assertEquals(val, roundtrip(val))
Beispiel #5
0
    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)))
Beispiel #6
0
 def fake_point_click(self):
     fake = PointStamped()
     fake.header.seq = 2 * 10
     fake.header.frame_id = "fake"
     fake.header.stamp = Time(1)
     fake.point = Point()
     fake.point.x = 3.0
     fake.point.y = 2.0
     fake.point.z = 0.
     self.fake_click_point.publish(fake)
Beispiel #7
0
    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_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
Beispiel #9
0
    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])))
Beispiel #10
0
    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))
Beispiel #11
0
    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)
Beispiel #12
0
    def test_strify_yaml(self):
        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

        # test YAML roundtrip. strify_message doesn't promise this
        # yet, but want to use it in this way in some demo toolchains
        from genpy.message import Message, strify_message, fill_message_args

        class M1(Message):
            __slots__ = []
            _slot_types = []

            def __init__(self):
                pass

        self.assertEquals(M1(), roundtrip(M1()))

        class M2(Message):
            __slots__ = ['str', 'int', 'float', 'bool', 'list']
            _slot_types = ['string', 'int32', 'float32', 'bool', 'int32[]']

            def __init__(self,
                         str_=None,
                         int_=None,
                         float_=None,
                         bool_=None,
                         list_=None):
                self.str = str_
                self.int = int_
                self.float = float_
                self.bool = bool_
                self.list = list_

        val = M2('string', 123456789101112, 5678., True, [1, 2, 3])
        self.assertEquals(val, roundtrip(val))
        # test with empty string and empty list
        val = M2('', -1, 0., False, [])
        self.assertEquals(val, roundtrip(val))

        class M3(Message):
            __slots__ = ['m2']
            _slot_types = ['test_roslib/M2']

            def __init__(self, m2=None):
                self.m2 = m2 or M2()

        val = M3(M2('string', -1, 0., False, []))
        self.assertEquals(val, roundtrip(val))

        # test array of Messages field. We can't use M4 or M5 because fill_message_args has to instantiate the embedded type
        if 0:
            from test_roslib_comm.msg import ArrayOfMsgs
            from std_msgs.msg import String, Time, MultiArrayLayout, MultiArrayDimension
            dims1 = [
                MultiArrayDimension(*args)
                for args in [('', 0, 0), ('x', 1, 2), ('y of z', 3, 4)]
            ]
            dims2 = [MultiArrayDimension('hello world', 91280, 1983274)]
            times = [
                Time(genpy.Time(*args))
                for args in [(0, ), (12345, 6789), (1, 1)]
            ]
            val = ArrayOfMsgs(
                [String(''), String('foo'),
                 String('bar of soap')],
                times,
                [MultiArrayLayout(dims1, 0),
                 MultiArrayLayout(dims2, 12354)],
            )
            self.assertEquals(val, roundtrip(val))
Beispiel #13
0
    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))
Beispiel #14
0
    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)
Beispiel #15
0
    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))
Beispiel #16
0
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