def test_read_pose(self): """ Test if we can connect to the pose data stream, and read from it. """ morse = Morse() pose_stream = morse.stream('Pose') pose = pose_stream.get() for coord in pose.values(): self.assertAlmostEqual(coord, 0.0, 2) morse.close()
def test_waypoint_services(self): with Morse() as simu: # Read the start position, it must be (0.0, 0.0, 0.0) pose_stream = simu.robot.pose pose = pose_stream.get() for key, coord in pose.items(): if key != 'timestamp': self.assertAlmostEqual(coord, 0.0, delta=0.02) logger.info("Moving 2m ahead...") simu.robot.motion.goto(2.0, 0.0, 0.0, 0.1, 1.0).result() # wait for completion pose = pose_stream.get() self.assertAlmostEqual(pose['x'], 2.0, delta=0.1) self.assertAlmostEqual(pose['y'], 0.0, delta=0.1) logger.info("Ok.") action = simu.robot.motion.goto(4.0, 0.0, 0.0, 0.1, 1.0) # do not wait for completion logger.info("Moving for 1 sec...") sleep(1) pose = pose_stream.get() #should have done 1m self.assertAlmostEqual(pose['x'], 3.0, delta=0.15) logger.info("Ok, reached correct position") self.assertTrue(action.running()) self.assertFalse(action.done()) logger.info("Cancelling motion and waiting for 0.5 sec...") action.cancel() sleep(0.1) self.assertFalse(action.running()) self.assertTrue(action.done()) with self.assertRaises(MorseServicePreempted): action.result() sleep(0.5) pose = pose_stream.get() #should not have moved self.assertAlmostEqual(pose['x'], 3.0, delta=0.15) logger.info("Ok, did not move") logger.info( "Moving again, waiting for 2 sec, and ensuring the action terminate" ) action = simu.robot.motion.goto(4.0, 0.0, 0.0, 0.1, 1.0) # do not wait for completion sleep(2) self.assertTrue(action.done()) self.assertFalse(action.running()) # Stop will stop the robot, but do not erase current goal action = simu.robot.motion.goto(6.0, 0.0, 0.0, 0.1, 1.0) # do not wait for completion logger.info("Moving for 1 sec...") sleep(1) self.assertFalse(action.done()) self.assertTrue(action.running()) status = simu.robot.motion.get_status().result() self.assertEqual(status, "Transit") simu.robot.motion.stop().result() # Stop does not change the fact that the goto is pending, # but stop the move self.assertFalse(action.done()) self.assertTrue(action.running()) status = simu.robot.motion.get_status().result() self.assertEqual(status, "Stop") pose = pose_stream.get() #should have done 1m self.assertAlmostEqual(pose['x'], 5.0, delta=0.15) sleep(0.5) pose = pose_stream.get() #should not have moved self.assertAlmostEqual(pose['x'], 5.0, delta=0.15) logger.info("Ok, did not move") # now resume the move simu.robot.motion.resume().result() sleep(0.5) # must move now pose = pose_stream.get() self.assertAlmostEqual(pose['x'], 5.5, delta=0.15) status = simu.robot.motion.get_status().result() self.assertEqual(status, "Transit") # wait for the end of the move sleep(1.0) 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")
def test_datastream(self): """ Test if we can connect to the pose data stream, and read from it. """ with Morse() as morse: gyro_stream = morse.robot.ptu.gyro posture_stream = morse.robot.ptu.posture ptu_stream = morse.robot.ptu angles = gyro_stream.get() posture = posture_stream.get() precision = 0.02 moving_precision = 0.1 self.assertAlmostEqual(posture['pan'], 0.0, delta=precision) self.assertAlmostEqual(posture['tilt'], 0.0, delta=precision) self.assertAlmostEqual(angles['yaw'], 0.0, delta=precision) self.assertAlmostEqual(angles['pitch'], 0.0, delta=precision) send_angles(ptu_stream, 1.0, 0.0) time.sleep(1.0) # here at speed of 0.5 rad / sec, we must be at the middle # of the trip, check it :) angles = gyro_stream.get() posture = posture_stream.get() self.assertAlmostEqual(posture['pan'], 0.5, delta=moving_precision) self.assertAlmostEqual(posture['tilt'], 0.0, delta=moving_precision) self.assertAlmostEqual(angles['yaw'], 0.5, delta=moving_precision) self.assertAlmostEqual(angles['pitch'], 0.0, delta=moving_precision) time.sleep(1.0) # now we must have achieve ptu rotation angles = gyro_stream.get() posture = posture_stream.get() self.assertAlmostEqual(posture['pan'], 1.0, delta=precision) self.assertAlmostEqual(posture['tilt'], 0.0, delta=precision) self.assertAlmostEqual(angles['yaw'], 1.0, delta=precision) self.assertAlmostEqual(angles['pitch'], 0.0, delta=precision) send_angles(ptu_stream, 1.0, -1.0) time.sleep(2.0) angles = gyro_stream.get() posture = posture_stream.get() self.assertAlmostEqual(posture['pan'], 1.0, delta=precision) self.assertAlmostEqual(posture['tilt'], -1.0, delta=precision) self.assertAlmostEqual(angles['yaw'], 1.0, delta=precision) self.assertAlmostEqual(angles['pitch'], -1.0, delta=precision) send_angles(ptu_stream, 0.0, 0.0) time.sleep(2.0) angles = gyro_stream.get() posture = posture_stream.get() self.assertAlmostEqual(posture['pan'], 0.0, delta=precision) self.assertAlmostEqual(posture['tilt'], 0.0, delta=precision) self.assertAlmostEqual(angles['yaw'], 0.0, delta=precision) self.assertAlmostEqual(angles['pitch'], 0.0, 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: gps_stream = morse.robot.gps gps_mod_stream = morse.robot.gps_mod teleport_stream = morse.robot.teleport teleport_mod_stream = morse.robot.teleport_mod pos = gps_stream.get() pos_mod = gps_mod_stream.get() precision = 0.02 geodetic_precision = 0.000002 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) 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) teleport_stream.publish({ 'x': 100.0, 'y': 200.0, 'z': 50.0, 'yaw': 0.0, 'pitch': 0.0, 'roll': 0.0 }) morse.sleep(0.01) 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)
def test_base_service_connection(self): """ Simply tests if the simulator is reachable by its socket interface. """ morse = Morse() morse.close()
For real applications, you may want to rely on a full middleware, like ROS (www.ros.org). """ import sys try: from pymorse import Morse except ImportError: print("you need first to install pymorse, the Python bindings for MORSE!") sys.exit(1) print("Use WASD to control the robot") with Morse() as simu: motion = simu.robot.motion pose = simu.robot.pose v = 0.0 w = 0.0 while True: key = input("WASD?") if key.lower() == "w": v += 0.1 elif key.lower() == "s": v -= 0.1 elif key.lower() == "a":
class MorseController(AbstractController): def __init__(self, red_pixels): AbstractController.__init__(self, red_pixels) self._morse = Morse() self._left_engine = 0 self._right_engine = 0 def __enter__(self): self._morse.__enter__() return self def __exit__(self, type, value, traceback): return self._morse.__exit__(type, value, traceback) def get_randomize_states(self): return 6 def randomize(self, state=None): if state is None: state = random.randint(0, self.get_randomize_states() - 1) for i in range(self.get_randomize_states()): x = i % 3 y = i // 3 box_nr = 0 if state == i else i if state < i else i + 1 getattr(self._morse, "box_%d" % box_nr).teleport.publish({ "x": (-10 + x * 5), "y": (-4 + y * 8), "z": 0, "yaw": 0, "pitch": 0, "roll": 0 }) self.reset_environment() def reset_environment(self): self._morse.reset() # self._morse.rpc("robot", "randomize") #print(self._morse.robot.pose.get()) #self._morse.robot.teleport.publish({'x':(random()-0.5)*3,'y':(random()-0.5)*3,'z':0,'roll':0,'yaw':(random()-0.5)*3.14*2,'pitch':0}) #self._morse.robot.translate(1.0, 0.0, 0.0) #self._morse.robot.rotate(0.0, 0.0, 0.0) self.pause() def reached_aim(self): print("found cube!") self.pause() def pause(self): self.left_engine(0) self.right_engine(0) def _update_motion(self): w = self._left_engine - self._right_engine v = self._left_engine + self._right_engine w /= 4 v /= 4 self._morse.robot.motion.publish({"v": v, "w": w}) def left_engine(self, power): self._left_engine = power self._update_motion() def right_engine(self, power): self._right_engine = power self._update_motion() def capture_camera(self): image = np.frombuffer(base64.b64decode( self._morse.robot.camera.get()['image']), dtype=np.uint8) #shape = image.shape return image.reshape((40, 40, 4)) def _infrared_val(self, sensor): range_list = sensor.get()['range_list'] return sum(range_list) / float(len(range_list)) def _sonar_val(self, sensor): range_list = sensor.get()['range_list'] return np.median(range_list) def ultrasonic(self): value = self._sonar_val(self._morse.robot.sonar) return value / 10 def infrared_left(self): return self._infrared_val(self._morse.robot.ir_left) <= 1.9 def infrared_right(self): return self._infrared_val(self._morse.robot.ir_right) <= 1.9
def test(self): with Morse() as morse: pose_stream = morse.robot.pose pose = pose_stream.get() precision = 0.02 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'], 0.0, delta=precision) self.assertAlmostEqual(pose['y'], 0.0, delta=precision) self.assertAlmostEqual(pose['z'], 20.0, delta=0.1) dest_client = morse.robot.destination send_goal(dest_client, 10.0, 0.0, 20.0) morse.sleep(3.0) # Only x has changed. Check that speed is respected 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'], 3.0 * 2.0, delta=0.1) self.assertAlmostEqual(pose['y'], 0.0, delta=precision) self.assertAlmostEqual(pose['z'], 20.0, delta=0.1) morse.sleep(2.0) # Only x has changed 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'], 9.7, delta=0.1) self.assertAlmostEqual(pose['y'], 0.0, delta=precision) self.assertAlmostEqual(pose['z'], 20.0, delta=0.1) x = pose['x'] send_goal(dest_client, x, 10.0, 20.0) morse.sleep(5.0) # Only Y has changed 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=0.1) self.assertAlmostEqual(pose['y'], 9.7, delta=0.1) self.assertAlmostEqual(pose['z'], 20.0, delta=0.1) x = pose['x'] y = pose['y'] 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, 0, 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.assertTrue(math.fabs(pose['x']) < 0.3) self.assertTrue(math.fabs(pose['y']) < 0.3) self.assertTrue(math.fabs(pose['z'] - 20) < 0.3)
def test_phi_c_control(self): with Morse() as morse: pose_stream = morse.robot.pose cmd_client = morse.robot.motion pose = pose_stream.get() send_ctrl(cmd_client, 0.0, 0.0, 0.0, 10.0) morse.sleep(3.0) pose = pose_stream.get() self.assertAlmostEqual(pose['x'], 0.0, delta=0.2) self.assertAlmostEqual(pose['y'], 0.0, delta=0.2) self.assertAlmostEqual(pose['z'], 10.0, delta=0.2) # phi_c permits to control the acceleration on y send_ctrl(cmd_client, 0.0, 0.1, 0.0, 10.0) morse.sleep(1.0) pose1 = pose_stream.get() dy1 = -0.4 self.assertAlmostEqual(pose1['y'], dy1, delta=0.2) self.assertAlmostEqual(pose1['x'], 0.0, delta=0.2) self.assertAlmostEqual(pose1['z'], 10.0, delta=0.2) self.assertAlmostEqual(pose1['yaw'], 0.0, delta=0.02) self.assertAlmostEqual(pose1['roll'], 0.1, delta=0.1) self.assertAlmostEqual(pose1['pitch'], 0.0, delta=0.0) # acceleration is constant as long ass we waintain theta_c morse.sleep(1.0) pose2 = pose_stream.get() dy2 = -1.4 self.assertAlmostEqual(pose2['y'], dy1 + dy2, delta=0.2) self.assertAlmostEqual(pose2['x'], 0.0, delta=0.2) self.assertAlmostEqual(pose2['z'], 10.0, delta=0.2) self.assertAlmostEqual(pose2['yaw'], 0.0, delta=0.02) self.assertAlmostEqual(pose2['roll'], 0.1, delta=0.1) self.assertAlmostEqual(pose2['pitch'], 0.0, delta=0.0) # if we setup phi_c to 0, acceleration on y is now null, # so v_y is constant (no friction) send_ctrl(cmd_client, 0.0, 0.0, 0.0, 10.0) morse.sleep(1.0) pose3 = pose_stream.get() dy3 = -2.2 self.assertAlmostEqual(pose3['y'], dy1 + dy2 + dy3, delta=0.2) self.assertAlmostEqual(pose3['x'], 0.0, delta=0.2) self.assertAlmostEqual(pose3['z'], 10.0, delta=0.2) self.assertAlmostEqual(pose3['yaw'], 0.0, delta=0.02) self.assertAlmostEqual(pose3['pitch'], 0.0, delta=0.0) self.assertAlmostEqual(pose3['roll'], 0.0, delta=0.1) morse.sleep(1.0) pose4 = pose_stream.get() self.assertAlmostEqual(pose4['y'], dy1 + dy2 + 2 * dy3, delta=0.2) self.assertAlmostEqual(pose4['x'], 0.0, delta=0.2) self.assertAlmostEqual(pose4['z'], 10.0, delta=0.2) self.assertAlmostEqual(pose4['yaw'], 0.0, delta=0.0) self.assertAlmostEqual(pose4['pitch'], 0.0, delta=0.0) self.assertAlmostEqual(pose4['roll'], 0.0, delta=0.1) # adding a negative value to theta_c gives a negative # acceleration on x, leading to some stabilization at some # point t send_ctrl(cmd_client, 0.0, -0.1, 0.0, 10.0) last_y = 0.0 pose = pose_stream.get() cur_y = pose['y'] while cur_y - last_y < -0.005: morse.sleep(0.01) last_y = cur_y pose = pose_stream.get() cur_y = pose['y'] send_ctrl(cmd_client, 0.0, 0.0, 0.0, 10.0) morse.sleep(0.2) pose = pose_stream.get() morse.sleep(1.0) pose1 = pose_stream.get() self.assertAlmostEqual(pose['x'] - pose1['x'], 0.0, delta=0.05) self.assertAlmostEqual(pose['y'] - pose1['y'], 0.0, delta=0.2) self.assertAlmostEqual(pose['z'] - pose1['z'], 0.0, delta=0.05) self.assertAlmostEqual(pose['yaw'] - pose1['yaw'], 0.0, delta=0.05) self.assertAlmostEqual(pose['pitch'] - pose1['pitch'], 0.0, delta=0.05) self.assertAlmostEqual(pose['roll'] - pose1['roll'], 0.0, delta=0.05)
def getjoint(name): with Morse() as simu: joints = simu.pr2.posture.get() return joints[name]
def test_accel_sensor(self): with Morse() as morse: delta = 0.06 xyw = morse.robot.motion accel_stream = morse.robot.accel accel_pi_stream = morse.robot.accel_pi xyw.publish({'x': 1.0, 'y': 0.0, 'w': 0.0}) morse.sleep(0.01) acc = accel_stream.get() acc_pi = accel_pi_stream.last() self.assertGreater(acc['acceleration'][0], 50) self.assert_accel_almost_null(acc, 1) self.assertAlmostEqual(acc['velocity'][0], 1.0, delta=delta) self.assertAlmostEqual(acc['velocity'][1], 0.0, delta=delta) # acceleration phase self.assertLess(acc_pi['acceleration'][1], -50) self.assert_accel_almost_null(acc_pi, 0) self.assertAlmostEqual(acc_pi['velocity'][0], 0.0, delta=delta) self.assertAlmostEqual(acc_pi['velocity'][1], -1.0, delta=delta) morse.sleep(0.1) acc = accel_stream.get() acc_pi = accel_pi_stream.get() self.assert_accel_almost_null(acc, 0) self.assert_accel_almost_null(acc, 1) self.assertAlmostEqual(acc['velocity'][0], 1.0, delta=delta) self.assertAlmostEqual(acc['velocity'][1], 0.0, delta=delta) self.assertAlmostEquals(acc['distance'], 1 / 60, delta=delta) self.assert_accel_almost_null(acc_pi, 0) self.assert_accel_almost_null(acc_pi, 1) self.assertAlmostEqual(acc_pi['velocity'][1], -1.0, delta=delta) self.assertAlmostEqual(acc_pi['velocity'][0], 0.0, delta=delta) self.assertAlmostEquals(acc_pi['distance'], 1 / 60, delta=delta) xyw.publish({'x': 0.0, 'y': 0.0, 'w': 0.0}) morse.sleep(0.01) # decacceleration phase acc = accel_stream.last() acc_pi = accel_pi_stream.last() self.assertLess(acc['acceleration'][0], -50) self.assert_accel_almost_null(acc, 1) self.assertAlmostEqual(acc['velocity'][0], 0.0, delta=delta) self.assertAlmostEqual(acc['velocity'][1], 0.0, delta=delta) self.assertAlmostEquals(acc['distance'], 0.0, delta=delta) self.assert_accel_almost_null(acc_pi, 0) self.assertGreater(acc_pi['acceleration'][1], 50) self.assertAlmostEqual(acc_pi['velocity'][0], 0.0, delta=delta) self.assertAlmostEqual(acc_pi['velocity'][1], 0.0, delta=delta) self.assertAlmostEquals(acc_pi['distance'], 0.0, delta=delta) morse.sleep(0.1) acc = accel_stream.get() acc_pi = accel_pi_stream.get() self.assert_accel_almost_null(acc, 0) self.assert_accel_almost_null(acc, 1) self.assertAlmostEqual(acc['velocity'][0], 0.0, delta=delta) self.assertAlmostEqual(acc['velocity'][1], 0.0, delta=delta) self.assertAlmostEquals(acc['distance'], 0.0, delta=delta) self.assert_accel_almost_null(acc_pi, 0) self.assert_accel_almost_null(acc_pi, 1) self.assertAlmostEqual(acc_pi['velocity'][0], 0.0, delta=delta) self.assertAlmostEqual(acc_pi['velocity'][1], 0.0, delta=delta) self.assertAlmostEquals(acc_pi['distance'], 0.0, delta=delta) xyw.publish({'x': 0.0, 'y': 1.0, 'w': 0.0}) morse.sleep(0.1) acc = accel_stream.get() acc_pi = accel_pi_stream.get() self.assert_accel_almost_null(acc, 0) self.assert_accel_almost_null(acc, 1) self.assertAlmostEqual(acc['velocity'][0], 0.0, delta=delta) self.assertAlmostEqual(acc['velocity'][1], 1.0, delta=delta) self.assertAlmostEquals(acc['distance'], 1 / 60, delta=delta) self.assert_accel_almost_null(acc_pi, 0) self.assert_accel_almost_null(acc_pi, 1) self.assertAlmostEqual(acc_pi['velocity'][0], 1.0, delta=delta) self.assertAlmostEqual(acc_pi['velocity'][1], 0.0, delta=delta) self.assertAlmostEquals(acc_pi['distance'], 1 / 60, delta=delta)
#! /usr/bin/env python3 import sys import curses import os try: from pymorse import Morse except ImportError: print("you need first to install pymorse, the Python bindings for MORSE!") sys.exit(1) s = Morse() u1 = 0 u2 = 0 position = 0 linear_speed = 0 angular_speed = 0 position = 0 def pose_received(p): global position position = p def speed_received(s): global linear_speed
if floor in lift_commands: return lift_commands.append(floor) for t in ["call", "command"]: for i in range(0, 4): rospy.Subscriber("/lift_sim/%s%s" % (t, doors[i]), Bool, on_floor_call_command, callback_args=(t, i)) morse = None while morse is None: try: morse = Morse() except ConnectionError as e: # ConnectionError as e: rospy.loginfo("Waiting for Morse...") print("[!!!!!] ", e, "Waiting for Morse...") rospy.sleep(3) while not rospy.is_shutdown(): if len(lift_commands) > 1: rospy.loginfo("Closing door %s" % doors[lift_commands[0]]) morse.rpc('lift.door%s' % doors[lift_commands[0]], 'change_door_state', 0) lift_commands = lift_commands[1:] rospy.loginfo("Moving to floor %s" % doors[lift_commands[0]]) morse.rpc('lift.platform', 'move_to_floor', lift_commands[0] - 1) rospy.loginfo("Opening door %s" % doors[lift_commands[0]]) morse.rpc('lift.door%s' % doors[lift_commands[0]], 'change_door_state',
def test_theta_c_control(self): with Morse() as morse: pose_stream = morse.robot.pose cmd_client = morse.robot.motion pose = pose_stream.get() send_ctrl(cmd_client, 0.0, 0.0, 0.0, 10.0) time.sleep(3.0) pose = pose_stream.get() self.assertAlmostEqual(pose['x'], 0.0, delta=0.2) self.assertAlmostEqual(pose['y'], 0.0, delta=0.2) self.assertAlmostEqual(pose['z'], 10.0, delta=0.2) # theta_c permits to control the acceleration on x send_ctrl(cmd_client, 0.1, 0.0, 0.0, 10.0) time.sleep(1.0) pose1 = pose_stream.get() dx1 = 0.4 self.assertAlmostEqual(pose1['x'], dx1, delta=0.2) self.assertAlmostEqual(pose1['y'], 0.0, delta=0.2) self.assertAlmostEqual(pose1['z'], 10.0, delta=0.2) self.assertAlmostEqual(pose1['yaw'], 0.0, delta=0.02) self.assertAlmostEqual(pose1['pitch'], 0.1, delta=0.1) self.assertAlmostEqual(pose1['roll'], 0.0, delta=0.0) # acceleration is constant as long ass we waintain theta_c time.sleep(1.0) pose2 = pose_stream.get() dx2 = 1.4 self.assertAlmostEqual(pose2['x'], dx1 + dx2, delta=0.2) self.assertAlmostEqual(pose2['y'], 0.0, delta=0.2) self.assertAlmostEqual(pose2['z'], 10.0, delta=0.2) self.assertAlmostEqual(pose2['yaw'], 0.0, delta=0.02) self.assertAlmostEqual(pose2['pitch'], 0.1, delta=0.1) self.assertAlmostEqual(pose2['roll'], 0.0, delta=0.0) # if we setup theta_c to 0, acceleration on x is now null, # so v_x is constant (no friction) send_ctrl(cmd_client, 0.0, 0.0, 0.0, 10.0) time.sleep(1.0) pose3 = pose_stream.get() dx3 = 2.0 self.assertAlmostEqual(pose3['x'], dx1 + dx2 + dx3, delta=0.2) self.assertAlmostEqual(pose3['y'], 0.0, delta=0.2) self.assertAlmostEqual(pose3['z'], 10.0, delta=0.2) self.assertAlmostEqual(pose3['yaw'], 0.0, delta=0.02) self.assertAlmostEqual(pose3['pitch'], 0.0, delta=0.1) self.assertAlmostEqual(pose3['roll'], 0.0, delta=0.0) time.sleep(1.0) pose4 = pose_stream.get() self.assertAlmostEqual(pose4['x'], dx1 + dx2 + 2 * dx3, delta=0.2) self.assertAlmostEqual(pose4['y'], 0.0, delta=0.2) self.assertAlmostEqual(pose4['z'], 10.0, delta=0.2) self.assertAlmostEqual(pose4['yaw'], 0.0, delta=0.02) self.assertAlmostEqual(pose4['pitch'], 0.0, delta=0.1) self.assertAlmostEqual(pose4['roll'], 0.0, delta=0.0) # adding a negative value to theta_c gives a negative # acceleration on x, leading to some stabilization at some # point t send_ctrl(cmd_client, -0.1, 0.0, 0.0, 10.0) last_x = 0.0 pose = pose_stream.get() cur_x = pose['x'] while cur_x - last_x > 0.005: time.sleep(0.01) last_x = cur_x pose = pose_stream.get() cur_x = pose['x'] send_ctrl(cmd_client, 0.0, 0.0, 0.0, 10.0) time.sleep(0.2) pose = pose_stream.get() time.sleep(1.0) pose1 = pose_stream.get() self.assertAlmostEqual(pose['x'] - pose1['x'], 0.0, delta=0.2) self.assertAlmostEqual(pose['y'] - pose1['y'], 0.0, delta=0.05) self.assertAlmostEqual(pose['z'] - pose1['z'], 0.0, delta=0.05) self.assertAlmostEqual(pose['yaw'] - pose1['yaw'], 0.0, delta=0.05) self.assertAlmostEqual(pose['pitch'] - pose1['pitch'], 0.0, delta=0.05) self.assertAlmostEqual(pose['roll'] - pose1['roll'], 0.0, delta=0.05)
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.stream('Pose') pose = pose_stream.get() for key, coord in pose.items(): if key == 'z': self.assertAlmostEqual(coord, 0.10, delta=0.03) else: self.assertAlmostEqual(coord, 0.0, delta=0.03) # v_w socket port = morse.get_stream_port('Motion_Controller') v_w_client = socket.socket(socket.AF_INET, socket.SOCK_STREAM) v_w_client.connect(('localhost', port)) #send_speed(v_w_client, 0.0, -math.pi/4.0, 2.0) #pose = pose_stream.get() ## for non-null w, we have r = v / w #self.assertAlmostEqual(pose['x'], 0.0, delta=0.15) #self.assertAlmostEqual(pose['y'], 0.0, delta=0.15) #self.assertAlmostEqual(pose['z'], 0.10, delta=0.15) #self.assertAlmostEqual(pose['yaw'], -math.pi/2.0, delta=0.15) #self.assertAlmostEqual(pose['pitch'], 0.0, delta=0.15) #self.assertAlmostEqual(pose['roll'], 0.0, delta=0.15) #send_speed(v_w_client, 0.0, math.pi/4.0, 2.0) #pose = pose_stream.get() #for key,coord in pose.items(): # if key == 'z': # self.assertAlmostEqual(coord, 0.10, delta=0.15) # else: # self.assertAlmostEqual(coord, 0.0, delta=0.15) # PART ONE send_speed(v_w_client, 0.5, -math.pi / 8.0, 4.0) pose = pose_stream.get() # for non-null w, we have r = v / w self.assertAlmostEqual(pose['x'], 4.0 / math.pi, delta=0.20) self.assertAlmostEqual(pose['y'], -4.0 / math.pi, delta=0.20) self.assertAlmostEqual(pose['z'], 0.0, delta=0.20) self.assertAlmostEqual(pose['yaw'], -math.pi / 2.0, delta=0.20) self.assertAlmostEqual(pose['pitch'], 0.0, delta=0.20) self.assertAlmostEqual(pose['roll'], 0.0, delta=0.20) # PART TWO send_speed(v_w_client, 0.5, -math.pi / 16.0, 8.0) """ pose = pose_stream.get() self.assertAlmostEqual(pose['x'], 0.0/ math.pi , delta=0.20) self.assertAlmostEqual(pose['y'], -8.0/ math.pi , delta=0.20) self.assertAlmostEqual(pose['z'], 0.0, delta=0.20) self.assertAlmostEqual(pose['yaw'], -math.pi, delta=0.20) self.assertAlmostEqual(pose['pitch'], 0.0, delta=0.20) self.assertAlmostEqual(pose['roll'], 0.0, delta=0.20) """ send_speed(v_w_client, 0.5, -math.pi / 32.0, 16.0) """
def main(): with Morse() as morse: printPos(morse.robot1.pose) getYaw(morse.robot1.pose)
def test_xyw_controller(self): with Morse() as morse: precision = 0.08 # Read the start position, it must be (0.0, 0.0, 0.0) pose_stream = morse.robot.pose pose = pose_stream.get() for coord in pose.values(): self.assertAlmostEqual(coord, 0.0, delta=precision) # v_w socket xyw = morse.robot.motion send_speed(xyw, 1.0, 0.0, 0.0, 2.0) 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_speed(xyw, 0.0, -1.0, 0.0, 2.0) pose = pose_stream.get() self.assertAlmostEqual(pose['x'], 2.0, delta=precision) self.assertAlmostEqual(pose['y'], -2.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_speed(xyw, -1.0, 1.0, 0.0, 2.0) pose = pose_stream.get() for coord in pose.values(): self.assertAlmostEqual(coord, 0.0, delta=precision) send_speed(xyw, 1.0, 0.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) send_speed(xyw, 0.5, 0.0, -math.pi / 8.0, 12.0) pose = pose_stream.get() for coord in pose.values(): self.assertAlmostEqual(coord, 0.0, delta=precision) send_speed(xyw, -2.0, 0.0, math.pi / 2.0, 3.0) pose = pose_stream.get() self.assertAlmostEqual(pose['x'], 4.0 / math.pi, delta=0.1) self.assertAlmostEqual(pose['y'], -4.0 / math.pi, delta=0.1) self.assertAlmostEqual(pose['z'], 0.0, delta=0.1) self.assertAlmostEqual(pose['yaw'], -math.pi / 2.0, delta=0.1) self.assertAlmostEqual(pose['pitch'], 0.0, delta=0.1) self.assertAlmostEqual(pose['roll'], 0.0, delta=0.1)
def test_ecef_modifier(self): """ Test if we can connect to the pose data stream, and read from it. """ with Morse() as morse: gps_stream = morse.robot.gps gps_mod_stream = morse.robot.gps_mod teleport_stream = morse.robot.teleport teleport_mod_stream = morse.robot.teleport_mod pos = gps_stream.get() pos_mod = gps_mod_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) 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) teleport_stream.publish({ 'x': 100.0, 'y': 200.0, 'z': 50.0, 'yaw': 0.0, 'pitch': 0.0, 'roll': 0.0 }) morse.sleep(0.01) 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.329, delta=precision) self.assertAlmostEqual(pos_mod['y'], 4397317.326, delta=precision) self.assertAlmostEqual(pos_mod['z'], 158674.47893, delta=precision) 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)
def test_drag(self): """ The idea between the test is to verify that a robot against the wind is going slower than another one, using the same force. The acceleration stay positive in both case. """ with Morse() as morse: delta = 0.20 force_ref = morse.robot_ref.force accel_ref = morse.robot_ref.accel force = morse.robot.force accel = morse.robot.accel morse.sleep(0.01) acc_ref = accel_ref.get() acc = accel.last() self.assertAlmostEqual(acc['acceleration'][0], 0.0, delta=delta) self.assertAlmostEqual(acc['acceleration'][1], 0.0, delta=delta) self.assertAlmostEqual(acc_ref['acceleration'][0], 0.0, delta=delta) self.assertAlmostEqual(acc_ref['acceleration'][1], 0.0, delta=delta) self.assertAlmostEqual(acc['velocity'][0], 0.0, delta=delta) self.assertAlmostEqual(acc['velocity'][1], 0.0, delta=delta) self.assertAlmostEqual(acc_ref['velocity'][0], 0.0, delta=delta) self.assertAlmostEqual(acc_ref['velocity'][1], 0.0, delta=delta) send_force_x(force_ref, 100) send_force_x(force, 100) morse.sleep(1) acc_ref = accel_ref.get() acc = accel.last() # After 1 sec, the speed of robot_ref should be less than # the speed of robot due to the drag effect self.assertAlmostEqual(acc['acceleration'][0], 3.80, delta=delta) self.assertAlmostEqual(acc['acceleration'][1], 0.0, delta=delta) self.assertAlmostEqual(acc_ref['acceleration'][0], 4.80, delta=delta) self.assertAlmostEqual(acc_ref['acceleration'][1], 0.0, delta=delta) self.assertAlmostEqual(acc['velocity'][0], 4.6, delta=delta) self.assertAlmostEqual(acc['velocity'][1], 0.0, delta=delta) self.assertAlmostEqual(acc_ref['velocity'][0], 5.0, delta=delta) self.assertAlmostEqual(acc_ref['velocity'][1], 0.0, delta=delta) morse.sleep(1) acc_ref = accel_ref.get() acc = accel.last() # After one more second, the differnece between both speed # increases. acceleration of robot_ref is constant, while # acceleration of robot decrease (as drag effect increases # with speed) self.assertAlmostEqual(acc['acceleration'][0], 2.0, delta=delta) self.assertAlmostEqual(acc_ref['acceleration'][0], 4.80, delta=delta) self.assertAlmostEqual(acc['velocity'][0], 7.5, delta=delta) self.assertAlmostEqual(acc_ref['velocity'][0], 10.0, delta=delta) # When no force are applied anymore, the robot_ref will # continue at constant speed (well, it is is supposed too, # but there seems to have some friction somewhere :D) # while the robot speed will slowly decreases until stop send_force_x(force_ref, 0) send_force_x(force, 0) morse.sleep(1) acc_ref = accel_ref.get() acc = accel.last() self.assertAlmostEqual(acc['acceleration'][0], -1.6, delta=delta) self.assertAlmostEqual(acc_ref['acceleration'][0], -0.2, delta=delta) self.assertAlmostEqual(acc['velocity'][0], 5.30, delta=delta) self.assertAlmostEqual(acc_ref['velocity'][0], 9.80, delta=delta)
def __init__(self, red_pixels): AbstractController.__init__(self, red_pixels) self._morse = Morse() self._left_engine = 0 self._right_engine = 0
""" On the press of the call or inside command button for lift...""" rospy.loginfo("Lift %s: To floor %s"%(type, doors[floor])) if floor in lift_commands: return lift_commands.append(floor) for t in ["call","command"]: for i in range(0,4): rospy.Subscriber("/lift_sim/%s%s"%(t,doors[i]), Bool, on_floor_call_command, callback_args=(t,i)) morse = None while morse is None: try: morse = Morse() except ConnectionError as e:# ConnectionError as e: rospy.loginfo("Waiting for Morse...") print("[!!!!!] ",e,"Waiting for Morse...") rospy.sleep(3) while not rospy.is_shutdown(): if len(lift_commands)>1: rospy.loginfo("Closing door %s"%doors[lift_commands[0]]) morse.rpc('lift.door%s'%doors[lift_commands[0]],'change_door_state',0) lift_commands=lift_commands[1:] rospy.loginfo("Moving to floor %s"%doors[lift_commands[0]]) morse.rpc('lift.platform','move_to_floor',lift_commands[0]-1) rospy.loginfo("Opening door %s"%doors[lift_commands[0]]) morse.rpc('lift.door%s'%doors[lift_commands[0]],'change_door_state',1) rospy.sleep(8)
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()
from pymorse import Morse import sys import math try: from morse.builder import * except ImportError: pass morse = Morse() motion = morse.robot.motion pose = morse.robot.pose goal_x = 5 goal_y = -5 print("At first ,the robot is currently at: %s" % pose.get()) morse.sleep(0.1) while True: position = pose.get() current_x = position['x'] current_y = position['y'] current_theta = position['yaw'] delta_x = goal_x - current_x delta_y = goal_y - current_y dis_goal = (delta_x**2 + delta_y**2)**0.5 if delta_y >= 0.0: temp_theta = math.acos(delta_x / dis_goal)