def __init__(self,
dims = (0.0,0.0,2*math.pi),
publisher = False,
maxSpeed = 0.15,
maxTurn = 0.3,
dampSpeed = 1.0,
dampTurn = 1.0,
kSpeed = 1.0,
kTurn = 1.0,
stdThres=100.0,
zeroDistance=0.3,
targetFn=False,
signalFn=False,
debug=False, dbgdir="~/"):
self.maxSpeed = maxSpeed
self.maxTurn = maxTurn
self.dampSpeed = dampSpeed
self.dampTurn = dampTurn
self.kSpeed = kSpeed
self.kTurn = kTurn
self.dims = dims
self.pub = publisher
self.targetFn = targetFn
self.signalFn = signalFn
self.debug = debug
self.dbgdir = dbgdir
self.count = 0
# These are for tracking the position
self.debugX = 0.0
self.debugY = 0.0
self.debugTh = 0.0
if self.debug:
rospy.Subscriber("position", Position, self.debug_track)
self.fl = force.ForceList()
self.gs = goal.GoalStack()
self.zeroDistance = zeroDistance
self.position = [ 0.0 ] * len(self.dims)
self.realPosition = [ 0.0 ] * len(self.dims)
self.stds = [ 0.0 ] * len(self.dims)
self.curstds = 1000.0
# The threshold is a number beyond which we can't reliably
# seek a goal, so the behavior has to change from seeking a
# goal to determining the orientation.
self.stdThres = stdThres
# We maintain the following so we can do derivative-proportional
# damping of the motion. (It's a PD controller.)
self.twist = Twist()
# Use this to signal that the doMove should stop
# working for a moment. (i.e. we have some motion emergency
# to deal with, like a bump)
self.isbusy = False
# These two time stamps are used to control the bump behavior.
self.bumpTimeOne = 0.0
self.bumpTimeTwo = 0.0
# Use these to remember where the current bump was.
self.bumpLeft = False
self.bumpRight = False
# A flag to indicate whether we know where we are to within the
# stdThres. We use the flag to do stuff that has to happen *when*
# we get lost, as opposed to stuff that has to happen *while* we
# are lost, which is indicated with self.lost().
self.amLost = False
# This histogram is used to turn a force into a speed and
# direction. We try to keep the same histogram from one
# invocation of set_move to the next in order to minimize the
# adjustments to the speed and direction of the robot.
self.guesser = Guesser("forces", (0.0, 2*math.pi), "force", (3,3))
self.guesser.makeHist()
self.guesser.hist.all()
mapArray[xi,yi] = 100
if yi < 3 or yi > 15:
mapArray[xi,yi] = 100
if yi < 10 and xi > 10:
mapArray[xi,yi] = 75
mapArray.shape = 576
og.data = list(mapArray)
mapData = MapData(og)
bg.addMap(mapData)
import tgraph
tg = tgraph.Tgraph(350, 350)
g = Guesser("map_filter_test", bg.nPoints, (0.0, 0.0, 2*math.pi),
"location", HistogramData((10,10,1)))
g.uniform([[-5.0,5.0],[-5.0,5.0],[0.0,3.0]])
bg.handle_guess(g.outPoints())
plist = []
for pt in bg.mapPointList:
plist.append(pt.point)
tg.draw_scatter(np.array([[-5.0, -5.0, 0.0], [5.0, 5.0, 2.0]]), 0,1,2,"s")
tg.plot_points(np.array(plist), 0,1,2, "s")
tg.mainloop()
mapArray[xi, yi] = 1.0
mapData = MapData(mapArray, (-5.0, -5.0), (0.5, 0.5))
mg.addMap(mapData)
import tgraph
tg = tgraph.Tgraph(350, 200)
inarray = np.array([[-5.5, -5.5, 0.0], [5.5, 5.5, 0.6]])
tg.draw_scatter(inarray, 0, 1, 2, "s")
parray = np.array([p.point for p in mg.mapPointList])
tg.plot_points(parray, 0, 1, 2, "s")
g = Guesser("map_filter_test", (0.0, 0.0, 2 * math.pi), "location",
(10, 10, 1))
g.uniform(mg.nPoints, [[-5.0, 5.0], [-5.0, 5.0], [0.0, 3.0]])
tg.new_graph()
tg.draw_scatter(g.pointArray, 0, 1, 2, "s", recalc=False)
gr = GuessRequest(inPoints=g.outPoints(),
means=g.means(),
stds=g.stds(),
data_type=g.data_type,
pers=g.periods)
gresp = mg.handle_guess(gr)
tg.plot_points(mg.guesser.hist.plottable(), 0, 1, 2, "c")
