def __init__(self,
name,
soma_map,
soma_config,
minute_interval,
window_interval,
collection="occurrence_rates"):
self.topo_map = None
self.soma_map = soma_map
self.soma_config = soma_config
self.minute_interval = minute_interval
self.window_interval = window_interval
self.rotm = RegionObservationTimeManager(soma_map, soma_config)
self.tre = TrajectoryRegionEstimate(soma_map, soma_config,
minute_interval)
self.tof = TrajectoryOccurrenceFrequencies(
soma_map,
soma_config,
minute_interval=minute_interval,
window_interval=window_interval)
self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
rospy.loginfo("Connect to database collection %s..." % collection)
self._ms = MessageStoreProxy(collection=collection)
self._soma_db = MessageStoreProxy(collection="soma_roi")
rospy.loginfo("Create a service %s/service..." % name)
self.service = rospy.Service(name + '/service',
TrajectoryOccurrenceRate, self.srv_cb)
rospy.loginfo("Create an action server %s..." % name)
self._as = actionlib.SimpleActionServer(name,
OccurrenceRateLearningAction,
execute_cb=self.execute,
auto_start=False)
self._as.start()
def __init__(self, soma_map, soma_config, minute_interval=60):
self.soma_map = soma_map
self.soma_config = soma_config
self.minute_interval = minute_interval
self.region_knowledge = RegionObservationTimeManager(
soma_map, soma_config)
# Service and Proxy
rospy.loginfo("Connecting to geospatial_store and roslog database...")
self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
def __init__(self, soma_map, soma_config, minute_interval=60):
self.soma_map = soma_map
self.soma_config = soma_config
self.minute_interval = minute_interval
self.region_knowledge = RegionObservationTimeManager(soma_map, soma_config)
# Service and Proxy
rospy.loginfo("Connecting to geospatial_store and roslog database...")
self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
def __init__(self, name, soma_map, soma_config, minute_interval, window_interval, collection="occurrence_rates"):
self.soma_map = soma_map
self.soma_config = soma_config
self.minute_interval = minute_interval
self.window_interval = window_interval
self.rotm = RegionObservationTimeManager(soma_map, soma_config)
self.tre = TrajectoryRegionEstimate(soma_map, soma_config, minute_interval)
self.tof = TrajectoryOccurrenceFrequencies(
soma_map, soma_config, minute_interval=minute_interval, window_interval=window_interval
)
rospy.loginfo("Connect to database collection %s..." % collection)
self._ms = MessageStoreProxy(collection=collection)
rospy.loginfo("Create a service %s/service..." % name)
self.service = rospy.Service(name + "/service", TrajectoryOccurrenceRate, self.srv_cb)
rospy.loginfo("Create an action server %s..." % name)
self._as = actionlib.SimpleActionServer(
name, OccurrenceRateLearningAction, execute_cb=self.execute, auto_start=False
)
self._as.start()
class TrajectoryRegionEstimate(object):
def __init__(self, soma_map, soma_config, minute_interval=60):
self.soma_map = soma_map
self.soma_config = soma_config
self.minute_interval = minute_interval
self.region_knowledge = RegionObservationTimeManager(soma_map, soma_config)
# Service and Proxy
rospy.loginfo("Connecting to geospatial_store and roslog database...")
self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
def estimate_trajectories_daily(self, dates, month=1, year=2015):
"""
Estimate the number of trajectories in a region hourly each day for particular dates
assuming that the robot sees the complete region when it sees the region
"""
days = [date for date in dates if date <= calendar.monthrange(year, month)[1]]
days = [
datetime.datetime(year, month, i, 0, 0) for i in days
]
return self.estimate_trajectories(days)
def estimate_trajectories(self, days):
"""
Estimate the number of trajectories in a region for days,
assuming that the robot sees the complete region when it sees the region
"""
days_trajectories = dict()
roi_region_daily = self.region_knowledge.calculate_region_observation_duration(
days, self.minute_interval
)
trajectory_region_daily = self.obtain_number_of_trajectories(days)
query = {
"soma_map": self.soma_map, "soma_config": self.soma_config,
"soma_roi_id": {"$exists": True}
}
regions = self.gs.find_projection(query, {"soma_roi_id": 1})
minutes = [
i * self.minute_interval for i in range(1, (60/self.minute_interval) + 1)
]
for region in regions:
reg = region['soma_roi_id']
for day in days:
day = day.day
if not(day in roi_region_daily and day in trajectory_region_daily):
continue
if reg not in days_trajectories:
days_trajectories[reg] = dict()
if day not in days_trajectories[reg]:
temp = list()
for i in range(24):
temp.append({i: -1 for i in minutes})
days_trajectories[reg][day] = temp
for hour in range(24):
for minute in minutes:
if reg in roi_region_daily[day]:
days_trajectories = self._estimate_trajectories_by_minutes(
reg, day, hour, minute, days_trajectories,
roi_region_daily[day], trajectory_region_daily[day]
)
return days_trajectories
def _extrapolate_trajectory_estimate(self, len_min_interval, seconds_stay_duration, num_traj):
""" extrapolate the number of trajectories with specific upper_threshold.
upper_threshold is to ceil how long the robot was in an area
for one minute interval, if the robot was there for less than 20
seconds, then it will be boosted to 20 seconds.
"""
upper_threshold_duration = 0
while seconds_stay_duration > upper_threshold_duration:
upper_threshold_duration += self.minute_interval * 20
multiplier_estimator = 3600 / float(
len_min_interval * upper_threshold_duration
)
return math.ceil(multiplier_estimator * num_traj)
def _hard_trajectory_estimate(self, seconds_stay_duration, num_traj):
""" Estimating the number of trajectories with hard threshold.
This forces the seconds_stay_duration to be 60 seconds for num_traj to be registered.
"""
temp_num_traj = -1
if seconds_stay_duration >= self.minute_interval * 60:
temp_num_traj = num_traj
return temp_num_traj
def _estimate_trajectories_by_minutes(
self, region, day, hour, minute, days_trajectories, rois_day, trajectories_day
):
minutes = [
i * self.minute_interval for i in range(1, (60/self.minute_interval) + 1)
]
if len(trajectories_day) > hour:
if region in trajectories_day[hour]:
if minute in trajectories_day[hour][region]:
traj = trajectories_day[hour][region][minute]
if rois_day[region][hour][minute] > 0:
days_trajectories[region][day][hour][minute] = self._extrapolate_trajectory_estimate(
len(minutes), rois_day[region][hour][minute], traj
)
# days_trajectories[region][day][hour][minute] = self._hard_trajectory_estimate(
# rois_day[region][hour][minute], traj
# )
else:
if traj > 0:
rospy.logwarn(
"%d trajectories are detected in region %s at day %d hour %d minute %d, but no info about robot being there" % (
traj, region, day, hour, minute
)
)
else:
if rois_day[region][hour][minute] >= self.minute_interval * 60:
days_trajectories[region][day][hour][minute] = 0
else:
for i in minutes:
if rois_day[region][hour][i] >= self.minute_interval * 60:
days_trajectories[region][day][hour][i] = 0
else:
query = {
"soma_map": self.soma_map, "soma_config": self.soma_config,
"soma_roi_id": {"$exists": True}
}
regions = self.gs.find_projection(query, {"soma_roi_id": 1})
for j in regions:
reg = j['soma_roi_id']
if reg in rois_day and hour in rois_day[reg]:
for i in minutes:
if i in rois_day[reg][hour] and rois_day[reg][hour][i] >= self.minute_interval * 60:
days_trajectories[reg][day][hour][i] = 0
return days_trajectories
def obtain_number_of_trajectories(self, days):
"""
Obtaining the number of trajectories for each day in argument days,
The number of Trajectories will be split into day, hour, region, and minutes.
Trajectories = [day[hour[region[minutes]]]]
"""
rospy.loginfo('Updating frequency of trajectories in each region...')
trajectory_region_daily = dict()
for i in days:
daily_trajectories = self._trajectories_daily(
calendar.timegm(i.timetuple())
)
trajectory_region_daily.update({i.day: daily_trajectories})
return trajectory_region_daily
# count trajectories in regions for the whole day
# day is in the epoch, so in default it refers to 1 Jan 1970
def _trajectories_daily(self, day=0):
daily_trajectories = list()
for i in range(24):
hourly_trajectories = self._trajectories_hourly_for_day(day, i)
daily_trajectories.append(hourly_trajectories)
return daily_trajectories
# count trajectories in the region per hour for a certain day
# day is in the epoch, so in default it refers to 1 Jan 1970
def _trajectories_hourly_for_day(self, day=0, hour=0):
date = datetime.date.fromtimestamp(day)
rospy.loginfo(
'Getting trajectories for day %s hour %d...' % (
date.strftime("%d-%m-%Y"), hour
)
)
start_hour = day + (hour * 3600)
end_hour = day + ((hour + 1) * 3600)
trajectories_hourly = self._get_num_traj_by_epoch(
start_hour, end_hour, self.minute_interval*60
)
return trajectories_hourly
def _get_num_traj_by_epoch(self, start, end, min_inter=3600):
"""
Count the number of trajectories in areas defined by soma map and soma
config that is restricted by the query.
"""
start_hour = start
end_hour = start + min_inter
trajectories_hourly = dict()
while end_hour <= end:
query = {
"$or": [
{"start": {"$gte": start_hour, "$lt": end_hour}},
{"end": {"$gte": start_hour, "$lt": end_hour}}
]
}
for geo_traj in self.gs.find(query):
query = {
"soma_map": self.soma_map,
"soma_config": self.soma_config,
"soma_roi_id": {"$exists": "true"},
"loc": {
"$geoIntersects": {
"$geometry": {
"type": "LineString",
"coordinates": geo_traj['loc']['coordinates']
}
}
}
}
roi_traj = self.gs.find_projection(query, {"soma_roi_id": 1})
if roi_traj.count() < 1:
rospy.logwarn("No region covers the movement of uuid %s" % (geo_traj["uuid"]))
else:
for region in roi_traj:
if region['soma_roi_id'] not in trajectories_hourly:
trajectories_hourly[region['soma_roi_id']] = dict()
index = (end_hour - start) / 60
if index not in trajectories_hourly[region['soma_roi_id']]:
trajectories_hourly[region['soma_roi_id']][index] = 0
trajectories_hourly[region['soma_roi_id']][index] += 1
start_hour += min_inter
end_hour += min_inter
return trajectories_hourly
class TrajectoryArrivalRateManager(object):
def __init__(self,
name,
soma_map,
soma_config,
minute_interval,
window_interval,
collection="occurrence_rates"):
self.topo_map = None
self.soma_map = soma_map
self.soma_config = soma_config
self.minute_interval = minute_interval
self.window_interval = window_interval
self.rotm = RegionObservationTimeManager(soma_map, soma_config)
self.tre = TrajectoryRegionEstimate(soma_map, soma_config,
minute_interval)
self.tof = TrajectoryOccurrenceFrequencies(
soma_map,
soma_config,
minute_interval=minute_interval,
window_interval=window_interval)
self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
rospy.loginfo("Connect to database collection %s..." % collection)
self._ms = MessageStoreProxy(collection=collection)
self._soma_db = MessageStoreProxy(collection="soma_roi")
rospy.loginfo("Create a service %s/service..." % name)
self.service = rospy.Service(name + '/service',
TrajectoryOccurrenceRate, self.srv_cb)
rospy.loginfo("Create an action server %s..." % name)
self._as = actionlib.SimpleActionServer(name,
OccurrenceRateLearningAction,
execute_cb=self.execute,
auto_start=False)
self._as.start()
def execute(self, goal):
temp_start_time = rospy.Time.now()
curr_date = datetime.datetime.fromtimestamp(rospy.Time.now().secs)
# curr_date = datetime.datetime(2016, 3, 10, 0, 50)
if curr_date.hour >= 0 and curr_date.hour < 8:
curr_date = curr_date - datetime.timedelta(hours=24)
curr_date = datetime.datetime(curr_date.year, curr_date.month,
curr_date.day, 0, 0)
prev_date = curr_date - datetime.timedelta(hours=24)
self.rotm.calculate_region_observation_duration([prev_date, curr_date],
self.minute_interval)
self.rotm.store_to_mongo()
if self._as.is_preempt_requested():
self._as.set_preempted()
return
curr_traj_est = self.tre.estimate_trajectories_daily([curr_date.day],
curr_date.month,
curr_date.year)
curr_traj_est = trajectory_estimate_for_date(curr_traj_est, curr_date)
prev_traj_est = self.tre.estimate_trajectories_daily([prev_date.day],
prev_date.month,
prev_date.year)
prev_traj_est = trajectory_estimate_for_date(prev_traj_est, prev_date)
if self._as.is_preempt_requested():
self._as.set_preempted()
return
self.tof.update_tof_daily(curr_traj_est, prev_traj_est, curr_date)
self.tof.store_tof()
self.rotm.reinit()
self.tof.reinit()
temp_end_time = rospy.Time.now()
rospy.loginfo("Time needed to complete this is %d" %
(temp_end_time - temp_start_time).secs)
self._as.set_succeeded(OccurrenceRateLearningResult())
def _get_occurrence_rate_logs(self, start_time, end_time):
filtered_logs = list()
if (end_time -
start_time).total_seconds() <= self.window_interval * 60:
end_time = start_time + datetime.timedelta(
seconds=self.minute_interval * 60)
if start_time.hour == end_time.hour and start_time.day == end_time.day:
query = {
"soma": self.soma_map,
"soma_config": self.soma_config,
"duration.secs": self.window_interval * 60,
"day": start_time.weekday(),
"hour": start_time.hour,
"minute": {
"$gte": start_time.minute,
"$lt": end_time.minute
}
}
logs = self._ms.query(OccurrenceRate._type, message_query=query)
filtered_logs = logs
elif start_time.hour != end_time.hour and start_time.day == end_time.day:
query = {
"soma": self.soma_map,
"soma_config": self.soma_config,
"duration.secs": self.window_interval * 60,
"day": start_time.weekday(),
"hour": {
"$gte": start_time.hour,
"$lte": end_time.hour
},
}
logs = self._ms.query(OccurrenceRate._type, message_query=query)
for log in logs:
if log[0].hour == start_time.hour and log[
0].minute >= start_time.minute:
filtered_logs.append(log)
elif log[0].hour == end_time.hour and log[
0].minute < end_time.minute:
filtered_logs.append(log)
elif log[0].hour > start_time.hour and log[
0].hour < end_time.hour:
filtered_logs.append(log)
else:
day = start_time.weekday()
end_day = end_time.weekday()
query = {
"soma": self.soma_map,
"soma_config": self.soma_config,
"duration.secs": self.window_interval * 60,
"day": {
"$gte": day,
"$lte": end_day
}
}
logs = self._ms.query(OccurrenceRate._type, message_query=query)
for log in logs:
if log[0].day == day and log[0].hour >= start_time.hour and log[
0].minute >= start_time.minute:
filtered_logs.append(log)
if log[0].day == end_day and log[
0].hour <= end_time.hour and log[
0].minute < end_time.minute:
filtered_logs.append(log)
elif end_day > day:
if log[0].day > day and log[0].day < end_day:
filtered_logs.append(log)
elif end_day < day:
if log[0].day > day:
filtered_logs.append(log)
elif log[0].day < end_day:
filtered_logs.append(log)
return filtered_logs
def _choose_proper_region_to_observe(self, rois, wp_point):
accumulate_dist = dict()
for roi in rois:
query = {
"map": self.soma_map,
"config": self.soma_config,
"roi_id": roi
}
logs = self._soma_db.query(SOMAROIObject._type,
message_query=query)
for log in logs:
if roi not in accumulate_dist:
accumulate_dist[roi] = 0.0
accumulate_dist[roi] = distance_calc.euclidean(
[wp_point.pose.position.x, wp_point.pose.position.y],
[log[0].pose.position.x, log[0].pose.position.y])
accumulate_dist = sorted(accumulate_dist,
key=lambda i: accumulate_dist[i],
reverse=True)
return accumulate_dist[-1]
def _get_waypoints(self, region_ids):
waypoints = dict()
topo_sub = rospy.Subscriber("/topological_map", TopologicalMap,
self._topo_map_cb, None, 10)
while self.topo_map is None:
rospy.sleep(0.1)
rospy.logwarn("Trying to get information from /topological_map...")
topo_sub.unregister()
for wp in self.topo_map.nodes:
if wp.name == "ChargingPoint":
continue
_, _, yaw = euler_from_quaternion(
[0, 0, wp.pose.orientation.z, wp.pose.orientation.w])
coords = robot_view_cone(wp.pose.position.x, wp.pose.position.y,
yaw)
langitude_latitude = list()
for pt in coords:
langitude_latitude.append(
self.gs.coords_to_lnglat(pt[0], pt[1]))
langitude_latitude.append(
self.gs.coords_to_lnglat(coords[0][0], coords[0][1]))
if self.gs.observed_roi(langitude_latitude, self.soma_map,
self.soma_config).count() > 1:
rospy.logwarn(
"There are two or more regions covered by the sight of the robot in %s"
% wp.name)
roi = list()
for j in self.gs.observed_roi(langitude_latitude, self.soma_map,
self.soma_config):
roi.append(str(j['soma_roi_id']))
roi = [i for i in roi if i in region_ids]
if len(roi) < 1:
continue
elif len(roi) == 1:
waypoints[roi[0]] = wp.name
else:
roi = self._choose_proper_region_to_observe(roi, wp)
waypoints[roi] = wp.name
return waypoints
def _topo_map_cb(self, topo_map):
self.topo_map = topo_map
def _get_long_duration(self, four_tuple, start_time):
datetimes = list()
for i in four_tuple:
delta_day = (i[0] - start_time.weekday()) % 7
curr_date = start_time + datetime.timedelta(days=delta_day)
curr_date = datetime.datetime(curr_date.year, curr_date.month,
curr_date.day, i[1], i[2])
datetimes.append(curr_date)
datetimes = sorted(datetimes)
counter = 0
for i in range(1, len(datetimes)):
if (datetimes[i] -
datetimes[i - 1]).seconds == self.minute_interval * 60:
counter += 1
elif (datetimes[i] -
datetimes[i - 1]).seconds <= self.window_interval * 60:
counter += (datetimes[i] - datetimes[i - 1]).seconds / (
self.minute_interval * 60)
duration = rospy.Duration(self.window_interval * 60 +
(counter * self.minute_interval * 60))
best_time = rospy.Time(time.mktime(datetimes[0].timetuple()))
return best_time, duration
def _get_best_regions(self, logs, start_time):
or_regions = dict()
best_time = dict()
for log in logs:
if log[0].region_id not in or_regions:
or_regions[log[0].region_id] = 0.0
or_regions[log[0].region_id] += log[0].occurrence_rate
if log[0].region_id not in best_time:
best_time[log[0].region_id] = [[
log[0].day, log[0].hour, log[0].minute,
log[0].occurrence_rate
]]
elif best_time[log[0].region_id][-1][-1] < log[0].occurrence_rate:
best_time[log[0].region_id][-1] = [
log[0].day, log[0].hour, log[0].minute,
log[0].occurrence_rate
]
elif best_time[log[0].region_id][-1][-1] == log[0].occurrence_rate:
best_time[log[0].region_id].append([
log[0].day, log[0].hour, log[0].minute,
log[0].occurrence_rate
])
duration = dict()
for roi in best_time.keys():
temp = best_time[roi]
if len(temp) > 0:
best_time[roi], duration[roi] = self._get_long_duration(
temp, start_time)
else:
rospy.logwarn(
"There is no recommended visiting time for region %s" %
roi)
duration[roi] = rospy.Duration(0)
best_time[roi] = rospy.Time.now()
return or_regions, best_time, duration
def srv_cb(self, msg):
region_ids = list()
best_times_list = list()
durations_list = list()
waypoints_list = list()
rospy.loginfo("Got a request...")
print msg
rospy.loginfo(
"Retrieving trajectory occurrence frequencies from database...")
start_time = datetime.datetime.fromtimestamp(msg.start_time.secs)
end_time = datetime.datetime.fromtimestamp(msg.end_time.secs)
if end_time >= start_time:
logs = self._get_occurrence_rate_logs(start_time, end_time)
# calculate occurrence_rate per region
or_regions, best_times, durations = self._get_best_regions(
logs, start_time)
region_ids = sorted(or_regions,
key=lambda i: or_regions[i],
reverse=True)
# get the nearest waypoints somehow
waypoints = self._get_waypoints(region_ids)
# ordering
for roi in region_ids:
best_times_list.append(best_times[roi])
durations_list.append(durations[roi])
waypoints_list.append(waypoints[roi])
rospy.loginfo("Composing answer...")
print TrajectoryOccurrenceRateResponse(
region_ids[:msg.n_best], best_times_list[:msg.n_best],
durations_list[:msg.n_best], waypoints_list[:msg.n_best])
return TrajectoryOccurrenceRateResponse(region_ids[:msg.n_best],
best_times_list[:msg.n_best],
durations_list[:msg.n_best],
waypoints_list[:msg.n_best])
class TrajectoryOccurrenceRateManager(object):
def __init__(self, name, soma_map, soma_config, minute_interval, window_interval, collection="occurrence_rates"):
self.soma_map = soma_map
self.soma_config = soma_config
self.minute_interval = minute_interval
self.window_interval = window_interval
self.rotm = RegionObservationTimeManager(soma_map, soma_config)
self.tre = TrajectoryRegionEstimate(soma_map, soma_config, minute_interval)
self.tof = TrajectoryOccurrenceFrequencies(
soma_map, soma_config, minute_interval=minute_interval, window_interval=window_interval
)
rospy.loginfo("Connect to database collection %s..." % collection)
self._ms = MessageStoreProxy(collection=collection)
rospy.loginfo("Create a service %s/service..." % name)
self.service = rospy.Service(name + "/service", TrajectoryOccurrenceRate, self.srv_cb)
rospy.loginfo("Create an action server %s..." % name)
self._as = actionlib.SimpleActionServer(
name, OccurrenceRateLearningAction, execute_cb=self.execute, auto_start=False
)
self._as.start()
def execute(self, goal):
temp_start_time = rospy.Time.now()
curr_date = datetime.datetime.fromtimestamp(rospy.Time.now().secs)
# curr_date = datetime.datetime(2016, 3, 10, 0, 50)
if curr_date.hour >= 0 and curr_date.hour < 8:
curr_date = curr_date - datetime.timedelta(hours=24)
curr_date = datetime.datetime(curr_date.year, curr_date.month, curr_date.day, 0, 0)
prev_date = curr_date - datetime.timedelta(hours=24)
self.rotm.calculate_region_observation_duration([prev_date, curr_date], self.minute_interval)
self.rotm.store_to_mongo()
if self._as.is_preempt_requested():
self._as.set_preempted()
return
curr_traj_est = self.tre.estimate_trajectories_daily([curr_date.day], curr_date.month, curr_date.year)
curr_traj_est = trajectory_estimate_for_date(curr_traj_est, curr_date)
prev_traj_est = self.tre.estimate_trajectories_daily([prev_date.day], prev_date.month, prev_date.year)
prev_traj_est = trajectory_estimate_for_date(prev_traj_est, prev_date)
if self._as.is_preempt_requested():
self._as.set_preempted()
return
self.tof.update_tof_daily(curr_traj_est, prev_traj_est, curr_date)
self.tof.store_tof()
self.rotm.reinit()
self.tof.reinit()
temp_end_time = rospy.Time.now()
rospy.loginfo("Time needed to complete this is %d" % (temp_end_time - temp_start_time).secs)
self._as.set_succeeded(OccurrenceRateLearningResult())
def srv_cb(self, msg):
rospy.loginfo("Retrieving trajectory occurrence frequencies from database...")
print msg
query = {
"soma": self.soma_map,
"soma_config": self.soma_config,
"region_id": msg.region_id,
"day": msg.day,
"end_hour": msg.hour,
"end_minute": msg.minute,
}
logs = self._ms.query(OccurrenceRate._type, message_query=query)
occurrence_rate = 0.0
counter = 0
for log in logs:
occurrence_rate += log[0].occurrence_rate
counter += 1
if counter == 0:
counter += 1
if len(logs) > 0:
rospy.loginfo("Occurrence Rate is obtained, which has value %.2f" % (occurrence_rate / float(counter)))
else:
rospy.loginfo("No occurrence rate is obtained, returning zero value")
return TrajectoryOccurrenceRateResponse(occurrence_rate / float(counter))
class TrajectoryRegionEstimate(object):
def __init__(self, soma_map, soma_config, minute_interval=60):
self.soma_map = soma_map
self.soma_config = soma_config
self.minute_interval = minute_interval
self.region_knowledge = RegionObservationTimeManager(
soma_map, soma_config)
# Service and Proxy
rospy.loginfo("Connecting to geospatial_store and roslog database...")
self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
def estimate_trajectories_daily(self, dates, month=1, year=2015):
"""
Estimate the number of trajectories in a region hourly each day for particular dates
assuming that the robot sees the complete region when it sees the region
"""
days = [
date for date in dates
if date <= calendar.monthrange(year, month)[1]
]
days = [datetime.datetime(year, month, i, 0, 0) for i in days]
return self.estimate_trajectories(days)
def estimate_trajectories(self, days):
"""
Estimate the number of trajectories in a region for days,
assuming that the robot sees the complete region when it sees the region
"""
days_trajectories = dict()
roi_region_daily = self.region_knowledge.calculate_region_observation_duration(
days, self.minute_interval)
trajectory_region_daily = self.obtain_number_of_trajectories(days)
query = {
"soma_map": self.soma_map,
"soma_config": self.soma_config,
"soma_roi_id": {
"$exists": True
}
}
regions = self.gs.find_projection(query, {"soma_roi_id": 1})
minutes = [
i * self.minute_interval
for i in range(1, (60 / self.minute_interval) + 1)
]
for region in regions:
reg = region['soma_roi_id']
for day in days:
day = day.day
if not (day in roi_region_daily
and day in trajectory_region_daily):
continue
if reg not in days_trajectories:
days_trajectories[reg] = dict()
if day not in days_trajectories[reg]:
temp = list()
for i in range(24):
temp.append({i: -1 for i in minutes})
days_trajectories[reg][day] = temp
for hour in range(24):
for minute in minutes:
if reg in roi_region_daily[day]:
days_trajectories = self._estimate_trajectories_by_minutes(
reg, day, hour, minute, days_trajectories,
roi_region_daily[day],
trajectory_region_daily[day])
return days_trajectories
def _extrapolate_trajectory_estimate(self, len_min_interval,
seconds_stay_duration, num_traj):
""" extrapolate the number of trajectories with specific upper_threshold.
upper_threshold is to ceil how long the robot was in an area
for one minute interval, if the robot was there for less than 20
seconds, then it will be boosted to 20 seconds.
"""
upper_threshold_duration = 0
while seconds_stay_duration > upper_threshold_duration:
upper_threshold_duration += self.minute_interval * 20
multiplier_estimator = 3600 / float(
len_min_interval * upper_threshold_duration)
return math.ceil(multiplier_estimator * num_traj)
def _hard_trajectory_estimate(self, seconds_stay_duration, num_traj):
""" Estimating the number of trajectories with hard threshold.
This forces the seconds_stay_duration to be 60 seconds for num_traj to be registered.
"""
temp_num_traj = -1
if seconds_stay_duration >= self.minute_interval * 60:
temp_num_traj = num_traj
return temp_num_traj
def _estimate_trajectories_by_minutes(self, region, day, hour, minute,
days_trajectories, rois_day,
trajectories_day):
minutes = [
i * self.minute_interval
for i in range(1, (60 / self.minute_interval) + 1)
]
if len(trajectories_day) > hour:
if region in trajectories_day[hour]:
if minute in trajectories_day[hour][region]:
traj = trajectories_day[hour][region][minute]
if rois_day[region][hour][minute] > 0:
days_trajectories[region][day][hour][
minute] = self._extrapolate_trajectory_estimate(
len(minutes), rois_day[region][hour][minute],
traj)
# days_trajectories[region][day][hour][minute] = self._hard_trajectory_estimate(
# rois_day[region][hour][minute], traj
# )
else:
if traj > 0:
rospy.logwarn(
"%d trajectories are detected in region %s at day %d hour %d minute %d, but no info about robot being there"
% (traj, region, day, hour, minute))
else:
if rois_day[region][hour][
minute] >= self.minute_interval * 60:
days_trajectories[region][day][hour][minute] = 0
else:
for i in minutes:
if rois_day[region][hour][i] >= self.minute_interval * 60:
days_trajectories[region][day][hour][i] = 0
else:
query = {
"soma_map": self.soma_map,
"soma_config": self.soma_config,
"soma_roi_id": {
"$exists": True
}
}
regions = self.gs.find_projection(query, {"soma_roi_id": 1})
for j in regions:
reg = j['soma_roi_id']
if reg in rois_day and hour in rois_day[reg]:
for i in minutes:
if i in rois_day[reg][hour] and rois_day[reg][hour][
i] >= self.minute_interval * 60:
days_trajectories[reg][day][hour][i] = 0
return days_trajectories
def obtain_number_of_trajectories(self, days):
"""
Obtaining the number of trajectories for each day in argument days,
The number of Trajectories will be split into day, hour, region, and minutes.
Trajectories = [day[hour[region[minutes]]]]
"""
rospy.loginfo('Updating frequency of trajectories in each region...')
trajectory_region_daily = dict()
for i in days:
daily_trajectories = self._trajectories_daily(
calendar.timegm(i.timetuple()))
trajectory_region_daily.update({i.day: daily_trajectories})
return trajectory_region_daily
# count trajectories in regions for the whole day
# day is in the epoch, so in default it refers to 1 Jan 1970
def _trajectories_daily(self, day=0):
daily_trajectories = list()
for i in range(24):
hourly_trajectories = self._trajectories_hourly_for_day(day, i)
daily_trajectories.append(hourly_trajectories)
return daily_trajectories
# count trajectories in the region per hour for a certain day
# day is in the epoch, so in default it refers to 1 Jan 1970
def _trajectories_hourly_for_day(self, day=0, hour=0):
date = datetime.date.fromtimestamp(day)
rospy.loginfo('Getting trajectories for day %s hour %d...' %
(date.strftime("%d-%m-%Y"), hour))
start_hour = day + (hour * 3600)
end_hour = day + ((hour + 1) * 3600)
trajectories_hourly = self._get_num_traj_by_epoch(
start_hour, end_hour, self.minute_interval * 60)
return trajectories_hourly
def _get_num_traj_by_epoch(self, start, end, min_inter=3600):
"""
Count the number of trajectories in areas defined by soma map and soma
config that is restricted by the query.
"""
start_hour = start
end_hour = start + min_inter
trajectories_hourly = dict()
while end_hour <= end:
query = {
"$or": [{
"start": {
"$gte": start_hour,
"$lt": end_hour
}
}, {
"end": {
"$gte": start_hour,
"$lt": end_hour
}
}]
}
for geo_traj in self.gs.find(query):
query = {
"soma_map": self.soma_map,
"soma_config": self.soma_config,
"soma_roi_id": {
"$exists": "true"
},
"loc": {
"$geoIntersects": {
"$geometry": {
"type": "LineString",
"coordinates": geo_traj['loc']['coordinates']
}
}
}
}
roi_traj = self.gs.find_projection(query, {"soma_roi_id": 1})
if roi_traj.count() < 1:
rospy.logwarn("No region covers the movement of uuid %s" %
(geo_traj["uuid"]))
else:
for region in roi_traj:
if region['soma_roi_id'] not in trajectories_hourly:
trajectories_hourly[region['soma_roi_id']] = dict()
index = (end_hour - start) / 60
if index not in trajectories_hourly[
region['soma_roi_id']]:
trajectories_hourly[
region['soma_roi_id']][index] = 0
trajectories_hourly[region['soma_roi_id']][index] += 1
start_hour += min_inter
end_hour += min_inter
return trajectories_hourly
