コード例 #1
0
    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()
コード例 #2
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')
コード例 #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, name, soma_map, soma_config, minute_interval, window_interval, collection="occurrence_rates"):
        self.soma_map = soma_map
        self.soma_config = soma_config
        self.minute_interval = minute_interval
        self.window_interval = window_interval
        self.rotm = RegionObservationTimeManager(soma_map, soma_config)
        self.tre = TrajectoryRegionEstimate(soma_map, soma_config, minute_interval)
        self.tof = TrajectoryOccurrenceFrequencies(
            soma_map, soma_config, minute_interval=minute_interval, window_interval=window_interval
        )

        rospy.loginfo("Connect to database collection %s..." % collection)
        self._ms = MessageStoreProxy(collection=collection)
        rospy.loginfo("Create a service %s/service..." % name)
        self.service = rospy.Service(name + "/service", TrajectoryOccurrenceRate, self.srv_cb)
        rospy.loginfo("Create an action server %s..." % name)
        self._as = actionlib.SimpleActionServer(
            name, OccurrenceRateLearningAction, execute_cb=self.execute, auto_start=False
        )
        self._as.start()
コード例 #5
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
コード例 #6
0
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])
コード例 #7
0
class TrajectoryOccurrenceRateManager(object):
    def __init__(self, name, soma_map, soma_config, minute_interval, window_interval, collection="occurrence_rates"):
        self.soma_map = soma_map
        self.soma_config = soma_config
        self.minute_interval = minute_interval
        self.window_interval = window_interval
        self.rotm = RegionObservationTimeManager(soma_map, soma_config)
        self.tre = TrajectoryRegionEstimate(soma_map, soma_config, minute_interval)
        self.tof = TrajectoryOccurrenceFrequencies(
            soma_map, soma_config, minute_interval=minute_interval, window_interval=window_interval
        )

        rospy.loginfo("Connect to database collection %s..." % collection)
        self._ms = MessageStoreProxy(collection=collection)
        rospy.loginfo("Create a service %s/service..." % name)
        self.service = rospy.Service(name + "/service", TrajectoryOccurrenceRate, self.srv_cb)
        rospy.loginfo("Create an action server %s..." % name)
        self._as = actionlib.SimpleActionServer(
            name, OccurrenceRateLearningAction, execute_cb=self.execute, auto_start=False
        )
        self._as.start()

    def execute(self, goal):
        temp_start_time = rospy.Time.now()
        curr_date = datetime.datetime.fromtimestamp(rospy.Time.now().secs)
        # curr_date = datetime.datetime(2016, 3, 10, 0, 50)
        if curr_date.hour >= 0 and curr_date.hour < 8:
            curr_date = curr_date - datetime.timedelta(hours=24)
        curr_date = datetime.datetime(curr_date.year, curr_date.month, curr_date.day, 0, 0)
        prev_date = curr_date - datetime.timedelta(hours=24)
        self.rotm.calculate_region_observation_duration([prev_date, curr_date], self.minute_interval)
        self.rotm.store_to_mongo()
        if self._as.is_preempt_requested():
            self._as.set_preempted()
            return

        curr_traj_est = self.tre.estimate_trajectories_daily([curr_date.day], curr_date.month, curr_date.year)
        curr_traj_est = trajectory_estimate_for_date(curr_traj_est, curr_date)
        prev_traj_est = self.tre.estimate_trajectories_daily([prev_date.day], prev_date.month, prev_date.year)
        prev_traj_est = trajectory_estimate_for_date(prev_traj_est, prev_date)
        if self._as.is_preempt_requested():
            self._as.set_preempted()
            return

        self.tof.update_tof_daily(curr_traj_est, prev_traj_est, curr_date)
        self.tof.store_tof()
        self.rotm.reinit()
        self.tof.reinit()
        temp_end_time = rospy.Time.now()
        rospy.loginfo("Time needed to complete this is %d" % (temp_end_time - temp_start_time).secs)
        self._as.set_succeeded(OccurrenceRateLearningResult())

    def srv_cb(self, msg):
        rospy.loginfo("Retrieving trajectory occurrence frequencies from database...")
        print msg
        query = {
            "soma": self.soma_map,
            "soma_config": self.soma_config,
            "region_id": msg.region_id,
            "day": msg.day,
            "end_hour": msg.hour,
            "end_minute": msg.minute,
        }
        logs = self._ms.query(OccurrenceRate._type, message_query=query)
        occurrence_rate = 0.0
        counter = 0
        for log in logs:
            occurrence_rate += log[0].occurrence_rate
            counter += 1
        if counter == 0:
            counter += 1
        if len(logs) > 0:
            rospy.loginfo("Occurrence Rate is obtained, which has value %.2f" % (occurrence_rate / float(counter)))
        else:
            rospy.loginfo("No occurrence rate is obtained, returning zero value")
        return TrajectoryOccurrenceRateResponse(occurrence_rate / float(counter))
コード例 #8
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