Source code for base.steer_force_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 os
import sys
from pymorse import Morse

[docs]def send_force(s, steer, force, brake): s.publish({'steer' : steer, 'force' : force, 'brake' : brake})
[docs]class SteerForceTest(MorseTestCase):
[docs] def setUpEnv(self): robot = Hummer('robot') pose = Pose() pose.add_stream('socket') robot.append(pose) steer_force = SteerForce() robot.append(steer_force) steer_force.add_stream('socket') env = Environment('land-1/rosace_1', fastmode = True) env.set_physics_step_sub(1) env.set_camera_location([50.0, -20.0, 50.0])
[docs] def test(self): with Morse() as morse: pose_stream = morse.robot.pose pose = pose_stream.get() # It is not really precise to control the robot in this way # in open loop. So it is a highly qualitative test. x = pose['x'] y = pose['y'] # destination socket steer_client = morse.robot.steer_force send_force(steer_client, 0.0, -20.0, 0.0) morse.sleep(3.0) send_force(steer_client, 0.0, 0.0, 10.0) morse.sleep(1.0) pose = pose_stream.get() self.assertAlmostEqual(pose['x'], x + 8.5, delta = 1.0) self.assertAlmostEqual(pose['y'], y, delta = 1.0) # Doubling the force send_force(steer_client, 0.0, -40.0, 0.0) morse.sleep(3.0) send_force(steer_client, 0.0, 0.0, 10.0) morse.sleep(2.0) pose = pose_stream.get() self.assertAlmostEqual(pose['x'], x + 28.0, delta = 1.0) self.assertAlmostEqual(pose['y'], y, delta = 1.5) # Backward move send_force(steer_client, 0.0, 10.0, 0.0) morse.sleep(11.0) send_force(steer_client, 0.0, 0.0, 10.0) morse.sleep(2.0) pose = pose_stream.get() self.assertAlmostEqual(pose['x'], x, delta = 1.5) self.assertAlmostEqual(pose['y'], y, delta = 1.0) # Turning send_force(steer_client, -1.0, -10.0, 0.0) morse.sleep(10) send_force(steer_client, 0.0, 0.0, 10.0) morse.sleep(2.0) pose = pose_stream.get() self.assertAlmostEqual(pose['yaw'], 2.25, delta = 0.2) self.assertAlmostEqual(pose['x'], x - 2.5, delta = 1.0) self.assertAlmostEqual(pose['y'], x - 7.0, delta = 1.0)
########################## Run these tests ########################## if __name__ == "__main__": from morse.testing.testing import main main(SteerForceTest)