示例#1
0
文件: soma.py 项目: seaun163/soma
    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()
示例#3
0
 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')
示例#4
0
    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
示例#7
0
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()
示例#9
0
文件: soma_roi.py 项目: abyssxsy/soma
    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()
示例#10
0
    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()
示例#13
0
    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)
示例#15
0
    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()
示例#16
0
    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
示例#19
0
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
示例#20
0
文件: soma.py 项目: seaun163/soma
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
示例#21
0
文件: soma_roi.py 项目: seaun163/soma
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
示例#22
0
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
示例#23
0
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
示例#24
0
            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
示例#25
0
        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
示例#27
0
文件: soma_roi.py 项目: abyssxsy/soma
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
示例#28
0
    """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
示例#32
0
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