def __init__(self, soma_map, soma_conf, config_file=None):
self.soma_map = soma_map
self.soma_conf = soma_conf
if config_file:
self._config_file = config_file
else:
# default file
rp = RosPack()
path = rp.get_path('soma_objects') + '/config/'
filename = 'default.json'
self._config_file=path+filename
self._soma_obj_ids = dict()
self._soma_obj_msg = dict()
self._interactive = True
self._msg_store=MessageStoreProxy(collection="soma")
self._gs_store=GeoSpatialStoreProxy(db="geospatial_store", collection="soma")
self._server = InteractiveMarkerServer("soma")
self._init_types()
self._init_menu()
self.load_objects()
rospy.spin()
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):
self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
self.ms = MessageStoreProxy(collection="people_trajectory")
# setting up the service
self.ser = rospy.Service('/trajectory_query', TrajectoryQuery,
self.service_cb)
self.topic = 'trajectory_query'
self.vis = TrajectoryVisualizer(self.topic)
def __init__(self, soma_map, soma_config):
self.soma_map = soma_map
self.soma_config = soma_config
self.ms = MessageStoreProxy(collection="region_observation_time")
self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
self.roslog = pymongo.MongoClient(
rospy.get_param("mongodb_host", "localhost"),
rospy.get_param("mongodb_port", 62345)
).roslog.robot_pose
self.reinit()
def filtered_trajectory_uuids(vis=False):
"""Find the completed (and filtered) trajectories from people_trajectory store"""
msg_store = GeoSpatialStoreProxy('message_store', 'people_trajectory')
query = {"uuid": {"$exists": "True"}}
rospy.loginfo("Query: %s" % query )
res = msg_store.find_projection(query, {"uuid":1})
rospy.loginfo("Result: %s filtered trajectories" % res.count() )
list_of_uuids = []
for i in res:
list_of_uuids.append(i["uuid"])
return list_of_uuids
class TrajectoryQueryService():
def __init__(self):
self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
self.ms = MessageStoreProxy(collection="people_trajectory")
# setting up the service
self.ser = rospy.Service('/trajectory_query', TrajectoryQuery,
self.service_cb)
self.topic = 'trajectory_query'
self.vis = TrajectoryVisualizer(self.topic)
def retrieve_msg(self, uuid):
res = self.ms.query(Trajectory._type, message_query={"uuid": uuid})
if len(res) < 1:
rospy.logerr("Trajectory not found: %s" % uuid)
return None
elif len(res) > 1:
rospy.logerr("Multiple trajectories found: %s" % uuid)
t = res[0][0]
return t
t = res[0][0]
return t
def service_cb(self, req):
rospy.loginfo("Request received: %s" % req)
if req.visualize:
self.vis.clear()
res = TrajectoryQueryResponse()
res.trajectories = Trajectories()
try:
json_query = json.loads(req.query)
trajectories = self.gs.find(json_query)
except:
rospy.logerr("Invalid json => re-check syntax")
res.error = True
return res
count = 0
for t in trajectories:
if t.has_key('uuid'):
count += 1
#rospy.loginfo("retrieve msg for uuid: %s" % t['uuid'])
# otherwise result is not a trajectory => ignore
msg = self.retrieve_msg(t['uuid'])
if msg:
res.trajectories.trajectories.append(msg)
rospy.loginfo("Query result: %s trajectories" % count)
if req.visualize:
rospy.loginfo("Visualize result on topic: %s" % self.topic)
self.vis.visualize_trajectories(res.trajectories)
rospy.loginfo("Response returned")
res.error = False
return res
def main(self):
rospy.spin()
class TrajectoryQueryService():
def __init__(self):
self.gs = GeoSpatialStoreProxy('geospatial_store','soma')
self.ms = MessageStoreProxy(collection="people_trajectory")
# setting up the service
self.ser = rospy.Service('/trajectory_query', TrajectoryQuery, self.service_cb)
self.topic = 'trajectory_query'
self.vis = TrajectoryVisualizer(self.topic)
def retrieve_msg(self, uuid):
res = self.ms.query(Trajectory._type, message_query={"uuid": uuid})
if len(res) < 1:
rospy.logerr("Trajectory not found: %s" % uuid)
return None
elif len(res) > 1:
rospy.logerr("Multiple trajectories found: %s" % uuid)
t = res[0][0]
return t
t = res[0][0]
return t
def service_cb(self, req):
rospy.loginfo("Request received: %s" % req)
if req.visualize:
self.vis.clear()
res = TrajectoryQueryResponse()
res.trajectories = Trajectories()
try:
json_query = json.loads(req.query)
trajectories = self.gs.find(json_query)
except:
rospy.logerr("Invalid json => re-check syntax")
res.error = True
return res
count = 0
for t in trajectories:
if t.has_key('uuid'):
count += 1
#rospy.loginfo("retrieve msg for uuid: %s" % t['uuid'])
# otherwise result is not a trajectory => ignore
msg = self.retrieve_msg(t['uuid'])
if msg:
res.trajectories.trajectories.append(msg)
rospy.loginfo("Query result: %s trajectories" % count)
if req.visualize:
rospy.loginfo("Visualize result on topic: %s" % self.topic)
self.vis.visualize_trajectories(res.trajectories)
rospy.loginfo("Response returned")
res.error = False
return res
def main(self):
rospy.spin()
def __init__(self, soma_map, soma_conf, config_file=None):
self.soma_map = soma_map
self.soma_conf = soma_conf
if config_file:
self._config_file = config_file
else:
# default file
rp = RosPack()
path = rp.get_path('soma_roi_manager') + '/config/'
filename = 'default.json'
self._config_file=path+filename
self._soma_obj_ids = dict()
self._soma_obj_msg = dict()
self._soma_obj_roi_ids = dict()
self._soma_obj_roi = dict()
self._soma_obj_type = dict()
self._soma_obj_pose = dict()
self._interactive = True
self._msg_store=MessageStoreProxy(collection="soma_roi")
self._gs_store=GeoSpatialStoreProxy(db="geospatial_store", collection="soma")
self._server = InteractiveMarkerServer("soma_roi")
self._init_types()
self._init_menu()
self.load_objects()
rospy.spin()
def __init__(self):
self.gs = GeoSpatialStoreProxy('geospatial_store','soma')
self.ms = MessageStoreProxy(collection="people_trajectory")
# setting up the service
self.ser = rospy.Service('/trajectory_query', TrajectoryQuery, self.service_cb)
self.topic = 'trajectory_query'
self.vis = TrajectoryVisualizer(self.topic)
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):
self.soma_map = soma_map
self.soma_config = soma_config
self.ms = MessageStoreProxy(collection="region_observation_time")
self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
self.roslog = pymongo.MongoClient(
rospy.get_param("mongodb_host", "localhost"),
rospy.get_param("mongodb_port", 62345)
).roslog.robot_pose
self.reinit()
def __init__(self, online):
self._traj = dict()
rospy.loginfo("Connecting to mongodb...")
self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
if not online:
# self._client = pymongo.MongoClient(rospy.get_param("mongodb_host"),
# rospy.get_param("mongodb_port"))
# self._store_client = MessageStoreProxy(
# collection="people_trajectory"
# )
self._traj = dict()
trajs = OfflineTrajectories()
for uuid, traj in trajs.traj.iteritems():
self._traj[uuid] = traj.get_trajectory_message()
else:
rospy.loginfo("Subscribing to /human_trajectories/trajectories...")
rospy.Subscriber("/human_trajectories/trajectories/complete",
Trajectories,
self.traj_callback,
None,
queue_size=10)
def __init__(self, soma_map, soma_config, prefix='spatial_model'):
super(SpatialModelServer, self).__init__()
self.model_cloud = rospy.Publisher("/spatial_model", PointCloud2, queue_size=1, latch=True)
self.best_pose = rospy.Publisher("/spatial_model_best_pose", PoseStamped, queue_size=1, latch=True)
self.geospatial_store = GeoSpatialStoreProxy('geospatial_store', 'soma')
self.soma_map = soma_map
self.soma_config = soma_config
self.soma_roi_query = SOMAROIQuery(self.soma_map, self.soma_config)
self.soma_proxy = MessageStoreProxy(collection="soma")
self.aggregate_models_dictionary = {}
# advertise ros services
for attr in dir(self):
if attr.endswith("_ros_srv"):
service = getattr(self, attr)
rospy.Service(prefix + "/" +attr[:-8], service.type, service)
def __init__(self, online, traj_topic):
self._traj = dict()
rospy.loginfo("Connecting to mongodb...")
self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
if online:
if traj_topic[0] != '/':
traj_topic = '/' + traj_topic
if traj_topic[-1] == '/':
traj_topic = traj_topic[:-1]
rospy.loginfo("Subscribing to %s..." % traj_topic)
rospy.Subscriber(
"%s/complete" % traj_topic, Trajectories,
self.traj_callback, None, queue_size=10
)
else:
self._traj = dict()
trajs = OfflineTrajectories()
for uuid, traj in trajs.traj.iteritems():
self._traj[uuid] = traj.get_trajectory_message()
def __init__(self, online):
self._traj = dict()
rospy.loginfo("Connecting to mongodb...")
self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
if not online:
# self._client = pymongo.MongoClient(rospy.get_param("mongodb_host"),
# rospy.get_param("mongodb_port"))
# self._store_client = MessageStoreProxy(
# collection="people_trajectory"
# )
self._traj = dict()
trajs = OfflineTrajectories()
for uuid, traj in trajs.traj.iteritems():
self._traj[uuid] = traj.get_trajectory_message()
else:
rospy.loginfo("Subscribing to /human_trajectories/trajectories...")
rospy.Subscriber(
"/human_trajectories/trajectories/complete", Trajectories,
self.traj_callback, None, queue_size=10
)
"""Recursively converts dictionary keys to strings."""
if isinstance(dictionary, unicode):
return str(dictionary)
elif isinstance(dictionary, list):
return dictionary
return dict((str(k), convert_keys_to_string(v))
for k, v in dictionary.items())
if __name__ == "__main__":
global __out
__out = True
rospy.init_node("trajectory_obtainer")
rospy.loginfo("Running trajectoy/ROI query ")
gs = GeoSpatialStoreProxy('geospatial_store','soma')
soma_map = 'uob_library'
soma_config = 'uob_lib_conf'
cnt=0
for roi in gs.roi_ids(soma_map, soma_config):
cnt+=1
print 'ROI: ', gs.type_of_roi(roi, soma_map, soma_config), roi
geom = gs.geom_of_roi(roi, soma_map, soma_config)
res = gs.objs_within_roi(geom, soma_map, soma_config)
if res == None:
print "No Objects in this Region"
continue
objects_in_roi = {}
for i in res:
key = i['type'] +'_'+ i['soma_id']
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 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 SOMAManager():
def __init__(self, soma_map, soma_conf, config_file=None):
self.soma_map = soma_map
self.soma_conf = soma_conf
if config_file:
self._config_file = config_file
else:
# default file
rp = RosPack()
path = rp.get_path('soma_objects') + '/config/'
filename = 'default.json'
self._config_file=path+filename
self._soma_obj_ids = dict()
self._soma_obj_msg = dict()
self._interactive = True
self._msg_store=MessageStoreProxy(collection="soma")
self._gs_store=GeoSpatialStoreProxy(db="geospatial_store", collection="soma")
self._server = InteractiveMarkerServer("soma")
self._init_types()
self._init_menu()
self.load_objects()
rospy.spin()
def _init_types(self):
# read from config in soma_objects
with open(self._config_file) as config_file:
config = json.load(config_file)
self.mesh = dict()
self.marker = dict()
if '2D' in config:
for k, v in config['2D'].iteritems():
self.mesh[k] = v
self.marker[k] = '2D'
if '3D' in config:
for k, v in config['3D'].iteritems():
self.mesh[k] = v
self.marker[k] = '3D'
def _init_menu(self):
self.menu_handler = MenuHandler()
add_entry = self.menu_handler.insert( "Add object" )
self.menu_item = dict()
for k in sorted(self.mesh.keys()):
entry = self.menu_handler.insert(k, parent=add_entry, callback=self._add_cb)
self.menu_item[entry] = k
del_entry = self.menu_handler.insert( "Delete object", callback=self._del_cb)
enable_entry = self.menu_handler.insert( "Movement control", callback=self._enable_cb )
self.menu_handler.setCheckState( enable_entry, MenuHandler.CHECKED )
def _add_cb(self, feedback):
rospy.loginfo("Add marker: %s", self.menu_item[feedback.menu_entry_id])
self.add_object(self.menu_item[feedback.menu_entry_id], feedback.pose)
def _del_cb(self, feedback):
rospy.loginfo("Delete marker: %s", feedback.marker_name)
self.delete_object(feedback.marker_name)
def _update_cb(self, feedback):
p = feedback.pose.position
print "Marker " + feedback.marker_name + " position: " + str(round(p.x,2)) + ", " + str(round(p.y,2)) + ", " + str(round(p.z,2))
if hasattr(self, "vp_timer_"+feedback.marker_name):
getattr(self, "vp_timer_"+feedback.marker_name).cancel()
setattr(self, "vp_timer_"+feedback.marker_name,
Timer(3, self.update_object, [feedback]))
getattr(self, "vp_timer_"+feedback.marker_name).start()
def _enable_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState( handle )
if state == MenuHandler.CHECKED:
self.menu_handler.setCheckState( handle, MenuHandler.UNCHECKED )
self._interactive = False
else:
self.menu_handler.setCheckState( handle, MenuHandler.CHECKED )
self._interactive = True
self.menu_handler.reApply( self._server )
self.load_objects()
self._server.applyChanges()
def _next_id(self):
self._soma_id += 1
return self._soma_id
def _retrieve_objects(self):
objs = self._msg_store.query(SOMAObject._type, message_query={"map": self.soma_map,
"config": self.soma_conf})
max_id = 0
for o,om in objs:
if int(o.id) > max_id:
max_id = int(o.id)
self._soma_id = max_id
return objs
def load_objects(self):
objs = self._retrieve_objects()
# if collection is empty insert initial object
if not objs:
pose = Pose()
self.add_object('Table', pose)
return
# otherwise, load all object from collection
for o, om in objs:
self._soma_obj_ids[o.id] = om['_id']
self._soma_obj_msg[o.id] = o
self.load_object(o.id, o.type, o.pose)
def load_object(self, soma_id, soma_type, pose):
int_marker = self.create_object_marker(soma_id, soma_type, pose)
self._server.insert(int_marker, self._update_cb)
self.menu_handler.apply( self._server, soma_id )
self._server.applyChanges()
def add_object(self, soma_type, pose):
# todo: add to mongodb
soma_id = self._next_id()
soma_obj = SOMAObject()
soma_obj.id = str(soma_id)
soma_obj.map = str(self.soma_map)
soma_obj.config = str(self.soma_conf)
soma_obj.type = soma_type
soma_obj.pose = pose
soma_obj.pose.position.z = 0.0
soma_obj.frame = 'map'
soma_obj.mesh = self.mesh[soma_type]
_id = self._msg_store.insert(soma_obj)
self._soma_obj_ids[soma_obj.id] = _id
self._soma_obj_msg[soma_obj.id] = soma_obj
# add object to geospatial store
self._gs_store.insert(self.geo_json_from_soma_obj(soma_obj))
print "GS Store: added obj"
self.load_object(str(soma_id), soma_type, soma_obj.pose)
def geo_json_from_soma_obj(self, soma_obj):
geo_json = {}
geo_json['soma_id'] = soma_obj.id
geo_json['soma_map'] = soma_obj.map
geo_json['soma_config'] = soma_obj.config
geo_json['type'] = soma_obj.type
geo_json['loc'] = {'type': 'Point',
'coordinates': self._gs_store.coords_to_lnglat(soma_obj.pose.position.x,
soma_obj.pose.position.y)}
return geo_json
def delete_object(self, soma_id):
# geospatial store
res = self._gs_store.find_one({'soma_id': soma_id,
'soma_map': self.soma_map,
'soma_config': self.soma_conf})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
print "GS Store: deleted obj"
# message store
_id = self._soma_obj_ids[str(soma_id)]
self._msg_store.delete(str(_id))
self._server.erase(soma_id)
self._server.applyChanges()
def update_object(self, feedback):
print "Updated marker " + feedback.marker_name
_id = self._soma_obj_ids[feedback.marker_name]
msg = self._soma_obj_msg[feedback.marker_name]
new_msg = copy.deepcopy(msg)
new_msg.pose = feedback.pose
self._msg_store.update_id(_id, new_msg)
# geospatial store
# delete old message
res = self._gs_store.find_one({'soma_id': new_msg.id,
'soma_map': self.soma_map,
'soma_config': self.soma_conf})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
print "GS Store: deleted obj"
# add new object to geospatial store
self._gs_store.insert(self.geo_json_from_soma_obj(new_msg))
print "GS Store: added obj"
def create_object_marker(self, soma_obj, soma_type, pose):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = soma_obj
int_marker.description = "id" + soma_obj
int_marker.pose = pose
int_marker.pose.position.z = 0.01
mesh_marker = Marker()
mesh_marker.type = Marker.MESH_RESOURCE
mesh_marker.scale.x = 1
mesh_marker.scale.y = 1
mesh_marker.scale.z = 1
random.seed(soma_type)
val = random.random()
mesh_marker.color.r = r_func(val)
mesh_marker.color.g = g_func(val)
mesh_marker.color.b = b_func(val)
mesh_marker.color.a = 1.0
#mesh_marker.pose = pose
mesh_marker.mesh_resource = self.mesh[soma_type]
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
if self._interactive:
int_marker.controls.append(copy.deepcopy(control))
# add the control to the interactive marker
if self.marker[soma_type] == '3D':
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
# add menu control
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
menu_control.markers.append( mesh_marker) #makeBox(int_marker) )
int_marker.controls.append(menu_control)
return int_marker
class SOMAROIManager():
def __init__(self, soma_map, soma_conf, config_file=None):
self.soma_map = soma_map
self.soma_conf = soma_conf
if config_file:
self._config_file = config_file
else:
# default file
rp = RosPack()
path = rp.get_path('soma_roi_manager') + '/config/'
filename = 'default.json'
self._config_file = path + filename
self._soma_obj_ids = dict()
self._soma_obj_msg = dict()
self._soma_obj_roi_ids = dict()
self._soma_obj_roi = dict()
self._soma_obj_type = dict()
self._soma_obj_pose = dict()
self._interactive = True
self._msg_store = MessageStoreProxy(collection="soma_roi")
self._gs_store = GeoSpatialStoreProxy(db="geospatial_store",
collection="soma")
self._server = InteractiveMarkerServer("soma_roi")
self._init_types()
self._init_menu()
self.load_objects()
rospy.spin()
def _init_types(self):
# read from config in soma_objects
with open(self._config_file) as config_file:
config = json.load(config_file)
self.mesh = dict()
for k, v in config['roi'].iteritems():
self.mesh[k] = v
def _init_menu(self):
self.menu_handler = MenuHandler()
add_point_entry = self.menu_handler.insert("Add Point",
callback=self._add_point_cb)
del_point_entry = self.menu_handler.insert("Delete Point",
callback=self._del_point_cb)
add_entry = self.menu_handler.insert("Add ROI")
self.menu_item = dict()
for k in sorted(self.mesh.keys()):
entry = self.menu_handler.insert(k,
parent=add_entry,
callback=self._add_cb)
self.menu_item[entry] = k
del_entry = self.menu_handler.insert("Delete ROI",
callback=self._del_cb)
enable_entry = self.menu_handler.insert("Movement control",
callback=self._enable_cb)
self.menu_handler.setCheckState(enable_entry, MenuHandler.CHECKED)
def _add_cb(self, feedback):
rospy.loginfo("Add ROI: %s", self.menu_item[feedback.menu_entry_id])
self.add_object(self.menu_item[feedback.menu_entry_id], feedback.pose)
def _del_cb(self, feedback):
rospy.loginfo("Delete ROI: %s", feedback.marker_name)
roi = self._soma_obj_roi[feedback.marker_name]
for r in self._soma_obj_roi_ids[roi]:
self.delete_object(r)
self.undraw_roi(roi)
def _add_point_cb(self, feedback):
rospy.loginfo("Add point: %s", feedback.marker_name)
roi = self._soma_obj_roi[feedback.marker_name]
t = self._soma_obj_type[feedback.marker_name]
self.add_object(t, feedback.pose, roi)
#self.draw_roi(roi)
def _del_point_cb(self, feedback):
rospy.loginfo("Delete point: %s", feedback.marker_name)
self.delete_object(feedback.marker_name)
roi = self._soma_obj_roi[feedback.marker_name]
self.draw_roi(roi)
def _update_poly(self, feedback):
return
def _update_cb(self, feedback):
p = feedback.pose.position
#print "Marker " + feedback.marker_name + " is now at " + str(p.x) + ", " + str(p.y)
self._soma_obj_pose[feedback.marker_name] = feedback.pose
roi = self._soma_obj_roi[feedback.marker_name]
self.draw_roi(roi)
if hasattr(self, "vp_timer_" + feedback.marker_name):
getattr(self, "vp_timer_" + feedback.marker_name).cancel()
setattr(self, "vp_timer_" + feedback.marker_name,
Timer(3, self.update_object, [feedback]))
getattr(self, "vp_timer_" + feedback.marker_name).start()
def _enable_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState(handle)
if state == MenuHandler.CHECKED:
self.menu_handler.setCheckState(handle, MenuHandler.UNCHECKED)
self._interactive = False
else:
self.menu_handler.setCheckState(handle, MenuHandler.CHECKED)
self._interactive = True
self.menu_handler.reApply(self._server)
self.load_objects()
self._server.applyChanges()
def _next_id(self):
self._soma_id += 1
return self._soma_id
def _next_roi_id(self):
self._soma_roi_id += 1
return self._soma_roi_id
def _retrieve_objects(self):
objs = self._msg_store.query(SOMAROIObject._type,
message_query={
"map": self.soma_map,
"config": self.soma_conf
})
max_id = 0
max_roi_id = 0
for o, om in objs:
if int(o.id) > max_id:
max_id = int(o.id)
if int(o.roi_id) > max_roi_id:
max_roi_id = int(o.roi_id)
self._soma_id = max_id
self._soma_roi_id = max_roi_id
return objs
def load_objects(self):
self._soma_obj_roi_ids = dict()
objs = self._retrieve_objects()
# if collection is empty insert initial object
if not objs:
pose = Pose()
self.add_object('Office', pose)
return
# otherwise, load all object from collection
for o, om in objs:
self._soma_obj_ids[o.id] = om['_id']
self._soma_obj_msg[o.id] = o
if o.roi_id in self._soma_obj_roi_ids:
self._soma_obj_roi_ids[o.roi_id].append(o.id)
else:
self._soma_obj_roi_ids[o.roi_id] = list()
self._soma_obj_roi_ids[o.roi_id].append(o.id)
self._soma_obj_roi[o.id] = o.roi_id
self._soma_obj_type[o.id] = o.type
self._soma_obj_pose[o.id] = o.pose
self.load_object(o.id, o.roi_id, o.type, o.pose)
self.draw_all_roi()
def draw_all_roi(self):
for key in self._soma_obj_roi_ids:
self.draw_roi(key)
def undraw_all_roi(self):
for key in self._soma_obj_roi_ids:
self.undraw_roi(key)
def draw_roi(self, roi):
v = self._soma_obj_roi_ids[roi]
t = self._soma_obj_type[v[0]]
p = self._soma_obj_pose[v[0]]
int_marker = self.create_roi_marker(roi, t, p, v)
self._server.erase("ROI-" + roi)
self._server.applyChanges()
self._server.insert(int_marker, self._update_poly)
self._server.applyChanges()
def undraw_roi(self, roi):
self._server.erase("ROI-" + roi)
self._server.applyChanges()
def load_object(self, soma_id, roi, soma_type, pose):
int_marker = self.create_object_marker(soma_id, roi, soma_type, pose)
self._server.insert(int_marker, self._update_cb)
self.menu_handler.apply(self._server, soma_id)
self._server.applyChanges()
def add_object(self, soma_type, pose, roi_id=None):
# todo: add to mongodb
soma_id = self._next_id()
if roi_id == None:
soma_roi_id = self._next_roi_id()
self._soma_obj_roi_ids[str(soma_roi_id)] = list()
else:
soma_roi_id = roi_id
soma_obj = SOMAROIObject()
soma_obj.id = str(soma_id)
soma_obj.roi_id = str(soma_roi_id)
soma_obj.map = str(self.soma_map)
soma_obj.config = str(self.soma_conf)
soma_obj.type = soma_type
soma_obj.pose = pose
soma_obj.frame = 'map'
_id = self._msg_store.insert(soma_obj)
self._soma_obj_ids[soma_obj.id] = _id
self._soma_obj_msg[soma_obj.id] = soma_obj
self._soma_obj_roi_ids[soma_obj.roi_id].append(soma_obj.id)
self._soma_obj_roi[soma_obj.id] = soma_obj.roi_id
self._soma_obj_type[soma_obj.id] = soma_type
self._soma_obj_pose[soma_obj.id] = pose
self.load_object(str(soma_id), soma_obj.roi_id, soma_type, pose)
# geospatial store
# delete old message
res = self._gs_store.find_one({
'soma_roi_id': soma_roi_id,
'soma_map': self.soma_map,
'soma_config': self.soma_conf
})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
#rospy.loginfo("GS Store: deleted roi")
# add new object to geospatial store
geo_json = self.geo_json_from_soma_obj(soma_obj)
if geo_json:
try:
self._gs_store.insert(geo_json)
rospy.loginfo("GS Store: updated roi (%s %s)" %
(soma_obj.type, soma_obj.roi_id))
except:
rospy.logerr(
"The polygon of %s %s is malformed (self-intersecting) => Please update geometry."
% (soma_type, soma_roi_id))
def geo_json_from_soma_obj(self, soma_obj):
geo_json = {}
geo_json['soma_roi_id'] = soma_obj.roi_id
geo_json['soma_map'] = soma_obj.map
geo_json['soma_config'] = soma_obj.config
geo_json['type'] = soma_obj.type
if len(self._soma_obj_roi_ids[soma_obj.roi_id]) < 3:
rospy.logerr(
"GS Store: %s %s, less then 3 points => Add more points or delete ROI."
% (soma_obj.type, soma_obj.roi_id))
return None
coordinates = []
for obj_id in self._soma_obj_roi_ids[soma_obj.roi_id]:
p = self._soma_obj_pose[obj_id]
coordinates.append(
self._gs_store.coords_to_lnglat(p.position.x, p.position.y))
p = self._soma_obj_pose[self._soma_obj_roi_ids[soma_obj.roi_id][0]]
coordinates.append(
self._gs_store.coords_to_lnglat(p.position.x, p.position.y))
geo_json['loc'] = {'type': 'Polygon', 'coordinates': [coordinates]}
return geo_json
def delete_object(self, soma_id):
# todo: delete from mongodb
_id = self._soma_obj_ids[str(soma_id)]
self._msg_store.delete(str(_id))
self._server.erase(soma_id)
self._server.applyChanges()
roi = self._soma_obj_roi[str(soma_id)]
nodes = self._soma_obj_roi_ids[roi]
new_nodes = []
for n in nodes:
if n != str(soma_id):
new_nodes.append(n)
self._soma_obj_roi_ids[roi] = new_nodes
# geospatial store
# delete old message
old_msg = self._soma_obj_msg[soma_id]
res = self._gs_store.find_one({
'soma_roi_id': roi,
'soma_map': self.soma_map,
'soma_config': self.soma_conf
})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
#rospy.loginfo("GS Store: deleted roi")
# add new object to geospatial store
geo_json = self.geo_json_from_soma_obj(old_msg)
if geo_json:
try:
self._gs_store.insert(geo_json)
rospy.loginfo("GS Store: updated roi (%s %s)" %
(old_msg.type, old_msg.roi_id))
except:
rospy.logerr(
"The polygon of %s %s is malformed (self-intersecting) => Please update geometry."
% (old_msg.type, old_msg.roi_id))
def update_object(self, feedback):
rospy.loginfo("Updated marker: %s", feedback.marker_name)
_id = self._soma_obj_ids[feedback.marker_name]
msg = self._soma_obj_msg[feedback.marker_name]
new_msg = copy.deepcopy(msg)
new_msg.pose = feedback.pose
self._soma_obj_pose[feedback.marker_name] = feedback.pose
self._msg_store.update_id(_id, new_msg)
# geospatial store
# delete old message
res = self._gs_store.find_one({
'soma_roi_id': new_msg.roi_id,
'soma_map': self.soma_map,
'soma_config': self.soma_conf
})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
#rospy.loginfo("GS Store: deleted roi")
# add new object to geospatial store
geo_json = self.geo_json_from_soma_obj(new_msg)
if geo_json:
try:
self._gs_store.insert(geo_json)
rospy.loginfo("GS Store: updated roi (%s %s)" %
(new_msg.type, new_msg.roi_id))
except:
rospy.logerr(
"The polygon of %s %s is malformed (self-intersecting) => Please update geometry."
% (new_msg.type, new_msg.roi_id))
def create_object_marker(self, soma_obj, roi, soma_type, pose):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = soma_obj
int_marker.description = soma_type + ' (' + roi + ')'
int_marker.pose = pose
int_marker.pose.position.z = 0.01
marker = Marker()
marker.type = Marker.SPHERE
marker.scale.x = 0.25
marker.scale.y = 0.25
marker.scale.z = 0.25
int_marker.pose.position.z = (marker.scale.z / 2)
random.seed(soma_type)
val = random.random()
marker.color.r = r_func(val)
marker.color.g = g_func(val)
marker.color.b = b_func(val)
marker.color.a = 1.0
#marker.pose = pose
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
if self._interactive:
int_marker.controls.append(copy.deepcopy(control))
# add the control to the interactive marker
int_marker.controls.append(control)
# add menu control
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
menu_control.markers.append(marker) #makeBox(int_marker) )
int_marker.controls.append(menu_control)
return int_marker
def create_roi_marker(self, roi, soma_type, pose, points):
#print "POINTS: " + str(points)
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = "ROI-" + roi
int_marker.description = roi
int_marker.pose = pose
marker = Marker()
marker.type = Marker.LINE_STRIP
marker.scale.x = 0.1
random.seed(soma_type)
val = random.random()
marker.color.r = r_func(val)
marker.color.g = g_func(val)
marker.color.b = b_func(val)
marker.color.a = 1.0
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(marker)
int_marker.controls.append(control)
marker.points = []
for point in points:
p = Point()
pose = self._soma_obj_pose[point]
p.x = pose.position.x - int_marker.pose.position.x
p.y = pose.position.y - int_marker.pose.position.y
marker.points.append(p)
p = Point()
pose = self._soma_obj_pose[points[0]]
p.x = pose.position.x - int_marker.pose.position.x
p.y = pose.position.y - int_marker.pose.position.y
marker.points.append(p)
return int_marker
class TrajectoryImporter(object):
def __init__(self, online):
self._traj = dict()
rospy.loginfo("Connecting to mongodb...")
self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
if not online:
# self._client = pymongo.MongoClient(rospy.get_param("mongodb_host"),
# rospy.get_param("mongodb_port"))
# self._store_client = MessageStoreProxy(
# collection="people_trajectory"
# )
self._traj = dict()
trajs = OfflineTrajectories()
for uuid, traj in trajs.traj.iteritems():
self._traj[uuid] = traj.get_trajectory_message()
else:
rospy.loginfo("Subscribing to /human_trajectories/trajectories...")
rospy.Subscriber("/human_trajectories/trajectories/complete",
Trajectories,
self.traj_callback,
None,
queue_size=10)
def store_all(self):
for uuid, t in self._traj.items():
# msg = t.get_trajectory_message()
# geo_json = self.geojson_from_trajectory(msg)
geo_json = self.geojson_from_trajectory(t)
# in case trajectory is already in there => replace
_uuid = geo_json['uuid']
self.gs.remove(_uuid)
rospy.loginfo("Storing %s data to geospatial_store", uuid)
self.gs.insert(geo_json)
# meta = dict()
# meta["map"] = map
# rospy.loginfo("Storing %s in geo
# self._store_client.insert(msg, meta)
del self._traj[uuid]
rospy.sleep(0.5)
def traj_callback(self, msg):
self._traj.update(
{traj.uuid: traj
for i, traj in enumerate(msg.trajectories)})
def geojson_from_trajectory(self, msg):
geojson = {}
# trajectory UUID
geojson['uuid'] = msg.uuid
# space
loc = {}
loc['type'] = 'LineString'
loc['coordinates'] = []
for ps in msg.trajectory:
xy_coord = self.gs.coords_to_lnglat(ps.pose.position.x,
ps.pose.position.y)
loc['coordinates'].append(xy_coord)
geojson['loc'] = loc
# time
geojson['start'] = msg.start_time.to_sec()
geojson['end'] = msg.end_time.to_sec()
start_dt = datetime.fromtimestamp(msg.start_time.to_sec())
geojson['start_weekday'] = start_dt.weekday()
geojson['start_hour'] = start_dt.hour
geojson['start_minute'] = start_dt.minute
end_dt = datetime.fromtimestamp(msg.end_time.to_sec())
geojson['end_weekday'] = end_dt.weekday()
geojson['end_hour'] = end_dt.hour
geojson['end_minute'] = end_dt.minute
return geojson
class TrajectoryImporter(object):
def __init__(self, online, traj_topic):
self._traj = dict()
rospy.loginfo("Connecting to mongodb...")
self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
if online:
if traj_topic[0] != '/':
traj_topic = '/' + traj_topic
if traj_topic[-1] == '/':
traj_topic = traj_topic[:-1]
rospy.loginfo("Subscribing to %s..." % traj_topic)
rospy.Subscriber(
"%s/complete" % traj_topic, Trajectories,
self.traj_callback, None, queue_size=10
)
else:
self._traj = dict()
trajs = OfflineTrajectories()
for uuid, traj in trajs.traj.iteritems():
self._traj[uuid] = traj.get_trajectory_message()
def store_all(self):
for uuid, t in self._traj.items():
geo_json = self.geojson_from_trajectory(t)
# in case trajectory is already in there => replace
ids = self.gs.find_projection({"uuid": uuid}, {"_id": 1})
for _id in ids:
self.gs.remove(_id["_id"])
rospy.loginfo("Storing %s data to geospatial_store", uuid)
self.gs.insert(geo_json)
# meta = dict()
# meta["map"] = map
# self._store_client.insert(msg, meta)
del self._traj[uuid]
rospy.sleep(0.5)
def traj_callback(self, msg):
self._traj.update({
traj.uuid: traj for i, traj in enumerate(msg.trajectories)
})
def geojson_from_trajectory(self, msg):
geojson = {}
# trajectory UUID
geojson['uuid'] = msg.uuid
# space
loc = {}
loc['type'] = 'LineString'
loc['coordinates'] = []
for ps in msg.trajectory:
xy_coord = self.gs.coords_to_lnglat(
ps.pose.position.x, ps.pose.position.y
)
loc['coordinates'].append(xy_coord)
geojson['loc'] = loc
# time
geojson['start'] = msg.start_time.to_sec()
geojson['end'] = msg.end_time.to_sec()
start_dt = datetime.fromtimestamp(msg.start_time.to_sec())
geojson['start_weekday'] = start_dt.weekday()
geojson['start_hour'] = start_dt.hour
geojson['start_minute'] = start_dt.minute
end_dt = datetime.fromtimestamp(msg.end_time.to_sec())
geojson['end_weekday'] = end_dt.weekday()
geojson['end_hour'] = end_dt.hour
geojson['end_minute'] = end_dt.minute
return geojson
req = TrajectoryQueryRequest()
req.query = query
req.visualize = vis
res = self.ser(req)
return res
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
if __name__ == "__main__":
rospy.init_node("query_examples")
rospy.loginfo("Running query examples")
client = QueryClient()
gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
soma_map = 'uob_library'
soma_config = 'uob_lib_conf'
for obj in gs.obj_ids(soma_map, soma_config):
print 'OBJ', obj, gs.type_of_obj(obj, soma_map, soma_config)
geom = gs.geom_of_obj(obj, soma_map, soma_config)
print 'OBJ', obj, geom
for roi in gs.roi_ids(soma_map, soma_config):
print 'ROI', gs.type_of_roi(roi, soma_map, soma_config)
geom = gs.geom_of_roi(roi, soma_map, soma_config)
print 'ROI', roi, geom
geom = gs.geom_of_trajectory('328e2f8c-6147-5525-93c4-1b281887623b')
print 'TRAJECTORY', geom
try:
req = TrajectoryQueryRequest()
req.query = query
req.visualize = vis
res = self.ser(req)
return res
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s"%e)
if __name__=="__main__":
rospy.init_node("query_examples")
rospy.loginfo("Running query examples")
client = QueryClient()
gs = GeoSpatialStoreProxy('geospatial_store','soma')
soma_map = 'uob_library'
soma_config = 'uob_lib_conf'
for obj in gs.obj_ids(soma_map, soma_config):
print 'OBJ', obj, gs.type_of_obj(obj, soma_map, soma_config)
geom = gs.geom_of_obj(obj, soma_map, soma_config)
print 'OBJ', obj, geom
for roi in gs.roi_ids(soma_map, soma_config):
print 'ROI',gs.type_of_roi(roi, soma_map, soma_config)
geom = gs.geom_of_roi(roi, soma_map, soma_config)
print 'ROI', roi, geom
geom = gs.geom_of_trajectory('328e2f8c-6147-5525-93c4-1b281887623b')
print 'TRAJECTORY', geom
class RegionObservationTimeManager(object):
def __init__(self, soma_map, soma_config):
self.soma_map = soma_map
self.soma_config = soma_config
self.ms = MessageStoreProxy(collection="region_observation_time")
self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
self.roslog = pymongo.MongoClient(
rospy.get_param("mongodb_host", "localhost"),
rospy.get_param("mongodb_port", 62345)
).roslog.robot_pose
self.reinit()
def reinit(self):
self.region_observation_duration = dict()
self._month_year_observation_taken = dict()
def load_from_mongo(self, days, minute_interval=60):
"""
Load robot-region observation time from database (mongodb) and store them in
self.region_observation_duration.
Returning (region observation time, total duration of observation)
"""
roi_region_daily = dict()
total_duration = 0
self.minute_interval = minute_interval
for i in days:
end_date = i + datetime.timedelta(hours=24)
rospy.loginfo(
"Load region observation time from %s to %s..." % (str(i), str(end_date))
)
query = {
"soma": self.soma_map, "soma_config": self.soma_config,
"start_from.secs": {
"$gte": time.mktime(i.timetuple()),
"$lt": time.mktime(end_date.timetuple())
}
}
roi_reg_hourly = dict()
for log in self.ms.query(RegionObservationTime._type, message_query=query):
end = datetime.datetime.fromtimestamp(log[0].until.secs)
if log[0].region_id not in roi_reg_hourly:
temp = list()
start = datetime.datetime.fromtimestamp(log[0].start_from.secs)
interval = (end.minute + 1) - start.minute
if interval != minute_interval:
continue
for j in range(24):
group_mins = {k*interval: 0 for k in range(1, (60/interval)+1)}
temp.append(group_mins)
roi_reg_hourly.update({log[0].region_id: temp})
roi_reg_hourly[log[0].region_id][end.hour][end.minute+1] = log[0].duration.secs
roi_reg_hourly[log[0].region_id][end.hour][end.minute+1] += 0.000000001 * log[0].duration.nsecs
total_duration += log[0].duration.secs
roi_region_daily.update({i.day: roi_reg_hourly})
self.region_observation_duration = roi_region_daily
return roi_region_daily, total_duration
def store_to_mongo(self):
"""
Store region observation time from self.region_observation_duration to mongodb.
It will store soma map, soma configuration, region_id, the starting and end time where
robot sees a region in some interval, and the duration of how long the robot during
the interval.
"""
rospy.loginfo("Storing region observation time to region_observation_time collection...")
data = self.region_observation_duration
try:
minute_interval = self.minute_interval
except:
rospy.logwarn("Minute interval is not defined, please call either load_from_mongo or calculate_region_observation_duration first")
return
for day, daily_data in data.iteritems():
for reg, reg_data in daily_data.iteritems():
for hour, hourly_data in enumerate(reg_data):
for minute, duration in hourly_data.iteritems():
date_until = datetime.datetime(
self._month_year_observation_taken[day][1],
self._month_year_observation_taken[day][0],
day, hour, minute-1,
59
)
until = time.mktime(date_until.timetuple())
start_from = until - (60 * minute_interval) + 1
msg = RegionObservationTime(
self.soma_map, self.soma_config, reg,
rospy.Time(start_from), rospy.Time(until),
rospy.Duration(duration)
)
self._store_to_mongo(msg)
def _store_to_mongo(self, msg):
query = {
"soma": msg.soma, "soma_config": msg.soma_config,
"region_id": msg.region_id, "start_from.secs": msg.start_from.secs,
"until.secs": msg.until.secs
}
if msg.duration.nsecs > 0:
if len(self.ms.query(RegionObservationTime._type, message_query=query)) > 0:
self.ms.update(msg, message_query=query)
else:
self.ms.insert(msg)
def calculate_region_observation_duration(self, days, minute_interval=60):
"""
Calculating the region observation duration for particular days, splitted by
minute_interval.
Returns the ROIs the robot has monitored at each logged robot pose
for particular days specified up to minutes interval.
"""
rospy.loginfo('Calculation region observation duration...')
roi_region_daily = dict()
self.minute_interval = minute_interval
for i in days:
loaded_roi_reg_day = self.load_from_mongo([i], minute_interval)
if loaded_roi_reg_day[1] == 0:
end_date = i + datetime.timedelta(hours=24)
roi_region_hourly = self._get_robot_region_stay_duration(i, end_date, minute_interval)
roi_region_daily.update({i.day: roi_region_hourly})
else:
roi_region_daily.update({i.day: loaded_roi_reg_day[0][i.day]})
self._month_year_observation_taken.update({i.day: (i.month, i.year)})
self.region_observation_duration = roi_region_daily
return roi_region_daily
# hidden function for get_robot_region_stay_duration
def _get_robot_region_stay_duration(self, start_date, end_date, minute_interval=60):
sampling_rate = 10
roi_temp_list = dict()
rospy.loginfo("Querying from " + str(start_date) + " to " + str(end_date))
query = {
"_id": {"$exists": "true"},
"_meta.inserted_at": {"$gte": start_date, "$lt": end_date}
}
for i, pose in enumerate(self.roslog.find(query)):
if i % sampling_rate == 0:
_, _, yaw = euler_from_quaternion(
[0, 0, pose['orientation']['z'], pose['orientation']['w']]
)
coords = robot_view_cone(pose['position']['x'], pose['position']['y'], yaw)
# coords = robot_view_area(pose['position']['x'], 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]))
for j in self.gs.observed_roi(langitude_latitude, self.soma_map, self.soma_config):
region = str(j['soma_roi_id'])
hour = pose['_meta']['inserted_at'].time().hour
minute = pose['_meta']['inserted_at'].time().minute
# Region Knowledge per hour. Bin them by hour and minute_interval.
if region not in roi_temp_list:
temp = list()
for i in range(24):
group_mins = {
i*minute_interval: 0 for i in range(1, (60/minute_interval)+1)
}
temp.append(group_mins)
roi_temp_list[region] = temp
index = ((minute / minute_interval) + 1) * minute_interval
roi_temp_list[region][hour][index] += 1
roi_temp_list = self._normalizing(roi_temp_list, sampling_rate)
rospy.loginfo("Region observation duration for the query is " + str(roi_temp_list))
return roi_temp_list
def _normalizing(self, roi_temp_list, sampling_rate):
"""
Checking if all robot region relation based on its stay duration is capped
by minute_interval * 60 (total seconds). If it is not, then normalization is applied
"""
regions = roi_temp_list.keys()
_hourly_poses = [0] * 24
for i in range(24):
for region in regions:
_hourly_poses[i] += sum(roi_temp_list[region][i].values())
normalizing = sum([1 for i in _hourly_poses if i > 3601]) > 0
max_hourly_poses = max(_hourly_poses)
for reg, hourly_poses in roi_temp_list.iteritems():
if normalizing:
for ind, val in enumerate(hourly_poses):
for minute, seconds in val.iteritems():
roi_temp_list[reg][ind][minute] = 3600 / float(max_hourly_poses) * seconds
return roi_temp_list
class SOMAROIManager():
def __init__(self, soma_map, soma_conf, config_file=None):
self.soma_map = soma_map
self.soma_conf = soma_conf
if config_file:
self._config_file = config_file
else:
# default file
rp = RosPack()
path = rp.get_path('soma_roi_manager') + '/config/'
filename = 'default.json'
self._config_file=path+filename
self._soma_obj_ids = dict()
self._soma_obj_msg = dict()
self._soma_obj_roi_ids = dict()
self._soma_obj_roi = dict()
self._soma_obj_type = dict()
self._soma_obj_pose = dict()
self._interactive = True
self._msg_store=MessageStoreProxy(collection="soma_roi")
self._gs_store=GeoSpatialStoreProxy(db="geospatial_store", collection="soma")
self._server = InteractiveMarkerServer("soma_roi")
self._init_types()
self._init_menu()
self.load_objects()
rospy.spin()
def _init_types(self):
# read from config in soma_objects
with open(self._config_file) as config_file:
config = json.load(config_file)
self.mesh = dict()
for k, v in config['roi'].iteritems():
self.mesh[k] = v
def _init_menu(self):
self.menu_handler = MenuHandler()
add_point_entry = self.menu_handler.insert( "Add Point", callback=self._add_point_cb)
del_point_entry = self.menu_handler.insert( "Delete Point", callback=self._del_point_cb)
add_entry = self.menu_handler.insert( "Add ROI" )
self.menu_item = dict()
for k in sorted(self.mesh.keys()):
entry = self.menu_handler.insert(k, parent=add_entry, callback=self._add_cb)
self.menu_item[entry] = k
del_entry = self.menu_handler.insert( "Delete ROI", callback=self._del_cb)
enable_entry = self.menu_handler.insert( "Movement control", callback=self._enable_cb )
self.menu_handler.setCheckState( enable_entry, MenuHandler.CHECKED )
def _add_cb(self, feedback):
rospy.loginfo("Add ROI: %s", self.menu_item[feedback.menu_entry_id])
self.add_object(self.menu_item[feedback.menu_entry_id], feedback.pose)
def _del_cb(self, feedback):
rospy.loginfo("Delete ROI: %s", feedback.marker_name)
roi = self._soma_obj_roi[feedback.marker_name]
for r in self._soma_obj_roi_ids[roi]:
self.delete_object(r)
self.undraw_roi(roi)
def _add_point_cb(self, feedback):
rospy.loginfo("Add point: %s", feedback.marker_name)
roi = self._soma_obj_roi[feedback.marker_name]
t = self._soma_obj_type[feedback.marker_name]
self.add_object(t, feedback.pose, roi)
#self.draw_roi(roi)
def _del_point_cb(self, feedback):
rospy.loginfo("Delete point: %s", feedback.marker_name)
self.delete_object(feedback.marker_name)
roi = self._soma_obj_roi[feedback.marker_name]
self.draw_roi(roi)
def _update_poly(self, feedback):
return
def _update_cb(self, feedback):
p = feedback.pose.position
#print "Marker " + feedback.marker_name + " is now at " + str(p.x) + ", " + str(p.y)
self._soma_obj_pose[feedback.marker_name] = feedback.pose
roi = self._soma_obj_roi[feedback.marker_name]
self.draw_roi(roi)
if hasattr(self, "vp_timer_"+feedback.marker_name):
getattr(self, "vp_timer_"+feedback.marker_name).cancel()
setattr(self, "vp_timer_"+feedback.marker_name,
Timer(3, self.update_object, [feedback]))
getattr(self, "vp_timer_"+feedback.marker_name).start()
def _enable_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState( handle )
if state == MenuHandler.CHECKED:
self.menu_handler.setCheckState( handle, MenuHandler.UNCHECKED )
self._interactive = False
else:
self.menu_handler.setCheckState( handle, MenuHandler.CHECKED )
self._interactive = True
self.menu_handler.reApply( self._server )
self.load_objects()
self._server.applyChanges()
def _next_id(self):
self._soma_id += 1
return self._soma_id
def _next_roi_id(self):
self._soma_roi_id += 1
return self._soma_roi_id
def _retrieve_objects(self):
objs = self._msg_store.query(SOMAROIObject._type, message_query={"map": self.soma_map,
"config": self.soma_conf})
max_id = 0
max_roi_id = 0
for o,om in objs:
if int(o.id) > max_id:
max_id = int(o.id)
if int(o.roi_id) > max_roi_id:
max_roi_id = int(o.roi_id)
self._soma_id = max_id
self._soma_roi_id = max_roi_id
return objs
def load_objects(self):
self._soma_obj_roi_ids = dict()
objs = self._retrieve_objects()
# if collection is empty insert initial object
if not objs:
pose = Pose()
self.add_object('Office', pose)
return
# otherwise, load all object from collection
for o, om in objs:
self._soma_obj_ids[o.id] = om['_id']
self._soma_obj_msg[o.id] = o
if o.roi_id in self._soma_obj_roi_ids:
self._soma_obj_roi_ids[o.roi_id].append(o.id)
else:
self._soma_obj_roi_ids[o.roi_id] = list()
self._soma_obj_roi_ids[o.roi_id].append(o.id)
self._soma_obj_roi[o.id] = o.roi_id
self._soma_obj_type[o.id] = o.type
self._soma_obj_pose[o.id] = o.pose
self.load_object(o.id, o.roi_id, o.type, o.pose)
self.draw_all_roi()
def draw_all_roi(self):
for key in self._soma_obj_roi_ids:
self.draw_roi(key)
def undraw_all_roi(self):
for key in self._soma_obj_roi_ids:
self.undraw_roi(key)
def draw_roi(self, roi):
v = self._soma_obj_roi_ids[roi]
t = self._soma_obj_type[v[0]]
p = self._soma_obj_pose[v[0]]
int_marker = self.create_roi_marker(roi, t, p, v)
self._server.erase("ROI-" + roi)
self._server.applyChanges()
self._server.insert(int_marker, self._update_poly)
self._server.applyChanges()
def undraw_roi(self, roi):
self._server.erase("ROI-" + roi)
self._server.applyChanges()
def load_object(self, soma_id, roi, soma_type, pose):
int_marker = self.create_object_marker(soma_id, roi, soma_type, pose)
self._server.insert(int_marker, self._update_cb)
self.menu_handler.apply( self._server, soma_id )
self._server.applyChanges()
def add_object(self, soma_type, pose, roi_id=None):
# todo: add to mongodb
soma_id = self._next_id()
if roi_id == None:
soma_roi_id = self._next_roi_id()
self._soma_obj_roi_ids[str(soma_roi_id)] = list()
else:
soma_roi_id = roi_id
soma_obj = SOMAROIObject()
soma_obj.id = str(soma_id)
soma_obj.roi_id = str(soma_roi_id)
soma_obj.map = str(self.soma_map)
soma_obj.config = str(self.soma_conf)
soma_obj.type = soma_type
soma_obj.pose = pose
soma_obj.frame = 'map'
_id = self._msg_store.insert(soma_obj)
self._soma_obj_ids[soma_obj.id] = _id
self._soma_obj_msg[soma_obj.id] = soma_obj
self._soma_obj_roi_ids[soma_obj.roi_id].append(soma_obj.id)
self._soma_obj_roi[soma_obj.id] = soma_obj.roi_id
self._soma_obj_type[soma_obj.id] = soma_type
self._soma_obj_pose[soma_obj.id] = pose
self.load_object(str(soma_id), soma_obj.roi_id, soma_type, pose)
# geospatial store
# delete old message
res = self._gs_store.find_one({'soma_roi_id': soma_roi_id,
'soma_map': self.soma_map,
'soma_config': self.soma_conf})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
#rospy.loginfo("GS Store: deleted roi")
# add new object to geospatial store
geo_json = self.geo_json_from_soma_obj(soma_obj)
if geo_json:
try:
self._gs_store.insert(geo_json)
rospy.loginfo("GS Store: updated roi (%s %s)" % (soma_obj.type, soma_obj.roi_id) )
except:
rospy.logerr("The polygon of %s %s is malformed (self-intersecting) => Please update geometry." % (soma_type, soma_roi_id))
def geo_json_from_soma_obj(self, soma_obj):
geo_json = {}
geo_json['soma_roi_id'] = soma_obj.roi_id
geo_json['soma_map'] = soma_obj.map
geo_json['soma_config'] = soma_obj.config
geo_json['type'] = soma_obj.type
if len(self._soma_obj_roi_ids[soma_obj.roi_id]) < 3:
rospy.logerr("GS Store: %s %s, less then 3 points => Add more points or delete ROI." % (soma_obj.type, soma_obj.roi_id) )
return None
coordinates = []
for obj_id in self._soma_obj_roi_ids[soma_obj.roi_id]:
p = self._soma_obj_pose[obj_id]
coordinates.append(self._gs_store.coords_to_lnglat(p.position.x, p.position.y))
p = self._soma_obj_pose[self._soma_obj_roi_ids[soma_obj.roi_id][0]]
coordinates.append(self._gs_store.coords_to_lnglat(p.position.x, p.position.y))
geo_json['loc'] = {'type': 'Polygon',
'coordinates': [coordinates]}
return geo_json
def delete_object(self, soma_id):
# todo: delete from mongodb
_id = self._soma_obj_ids[str(soma_id)]
self._msg_store.delete(str(_id))
self._server.erase(soma_id)
self._server.applyChanges()
roi = self._soma_obj_roi[str(soma_id)]
nodes = self._soma_obj_roi_ids[roi]
new_nodes = []
for n in nodes:
if n != str(soma_id):
new_nodes.append(n)
self._soma_obj_roi_ids[roi] = new_nodes
# geospatial store
# delete old message
old_msg = self._soma_obj_msg[soma_id]
res = self._gs_store.find_one({'soma_roi_id': roi,
'soma_map': self.soma_map,
'soma_config': self.soma_conf})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
#rospy.loginfo("GS Store: deleted roi")
# add new object to geospatial store
geo_json = self.geo_json_from_soma_obj(old_msg)
if geo_json:
try:
self._gs_store.insert(geo_json)
rospy.loginfo("GS Store: updated roi (%s %s)" % (old_msg.type, old_msg.roi_id) )
except:
rospy.logerr("The polygon of %s %s is malformed (self-intersecting) => Please update geometry." % (old_msg.type, old_msg.roi_id))
def update_object(self, feedback):
rospy.loginfo("Updated marker: %s", feedback.marker_name)
_id = self._soma_obj_ids[feedback.marker_name]
msg = self._soma_obj_msg[feedback.marker_name]
new_msg = copy.deepcopy(msg)
new_msg.pose = feedback.pose
self._soma_obj_pose[feedback.marker_name] = feedback.pose
self._msg_store.update_id(_id, new_msg)
# geospatial store
# delete old message
res = self._gs_store.find_one({'soma_roi_id': new_msg.roi_id,
'soma_map': self.soma_map,
'soma_config': self.soma_conf})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
#rospy.loginfo("GS Store: deleted roi")
# add new object to geospatial store
geo_json = self.geo_json_from_soma_obj(new_msg)
if geo_json:
try:
self._gs_store.insert(geo_json)
rospy.loginfo("GS Store: updated roi (%s %s)" % (new_msg.type, new_msg.roi_id) )
except:
rospy.logerr("The polygon of %s %s is malformed (self-intersecting) => Please update geometry." % (new_msg.type, new_msg.roi_id))
def create_object_marker(self, soma_obj, roi, soma_type, pose):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = soma_obj
int_marker.description = soma_type + ' (' + roi + ')'
int_marker.pose = pose
int_marker.pose.position.z = 0.01
marker = Marker()
marker.type = Marker.SPHERE
marker.scale.x = 0.25
marker.scale.y = 0.25
marker.scale.z = 0.25
int_marker.pose.position.z = (marker.scale.z / 2)
random.seed(soma_type)
val = random.random()
marker.color.r = r_func(val)
marker.color.g = g_func(val)
marker.color.b = b_func(val)
marker.color.a = 1.0
#marker.pose = pose
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
if self._interactive:
int_marker.controls.append(copy.deepcopy(control))
# add the control to the interactive marker
int_marker.controls.append(control);
# add menu control
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
menu_control.markers.append( marker) #makeBox(int_marker) )
int_marker.controls.append(menu_control)
return int_marker
def create_roi_marker(self, roi, soma_type, pose, points):
#print "POINTS: " + str(points)
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = "ROI-" + roi
int_marker.description = roi
int_marker.pose = pose
marker = Marker()
marker.type = Marker.LINE_STRIP
marker.scale.x = 0.1
random.seed(soma_type)
val = random.random()
marker.color.r = r_func(val)
marker.color.g = g_func(val)
marker.color.b = b_func(val)
marker.color.a = 1.0
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append( marker )
int_marker.controls.append(control )
marker.points = []
for point in points:
p = Point()
pose = self._soma_obj_pose[point]
p.x = pose.position.x - int_marker.pose.position.x
p.y = pose.position.y - int_marker.pose.position.y
marker.points.append(p)
p = Point()
pose = self._soma_obj_pose[points[0]]
p.x = pose.position.x - int_marker.pose.position.x
p.y = pose.position.y - int_marker.pose.position.y
marker.points.append(p)
return int_marker
"""Recursively converts dictionary keys to strings."""
if isinstance(dictionary, unicode):
return str(dictionary)
elif isinstance(dictionary, list):
return dictionary
return dict(
(str(k), convert_keys_to_string(v)) for k, v in dictionary.items())
if __name__ == "__main__":
global __out
__out = True
rospy.init_node("trajectory_obtainer")
rospy.loginfo("Running trajectoy/ROI query ")
gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
soma_map = 'uob_library'
soma_config = 'uob_lib_conf'
cnt = 0
for roi in gs.roi_ids(soma_map, soma_config):
cnt += 1
print 'ROI: ', gs.type_of_roi(roi, soma_map, soma_config), roi
geom = gs.geom_of_roi(roi, soma_map, soma_config)
res = gs.objs_within_roi(geom, soma_map, soma_config)
if res == None:
print "No Objects in this Region"
continue
objects_in_roi = {}
for i in res:
key = i['type'] + '_' + i['soma_id']
class RegionObservationTimeManager(object):
def __init__(self, soma_map, soma_config):
self.soma_map = soma_map
self.soma_config = soma_config
self.ms = MessageStoreProxy(collection="region_observation_time")
self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
self.roslog = pymongo.MongoClient(
rospy.get_param("mongodb_host", "localhost"),
rospy.get_param("mongodb_port", 62345)
).roslog.robot_pose
self.reinit()
def reinit(self):
"""
Reinitialising region_observation_duration to an empty dictionary
"""
self.region_observation_duration = dict()
self._month_year_observation_taken = dict()
def load_from_mongo(self, days, minute_interval=60):
"""
Load robot-region observation time from database (mongodb) and store them in
self.region_observation_duration.
Returning (region observation time, total duration of observation)
"""
roi_region_daily = dict()
total_duration = 0
self.minute_interval = minute_interval
for i in days:
end_date = i + datetime.timedelta(hours=24)
rospy.loginfo(
"Load region observation time from %s to %s..." % (str(i), str(end_date))
)
query = {
"soma": self.soma_map, "soma_config": self.soma_config,
"start_from.secs": {
"$gte": time.mktime(i.timetuple()),
"$lt": time.mktime(end_date.timetuple())
}
}
roi_reg_hourly = dict()
for log in self.ms.query(RegionObservationTime._type, message_query=query):
end = datetime.datetime.fromtimestamp(log[0].until.secs)
if log[0].region_id not in roi_reg_hourly:
temp = list()
start = datetime.datetime.fromtimestamp(log[0].start_from.secs)
interval = (end.minute + 1) - start.minute
if interval != minute_interval:
continue
for j in range(24):
group_mins = {k*interval: 0 for k in range(1, (60/interval)+1)}
temp.append(group_mins)
roi_reg_hourly.update({log[0].region_id: temp})
roi_reg_hourly[log[0].region_id][end.hour][end.minute+1] = log[0].duration.secs
roi_reg_hourly[log[0].region_id][end.hour][end.minute+1] += 0.000000001 * log[0].duration.nsecs
total_duration += log[0].duration.secs
roi_region_daily.update({i.day: roi_reg_hourly})
self.region_observation_duration = roi_region_daily
return roi_region_daily, total_duration
def store_to_mongo(self):
"""
Store region observation time from self.region_observation_duration to mongodb.
It will store soma map, soma configuration, region_id, the starting and end time where
robot sees a region in some interval, and the duration of how long the robot during
the interval.
"""
rospy.loginfo("Storing region observation time to region_observation_time collection...")
data = self.region_observation_duration
try:
minute_interval = self.minute_interval
except:
rospy.logwarn("Minute interval is not defined, please call either load_from_mongo or calculate_region_observation_duration first")
return
for day, daily_data in data.iteritems():
for reg, reg_data in daily_data.iteritems():
for hour, hourly_data in enumerate(reg_data):
for minute, duration in hourly_data.iteritems():
date_until = datetime.datetime(
self._month_year_observation_taken[day][1],
self._month_year_observation_taken[day][0],
day, hour, minute-1,
59
)
until = time.mktime(date_until.timetuple())
start_from = until - (60 * minute_interval) + 1
msg = RegionObservationTime(
self.soma_map, self.soma_config, reg,
rospy.Time(start_from), rospy.Time(until),
rospy.Duration(duration)
)
self._store_to_mongo(msg)
def _store_to_mongo(self, msg):
query = {
"soma": msg.soma, "soma_config": msg.soma_config,
"region_id": msg.region_id, "start_from.secs": msg.start_from.secs,
"until.secs": msg.until.secs
}
if msg.duration.nsecs > 0:
if len(self.ms.query(RegionObservationTime._type, message_query=query)) > 0:
self.ms.update(msg, message_query=query)
else:
self.ms.insert(msg)
def calculate_region_observation_duration(self, days, minute_interval=60):
"""
Calculating the region observation duration for particular days, splitted by
minute_interval.
Returns the ROIs the robot has monitored at each logged robot pose
for particular days specified up to minutes interval.
"""
rospy.loginfo('Calculation region observation duration...')
roi_region_daily = dict()
self.minute_interval = minute_interval
for i in days:
loaded_roi_reg_day = self.load_from_mongo([i], minute_interval)
if loaded_roi_reg_day[1] == 0:
end_date = i + datetime.timedelta(hours=24)
roi_region_hourly = self._get_robot_region_stay_duration(i, end_date, minute_interval)
roi_region_daily.update({i.day: roi_region_hourly})
else:
roi_region_daily.update({i.day: loaded_roi_reg_day[0][i.day]})
self._month_year_observation_taken.update({i.day: (i.month, i.year)})
self.region_observation_duration = roi_region_daily
return roi_region_daily
# hidden function for get_robot_region_stay_duration
def _get_robot_region_stay_duration(self, start_date, end_date, minute_interval=60):
sampling_rate = 10
roi_temp_list = dict()
rospy.loginfo("Querying from " + str(start_date) + " to " + str(end_date))
query = {
"_id": {"$exists": "true"},
"_meta.inserted_at": {"$gte": start_date, "$lt": end_date}
}
for i, pose in enumerate(self.roslog.find(query)):
if i % sampling_rate == 0:
_, _, yaw = euler_from_quaternion(
[0, 0, pose['orientation']['z'], pose['orientation']['w']]
)
coords = robot_view_cone(pose['position']['x'], pose['position']['y'], yaw)
# coords = robot_view_area(pose['position']['x'], 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]))
for j in self.gs.observed_roi(langitude_latitude, self.soma_map, self.soma_config):
region = str(j['soma_roi_id'])
hour = pose['_meta']['inserted_at'].time().hour
minute = pose['_meta']['inserted_at'].time().minute
# Region Knowledge per hour. Bin them by hour and minute_interval.
if region not in roi_temp_list:
temp = list()
for i in range(24):
group_mins = {
i*minute_interval: 0 for i in range(1, (60/minute_interval)+1)
}
temp.append(group_mins)
roi_temp_list[region] = temp
index = ((minute / minute_interval) + 1) * minute_interval
roi_temp_list[region][hour][index] += 1
roi_temp_list = self._normalizing(roi_temp_list, sampling_rate)
rospy.loginfo("Region observation duration for the query is " + str(roi_temp_list))
return roi_temp_list
def _normalizing(self, roi_temp_list, sampling_rate):
"""
Checking if all robot region relation based on its stay duration is capped
by minute_interval * 60 (total seconds). If it is not, then normalization is applied
"""
regions = roi_temp_list.keys()
_hourly_poses = [0] * 24
for i in range(24):
for region in regions:
_hourly_poses[i] += sum(roi_temp_list[region][i].values())
normalizing = sum([1 for i in _hourly_poses if i > 3601]) > 0
max_hourly_poses = max(_hourly_poses)
for reg, hourly_poses in roi_temp_list.iteritems():
if normalizing:
for ind, val in enumerate(hourly_poses):
for minute, seconds in val.iteritems():
roi_temp_list[reg][ind][minute] = 3600 / float(max_hourly_poses) * seconds
return roi_temp_list
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 SpatialModelServer(object):
def __init__(self, soma_map, soma_config, prefix='spatial_model'):
super(SpatialModelServer, self).__init__()
self.model_cloud = rospy.Publisher("/spatial_model", PointCloud2, queue_size=1, latch=True)
self.best_pose = rospy.Publisher("/spatial_model_best_pose", PoseStamped, queue_size=1, latch=True)
self.geospatial_store = GeoSpatialStoreProxy('geospatial_store', 'soma')
self.soma_map = soma_map
self.soma_config = soma_config
self.soma_roi_query = SOMAROIQuery(self.soma_map, self.soma_config)
self.soma_proxy = MessageStoreProxy(collection="soma")
self.aggregate_models_dictionary = {}
# advertise ros services
for attr in dir(self):
if attr.endswith("_ros_srv"):
service = getattr(self, attr)
rospy.Service(prefix + "/" +attr[:-8], service.type, service)
def get_target_object(self, id):
query = {'id': id, 'map': self.soma_map, 'config': self.soma_config}
return self.soma_proxy.query(SOMAObject._type, message_query=query, single=True)[0]
def get_sample_poses(self, pose, model_name):
good_glob_poses = []
bad_glob_poses = []
good_rel_poses = self.aggregate_models_dictionary[model_name].models_list[0].good_pref_rel_points
bad_rel_poses = self.aggregate_models_dictionary[model_name].models_list[0].bad_pref_rel_points
target_x, target_y = support_functions.pose_to_xy(pose)
for pose in good_rel_poses:
good_glob_poses.append(support_functions.mkpose(pose[0], pose[1]))
for pose in bad_rel_poses:
bad_glob_poses.append(support_functions.mkpose(pose[0], pose[1]))
return (good_glob_poses, bad_glob_poses)
def get_similar_room_model_name(self, name, target_id):
rois = self.geospatial_store.rois_containing_obj(target_id, self.soma_map, self.soma_config)
if len(rois) == 0:
raise rospy.ROSException('The object with id %s is not in a SoMa ROI' % target_id)
for key in self.aggregate_models_dictionary:
#FIXME also check that the target_id encoded in the key is in a room similar to the one in which the input target_id is located
if key.startswith(name):
n_feedback_points = len(self.aggregate_models_dictionary[key].models_list[0].good_pref_rel_points) + len(self.aggregate_models_dictionary[key].models_list[0].bad_pref_rel_points)
if n_feedback_points > N_FEEDBACK_THRESHOLD:
return key
return None
def get_similar_object(self, target_obj, objects_in_similar_room):
print objects_in_similar_room
for obj in objects_in_similar_room:
print obj
if obj.type == target_obj.type:
return obj
raise rospy.ROSException('Could not find a similar object of type %s in similar room'%target_obj.type)
def get_objects_in_room(self, room_id):
area_roi = self.geospatial_store.geom_of_roi(room_id, self.soma_map, self.soma_config)
obj_list = self.geospatial_store.objs_within_roi(area_roi, self.soma_map, self.soma_config)
if (obj_list == None):
raise rospy.ROSException('Could not find any object in %s SoMa ROI' %room_id)
result = []
for obj in obj_list:
result.append(self.get_target_object(obj["soma_id"]))
return result
def get_room_id_containing_object_id(self, target_id):
print "retrieving room with object id '%s'"%target_id
rois = self.geospatial_store.rois_containing_obj(target_id, self.soma_map, self.soma_config)
if len(rois) == 0:
raise rospy.ROSException('The object with id %s is not in a SoMa ROI' % target_id)
# just choose the first ROI
return rois[0]
def get_room_id_from_model_name(self, model_name):
object_id = model_name.split("_")[1]
return self.get_room_id_containing_object_id(object_id)
def get_best_pose(self, bounds, model, samples = 100):
"""
Randomly samples poses within the bounds and returns the highest scoring sample according to the model.
"""
# subscribe to the service for generating nav goals
rospy.loginfo('waiting for nav_goals')
rospy.wait_for_service('nav_goals')
rospy.loginfo('done')
nav_goals = rospy.ServiceProxy('nav_goals', NavGoals)
inflation_radius = 0.5
# generate legal poses in the bounds provided
pose_array = nav_goals(samples, inflation_radius, bounds).goals.poses
# score them with the model
points = np.array([[pose.position.x, pose.position.y] for pose in pose_array])
scores = model.score_samples(points)
idx_of_max = scores.argmax()
return pose_array[idx_of_max]
def get_feedback_ros_srv(self, req):
"""
Updates the preference model related to the spatial predicate and object id given a point and a good/bad feedback
"""
key = support_functions.predicate_to_key(req.predicate)
point_x = req.pose.pose.position.x
point_y = req.pose.pose.position.y
good = req.good
target_obj = self.get_target_object(req.predicate.arguments[0])
if target_obj is None:
raise rospy.ROSException('The object with id %s is not in the SoMa message store' % target_id)
if not (key in self.aggregate_models_dictionary):
raise rospy.ROSException('Could not find aggregate model for key %s' % key)
self.aggregate_models_dictionary[key].add_preference_point(point_x, point_y, good)
print "updated aggregate model:\n%s"%self.aggregate_models_dictionary[key]
if rospy.get_param('~visualise_model', True):
map_width = 5
model_pcloud = support_functions.model_to_pc2(self.aggregate_models_dictionary[key], target_obj.pose.position.x - map_width / 2, target_obj.pose.position.y - map_width / 2, 0.02, map_width, map_width)
self.model_cloud.publish(model_pcloud)
return True
def get_pose_ros_srv(self, req):
"""
Generates a pose that sastisfies the given spatial predicate
"""
# assume just near for now
assert req.predicate.name == 'near'
assert len(req.predicate.arguments) == 1
name = req.predicate.name
target_id = req.predicate.arguments[0]
target_room_id = self.get_room_id_containing_object_id(target_id)
# create the model centred at the object given
target_obj = self.get_target_object(target_id)
if target_obj is None:
raise rospy.ROSException('The object with id %s is not in the SoMa message store' % target_id)
model_name = support_functions.predicate_to_key(req.predicate)
similar_model_name = self.get_similar_room_model_name(name, target_id)
# if I know the model I use the previous one otherwise I instantiate a new model
# if I am visiting a new room and the previous one has at least n points I transfer, otherwise I learn starting from my default model
if (model_name in self.aggregate_models_dictionary):
# I already have a model for this
print "Model %s already known. Retrieving the old one..."%model_name
model = self.aggregate_models_dictionary[model_name]
elif (similar_model_name != None):
# I can retrieve a similar model for the new room
similar_room_id = self.get_room_id_from_model_name(similar_model_name)
objects_in_similar_room = self.get_objects_in_room(similar_room_id)
objects_in_target_room = self.get_objects_in_room(target_room_id)
similar_target_obj = self.get_similar_object(target_obj, objects_in_similar_room)
good_sample_poses, bad_sample_poses = self.get_sample_poses(similar_target_obj.pose, similar_model_name)
objects_in_target_room = support_functions.get_reordered_object_lists(objects_in_similar_room, objects_in_target_room)
print "lista 1: "
for i in objects_in_similar_room:
print i.id
print "lista 2:"
for i in objects_in_target_room:
print i.id
print "Transferring model from room %s to room %s..."%(similar_room_id, target_room_id)
default_model = transfer.build_relational_models(similar_target_obj, target_obj, bad_sample_poses, good_sample_poses, objects_in_similar_room, objects_in_target_room, {'near': support_functions.distance,'relative_angle': support_functions.unit_circle_position}, self)
model = models.AggregateModel(default_model)
self.aggregate_models_dictionary[model_name] = model
else:
# I use the default near model
print "Uknown model %s initializing a new one..."%model_name
model = models.AggregateModel(models.NearModel(target_obj.pose, 1.5))
self.aggregate_models_dictionary[model_name] = model
map_width = 10
pcloud = support_functions.model_to_pc2(model, target_obj.pose.position.x - map_width / 2, target_obj.pose.position.y - map_width / 2, 0.04, map_width, map_width)
self.model_cloud.publish(pcloud)
bounds = self.soma_roi_query.get_polygon(target_room_id)
pose = self.get_best_pose(bounds, model)
# rotate to point
pose.orientation = support_functions.quarternion_to_point(pose.position, target_obj.pose.position)
# stamp it so that we know the frame
stamped_pose = PoseStamped(pose = pose)
stamped_pose.header.stamp = rospy.get_rostime()
stamped_pose.header.frame_id = 'map'
self.best_pose.publish(stamped_pose)
return stamped_pose
get_pose_ros_srv.type=sm_srv.GetPoseForPredicate
get_feedback_ros_srv.type=sm_srv.AddPoseSampleForPredicate
class TrajectoryImporter(object):
def __init__(self, online):
self._traj = dict()
rospy.loginfo("Connecting to mongodb...")
self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
if not online:
# self._client = pymongo.MongoClient(rospy.get_param("mongodb_host"),
# rospy.get_param("mongodb_port"))
# self._store_client = MessageStoreProxy(
# collection="people_trajectory"
# )
self._traj = dict()
trajs = OfflineTrajectories()
for uuid, traj in trajs.traj.iteritems():
self._traj[uuid] = traj.get_trajectory_message()
else:
rospy.loginfo("Subscribing to /human_trajectories/trajectories...")
rospy.Subscriber(
"/human_trajectories/trajectories/complete", Trajectories,
self.traj_callback, None, queue_size=10
)
def store_all(self):
for uuid, t in self._traj.items():
# msg = t.get_trajectory_message()
# geo_json = self.geojson_from_trajectory(msg)
geo_json = self.geojson_from_trajectory(t)
# in case trajectory is already in there => replace
_uuid = geo_json['uuid']
self.gs.remove(_uuid)
rospy.loginfo("Storing %s data to geospatial_store", uuid)
self.gs.insert(geo_json)
# meta = dict()
# meta["map"] = map
# rospy.loginfo("Storing %s in geo
# self._store_client.insert(msg, meta)
del self._traj[uuid]
rospy.sleep(0.5)
def traj_callback(self, msg):
self._traj.update({
traj.uuid: traj for i, traj in enumerate(msg.trajectories)
})
def geojson_from_trajectory(self, msg):
geojson = {}
# trajectory UUID
geojson['uuid'] = msg.uuid
# space
loc = {}
loc['type'] = 'LineString'
loc['coordinates'] = []
for ps in msg.trajectory:
xy_coord = self.gs.coords_to_lnglat(
ps.pose.position.x, ps.pose.position.y
)
loc['coordinates'].append(xy_coord)
geojson['loc'] = loc
# time
geojson['start'] = msg.start_time.to_sec()
geojson['end'] = msg.end_time.to_sec()
start_dt = datetime.fromtimestamp(msg.start_time.to_sec())
geojson['start_weekday'] = start_dt.weekday()
geojson['start_hour'] = start_dt.hour
geojson['start_minute'] = start_dt.minute
end_dt = datetime.fromtimestamp(msg.end_time.to_sec())
geojson['end_weekday'] = end_dt.weekday()
geojson['end_hour'] = end_dt.hour
geojson['end_minute'] = end_dt.minute
return geojson