← Back to branch summary

~mrol-dev/mrol/trunk

« back to all changes in this revision

Viewing changes to mrol_mapping/util.py

  • Committer: Julian Ryde
  • Date: 2012-03-04 17:10:07 UTC
  • mto: (16.1.12 trunk) (29.1.2 trunk)
  • mto: This revision was merged to the branch mainline in revision 25.
  • Revision ID: julian_ryde-20120304171007-hfbttvxk2nhmqoad
Changed some functions to take pose objects rather than pose tuples.  Tidied up cost_min so it considers quantised pose steps.

Show diffs side-by-side

added added

removed removed

Lines of Context:
mrol_mapping/util.py
52
52
    # it part of the public api of occupiedlist?
53
53
    occupiedlist._pack(inds)
54
54
    # This introduces a systematic error as inds_unique is just the first index encountered.
55
 
    ids_unique, inds_unique = np.unique(inds, return_index=True)
 
55
    ids_unique, inds_unique = np.unique1d(inds, return_index=True)
56
56
    if return_inds:
57
57
        return P[inds_unique], inds_unique
58
58
    else:
132
132
    newPose = poseutil.Pose3D()
133
133
    newPose.setMatrix(Ppose)
134
134
    return newPose.getTuple()
135
 
 
136
 
import Queue as _Queue
137
 
class Queue(_Queue.Queue):
138
 
    def set_timeout(self, func):
139
 
        self._timeout_func = func
140
 
 
141
 
    def next(self, **kwargs):
142
 
        return self.get(timeout=self._timeout_func(), **kwargs)
143
 
 
144
 
    def put_force(self, item, **kwargs):
145
 
        """Force put an item to Queue. If the Queue is full, pop out one item
146
 
        and push this one."""
147
 
        if ("block" in kwargs) or ("timeout" in kwargs):
148
 
            raise ValueError("block or timeout do not make sense with put"
149
 
                             + "force")
150
 
 
151
 
        self.not_full.acquire()
152
 
        try:
153
 
            if self.maxsize > 0:
154
 
                if self._qsize() == self.maxsize:
155
 
                    #print("Queue full. flushing") 
156
 
                    _ = self._get()
157
 
            self._put(item)
158
 
            self.unfinished_tasks += 1
159
 
            self.not_empty.notify()
160
 
        finally:
161
 
            self.not_full.release()