def test_subtract(self):
a = Time(5, 5 * 10e8)
b = a - Time(4, 6 * 10e8)
self.assertEqual(b.sec, 0)
self.assertEqual(b.nsec, 9 * 10e8)
b = a - 1.1
self.assertEqual(b.sec, 4)
self.assertEqual(b.nsec, 4 * 10e8)
b = 9.9 - a
self.assertEqual(b.sec, 4)
self.assertEqual(b.nsec, 4 * 10e8)
a -= a
self.assertEqual(a.sec, 0)
self.assertEqual(a.nsec, 0)
a -= Time(2, 10e8)
self.assertEqual(a.sec, -3)
self.assertEqual(a.nsec, 9 * 10e8)
a += 1.1
self.assertEqual(a.sec, -1)
self.assertEqual(a.nsec, 0)
def sleep_sim_time(world, seconds):
start = world.last_time if world.last_time else Time()
remain = seconds
while remain > 0:
yield From(trollius.sleep(0.05))
now = world.last_time if world.last_time else Time()
remain = seconds - float(now - start)
def _robot_inserted(self, robot_name, tree, robot, parents, msg,
return_future):
"""
Registers a newly inserted robot and marks the insertion
message response as handled.
:param tree:
:param robot_name:
:param tree:
:param robot:
:param parents:
:param msg:
:type msg: pygazebo.msgs.response_pb2.Response
:param return_future: Future to resolve with the created robot object.
:type return_future: Future
:return:
"""
inserted = ModelInserted()
inserted.ParseFromString(msg.serialized_data)
model = inserted.model
time = Time(msg=inserted.time)
p = model.pose.position
position = Vector3(p.x, p.y, p.z)
robot = Robot(robot_name, tree, robot, position, time, parents)
self.register_robot(robot)
return_future.set_result(robot)
def age(self):
"""
Returns this robot's age as a Time object.
Depends on the last and first update times.
:return:
:rtype: Time
"""
return Time() if self.last_update is None else self.last_update - self.starting_time
def test_cmp(self):
a = Time(dbl=1.0)
b = Time(dbl=2.0)
c = Time(dbl=1.0)
self.assertTrue(a < b)
self.assertFalse(a > b)
self.assertTrue(b > a)
self.assertFalse(b < a)
self.assertTrue(b >= a)
self.assertFalse(b <= a)
self.assertTrue(a <= b)
self.assertFalse(a >= b)
self.assertTrue(a <= c)
self.assertTrue(a >= c)
self.assertTrue(c >= a)
self.assertTrue(a >= c)
def sleep_sim_time(world, seconds, state_break=[False]):
"""
Sleeps for a certain number of simulation seconds,
note that this is always approximate as it relies
on real world sleeping.
:param world:
:param seconds:
:param state_break: List containing one boolean, the
wait will be terminated if this
is set to True (basically a hack
to break out of automatic mode early).
:return:
"""
start = world.last_time if world.last_time else Time()
remain = seconds
while remain > 0 and not state_break[0]:
yield From(trollius.sleep(0.1))
now = world.last_time if world.last_time else Time()
remain = seconds - float(now - start)
def displacement(self):
"""
Returns a tuple of the displacement in both time and space
between the first and last registered element in the speed
window.
:return: Tuple where the first item is a displacement vector
and the second a `Time` instance.
:rtype: tuple(Vector3, Time)
"""
if self.last_position is None:
return Vector3(0, 0, 0), Time()
return (self._positions[-1] - self._positions[0],
self._times[-1] - self._times[0])
def test_add(self):
a = Time(5, 5 * 10e8)
b = a + a
self.assertEqual(b.sec, 11)
self.assertEqual(b.nsec, 0)
b = a + 1.1
self.assertEqual(b.sec, 6)
self.assertEqual(b.nsec, 6 * 10e8)
b = 1.1 + a
self.assertEqual(b.sec, 6)
self.assertEqual(b.nsec, 6 * 10e8)
a += a
self.assertEqual(a.sec, 11)
self.assertEqual(a.nsec, 0)
a += 1.1
self.assertEqual(a.sec, 12)
self.assertEqual(a.nsec, 10e8)
def _update_poses(self, msg):
"""
Handles the pose info message by updating robot positions.
:param msg:
:return:
"""
poses = poses_stamped_pb2.PosesStamped()
poses.ParseFromString(msg)
self.last_time = t = Time(msg=poses.time)
if self.start_time is None:
self.start_time = t
for pose in poses.pose:
robot = self.robots.get(pose.name, None)
if not robot:
continue
position = Vector3(pose.position.x, pose.position.y,
pose.position.z)
# robot.update_position(t, position, self.write_poses)
self.call_update_triggers()
def test_create(self):
a = Time(0, -10)
self.assertEqual(a.sec, -1)
self.assertEqual(a.nsec, 10e9 - 10)
def test_eq(self):
a = Time(5, 5 * 10e8)
self.assertEqual(a, 5.5)
self.assertEqual(a, Time(5, 5 * 10e8))
self.assertNotEqual(a, Time(5, 6 * 10e8))
def test_float(self):
a = Time(1, 5 * 10e8)
self.assertAlmostEqual(1.5, float(a))
def run_single(self):
"""
:return:
"""
conf = self.conf
insert_queue = []
if not self.do_restore:
if self.current_run == 0:
# Only build arena on first run
yield From(wait_for(self.build_arena()))
# Set initial battery charge
self.current_charge = self.conf.initial_charge
self.last_charge_update = 0.0
self.births = 0
self.deaths = 0
# Generate a starting population
trees, bboxes = yield From(self.generate_population(conf.initial_population_size))
insert_queue = zip(trees, bboxes, [None for _ in range(len(trees))])
# Simple loop timing mechanism
timers = {k: Time() for k in ['reproduce', 'death', 'snapshot',
'log_fitness', 'rtf', 'insert_queue']}
this = self
def timer(name, t):
"""
:param t:
:param name:
:return:
"""
if this.last_time is not None and float(this.last_time - timers[name]) > t:
timers[name] = this.last_time
return True
return False
# Some variables
real_time = time.time()
rtf_interval = 10.0
sleep_time = 0.1
run_result = 'unknown'
started = False
while True:
if insert_queue and (not started or timer('insert_queue', 1.0)):
tree, bbox, parents = insert_queue.pop()
res = yield From(self.birth(tree, bbox, parents))
if res:
yield From(res)
if not started:
# Start the world
yield From(wait_for(self.pause(False)))
started = True
# Perform operations only if there are no items
# in the insert queue, makes snapshotting easier.
if insert_queue:
# Space out robot inserts with one simulation second
# to allow them to drop in case they are too close.
# Sleep for a very small interval every time until
# all inserts are done
yield From(trollius.sleep(0.01))
continue
if timer('snapshot', 100.0):
# Snapshot the world every 100 simulation seconds
yield From(self.create_snapshot())
yield From(wait_for(self.pause(False)))
if timer('death', 3.0):
# Kill off robots over their age every 5 simulation seconds
futs = yield From(self.kill_old_robots())
if futs:
yield From(multi_future(futs))
if timer('reproduce', 3.0):
# Attempt a reproduction every 3 simulation seconds
potential_parents = self.select_parents()
if potential_parents:
ra = random.choice(potential_parents)
rb = self.select_optimal_mate(ra)
result = yield From(self.attempt_mate(ra, rb))
if result:
child, bbox = result
insert_queue.append((child, bbox, (ra, rb)))
if timer('log_fitness', 2.0):
# Log overall fitness every 2 simulation seconds
self.log_fitness()
self.log_summary()
if timer('rtf', rtf_interval):
# Print RTF to screen every so often
nw = time.time()
diff = nw - real_time
real_time = nw
print("RTF: %f" % (rtf_interval / diff))
# Stop conditions
num_bots = len(self.robots)
age = float(self.age())
if num_bots <= conf.extinction_cutoff:
print("%d or fewer robots left in population - extinction." %
conf.extinction_cutoff)
run_result = 'extinction'
break
elif age > conf.stability_cutoff:
print("World older than %f seconds, stable." % conf.stability_cutoff)
run_result = 'stable'
break
yield From(trollius.sleep(sleep_time))
# Delete all robots and reset the world, just in case a new run
# will be started.
self.write_result(run_result)
yield From(wait_for(self.delete_all_robots()))
yield From(wait_for(self.reset()))
yield From(wait_for(self.pause(True)))
yield From(trollius.sleep(0.5))
def run(self):
"""
:return:
"""
conf = self.conf
insert_queue = []
if not self.do_restore:
# Build the arena
yield From(wait_for(self.build_arena()))
# Generate a starting population
trees, bboxes = yield From(
self.generate_population(conf.initial_population_size))
insert_queue = zip(trees, bboxes,
[None for _ in range(len(trees))])
# Simple loop timing mechanism
timers = {
k: Time()
for k in ['reproduce', 'death', 'snapshot', 'log_fitness', 'rtf']
}
this = self
def timer(name, t):
"""
:param t:
:param name:
:return:
"""
if this.last_time is not None and float(this.last_time -
timers[name]) > t:
timers[name] = this.last_time
return True
return False
# Start the world
real_time = time.time()
yield From(wait_for(self.pause(False)))
while True:
if insert_queue:
tree, bbox, parents = insert_queue.pop()
yield From(wait_for(self.birth(tree, bbox, parents)))
# Perform operations only if there are no items
# in the insert queue, makes snapshotting easier.
if insert_queue:
yield From(trollius.sleep(0.2))
continue
if timer('snapshot', 10.0):
# Snapshot the world every 10 simulation seconds
yield From(self.create_snapshot())
yield From(wait_for(self.pause(False)))
if timer('death', 5.0):
# Kill off robots over their age every 5 simulation seconds
futs = yield From(self.kill_old_robots())
if futs:
yield From(multi_future(futs))
if len(self.robots) <= 1:
print("Less than two robots left in population - extinction.")
break
if timer('reproduce',
3.0) and len(self.robots) < conf.max_population_size:
# Attempt a reproduction every 3 simulation seconds
potential_mates = self.select_mates()
if potential_mates:
ra, rb = random.choice(potential_mates)
result = yield From(self.attempt_mate(ra, rb))
if result:
ra.did_mate_with(rb)
rb.did_mate_with(ra)
child, bbox = result
insert_queue.append((child, bbox, (ra, rb)))
if timer('log_fitness', 2.0):
# Log overall fitness every 2 simulation seconds
self.log_fitness()
if timer('rtf', 1.0):
# Print RTF to screen every second
nw = time.time()
diff = nw - real_time
real_time = nw
print("RTF: %f" % (1.0 / diff))
yield From(trollius.sleep(0.2))
