Esempio n. 1
0
            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)
Esempio n. 4
0
            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)
Esempio n. 5
0
        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)
Esempio n. 6
0
            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)
Esempio n. 7
0
    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)
Esempio n. 8
0
    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)
Esempio n. 11
0
            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)
Esempio n. 12
0
        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)
Esempio n. 13
0
        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])
Esempio n. 14
0
            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)
Esempio n. 16
0
            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)
Esempio n. 17
0
            # 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)
Esempio n. 18
0
    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)
Esempio n. 19
0
        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)
Esempio n. 20
0
            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)
Esempio n. 21
0
        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)
Esempio n. 23
0
        ######

        # 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)
Esempio n. 24
0
                '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)
Esempio n. 25
0
        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])
Esempio n. 26
0
            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])
Esempio n. 27
0
            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)

Esempio n. 28
0
            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)
Esempio n. 29
0
            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)
Esempio n. 30
0
        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)
Esempio n. 31
0
    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)
Esempio n. 32
0
            # 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)
Esempio n. 33
0
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)
Esempio n. 34
0
            pose = pose_stream.get()
            self.assertAlmostEqual(pose['x'], x + 28.0, delta=1.0)
            self.assertAlmostEqual(pose['y'], y, delta=1.5)

            # Backward move
            send_force(steer_client, 0.0, 10.0, 0.0)
            morse.sleep(11.0)
            send_force(steer_client, 0.0, 0.0, 10.0)
            morse.sleep(2.0)

            pose = pose_stream.get()
            self.assertAlmostEqual(pose['x'], x, delta=1.5)
            self.assertAlmostEqual(pose['y'], y, delta=1.0)

            # Turning
            send_force(steer_client, -1.0, -10.0, 0.0)
            morse.sleep(10)
            send_force(steer_client, 0.0, 0.0, 10.0)
            morse.sleep(2.0)
            pose = pose_stream.get()

            self.assertAlmostEqual(pose['yaw'], 2.25, delta=0.2)
            self.assertAlmostEqual(pose['x'], x - 2.5, delta=1.0)
            self.assertAlmostEqual(pose['y'], x - 7.0, delta=1.0)


########################## Run these tests ##########################
if __name__ == "__main__":
    from morse.testing.testing import main
    main(SteerForceTest)
Esempio n. 35
0
            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])
Esempio n. 36
0
    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)
Esempio n. 37
0
        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])
Esempio n. 38
0
        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)
Esempio n. 39
0
		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)
Esempio n. 41
0
        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])
Esempio n. 42
0
        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)
Esempio n. 43
0
        
        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)
Esempio n. 44
0
        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])
Esempio n. 45
0
        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)
Esempio n. 46
0
        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])
Esempio n. 47
0
            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)
Esempio n. 48
0
        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)
Esempio n. 49
0
            # 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)
Esempio n. 50
0
            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)
Esempio n. 51
0
            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)
Esempio n. 53
0
            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)
Esempio n. 54
0
    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)
Esempio n. 55
0
            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)
Esempio n. 56
0
        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)
Esempio n. 57
0
    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)
Esempio n. 58
0
            # 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)
Esempio n. 59
0
                    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)
Esempio n. 60
0
            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)