self.assertAlmostEqual(pose['y'], 0.0, delta=precision)
self.assertAlmostEqual(pose['z'], 0.0, delta=precision)
self.assertAlmostEqual(pose['yaw'], 0.0, delta=precision)
self.assertAlmostEqual(pose['pitch'], 0.0, delta=precision)
self.assertAlmostEqual(pose['roll'], 0.0, delta=precision)
send_service_speed(v_w, -1.0, 0.0, 2.0)
pose = pose_stream.get()
for coord in pose.values():
self.assertAlmostEqual(coord, 0.0, delta=precision)
send_service_speed(v_w, 1.0, -math.pi / 4.0, 2.0)
pose = pose_stream.get()
# for non-null w, we have r = v / w
self.assertAlmostEqual(pose['x'], 4.0 / math.pi, delta=precision)
self.assertAlmostEqual(pose['y'], -4.0 / math.pi, delta=precision)
self.assertAlmostEqual(pose['z'], 0.0, delta=precision)
self.assertAlmostEqual(pose['yaw'],
-math.pi / 2.0,
delta=precision)
self.assertAlmostEqual(pose['pitch'], 0.0, delta=precision)
self.assertAlmostEqual(pose['roll'], 0.0, delta=precision)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(VW_Test)
def test_semantic_camera(self):
""" This test is guaranteed to be started only when the simulator
is ready.
"""
with Morse() as morse:
# not enough argument
with self.assertRaises(MorseServiceFailed):
res = morse.rpc('communication', 'distance_and_view')
# unknown robot does not exit
with self.assertRaises(MorseServiceFailed):
res = morse.rpc('communication', 'distance_and_view', 'mana', 'unknow_robot')
res = morse.rpc('communication', 'distance_and_view', 'mana', 'minnie')
self.assertAlmostEquals(res[0], 10.0, delta=0.01)
self.assertTrue(res[1])
res = morse.rpc('communication', 'distance_and_view', 'mana', 'munu')
self.assertAlmostEquals(res[0], 10.0, delta=0.01)
self.assertFalse(res[1])
res = morse.rpc('communication', 'distance_and_view', 'minnie', 'munu')
self.assertAlmostEquals(res[0], 20.0, delta=0.01)
self.assertFalse(res[1])
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(Communication_Service_Testing)
self.assertAlmostEqual(pose['yaw'], 2 * delta_yaw, delta=0.1)
self.assertAlmostEqual(pose['pitch'], 0.0, delta=0.1)
self.assertAlmostEqual(pose['roll'], 0.0, delta=0.1)
send_ctrl(cmd_client, 0.0, 0.0, 0.0, z)
morse.sleep(1.0)
pose = pose_stream.get()
self.assertAlmostEqual(pose['x'], 0.0, delta=0.1)
self.assertAlmostEqual(pose['y'], 0.0, delta=0.1)
self.assertAlmostEqual(pose['z'], z, delta=0.2)
self.assertAlmostEqual(pose['yaw'], 2 * delta_yaw, delta=0.2)
self.assertAlmostEqual(pose['pitch'], 0.0, delta=0.1)
self.assertAlmostEqual(pose['roll'], 0.0, delta=0.1)
send_ctrl(cmd_client, 0.0, 0.0, -0.1, z)
morse.sleep(1.0)
pose = pose_stream.get()
self.assertAlmostEqual(pose['x'], 0.0, delta=0.1)
self.assertAlmostEqual(pose['y'], 0.0, delta=0.1)
self.assertAlmostEqual(pose['z'], z, delta=0.2)
self.assertAlmostEqual(pose['yaw'], delta_yaw, delta=0.2)
self.assertAlmostEqual(pose['pitch'], 0.0, delta=0.1)
self.assertAlmostEqual(pose['roll'], 0.0, delta=0.1)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(StabilizedQuadrirotorTest)
self.assertAlmostEqual(pose['x'], _x, delta=0.03)
logger.info("Ok, did not move")
# now resume the move
simu.robot.motion.resume().result()
simu.sleep(0.5)
# must move now
pose = pose_stream.get()
self.assertAlmostEqual(pose['x'], 5.7, delta=0.20)
status = simu.robot.motion.get_status().result()
self.assertEqual(status, "Transit")
# wait for the end of the move
simu.sleep(1.5)
self.assertTrue(action.done())
self.assertFalse(action.running())
pose = pose_stream.get()
self.assertAlmostEqual(pose['x'], 6.0, delta=0.15)
status = simu.robot.motion.get_status().result()
self.assertEqual(status, "Arrived")
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(Waypoints_Test)
IK_TARGET = "ik_target.robot.arm.kuka_7"
with Morse() as simu:
self.assertEqual(simu.robot.arm.list_IK_targets(), [IK_TARGET])
self._check_pose(simu, 0., 0., 1.3105, 0.)
simu.robot.arm.move_IK_target(IK_TARGET, [0,0,2], None, False).result() # absolute location
self._check_pose(simu, 0., 0., 1.3105, 0.)
simu.robot.arm.move_IK_target(IK_TARGET, [1,0,0.3105], None, False).result()
self._check_pose(simu, 0.778, 0., 0.363, 0.02)
simu.robot.arm.move_IK_target(IK_TARGET, [1,0,0.3105], [math.pi/2, -math.pi/2, -math.pi], False).result() # arm should be horizontal
self._check_pose(simu, 1.0, 0., 0.3105, math.radians(90))
# back to original position
simu.robot.arm.move_IK_target(IK_TARGET, [0,0,2], [math.pi/2, 0., -math.pi], False).result() # absolute location
self._check_pose(simu, 0., 0., 1.3105, 0.)
simu.robot.arm.move_IK_target(IK_TARGET, [-1, 0, -1.6895], None).result() # relative position
self._check_pose(simu, -0.778, 0., 0.363, -0.02)
simu.robot.arm.move_IK_target(IK_TARGET, [0.,0.,0.], [0., -math.pi/2, 0.]).result() # relative rotation
self._check_pose(simu, -1.0, 0., 0.3105, -math.radians(90))
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(ArmatureTest)
self.assertAlmostEqual(pose['y'], 8.0, delta=self.precision)
self.assertAlmostEqual(pose['z'], 20.0, delta=self.precision)
# Test only one rotation each time, otherwise, it a bit more
# complex to check that it does the good transformation
# (without a matrix transformation library)
for i in range(0, 5):
self._test_one_pose(random.uniform(-30.0, 30.0),
random.uniform(-30.0, 30.0),
random.uniform(10.0, 50.0),
random.uniform(-math.pi, math.pi),
0, 0)
self._test_one_pose(random.uniform(-30.0, 30.0),
random.uniform(-30.0, 30.0),
random.uniform(10.0, 50.0),
0,
random.uniform(-math.pi, math.pi),
0)
self._test_one_pose(random.uniform(-30.0, 30.0),
random.uniform(-30.0, 30.0),
random.uniform(10.0, 50.0),
0, 0,
random.uniform(-math.pi, math.pi))
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(TeleportTest)
def test_base_service_connection(self):
""" Simply tests if the simulator is reachable by its socket interface.
"""
morse = Morse()
morse.close()
def test_get_pose_streams_service(self):
""" Tests if we can retrieve the list of published data streams.
"""
morse = Morse()
self.assertEquals(set(morse.streams()), set(["robot.pose"]))
morse.close()
def test_read_pose(self):
""" Test if we can connect to the pose data stream, and read from it.
"""
with Morse() as morse:
pose_stream = morse.robot.pose
pose = pose_stream.get()
for coord in pose.values():
self.assertAlmostEqual(coord, 0.0, 2)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(PoseTest)
def test_tf(self):
rospy.init_node('morse_ros_tf_test')
self.tf = TransformListener()
sleep(1)
self.assertTrue(self.tf.frameExists("/odom"))
self.assertTrue(self.tf.frameExists("/base_footprint"))
self.assertTrue(self.tf.frameExists("/map"))
self.assertTrue(self.tf.frameExists("/robot2"))
self._check_pose("odom", "base_footprint", [0, 0, 0], [0, 0, 0, 1])
self._check_pose("map", "robot2", [0, 0, 0], [0, 0, 0, 1])
with Morse() as morse:
teleport = morse.robot.motion
teleport.publish({'x' : 2, 'y' : 0, 'z' : 0, \
'yaw' : 0, 'pitch' : 0.0, 'roll' : 0.0})
morse.sleep(0.1)
self._check_pose("odom", "base_footprint", [2, 0, -0.1], [0, 0, 0, 1])
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(TfTest, time_modes=[TimeStrategies.BestEffort])
pose2.add_stream('socket')
env = Environment('empty', fastmode = True)
env.add_service('socket')
def test_ned_pose(self):
""" Test if the NED conversion is correctly done
"""
with Morse() as morse:
pose_stream = morse.robot.pose
pose_ned_stream = morse.robot.pose2
pose = pose_stream.get()
pose_ned = pose_ned_stream.get()
self.assertAlmostEqual(pose['x'], 42.0, delta=0.01)
self.assertAlmostEqual(pose['y'], -10.0, delta=0.01)
self.assertAlmostEqual(pose['z'], 40.0, delta=0.1)
self.assertAlmostEqual(pose['x'], pose_ned['y'])
self.assertAlmostEqual(pose['y'], pose_ned['x'])
self.assertAlmostEqual(pose['z'], -pose_ned['z'])
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(NedTest)
'l_gripper_motor_slider_joint'
}
class PR2TorsoTest(MorseTestCase):
def setUpEnv(self):
from morse.builder.robots import BasePR2
pr2 = BasePR2()
pr2.add_interface("socket")
env = Environment('empty', fastmode=True)
env.set_camera_rotation([1.0470, 0, 0.7854])
def test_joints(self):
with Morse() as simu:
joints = simu.pr2.joint_state.get()
self.assertEqual(len(set(joints.keys())), len(joints.keys()),
'Some joints are duplicated!')
self.assertEqual(
set(joints.keys()), PR2_JOINTS,
'Could not find all joints of the PR2. Please check if you named the joints correctly in your pr2_posture sensor and middleware!'
)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(PR2TorsoTest)
self.assert_velocity(morse, {
"x": 0.0,
"y": -1.0,
"w": 0.0
}, [0.0, -1.0, 0.0, 0.0, 0.0, 0.0])
self.assert_velocity(morse, {
"x": 0.0,
"y": 0.0,
"w": 0.0
}, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
self.assert_velocity(morse, {
"x": 1.0,
"y": 0.0,
"w": 1.0
}, [1.0, 0.0, 0.0, 0.0, 0.0, 1.0])
self.assert_velocity(morse, {
"x": 0.0,
"y": 0.0,
"w": 0.0
}, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(Velocity_Test)
collision = Collision()
collision.properties(only_objects_with_property="obstacle")
collision.add_stream('socket')
collision.translate(x=0.7, z=0.2)
robot.append(collision)
robot.translate(x=-3.0)
env = Environment('empty', fastmode=True)
def test_collision(self):
with Morse() as sim:
collision = sim.robot.collision.get(timeout=0.1)
self.assertEqual(collision, None)
send_speed(sim.robot.motion, sim, 1.0, 0.0, 1.0)
collision = sim.robot.collision.get(timeout=0.1)
self.assertEqual(collision, None)
send_speed(sim.robot.motion, sim, 1.0, 0.0)
sim.sleep(1.0)
collision = sim.robot.collision.get(timeout=0.1)
self.assertNotEqual(collision, None)
self.assertEqual(collision['objects'], "dala")
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(CollisionTest)
send_speed(cmd_stream, 0.5, -math.pi / 8.0, 12.0)
sleep(1)
self.assertAlmostEqual(self.pos.pose.pose.position.x,
0.0,
delta=precision)
self.assertAlmostEqual(self.pos.pose.pose.position.y,
0.0,
delta=precision)
self.assertAlmostEqual(self.pos.pose.pose.position.z,
0.0,
delta=precision)
send_speed(cmd_stream, -2.0, math.pi / 2.0, 3.0)
self.assertAlmostEqual(self.pos.pose.pose.position.x,
4.0 / math.pi,
delta=precision)
self.assertAlmostEqual(self.pos.pose.pose.position.y,
-4.0 / math.pi,
delta=precision)
self.assertAlmostEqual(self.pos.pose.pose.position.z,
0.0,
delta=precision)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(DataStreamTest, time_modes=[TimeStrategies.BestEffort])
morse.deactivate('robot.teleport')
teleport_mod_stream.publish({
'x': 4617522.748,
'y': 4397221.061,
'z': 158481.296875,
'yaw': 0.0,
'pitch': 0.0,
'roll': 0.0
})
morse.sleep(0.03)
pos = gps_stream.get()
pos_mod = gps_mod_stream.last()
self.assertAlmostEqual(pos['x'], 10.0, delta=precision)
self.assertAlmostEqual(pos['y'], 8.0, delta=precision)
self.assertAlmostEqual(pos['z'], 0.1, delta=precision)
self.assertAlmostEqual(pos_mod['x'], 4617522.748, delta=precision)
self.assertAlmostEqual(pos_mod['y'], 4397221.061, delta=precision)
self.assertAlmostEqual(pos_mod['z'],
158481.296875,
delta=precision)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(ECEFModifierTest)
"""
with Morse() as morse:
semantic_stream = morse.robot.camera
teleport_client = morse.robot.motion
o = semantic_stream.get()
objects= o['visible_objects']
self.assertEqual(objects, [])
# Change the orientation of the robot using the v_w socket
send_dest(teleport_client, morse, -5.0, 0.0, math.pi)
# Second test for the sensor, with objects in front
o = semantic_stream.get()
objects= o['visible_objects']
self.assertEqual(len(objects), 1)
self.assertEqual(objects[0]['name'],'RedBox')
# RedBox in camera frame:
position = [0, -0.2, 2.2]
quaternion = {'x':0.5, 'y':0.5, 'z':-0.5, 'w':0.5}
# quaternion is equal to euler (pi, pi, 0) in XYZ mode
for i in [0,1,2]:
self.assertAlmostEqual(objects[0]['position'][i], position[i], delta=0.1)
for i in ['x', 'y', 'z', 'w']:
self.assertAlmostEqual(objects[0]['orientation'][i], quaternion[i], delta=.1)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(Semantic_Camera_Test)
self.assertIn('Target1', prox['near_objects'])
# Don't care about the direction, only check the distance
send_dest(teleport_client, morse, -2.8, 0.0, 0.0)
prox = prox_stream.get()
self.assertEqual(len(prox['near_objects']), 1)
self.assertIn('Target3', prox['near_objects'])
# Call the set_range service and check if we can catch the
# two objects
prox_stream.set_range(20.0)
morse.sleep(0.1)
prox = prox_stream.get()
self.assertEqual(len(prox['near_objects']), 2)
self.assertIn('Target1', prox['near_objects'])
self.assertIn('Target3', prox['near_objects'])
# Call the set_tracked_tag service and check if we catch
# target2
prox_stream.set_tracked_tag('Catch_me2')
morse.sleep(0.1)
prox = prox_stream.get()
self.assertEqual(len(prox['near_objects']), 1)
self.assertIn('Target2', prox['near_objects'])
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(ProximityTest)
# search the green block in the image
cam = cam_stream.get()
for i in range(320*240):
o = cam['image'][i]
# Value computed with gimp help ...
if (o['r'] < 5 and o['g'] > 110 and o['b'] < 5):
res.append(i)
self.assertEqual(len(res), 0)
# Now, illuminate the scene
light_stream.publish({"emit": True})
morse.sleep(2.0)
cam = cam_stream.get()
# search the green block in the image
for i in range(320*240):
o = cam['image'][i]
# Value computed with gimp help ...
if (o['r'] < 5 and o['g'] > 110 and o['b'] < 5):
res.append(i)
self.assertTrue(len(res) > 10000)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(LightTest)
def test_renaming(self):
""" Tests the simulator can return the list of robots
This test is guaranteed to be started only when the simulator
is ready.
"""
with Morse() as morse:
p1 = morse.morsy.my_kuka.my_pose
p2 = morse.mana.arm.my_pose2
p3 = morse.dala.my_kuka.pose
p4 = morse.robot4.arm.pose
p5 = morse.foo.arm.pose
p6 = morse.bar.arm.pose
p7 = morse.m.arm.pose
p8 = morse.m_001.arm.pose
p9 = morse.m_002.arm.pose
p10 = morse.m_003.arm.pose
# ...
self.assertEqual(len(morse.ms), 10)
self.assertEqual(morse.m, morse.ms[0])
self.assertEqual(morse.m_001, morse.ms[1])
self.assertEqual(morse.m_002, morse.ms[2])
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(RenamingTest)
as expected (as if they were alone).
This tests the particular wheel parenting mechanism used on
this robot when several instance are present.
"""
with Morse() as morse:
pose1_stream = morse.robot1.pose1
pose2_stream = morse.robot2.pose2
pose1_x = pose1_stream.get()["x"]
self.assertAlmostEqual(pose1_x, 0.0, delta=0.03)
pose2_x = pose2_stream.get()["x"]
self.assertAlmostEqual(pose2_x, 0.0, delta=0.03)
set_speed(morse.robot1.motion1, morse, 1.0, 0.0, 2.0)
set_speed(morse.robot2.motion2, morse, 1.0, 0.0, 2.0)
pose1_x = pose1_stream.get()["x"]
self.assertAlmostEqual(pose1_x, 2.0, delta=0.10)
pose2_x = pose2_stream.get()["x"]
self.assertAlmostEqual(pose2_x, 2.0, delta=0.10)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(TwoRMP400Test)
bat_stream = morse.robot.barometer
teleport_stream = morse.robot.teleport
bat = bat_stream.get()
self.assertAlmostEqual(bat['pressure'], 101325.0, delta=0.1)
# pressure is independant of position (x,y)
send_pose(teleport_stream, 5.0, 2.0, 0.0)
morse.sleep(0.01)
bat = bat_stream.get()
self.assertAlmostEqual(bat['pressure'], 101325.0, delta=0.1)
# Pressure computed from
# http://www.digitaldutch.com/atmoscalc/
send_pose(teleport_stream, 0.0, 0.0, 100.0)
morse.sleep(0.01)
bat = bat_stream.get()
self.assertAlmostEqual(bat['pressure'], 100129.0, delta=0.1)
send_pose(teleport_stream, 0.0, 0.0, 1000.0)
morse.sleep(0.01)
bat = bat_stream.get()
self.assertAlmostEqual(bat['pressure'], 89871.0, delta=0.1)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(BarometerTest)
human3 = Human()
human3.armature.add_stream('socket')
env = Environment('empty', fastmode = True)
def test_pose(self):
""" Tests we can load the human model, attach a pose sensor, and
get back the pose.
"""
with Morse() as morse:
armature1 = morse.human1.armature
armature2 = morse.human2.armature
armature3 = morse.human3.armature
p1 = armature1.get()
p2 = armature2.get()
p3 = armature3.get()
self.assertAlmostEquals(p1['x'], 5.0, delta=0.01)
self.assertAlmostEquals(p2['x'], -5.0, delta=0.01)
self.assertAlmostEquals(p3['x'], 0.0, delta=0.01)
self.assertAlmostEquals(p1['y'], 0.0, delta=0.01)
self.assertAlmostEquals(p2['y'], 0.0, delta=0.01)
self.assertAlmostEquals(p3['y'], 0.0, delta=0.01)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(MultipleHumanTest)
pose = pose_stream.get()
self.assertAlmostEqual(pose['x'], 2.0, delta=precision)
self.assertAlmostEqual(pose['y'], 0.0, delta=precision)
self.assertAlmostEqual(pose['z'], 0.0, delta=precision)
self.assertAlmostEqual(pose['yaw'], 0.0, delta=precision)
self.assertAlmostEqual(pose['pitch'], 0.0, delta=precision)
self.assertAlmostEqual(pose['roll'], 0.0, delta=precision)
send_service_speed(v_w, morse, -1.0, 0.0, 2.0)
pose = pose_stream.get()
for key, coord in pose.items():
if key != 'timestamp':
self.assertAlmostEqual(coord, 0.0, delta=precision)
send_service_speed(v_w, morse, 1.0, -math.pi/4.0, 2.0)
pose = pose_stream.get()
# for non-null w, we have r = v / w
self.assertAlmostEqual(pose['x'], 4.0/ math.pi , delta=precision)
self.assertAlmostEqual(pose['y'], -4.0/ math.pi , delta=precision)
self.assertAlmostEqual(pose['z'], 0.0, delta=precision)
self.assertAlmostEqual(pose['yaw'], -math.pi/2.0, delta=precision)
self.assertAlmostEqual(pose['pitch'], 0.0, delta=precision)
self.assertAlmostEqual(pose['roll'], 0.0, delta=precision)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(Pioneer3DXTest)
######
# sending again the goal, and ask for cancellation
self.cb_fired = False
client.send_goal(goal1, done_cb = self.cb_preempted)
time.sleep(1)
client.cancel_goal()
self.check_not_moving()
self.assertTrue(self.cb_fired)
######
# sending again the goal, this time, interrupted by another one
self.cb_fired = False
client.send_goal(goal1, done_cb = self.cb_preempted)
time.sleep(1)
client2.send_goal(goal2, done_cb = self.cb_succeeded)
time.sleep(0.5)
self.assertTrue(self.cb_fired)
self.cb_fired = False
time.sleep(5)
self.assertTrue(self.cb_fired)
######
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(RosActionsTest)
'points': [{
'positions': [0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0],
'time_from_start': 1.0
}]
}
act = simu.robot.arm.trajectory(traj2)
time.sleep(0.5)
self.checkstate(simu, [0.0] * 7)
time.sleep(0.5)
time.sleep(1)
self.checkstate(simu, [0.0, 1.0, 0, 0, 0, 0, 0])
time.sleep(1)
self.checkstate(simu, [0.0, 1.0, 0, 0, 0, 0, 0])
# Check action cancellation
act = simu.robot.arm.set_rotations([0.0] * 7)
traj2['starttime'] = time.time() + 1
act = simu.robot.arm.trajectory(traj2)
time.sleep(0.5)
act.cancel()
time.sleep(1)
self.checkstate(simu, [0.0] * 7)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(ArmatureTest)
sick.create_laser_arc()
env = Environment('indoors-1/boxes', fastmode=True)
def test_sick_laser(self):
rospy.init_node('morse_ros_laser_testing', log_level=rospy.DEBUG)
motion_topic = '/robot/motion'
laser_topic = '/robot/sick'
pub_vw(motion_topic, 1, 1)
old = []
for step in range(5):
msg = rospy.wait_for_message(laser_topic, LaserScan, 10)
self.assertEqual(len(msg.ranges), 181) # 180 + center ray
self.assertNotEqual(msg.ranges, old)
old = msg.ranges
# assert that : near <= distance <= far
for distance in msg.ranges:
self.assertGreaterEqual(distance, 0.1)
self.assertLessEqual(distance, 30)
time.sleep(0.2) # wait for turning
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(SickLaserRosTest, time_modes = [TimeStrategies.BestEffort])
self.assertAlmostEqual(self.z, 0., delta=precision)
self.assertAlmostEqual(self.yaw, 0., delta=precision)
self.assertAlmostEqual(self.roll, 0., delta=precision)
self.assertAlmostEqual(self.pitch, 0., delta=precision)
send_speed(self.moos_comms, morse, -1., 0., 2.)
# read the new position, it must be (0., 0., 0., 0., 0., 0.)
self.assertAlmostEqual(self.x, 0., delta=precision)
self.assertAlmostEqual(self.y, 0., delta=precision)
self.assertAlmostEqual(self.z, 0., delta=precision)
self.assertAlmostEqual(self.yaw, 0., delta=precision)
self.assertAlmostEqual(self.roll, 0., delta=precision)
self.assertAlmostEqual(self.pitch, 0., delta=precision)
send_speed(self.moos_comms, morse, 1., -math.pi / 4., 2.)
# read the new position, it must be (0., 0., 0., 0., 0., 0.)
self.assertAlmostEqual(self.x, 4. / math.pi, delta=precision)
self.assertAlmostEqual(self.y, -4. / math.pi, delta=precision)
self.assertAlmostEqual(self.z, 0., delta=precision)
self.assertAlmostEqual(self.yaw, -math.pi / 2., delta=precision)
self.assertAlmostEqual(self.pitch, 0., delta=precision)
self.assertAlmostEqual(self.roll, 0., delta=precision)
# close moos_comms properly
self.moos_comms.close(True)
if __name__ == "__main__":
from morse.testing.testing import main
main(MOOS_MW_Test, time_modes=[TimeStrategies.BestEffort])
bat_stream = morse.robot.barometer
teleport_stream = morse.robot.teleport
bat = bat_stream.get()
self.assertAlmostEqual(bat['pressure'], 101325.0, delta = 0.1)
# pressure is independant of position (x,y)
send_pose(teleport_stream, 5.0, 2.0, 0.0)
morse.sleep(0.01)
bat = bat_stream.get()
self.assertAlmostEqual(bat['pressure'], 101325.0, delta = 0.1)
# Pressure computed from
# http://www.digitaldutch.com/atmoscalc/
send_pose(teleport_stream, 0.0, 0.0, 100.0)
morse.sleep(0.01)
bat = bat_stream.get()
self.assertAlmostEqual(bat['pressure'], 100129.0, delta = 0.1)
send_pose(teleport_stream, 0.0, 0.0, 1000.0)
morse.sleep(0.01)
bat = bat_stream.get()
self.assertAlmostEqual(bat['pressure'], 89871.0, delta = 0.1)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(BarometerTest)
pose = self.pose_stream.get()
self.assertAlmostEqual(pose['yaw'], 0.0, delta=self.precision)
self.assertAlmostEqual(pose['pitch'], 0.0, delta=self.precision)
self.assertAlmostEqual(pose['roll'], 0.0, delta=self.precision)
self.assertAlmostEqual(pose['x'], 10.0, delta=self.precision)
self.assertAlmostEqual(pose['y'], 8.0, delta=self.precision)
self.assertAlmostEqual(pose['z'], 20.0, delta=self.precision)
# Test only one rotation each time, otherwise, it a bit more
# complex to check that it does the good transformation
# (without a matrix transformation library)
for i in range(0, 5):
self._test_one_pose(random.uniform(-30.0, 30.0),
random.uniform(-30.0, 30.0),
random.uniform(10.0, 50.0),
random.uniform(-math.pi, math.pi), 0, 0)
self._test_one_pose(random.uniform(-30.0, 30.0),
random.uniform(-30.0, 30.0),
random.uniform(10.0, 50.0), 0,
random.uniform(-math.pi, math.pi), 0)
self._test_one_pose(random.uniform(-30.0, 30.0),
random.uniform(-30.0, 30.0),
random.uniform(10.0, 50.0), 0, 0,
random.uniform(-math.pi, math.pi))
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(TeleportTest)
pos = gps_stream.get()
pos_mod = gps_mod_stream.last()
self.assertAlmostEqual(pos['x'], 100.0, delta=precision)
self.assertAlmostEqual(pos['y'], 200.0, delta=precision)
self.assertAlmostEqual(pos['z'], 50.0, delta=precision)
self.assertAlmostEqual(pos_mod['x'], 43.6008970, delta=geodetic_precision)
self.assertAlmostEqual(pos_mod['y'], 1.43510869, delta=geodetic_precision)
self.assertAlmostEqual(pos_mod['z'], 185.0039, delta=precision)
morse.deactivate('robot.teleport')
teleport_mod_stream.publish({'x': 43.6000883, 'y': 1.433372470, 'z': 135.1000, 'yaw' : 0.0, 'pitch' : 0.0, 'roll': 0.0})
morse.sleep(0.03)
pos = gps_stream.get()
pos_mod = gps_mod_stream.last()
self.assertAlmostEqual(pos['x'], 10.0, delta=precision)
self.assertAlmostEqual(pos['y'], 8.0, delta=precision)
self.assertAlmostEqual(pos['z'], 0.1, delta=precision)
self.assertAlmostEqual(pos_mod['x'], 43.6000883, delta=geodetic_precision)
self.assertAlmostEqual(pos_mod['y'], 1.433372470, delta=geodetic_precision)
self.assertAlmostEqual(pos_mod['z'], 135.1000, delta=precision)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(GeodeticModifierTest)
motion = MotionVW()
motion.add_stream('socket')
robot.append(motion)
collision = Collision()
collision.properties(collision_property="obstacle")
collision.add_stream('socket')
collision.translate(x = 0.7, z = 0.2)
robot.append(collision)
robot.translate(x = -3.0)
env = Environment('empty', fastmode = True)
def test_friction(self):
with Morse() as sim:
collision = sim.robot.collision.get(timeout=0.1)
self.assertEqual(collision, None)
send_speed(sim.robot.motion, 1.0, 0.0, 1.0)
collision = sim.robot.collision.get(timeout=0.1)
self.assertNotEqual(collision, None)
self.assertEqual(collision['objects'], "dala")
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(FrictionTest)
def setUpEnv(self):
robot = ATRV()
imu = IMU()
robot.append(imu)
imu_noised = IMU()
imu_noised.alter("Noise", gyro_std=1, accel_std=1)
robot.append(imu_noised)
robot.add_default_interface("socket")
env = Environment("empty", fastmode=True)
env.add_interface("socket")
def test_noised_imu(self):
""" Test if the IMU data is noised
"""
with Morse() as morse:
d = morse.robot.imu.get()
dn = morse.robot.imu_noised.get()
for i in ["angular_velocity", "linear_acceleration"]:
for j in range(0, 3):
self.assertNotAlmostEqual(d[i][j], dn[i][j], delta=0.001)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(NoiseTest)
# get a new image from the camera in gray
imageA = capture8u(camera_stream)#, imageA_path)
# assert that the camera image differ < .1 percent from the expected
self.assert_image_file_diff_less(imageA_path, imageA, 0.1)
# command the robot to rotate and wait that he does for 5 seconds max
in_time = rotate_robot_and_wait(orientation_stream, \
gyroscope_stream, 2.70, 5, precision)
# XXX robot might have not graphically turned yet! happens randomly!
# gyroscope can give its new orientation while the physics didnt update yet.
if DEBUG_PGM:
print("debug: rotate in time: %s (False = timeout)"%str(in_time))
# "flush" the camera stream for 1 second
flush_camera(camera_stream, 1.0)
# assert robot orienation is correct
self.assert_orientation(gyroscope_stream, 2.70, 0.0, 0.0, precision)
# get a new image from the camera in gray
imageB = capture8u(camera_stream)#, imageB_path)
# assert that the camera image differ < .1 percent from the expected
self.assert_image_file_diff_less(imageB_path, imageB, 0.2)
# assert that the second image differ > 4 percent from the first
self.assert_images_diff_greater(imageA, imageB, 4)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(CameraTest)
class PR2JointStateTest(RosTestCase):
# from the output of "rostopic echo /joint_states" on the PR2
pr2_joints_list = ['fl_caster_rotation_joint', 'fl_caster_l_wheel_joint', 'fl_caster_r_wheel_joint', 'fr_caster_rotation_joint', 'fr_caster_l_wheel_joint', 'fr_caster_r_wheel_joint', 'bl_caster_rotation_joint', 'bl_caster_l_wheel_joint', 'bl_caster_r_wheel_joint', 'br_caster_rotation_joint', 'br_caster_l_wheel_joint', 'br_caster_r_wheel_joint', 'torso_lift_joint', 'torso_lift_motor_screw_joint', 'head_pan_joint', 'head_tilt_joint', 'laser_tilt_mount_joint', 'r_upper_arm_roll_joint', 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_forearm_roll_joint', 'r_elbow_flex_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint', 'r_gripper_joint', 'r_gripper_l_finger_joint', 'r_gripper_r_finger_joint', 'r_gripper_r_finger_tip_joint', 'r_gripper_l_finger_tip_joint', 'r_gripper_motor_screw_joint', 'r_gripper_motor_slider_joint', 'l_upper_arm_roll_joint', 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_forearm_roll_joint', 'l_elbow_flex_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint', 'l_gripper_joint', 'l_gripper_l_finger_joint', 'l_gripper_r_finger_joint', 'l_gripper_r_finger_tip_joint', 'l_gripper_l_finger_tip_joint', 'l_gripper_motor_screw_joint', 'l_gripper_motor_slider_joint']
def setUpEnv(self):
print("Adding a PR2 robot...")
pr2 = BasePR2()
pr2.add_interface("ros")
env = Environment('empty', fastmode = True)
env.aim_camera([1.0470, 0, 0.7854])
def test_base_jointstates(self):
rospy.loginfo("Creating listener node to check if posture of PR2 is published.")
rospy.init_node('pr2_jointstate_listener', log_level = rospy.DEBUG, disable_signals=True)
rospy.loginfo("Subscribing to pr2_posture topic.")
jointstate_msg = rospy.client.wait_for_message("joint_states", JointState)
name_len = len(jointstate_msg.name)
pos_len = len(jointstate_msg.position)
rospy.loginfo("Checking if number of jointstate names equals number of jointstate positions.")
self.assertEqual(pos_len, name_len, 'Found %s jointstate names but %s jointstate positions. Please check the configuration of your pr2_posture sensor and middleware!'%(name_len, pos_len))
rospy.loginfo("Checking is every jointstate is present.")
self.assertEqual(set(self.pr2_joints_list), set(jointstate_msg.name), 'Could not find all joints of the PR2. Please check if you named the joints correctly in your pr2_posture sensor and middleware!' )
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(PR2JointStateTest)
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)
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sync:
sync.connect(('localhost', 5000))
# Now the clock is blocked until we triggered it.
# Checking it :)
time.sleep(0.2)
prev_clock = clock_stream.last()
time.sleep(0.2)
clock = clock_stream.last()
self.assertTrue(clock['timestamp'] == prev_clock['timestamp'])
# triggering once
sync.send(bytes('foo', 'utf-8'))
clock = clock_stream.get()
self.assertAlmostEqual(clock['timestamp'] - prev_clock['timestamp'], 0.1, delta = 0.0001)
# So cool, isn't it :)
# Close the socket, no more control
prev_clock = clock_stream.last()
time.sleep(0.2)
clock = clock_stream.last()
self.assertTrue(clock['timestamp'] > prev_clock['timestamp'])
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(SocketSyncTest, time_modes = [TimeStrategies.FixedSimulationStep])
def test_base_jointstates(self):
rospy.loginfo(
"Creating listener node to check if posture of PR2 is published.")
rospy.init_node('pr2_jointstate_listener',
log_level=rospy.DEBUG,
disable_signals=True)
rospy.loginfo("Subscribing to pr2_posture topic.")
jointstate_msg = rospy.client.wait_for_message("joint_states",
JointState)
name_len = len(jointstate_msg.name)
pos_len = len(jointstate_msg.position)
rospy.loginfo(
"Checking if number of jointstate names equals number of jointstate positions."
)
self.assertEqual(
pos_len, name_len,
'Found %s jointstate names but %s jointstate positions. Please check the configuration of your pr2_posture sensor and middleware!'
% (name_len, pos_len))
rospy.loginfo("Checking is every jointstate is present.")
self.assertEqual(
set(self.pr2_joints_list), set(jointstate_msg.name),
'Could not find all joints of the PR2. Please check if you named the joints correctly in your pr2_posture sensor and middleware!'
)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(PR2JointStateTest)
IK_TARGET = "ik_target.robot.arm.kuka_7"
with Morse() as simu:
self.assertEqual(simu.robot.arm.list_IK_targets(), [IK_TARGET])
self._check_pose(simu, 0., 0., 1.3105, 0.)
simu.robot.arm.move_IK_target(IK_TARGET, [0,0,2], None, False).result() # absolute location
self._check_pose(simu, 0., 0., 1.3105, 0.)
simu.robot.arm.move_IK_target(IK_TARGET, [1,0,0.3105], None, False).result()
self._check_pose(simu, 0.778, 0., 0.363, 0.02)
simu.robot.arm.move_IK_target(IK_TARGET, [1,0,0.3105], [math.pi/2, -math.pi/2, -math.pi], False).result() # arm should be horizontal
self._check_pose(simu, 1.0, 0., 0.3105, math.radians(90))
# back to original position
simu.robot.arm.move_IK_target(IK_TARGET, [0,0,2], [math.pi/2, 0., -math.pi], False).result() # absolute location
self._check_pose(simu, 0., 0., 1.3105, 0.)
simu.robot.arm.move_IK_target(IK_TARGET, [-1, 0, -1.6895], None).result() # relative position
self._check_pose(simu, -0.778, 0., 0.363, -0.02)
simu.robot.arm.move_IK_target(IK_TARGET, [0.,0.,0.], [0., -math.pi/2, 0.]).result() # relative rotation
self._check_pose(simu, -1.0, 0., 0.3105, -math.radians(90))
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(ArmatureTest, time_modes = [TimeStrategies.BestEffort])
env = Environment('empty', fastmode = True)
env.add_service('socket')
def test_vw_controller(self):
with Morse() as morse:
# Read the start position, it must be (0.0, 0.0, 0.0)
pose_stream = morse.robot.pose
pose = pose_stream.get()
for key,coord in pose.items():
if key == 'z':
self.assertAlmostEqual(coord, 0.10, delta=0.03)
elif key == 'yaw':
self.assertAlmostEqual(coord, math.pi/2, delta=0.03)
else:
self.assertAlmostEqual(coord, 0.0, delta=0.03)
v_w = morse.robot.motion
send_speed(v_w, 1.0, -math.pi/4.0, 2.0)
pose = pose_stream.get()
self.assertAlmostEqual(pose['x'], 0.75, delta=0.15)
self.assertAlmostEqual(pose['y'], 1.75, delta=0.15)
self.assertAlmostEqual(pose['yaw'], 0.75, delta=0.15)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(Rotated_Segway_Test)
motion = MotionVW()
robot.append(motion)
motion.add_stream('socket')
camera = DepthCamera()
camera.translate(z=1)
camera.frequency(3)
robot.append(camera)
camera.add_stream('socket')
env = Environment('indoors-1/boxes')
def test_depth_camera(self):
with Morse() as morse:
morse.robot.publish({'v':1,'w',1})
for step in range(5):
msg = morse.robot.camera.get()
data = base64.b64decode(msg['points'])
for i in range(0,len(data)-12,12):
xyz = struct.unpack('fff',data[i:i+12])
self.assertTrue(xyz[2]>=1 and xyz[2]<=20)
morse.sleep(0.2)
if __name__==__'main'__:
from morse.testing.testing import main
main(DepthCameraTest)
""" Defines the test scenario, using the Builder API.
"""
robot = Morsy()
robot.name = "toto.toto"
env = Environment('empty', fastmode = True)
env.add_service('socket')
def test_builder_script(self):
pass # we only test the parsing of the script
class NoEnvironment_Test(MorseBuilderFailureTestCase):
def setUpEnv(self):
""" Defines the test scenario, using the Builder API.
"""
robot = Morsy()
env = Environment('i_do_not_exist', fastmode = True)
env.add_service('socket')
def test_builder_script(self):
pass # we only test the parsing of the script
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(InvalidName_Test, NoEnvironment_Test)
rospy.init_node('morse_ros_video_testing', log_level=rospy.DEBUG)
motion_topic = '/robot/motion'
camera_topic = '/robot/camera/image'
camnfo_topic = '/robot/camera/camera_info'
pub_vw(motion_topic, 1, 1)
old = []
for step in range(2):
msg = rospy.wait_for_message(camnfo_topic, CameraInfo, 10)
camera_info_frame = msg.header.frame_id
# might want to add more CameraInfo test here
msg = rospy.wait_for_message(camera_topic, Image, 10)
self.assertEqual(msg.header.frame_id, camera_info_frame)
self.assertEqual(len(msg.data), 128*128*4) # RGBA
# dont use assertNotEqual here
# dumping raw image data in log is not relevant
self.assertTrue(msg.data != old)
old = msg.data
time.sleep(0.2) # wait for turning
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(VideoCameraRosTest, time_modes = [TimeStrategies.BestEffort])
return self.sensor_message
def cleanup_sensor_test(self):
self.sensor_sub.unregister()
testlogger.debug("cleanup sensor")
def test_video_camera(self):
msg = self.init_sensor_test('/ATRV/Odometry', nav_msgs.msg.Odometry)
precision = 0.15 # we start at the origine
self.assertAlmostEqual(msg.pose.pose.position.x, 0.0, delta=precision)
self.assertAlmostEqual(msg.pose.pose.position.y, 0.0, delta=precision)
self.assertAlmostEqual(msg.pose.pose.position.z, 0.0, delta=precision)
# see http://ros.org/doc/api/nav_msgs/html/msg/Odometry.html
msg = self.init_sensor_test('/ATRV/Camera/image', Image)
self.assertEqual(len(msg.data), 128 * 128 * 4) # RGBA
self.send_speed(1.0, math.pi / 2.0, 2.0)
msg = self.wait_for_message()
# TODO: more test
self.cleanup_sensor_test()
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(VideoCameraTest)
robot = ATRV()
robot.translate(10.0, 8.0, 0.0)
gps = GPS()
gps.add_stream('socket')
robot.append(gps)
env = Environment('empty', fastmode = True)
env.add_service('socket')
def test_read_gps(self):
""" Test if we can connect to the pose data stream, and read from it.
"""
with Morse() as morse:
gps_stream = morse.robot.gps
pos = gps_stream.get()
precision = 0.02
self.assertAlmostEqual(pos['x'], 10.0, delta=precision)
self.assertAlmostEqual(pos['y'], 8.0, delta=precision)
# Z = 0.1 : pose of the ATRV's center relative to the world
self.assertAlmostEqual(pos['z'], 0.1, delta=precision)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(GPSTest)
robot.append(camera)
camera.add_stream('ros')
env = Environment('indoors-1/boxes')
# No fastmode here, no MaterialIndex in WIREFRAME mode: AttributeError:
# 'KX_PolygonMaterial' object has no attribute 'getMaterialIndex'
def test_depth_camera(self):
rospy.init_node('morse_ros_depth_testing', log_level=rospy.DEBUG)
motion_topic = '/robot/motion'
camera_topic = '/robot/camera'
pub_vw(motion_topic, 1, 1)
for step in range(5):
msg = rospy.wait_for_message(camera_topic, PointCloud2, 10)
# assert that : near <= z <= far
for i in range(0, len(msg.data) - 12, 12):
xyz = struct.unpack('fff', msg.data[i:i + 12])
self.assertTrue(1 <= xyz[2] <= 20)
time.sleep(0.2) # wait for turning
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(DepthCameraRosTest, time_modes=[TimeStrategies.BestEffort])
motion = MotionVW()
motion.add_stream('socket')
robot.append(motion)
collision = Collision()
collision.properties(collision_property="obstacle")
collision.add_stream('socket')
collision.translate(x = 0.7, z = 0.2)
robot.append(collision)
robot.translate(x = -3.0)
env = Environment('empty', fastmode = True)
def test_friction(self):
with Morse() as sim:
collision = sim.robot.collision.get(timeout=0.1)
self.assertEqual(collision, None)
send_speed(sim.robot.motion, sim, 1.0, 0.0, 1.0)
collision = sim.robot.collision.get(timeout=0.1)
self.assertNotEqual(collision, None)
self.assertEqual(collision['objects'], "dala")
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(FrictionTest)
sick.create_laser_arc()
env = Environment('indoors-1/boxes', fastmode=True)
def test_sick_laser(self):
rospy.init_node('morse_ros_laser_testing', log_level=rospy.DEBUG)
motion_topic = '/robot/motion'
laser_topic = '/robot/sick'
pub_vw(motion_topic, 1, 1)
old = []
for step in range(5):
msg = rospy.wait_for_message(laser_topic, LaserScan, 10)
self.assertEqual(len(msg.ranges), 181) # 180 + center ray
self.assertTrue(msg.ranges != old)
old = msg.ranges
# assert that : near <= distance <= far
for distance in msg.ranges:
self.assertTrue(0.1 <= distance <= 30)
time.sleep(0.2) # wait for turning
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(SickLaserRosTest, time_modes=[TimeStrategies.BestEffort])
z = pose['z']
send_goal(dest_client, x, y, -30.0)
morse.sleep(5.0)
# Only Z has changed
# XXX precision is not really good
pose = pose_stream.get()
self.assertAlmostEqual(pose['yaw'], 0.0, delta=precision)
self.assertAlmostEqual(pose['pitch'], 0.0, delta=precision)
self.assertAlmostEqual(pose['roll'], 0.0, delta=precision)
self.assertAlmostEqual(pose['x'], x, delta=precision)
self.assertAlmostEqual(pose['y'], y, delta=precision)
self.assertAlmostEqual(pose['z'], -30.0, delta=0.3)
send_goal(dest_client, 0, 200, -20)
morse.sleep(10.0)
pose = pose_stream.get()
self.assertAlmostEqual(pose['yaw'], 0.0, delta=precision)
self.assertAlmostEqual(pose['pitch'], 0.0, delta=precision)
self.assertAlmostEqual(pose['roll'], 0.0, delta=precision)
self.assertLess(math.fabs(pose['x']), 0.3)
self.assertLess(math.fabs(pose['y'] -200), 0.3)
self.assertLess(math.fabs(pose['z'] + 20), 0.3)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(DestinationTest)
with Morse() as morse:
# Read the start position, it must be (0.0, 0.0, 0.0)
self.pose_stream = morse.robot.pose
self.odo_stream = morse.robot.odo
self.integ_odo_stream = morse.robot.integ_odo
self.motion = morse.robot.motion
self.clear_datas(0.0, 0.0, 0.0)
self.odometry_test_helper(morse, 1.0, 0.0, 2.0)
self.verify(2.0, 0.0, 0.0)
self.odometry_test_helper(morse, -1.0, 0.0, 2.0)
self.verify(0.0, 0.0, 0.0)
self.odometry_test_helper(morse, 1.0, -math.pi / 4.0, 2.0)
self.verify(4.0 / math.pi, -4.0 / math.pi, -math.pi / 2.0)
self.odometry_test_helper(morse, 0.5, -math.pi / 8.0, 12.0)
self.verify(0.0, 0.0, 0.0)
# XXX fail Y with 0.11 delta
#self.odometry_test_helper(morse, -2.0, math.pi/2.0, 3.0)
#self.verify(4.0 / math.pi, -4.0/math.pi, -math.pi/2.0)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(OdometryTest)
# Read the start position, it must be (0.0, 0.0, 0.0)
self.assertAlmostEqualPositionThenFix(morse, [0.0, 0.0, 0.10, 0.0, 0.0, 0.0], precision)
send_speed(xyw, morse, 1.0, 0.0, 0.0, 2.0)
self.assertAlmostEqualPositionThenFix(morse, [2.0, 0.0, 0.10, 0.0, 0.0, 0.0], precision)
send_speed(xyw, morse, 0.0, -1.0, 0.0, 2.0)
self.assertAlmostEqualPositionThenFix(morse, [2.0, -2.0, 0.10, 0.0, 0.0, 0.0], precision)
send_speed(xyw, morse, -1.0, 1.0, 0.0, 2.0)
self.assertAlmostEqualPositionThenFix(morse, [0.0, 0.0, 0.10, 0.0, 0.0, 0.0], precision)
send_speed(xyw, morse, 1.0, 0.0, -math.pi/4.0, 2.0)
self.assertAlmostEqualPositionThenFix(morse, [4.0 / math.pi, -4.0 / math.pi, 0.10,
-math.pi / 2.0, 0.0, 0.0], precision)
send_speed(xyw, morse, 0.5, 0.0, -math.pi/8.0, 12.0)
self.assertAlmostEqualPositionThenFix(morse, [0.0, 0.0, 0.10, 0.0, 0.0, 0.0], precision)
send_speed(xyw, morse, -2.0, 0.0, math.pi/2.0, 3.0)
self.assertAlmostEqualPositionThenFix(morse, [4.0 / math.pi, -4.0 / math.pi, 0.10,
-math.pi / 2.0, 0.0, 0.0], precision*2)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(XYW_Test)
semantic_stream = morse.robot.camera
teleport_client = morse.robot.motion
o = semantic_stream.get()
objects = o['visible_objects']
self.assertEqual(objects, [])
# Change the orientation of the robot using the v_w socket
send_dest(teleport_client, morse, 0.0, 0.0, 5.0 / 4.0 * math.pi)
# Second test for the sensor, with BlueBox in front
o = semantic_stream.get()
objects = o['visible_objects']
self.assertEqual(objects, [])
send_dest(teleport_client, morse, -5.0, 0.0, math.pi)
# Third test for the sensor, with RedBox in front
o = semantic_stream.get()
objects = o['visible_objects']
self.assertEqual(len(objects), 1)
self.assertEqual(objects[0]['name'], 'RedBox')
self.assertAlmostEqual(objects[0]['position'][0], -7.48, delta=0.1)
self.assertAlmostEqual(objects[0]['position'][1], 0.0, delta=0.1)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(Semantic_Camera_Test)
pos = gps_stream.get()
pos_mod = gps_mod_stream.last()
self.assertAlmostEqual(pos['x'], 100.0, delta=precision)
self.assertAlmostEqual(pos['y'], 200.0, delta=precision)
self.assertAlmostEqual(pos['z'], 50.0, delta=precision)
self.assertAlmostEqual(pos_mod['x'], 4617493.0, delta=precision)
self.assertAlmostEqual(pos_mod['y'], 4397317.5, delta=precision)
self.assertAlmostEqual(pos_mod['z'], 158674.484375, delta=precision)
morse.deactivate('robot.teleport')
teleport_mod_stream.publish({'x': 4617522.5, 'y': 4397221.0, 'z':158481.296875, 'yaw' : 0.0, 'pitch' : 0.0, 'roll': 0.0})
morse.sleep(0.03)
pos = gps_stream.get()
pos_mod = gps_mod_stream.last()
self.assertAlmostEqual(pos['x'], 10.0, delta=0.15)
self.assertAlmostEqual(pos['y'], 8.0, delta=0.15)
self.assertAlmostEqual(pos['z'], 0.1, delta=0.15)
self.assertAlmostEqual(pos_mod['x'], 4617522.5, delta=precision)
self.assertAlmostEqual(pos_mod['y'], 4397221.0, delta=precision)
self.assertAlmostEqual(pos_mod['z'], 158481.296875, delta=precision)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(ECEFModifierTest)
self.assertAlmostEqual(pose['roll'], 0.0, delta=precision)
self.assertAlmostEqual(pose['x'], 10.0, delta=precision)
self.assertAlmostEqual(pose['y'], 8.0, delta=precision)
self.assertAlmostEqual(pose['z'], 20.0, delta=precision)
# Complete orientation settings
send_angles(orientation_stream, -math.pi / 2, math.pi / 8, 0.89)
morse.sleep(0.1)
pose = pose_stream.get()
angles = gyro_stream.get()
self.assertAlmostEqual(angles['yaw'],
-math.pi / 2,
delta=precision)
self.assertAlmostEqual(angles['pitch'],
math.pi / 8,
delta=precision)
self.assertAlmostEqual(angles['roll'], 0.89, delta=precision)
self.assertAlmostEqual(pose['yaw'], -math.pi / 2, delta=precision)
self.assertAlmostEqual(pose['pitch'], math.pi / 8, delta=precision)
self.assertAlmostEqual(pose['roll'], 0.89, delta=precision)
self.assertAlmostEqual(pose['x'], 10.0, delta=precision)
self.assertAlmostEqual(pose['y'], 8.0, delta=precision)
self.assertAlmostEqual(pose['z'], 20.0, delta=precision)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(OrientationTest)
self.assertAlmostEqualPositionThenFix(simu, [0.0, 0.0, 0.10, 0.0, 0.0, 0.0], precision)
send_speed(v_w, simu, -2.0, math.pi/2.0, 3.0)
self.assertAlmostEqualPositionThenFix(simu, [4.0 / math.pi, -4.0 / math.pi, 0.10,
-math.pi / 2.0, 0.0, 0.0], precision*2)
def test_vw_service_controller(self):
with Morse() as simu:
precision = 0.10
simu.deactivate('robot.teleport')
# Read the start position, it must be (0.0, 0.0, 0.0)
self.assertAlmostEqualPositionThenFix(simu, [0.0, 0.0, 0.10, 0.0, 0.0, 0.0], precision)
v_w = simu.robot.motion
send_service_speed(v_w, simu, 1.0, 0.0, 2.0)
self.assertAlmostEqualPositionThenFix(simu, [2.0, 0.0, 0.10, 0.0, 0.0, 0.0], precision)
send_service_speed(v_w, simu, -1.0, 0.0, 2.0)
self.assertAlmostEqualPositionThenFix(simu, [0.0, 0.0, 0.10, 0.0, 0.0, 0.0], precision)
send_service_speed(v_w, simu, 1.0, -math.pi/4.0, 2.0)
self.assertAlmostEqualPositionThenFix(simu, [4.0 / math.pi, -4.0 / math.pi, 0.10,
-math.pi / 2.0, 0.0, 0.0], precision)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(VW_Test)
def _test_movement(self):
""" Tests the human can accept an actuator, and that it
work as expected to move around the human.
Currently disabled (the waypoint actuator can not move yet the human)
"""
with Morse() as morse:
#Read the start position, it must be (0.0, 0.0, 0.0)
pose_stream = morse.human.pose
# waypoint controller socket
v_w_client = morse.human.motion
v_w_client.publish({'x' : 2.0, 'y': 3.0, 'z': 0.0,
'tolerance' : 0.3,
'speed' : 1.0})
morse.sleep(5)
pose = pose_stream.get()
self.assertAlmostEqual(pose['x'], 2.0, delta=0.5)
self.assertAlmostEqual(pose['y'], 3.0, delta=0.5)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(HumanPoseTest)
AbstractComponent.close_context()
env = Environment('empty', fastmode = True)
env.create()
def test_renaming(self):
""" Tests the simulator can return the list of robots
This test is guaranteed to be started only when the simulator
is ready.
"""
with Morse() as morse:
p1 = morse.morsy.my_kuka.my_pose
p2 = morse.mana.arm.my_pose2
p3 = morse.dala.my_kuka.pose
p4 = morse.robot4.arm.pose
p5 = morse.foo.arm.pose
p6 = morse.bar.arm.pose
p7 = morse.m.arm.pose
p8 = morse.m_001.arm.pose
p9 = morse.m_002.arm.pose
p10 = morse.m_003.arm.pose
# ...
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(RenamingTest)
integ_odo.level("integrated")
robot.append(integ_odo)
integ_odo.add_stream("socket")
env = Environment('empty', fastmode = True)
env.add_service('socket')
def test_levels(self):
with Morse() as morse:
odo = morse.robot.odo.get()
diff_odo = morse.robot.diff_odo.get()
raw_odo = morse.robot.raw_odo.get()
integ_odo = morse.robot.integ_odo.get()
self.assertEquals(set(['timestamp', 'dS']), set(raw_odo.keys()))
self.assertEquals(set(['timestamp',
'x', 'y', 'z', 'yaw', 'pitch', 'roll',
'vx', 'vy', 'vz', 'wz', 'wy', 'wx']),
set(integ_odo.keys()))
self.assertEquals(set(['timestamp', 'dx', 'dy', 'dz', 'dyaw', 'dpitch', 'droll']),
set(diff_odo.keys()))
self.assertEquals(set(integ_odo.keys()), set(odo.keys()))
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(LevelsTest)
def _teleport_and_test(self, x, y, z, mag_x, mag_y, mag_z):
precision = 0.1
self.teleport_stream.publish({'x': x, 'y': y, 'z' : z, 'yaw': 0.0,
'pitch': 0.0, 'roll': 0.0})
self.m.sleep(0.1)
mag = self.mag_stream.get()
self.assertAlmostEquals(mag['x'], mag_x, delta = precision)
self.assertAlmostEquals(mag['y'], mag_y, delta = precision)
self.assertAlmostEquals(mag['z'], mag_z, delta = precision)
def test_geodetic_modifier(self):
""" Test if we can connect to the pose data stream, and read from it.
"""
with Morse() as morse:
self.m = morse
self.mag_stream = morse.robot.mag
self.teleport_stream = morse.robot.teleport
# Tests from WMM2015
self._teleport_and_test(0, 80, 0, 6627.1, -445.9, 54432.3)
self._teleport_and_test(120, 0, 0, 39518.2, 392.9, -11252.4)
self._teleport_and_test(240, -80, 0, 5797.3, 15761.1, -52919.1)
self._teleport_and_test(0, 80, 100000, 6314.3, -471.6, 52269.8)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(MagnemoterTest)
# left hand, palm facing up
skeleton.move_IK_target(IK_TARGET_WRIST_L, [0.44,0.31,1.1], [-3,1.6,1.5], False).result()
self._check_pose(skeleton, -0.087, -0.016, -0.013, 0.33, -0.01)
# back to rest pose
skeleton.move_IK_target(IK_TARGET_WRIST_L, [0.03,0.29,0.87], [-1.97,0.,1.47], False).result()
self._check_pose(skeleton, -0.006, -0.016, -0.013, 0., 0.01)
# right hand above head
skeleton.move_IK_target(IK_TARGET_WRIST_R, [0.04,-0.15,1.76], [-1.41,1.8,-1.33], False).result()
self._check_pose(skeleton, 0.35, -0.011,-0.013, -0.059, 0.26)
# back to rest pose
skeleton.move_IK_target(IK_TARGET_WRIST_R, [0.05,-0.25,0.88], [1.33,2.91,-1.11], False).result()
self._check_pose(skeleton, -0.006, -0.016, -0.013, 0., 0.01)
# head looking left (relative motion)
skeleton.move_IK_target(IK_TARGET_HEAD, [0.,0.,0.], [0.,0.,1.], True).result()
self._check_pose(skeleton, 0.35, -0.016, -0.013, 0.01, 0.02)
# head looking right (relative motion)
skeleton.move_IK_target(IK_TARGET_HEAD, [0.,0.,0.], [0.,0.,-2.], True).result()
self._check_pose(skeleton, -0.34, -0.016, -0.013, -0.01, 0.02)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(HumanIkTest)
self.assertAlmostEqual(coord, 0.0, delta=0.1)
def _test_movement(self):
""" Tests the human can accept an actuator, and that it
work as expected to move around the human.
Currently disabled (the waypoint actuator can not move yet the human)
"""
with Morse() as morse:
# Read the start position, it must be (0.0, 0.0, 0.0)
pose_stream = morse.human.pose
# waypoint controller socket
v_w_client = morse.human.motion
v_w_client.publish({"x": 2.0, "y": 3.0, "z": 0.0, "tolerance": 0.3, "speed": 1.0})
morse.sleep(5)
pose = pose_stream.get()
self.assertAlmostEqual(pose["x"], 2.0, delta=0.5)
self.assertAlmostEqual(pose["y"], 3.0, delta=0.5)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(HumanPoseTest)
self.assertAlmostEqual(pose['x'], 10.0, delta=0.5)
self.assertAlmostEqual(pose['y'], 5.0, delta=0.5)
self.assertAlmostEqual(pose['z'], 10.0, delta=0.5)
self.assertAlmostEqual(pose['yaw'], 1.0, delta=0.05)
def test_waypoint_service_controller(self):
with Morse() as morse:
pose_stream = morse.robot.pose
pose = pose_stream.get()
self.assertAlmostEqual(pose['x'], -1.24, delta=0.05)
self.assertAlmostEqual(pose['y'], 1.70, delta=0.05)
self.assertAlmostEqual(pose['z'], 1.81, delta=0.05)
self.assertAlmostEqual(pose['yaw'], 0, delta=0.05)
morse.rpc('robot.motion', 'goto', 10.0, 5.0, 10.0, 1.0, 0.5)
pose = pose_stream.get()
self.assertAlmostEqual(pose['x'], 10.0, delta=0.5)
self.assertAlmostEqual(pose['y'], 5.0, delta=0.5)
self.assertAlmostEqual(pose['z'], 10.0, delta=0.5)
self.assertAlmostEqual(pose['yaw'], 1.0, delta=0.05)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(RotorcraftWaypoints_Test)
