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)