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, 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 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))
rospy.logerr(
"usage: tof soma config start_date end_date month year minute_interval window_time"
)
sys.exit(2)
# the interval minute must be the same for Trajectory Region Knowledge and
# Continuous TOF
interval = int(sys.argv[7])
window = int(sys.argv[8])
start_date = int(sys.argv[3])
end_date = int(sys.argv[4])
month = int(sys.argv[5])
year = int(sys.argv[6])
temp_start_time = rospy.Time.now()
tre = TrajectoryRegionEstimate(sys.argv[1], sys.argv[2], interval)
if start_date > 1:
trajectory_estimate = tre.estimate_trajectories_daily(
# range(1, calendar.monthrange(year, month)[1]+1), month, year
range(start_date - 1, end_date + 1),
month,
year)
else:
trajectory_estimate = tre.estimate_trajectories_daily(
# range(1, calendar.monthrange(year, month)[1]+1), month, year
range(start_date, end_date + 1),
month,
year)
tof = TrajectoryOccurrenceFrequencies(sys.argv[1],
sys.argv[2],
if len(sys.argv) < 5:
rospy.logerr(
"usage: periodicity map map_config minute_interval window_interval"
)
sys.exit(2)
interval = int(sys.argv[3])
window = int(sys.argv[4])
inp = raw_input("start_date end_date month year: ")
[start_date, end_date, month, year] = string.split(inp)
start_date = int(start_date)
end_date = int(end_date)
month = int(month)
year = int(year)
tre = TrajectoryRegionEstimate(sys.argv[1], sys.argv[2], interval)
if start_date > 1:
# if start_date is not the first date of the month, then add the
# previous date
trajectory_estimate = tre.estimate_trajectories_daily(
range(start_date - 1, end_date + 1), month, year)
else:
trajectory_estimate = tre.estimate_trajectories_daily(
range(start_date, end_date + 1), month, year)
tp = TrajectoryPeriodicity(sys.argv[1], sys.argv[2], interval, window)
inp = raw_input("MSE(0) or Prediction MSE(1): ")
if int(inp) == 0:
rospy.loginfo("Addition Best Amplitude Frequency Method")
# rospy.loginfo("Start model selection...")
# tp.model_selection(trajectory_estimate, month, year, True)
if len(sys.argv) < 9:
rospy.logerr("usage: tof soma config start_date end_date month year minute_interval window_time")
sys.exit(2)
# the interval minute must be the same for Trajectory Region Knowledge and
# Continuous TOF
interval = int(sys.argv[7])
window = int(sys.argv[8])
start_date = int(sys.argv[3])
end_date = int(sys.argv[4])
month = int(sys.argv[5])
year = int(sys.argv[6])
temp_start_time = rospy.Time.now()
tre = TrajectoryRegionEstimate(sys.argv[1], sys.argv[2], interval)
if start_date > 1:
trajectory_estimate = tre.estimate_trajectories_daily(
# range(1, calendar.monthrange(year, month)[1]+1), month, year
range(start_date-1, end_date+1), month, year
)
else:
trajectory_estimate = tre.estimate_trajectories_daily(
# range(1, calendar.monthrange(year, month)[1]+1), month, year
range(start_date, end_date+1), month, year
)
tof = TrajectoryOccurrenceFrequencies(
sys.argv[1], sys.argv[2], minute_interval=interval, window_interval=window
)
tof.load_tof()
if __name__ == "__main__":
rospy.init_node("trajectory_periodicity")
if len(sys.argv) < 5:
rospy.logerr("usage: periodicity map map_config minute_interval window_interval")
sys.exit(2)
interval = int(sys.argv[3])
window = int(sys.argv[4])
inp = raw_input("start_date end_date month year: ")
[start_date, end_date, month, year] = string.split(inp)
start_date = int(start_date)
end_date = int(end_date)
month = int(month)
year = int(year)
tre = TrajectoryRegionEstimate(sys.argv[1], sys.argv[2], interval)
if start_date > 1:
# if start_date is not the first date of the month, then add the
# previous date
trajectory_estimate = tre.estimate_trajectories_daily(
range(start_date-1, end_date+1), month, year
)
else:
trajectory_estimate = tre.estimate_trajectories_daily(
range(start_date, end_date+1), month, year
)
tp = TrajectoryPeriodicity(sys.argv[1], sys.argv[2], interval, window)
inp = raw_input("MSE(0) or Prediction MSE(1): ")
if int(inp) == 0:
rospy.loginfo("Addition Best Amplitude Frequency Method")
