Source code for base.attitude_testing

#! /usr/bin/env python
"""
This script tests the 'data stream' oriented feature of the socket interface.
"""

from morse.testing.testing import MorseTestCase

try:
    # Include this import to be able to use your test file as a regular 
    # builder script, ie, usable with: 'morse [run|exec] <your test>.py
    from morse.builder import *
except ImportError:
    pass

import sys
import math
from pymorse import Morse


[docs]class AttitudeTest(MorseTestCase):
[docs] def setUpEnv(self): robot = RMax('robot') robot.translate(10.0, 8.0, 20.0) attitude = Attitude() attitude.add_stream('socket') attitude.properties(ComputationMode = 'Velocity') robot.append(attitude) attitude_pos = Attitude() attitude_pos.add_stream('socket') attitude_pos.properties(ComputationMode = 'Position') robot.append(attitude_pos) orientation = Orientation('orientation') orientation.add_stream('socket') orientation.properties(speed = 0.5) robot.append(orientation) env = Environment('empty', fastmode = True) env.add_service('socket')
def _test_attitude(self, yaw, pitch, roll, wx, wy, wz): precision = 0.05 precision_speed = 0.003 attitude_pos = self.att_pos_stream.get() attitude = self.att_stream.last() self.assertAlmostEqual(attitude['rotation']['yaw'], yaw, delta = precision) self.assertAlmostEqual(attitude['rotation']['pitch'], pitch, delta = precision) self.assertAlmostEqual(attitude['rotation']['roll'], roll, delta = precision) self.assertAlmostEqual(attitude_pos['rotation']['yaw'], yaw, delta = precision) self.assertAlmostEqual(attitude_pos['rotation']['pitch'], pitch, delta = precision) self.assertAlmostEqual(attitude_pos['rotation']['roll'], roll, delta = precision) self.assertAlmostEqual(attitude['angular_velocity'][0], wx, delta = precision_speed) self.assertAlmostEqual(attitude['angular_velocity'][1], wy, delta = precision_speed) self.assertAlmostEqual(attitude['angular_velocity'][2], wz, delta = precision_speed) self.assertAlmostEqual(attitude_pos['angular_velocity'][0], wx, delta = precision_speed) self.assertAlmostEqual(attitude_pos['angular_velocity'][1], wy, delta = precision_speed) self.assertAlmostEqual(attitude_pos['angular_velocity'][2], wz, delta = precision_speed)
[docs] def send_angles(self, yaw, pitch, roll): self.orientation_stream.publish({'yaw' : yaw, 'pitch' : pitch, 'roll' : roll})
[docs] def test_attitude(self): """ Test if we can connect to the pose data stream, and read from it. """ with Morse() as morse: self.att_stream = morse.robot.attitude self.att_pos_stream = morse.robot.attitude_pos self.orientation_stream = morse.robot.orientation morse.sleep(0.01) self._test_attitude(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) self.send_angles(3.0, 0.0, 0.0) morse.sleep(1.0) self._test_attitude(0.5, 0.0, 0.0, 0.0, 0.0, 0.5) morse.sleep(6.0) self._test_attitude(3.0, 0.0, 0.0, 0.0, 0.0, 0.0) # Be smart on the rotation order self.send_angles(-3.0, 0.0, 0.0) morse.sleep(1.0) self._test_attitude(-3.0, 0.0, 0.0, 0.0, 0.0, 0.0) # Turn in the order direction self.send_angles(2.0, 0.0, 0.0) morse.sleep(1.0) self._test_attitude(2.8, 0.0, 0.0, 0.0, 0.0, -0.5) morse.sleep(2.0) self._test_attitude(2.0, 0.0, 0.0, 0.0, 0.0, 0.0) self.send_angles(3.0, 1.0, 0.0) morse.sleep(0.5) self._test_attitude(2.25, 0.25, 0.0, 0.0, 0.5, 0.5)
########################## Run these tests ########################## if __name__ == "__main__": from morse.testing.testing import main main(AttitudeTest)