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
def test_base_service_connection(self):
""" Simply tests if the simulator is reachable by its socket interface.
"""
morse = Morse()
morse.close()
""" 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()
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)
