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 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 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 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 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 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
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 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