class DummyBidder(object):
def __init__(self):
self.bidder = ExplorationBidder()
rospy.Timer(rospy.Duration(60), self.add_task)
##DEBUGGING SERVICES
#rospy.Service("add_budget", Empty, self.add_budget)
#rospy.Service("bid", Empty, self.bid)
#rospy.Service("get_info", Empty, self.get_info)
def add_task(self, timer_event):
print self.bidder.available_tokens
print self.bidder.currently_bid_tokens
task = Task(action='do_sweep',
start_node_id='CorpLocker6',
end_node_id='CorpLocker6',
start_after=rospy.get_rostime(),
end_before=rospy.get_rostime() + rospy.Duration(60 * 1),
max_duration=rospy.Duration(1 * 60))
bid = int(
ceil(0.1 * (self.bidder.available_tokens -
self.bidder.currently_bid_tokens)))
self.bidder.add_task_bid(task, bid)
#DEBUGGING SERVICES
#def add_budget(self, req):
#self.bidder.available_tokens+=20
#self.bidder.process_task_queue()
#return []
#def bid(self, req):
#task=Task(action= 'do_sweep',
#start_node_id='CorpLocker6',
#end_node_id='CorpLocker6',
#start_after=rospy.get_rostime(),
#end_before=rospy.get_rostime() + rospy.Duration(60*1),
#max_duration=rospy.Duration(2*60))
#bid = 10
#self.bidder.add_task_bid(task, bid)
#return []
#def get_info(self, req):
#print "AVAILABLE TOKENS: ", self.bidder.available_tokens
#print "BID TOKENS: ", self.bidder.currently_bid_tokens
#print "ADDED TOKENS: ", self.bidder.currently_added_tokens
#print "ADDED TASKS: ", self.bidder.added_tasks
#print "--------------------------------------------------------------------------------------------------------------------"
#print "QUEUED TASJS: ", self.bidder.queued_tasks
#print "\n\n\n\n\n\n\n"
#return []
def main(self):
# Wait for control-c
rospy.spin()
def __init__(self):
self.bidder = ExplorationBidder()
self.grids_schedule = ExplorationSchedule()
self.objects_schedule = ExplorationSchedule()
self.bidder_timer = rospy.get_param("~bidder_timer", 3600)
self.rescheduleInterval = rospy.get_param('~rescheduleInterval', 86400)
self.slot_duration = 0
self.num_slots = 0
self.last_bid = 0
#every 5 min check current time slot
rospy.Timer(rospy.Duration(60 * 5), self.add_task)
rospy.Subscriber("/exploration_schedule", ExplorationSchedule,
self.schedule_grids_listener)
rospy.Subscriber("/object_schedule", ExplorationSchedule,
self.schedule_objects_listener)
def __init__(self):
self.bidder = ExplorationBidder()
self.map_received = False
rospy.on_shutdown(self.shutdown)
self.service_timeout = rospy.get_param("~timeout", 10)
self.time_of_slots = rospy.get_param("~slot_time", 1200)
host = rospy.get_param("mongodb_host")
port = rospy.get_param("mongodb_port")
self.mongo_client = pymongo.MongoClient(host, port)
rospy.Subscriber('/topological_map', TopologicalMap, self.MapCallback)
rospy.loginfo("Waiting for Topological map ...")
while not self.map_received:
rospy.sleep(rospy.Duration(0.1))
rospy.loginfo("... Got Topological map")
self.get_info()
self.timer_obj = rospy.Timer(rospy.Duration(self.time_of_slots),
self.get_exploration_task_cb)
self.timer_obj2 = rospy.Timer(rospy.Duration(30), self.get_info_cb)
self.get_exploration_task()
rospy.spin()
def __init__(self,
start_time=(8, 0),
end_time=(18, 0),
observe_interval=rospy.Duration(1800)):
rospy.loginfo("Initiating budgetting control...")
# hand-allocated budget (between 8am to 6pm)
self.observe_interval = observe_interval
self._start_time = datetime.time(start_time[0], start_time[1])
self._end_time = datetime.time(end_time[0], end_time[1])
self.budget_alloc = list()
self.available_budget = 0
tmp = datetime.datetime.fromtimestamp(rospy.Time.now().secs)
self._update_budget_date = datetime.datetime(tmp.year, tmp.month,
tmp.day, 0, 0)
self.bidder = ExplorationBidder()
self.soma_config = rospy.get_param("~soma_config",
"activity_exploration")
self._db = MessageStoreProxy(collection="activity_exploration_log")
# all services to counters
people_srv_name = rospy.get_param(
"~people_srv", "/people_counter/people_best_time_estimate")
if people_srv_name != "":
rospy.loginfo("Connecting to %s service..." % people_srv_name)
self._people_srv = rospy.ServiceProxy(people_srv_name,
PeopleBestTimeEstimateSrv)
self._people_srv.wait_for_service()
act_srv_name = rospy.get_param(
"~activity_srv", "/activity_counter/activity_best_time_estimate")
if act_srv_name != "":
rospy.loginfo("Connecting to %s service..." % act_srv_name)
self._act_srv = rospy.ServiceProxy(act_srv_name,
ActivityBestTimeEstimateSrv)
self._act_srv.wait_for_service()
scene_srv_name = rospy.get_param(
"~scene_srv", "/scene_counter/scene_best_time_estimate")
if scene_srv_name != "":
rospy.loginfo("Connecting to %s service..." % scene_srv_name)
self._scene_srv = rospy.ServiceProxy(scene_srv_name,
SceneBestTimeEstimateSrv)
self._scene_srv.wait_for_service()
class SpatioTemporalBidder(object):
def __init__(self):
self.bidder = ExplorationBidder()
self.grids_schedule = ExplorationSchedule()
self.objects_schedule = ExplorationSchedule()
self.bidder_timer = rospy.get_param("~bidder_timer", 3600)
self.rescheduleInterval = rospy.get_param('~rescheduleInterval', 86400)
self.slot_duration = 0
self.num_slots = 0
self.last_bid = 0
#every 5 min check current time slot
rospy.Timer(rospy.Duration(60 * 5), self.add_task)
rospy.Subscriber("/exploration_schedule", ExplorationSchedule,
self.schedule_grids_listener)
rospy.Subscriber("/object_schedule", ExplorationSchedule,
self.schedule_objects_listener)
def getMidnightTime(self, givenTime):
midnight = (givenTime /
self.rescheduleInterval) * self.rescheduleInterval
return midnight
def schedule_grids_listener(self, data):
self.grids_schedule = data
self.num_slots = len(data.timeInfo)
#some debug messages:
rospy.loginfo(
"Bidder: Received 3D grids schedule -- nr of time slots: %d",
self.num_slots)
self.add_task('foo', test='bar')
def schedule_objects_listener(self, data):
self.objects_schedule = data
self.num_slots = len(data.timeInfo)
#some debug messages:
rospy.loginfo(
"Bidder: Received object schedule -- nr of time slots: %d",
self.num_slots)
self.add_task('foo', test='bar')
def getCurrentTimeSlot(self, slot_duration):
numSlots = 24 * 3600 / slot_duration
now = rospy.get_rostime().secs
currentSlot = (now - self.grids_schedule.timeInfo[0]) / slot_duration
if currentSlot > numSlots:
currentSlot = numSlots
return currentSlot
def add_task(self, *args, **kwargs):
# print "Availabe tokens: ", self.bidder.available_tokens
# print "Currently bid tokens: ", self.bidder.currently_bid_tokens
currentTime = rospy.Time.now()
midnight = self.getMidnightTime(currentTime.secs)
#print 'Current time: ', currentTime.secs, ' Midnight: ', midnight
if len(self.grids_schedule.timeInfo) == 0 or len(
self.objects_schedule.timeInfo) == 0:
rospy.loginfo("Bidder: waiting for both schedules.")
else:
if self.grids_schedule.timeInfo[
0] == midnight and self.objects_schedule.timeInfo[
0] == midnight:
rospy.loginfo("Received schedules for both explorations.")
self.slot_duration = self.grids_schedule.timeInfo[
2] - self.grids_schedule.timeInfo[1]
#rospy.loginfo("Time slot duration: %d", self.slot_duration)
#get current time slot:
currentSlot = self.getCurrentTimeSlot(self.slot_duration)
rospy.loginfo("Time slot: %d/%d", currentSlot, self.num_slots)
task_duration = rospy.Duration(secs=60 * 10)
if currentSlot is not self.last_bid:
if self.num_slots - currentSlot > 2:
rospy.loginfo("Adding task...")
#choose between full and mini:
if random.randint(0, 2) == 0:
#start_time = rospy.Time(secs = self.grids_schedule[currentSlot+1]['timeInfo'], nsecs = 0)
start_time = rospy.Time(
secs=self.grids_schedule.timeInfo[currentSlot +
1],
nsecs=0)
node_observation = self.grids_schedule.nodeID[
currentSlot + 1]
mode = self.grids_schedule.mode[currentSlot + 1]
else:
start_time = rospy.Time(
secs=self.objects_schedule.timeInfo[currentSlot
+ 1],
nsecs=0)
node_observation = self.objects_schedule.nodeID[
currentSlot + 1]
mode = self.objects_schedule.mode[currentSlot + 1]
#add task:
task = Task(
action='search_object',
start_node_id=node_observation,
end_node_id=node_observation,
start_after=start_time,
end_before=start_time + rospy.Duration(60 * 15),
max_duration=task_duration,
)
if mode == 'object_full':
rois = get_object_search_dfn_quasimodo(
node_observation)
else:
rois = get_object_search_dfn(node_observation)
if rois == None:
rospy.loginfo("waypoint not found in routine file")
else:
task_utils.add_string_argument(
task, node_observation)
task_utils.add_string_argument(task, rois[1])
task_utils.add_string_argument(task, rois[1])
task_utils.add_string_argument(task, mode)
bidRatio = self.num_slots - currentSlot
bid = int(
ceil((self.bidder.available_tokens -
self.bidder.currently_bid_tokens) /
bidRatio))
if bid > 0:
rospy.loginfo(
"Bidder: Bid Amount: %d -- Node: %s -- Start Time: %d -- Mode: %s -- Roi: %s -- Surface: %s",
bid, node_observation, start_time.secs,
mode, rois[1], rois[2])
self.bidder.add_task_bid(task, bid)
self.last_bid = currentSlot
else:
rospy.loginfo(
"Bidder: Bid value to low to add task!")
rospy.loginfo("Bidder: Current budget: %d",
self.bidder.available_tokens)
else:
rospy.loginfo(
"Last slot of the day, waiting for midnight!")
else:
rospy.loginfo(
"Bidder: Waiting for both schedules (3D changes and SOMA...)"
)
def main(self):
# Wait for control-c
rospy.spin()
class EdgeBider(object):
def __init__(self):
self.bidder = ExplorationBidder()
self.map_received = False
rospy.on_shutdown(self.shutdown)
self.service_timeout = rospy.get_param("~timeout", 10)
self.time_of_slots = rospy.get_param("~slot_time", 1200)
host = rospy.get_param("mongodb_host")
port = rospy.get_param("mongodb_port")
self.mongo_client = pymongo.MongoClient(host, port)
rospy.Subscriber('/topological_map', TopologicalMap, self.MapCallback)
rospy.loginfo("Waiting for Topological map ...")
while not self.map_received:
rospy.sleep(rospy.Duration(0.1))
rospy.loginfo("... Got Topological map")
self.get_info()
self.timer_obj = rospy.Timer(rospy.Duration(self.time_of_slots),
self.get_exploration_task_cb)
self.timer_obj2 = rospy.Timer(rospy.Duration(30), self.get_info_cb)
self.get_exploration_task()
rospy.spin()
def get_exploration_task_cb(self, timer_events):
self.get_exploration_task()
# exptsk=[]
# tskscr=[]
def get_exploration_task(self):
exptsk = []
tskscr = []
tsktoad = []
print "exploration time"
explotime = rospy.Duration.from_sec(self.time_of_slots)
self.timeslot_start = rospy.Time.now() + rospy.Duration.from_sec(10)
timeslot = self.timeslot_start + rospy.Duration.from_sec(
self.time_of_slots / 2)
self.timeslot_end = self.timeslot_start + rospy.Duration.from_sec(
self.time_of_slots)
print timeslot.secs, explotime.secs
ent = self.predict_entropy(timeslot)
est = self.predict_edges(timeslot)
self.fill_values(ent, est)
self.estimate_scores()
print self.eids
# print self.edges_to_explote
results = copy.copy(self.eids)
results = sorted(results, key=lambda k: k['score'], reverse=True)
task_time = 0
for i in results:
if i['probs'] > 0.3:
#print task_time, i['time'].secs*2
task_time += (i['time'].secs * 4)
if task_time < (explotime.secs / 4):
#print i['edge_id'], i['samples'], i['time'].secs, i['entropy'], i['score'], i['probs']
exptsk.append(i['edge_id'])
tskscr.append(i['score'])
else:
break
#tokens_to_use = self.bidder.available_tokens/((3600*24)/self.time_of_slots)
x_slots = np.linspace(0.0, 24.0, (3 * 24))
myd = self.bidder.available_tokens * norm.pdf(x_slots, 12, 4)
now = datetime.datetime.now()
slot = int(now.hour * 3) + (int(np.floor(now.minute / 20.0)) + 1)
if slot >= len(myd):
slot = len(myd) - 1
tokens_to_use = np.ceil(myd[slot])
total_entropy = sum(tskscr)
total_tokens = 0
for i in range(len(tskscr)):
e = {}
tstr = exptsk[i].split('_')
e['tokens'] = int(
math.ceil((tskscr[i] * tokens_to_use) / total_entropy))
if e['tokens'] <= 0:
e['tokens'] = 1
total_tokens += e['tokens']
e['origin'] = tstr[0]
e['goal'] = tstr[1]
e['action'] = '(F ("' + tstr[0] + '" & (X "' + tstr[1] + '")))'
tsktoad.append(e)
#print tokens_to_use, total_tokens, total_entropy
#print exptsk, tskscr
#print tsktoad
self.add_tasks(tsktoad)
def estimate_scores(self):
samples = np.array([x['samples'] for x in self.eids])
mean = np.mean(samples)
minimum = np.min(samples)
maximum = np.max(samples)
#print "Mean: %f Max: %f Min: %f"%(mean, minimum, maximum)
print mean, minimum, maximum
for i in range(len(self.eids)):
if self.eids[i]['samples'] >= mean:
sbs = (float(self.eids[i]['samples']) -
maximum) / (2 * (mean - maximum))
else:
sbs = 1 + (float(self.eids[i]['samples']) -
minimum) / (2 * (minimum - mean))
if sbs >= 0.9:
wsbs = (math.sqrt(1 - sbs))
self.eids[i]['score'] = (wsbs * self.eids[i]['entropy']) + (
(1 - wsbs) * sbs)
else:
self.eids[i]['score'] = self.eids[i][
'entropy'] #((1-(1/(float(self.eids[i]['samples'])+1)))*self.eids[i]['entropy'])+(1/(float(self.eids[i]['samples'])+1))
if self.eids[i]['score'] < 0.0:
self.eids[i]['score'] = 0.0
elif self.eids[i]['score'] > 1.0:
self.eids[i]['score'] = 1.0
if self.eids[i]['samples'] <= 10:
self.eids[i]['probs'] = 0.5
# print "+++++"
# print i, self.eids[i]['edge_id'], self.eids[i]['samples'], self.eids[i]['entropy'], self.eids[i]['score']
#print wsbs, self.eids[i]['entropy'], 1-wsbs, sbs, self.eids[i]['score']
# #eids, ents, nothing = self.predict_entropy(timeslot)
# v= self.predict_entropy(timeslot)
# self.edges_to_explote=[]
# for i in range(len(v.edge_ids)):
# f={}
# f['eid']=v.edge_ids[i]
# f['ent']=v.probs[i]
# self.edges_to_explote.append(f)
def add_tasks(self, tasks_to_add):
#print self.bidder.available_tokens
#print self.bidder.currently_bid_tokens
for i in tasks_to_add:
wp1 = i['origin']
wp2 = i['goal'] #+'"'
task = Task(action=i['action'],
start_node_id=wp1,
end_node_id=wp2,
start_after=self.timeslot_start,
end_before=self.timeslot_end,
max_duration=rospy.Duration(2 * 60))
bid = i['tokens']
self.bidder.add_task_bid(task, bid)
def fill_values(self, entropies, estimations):
db = self.mongo_client.message_store
collection = db['nav_stats']
for i in range(len(entropies.edge_ids)):
ind = self.edgid.index(entropies.edge_ids[i])
self.eids[ind]['entropy'] = entropies.probs[i]
for i in range(len(estimations.edge_ids)):
ind = self.edgid.index(estimations.edge_ids[i])
self.eids[ind]['time'] = estimations.durations[i]
self.eids[ind]['probs'] = estimations.probs[i]
for i in range(len(self.eids)):
query = {
'topological_map': self.top_map.pointset,
'edge_id': self.eids[i]['edge_id']
}
self.eids[i]['samples'] = collection.find(query).count()
def predict_entropy(self, epoch):
rospy.wait_for_service('/topological_prediction/edge_entropies',
timeout=self.service_timeout)
try:
get_prediction = rospy.ServiceProxy(
'/topological_prediction/edge_entropies',
strands_navigation_msgs.srv.PredictEdgeState)
print "Requesting prediction for %f" % epoch.secs
resp1 = get_prediction(epoch)
print "got %d edge ids, %d probs" % (len(
resp1.edge_ids), len(resp1.probs))
return resp1
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def __init__(self):
self.bidder = ExplorationBidder()
rospy.Timer(rospy.Duration(60), self.add_task)