Skip to content
Permalink
Browse files

followme - step1 go straight to an obstacle and wait until is removed

  • Loading branch information
martind
martind committed Feb 11, 2015
1 parent a8a1b7e commit 8190af39aa125ce51604d4806741b3318baba125
Showing with 39 additions and 4 deletions.
  1. +39 −4 ros/followme.py
@@ -11,7 +11,8 @@

sys.path.append( ".."+os.sep+"openni2")
from getpic import Sensor3D

import numpy as np
import datetime

class ScannerThread( Thread ):
def __init__( self ):
@@ -20,11 +21,22 @@ def __init__( self ):
self.shouldIRun = Event()
self.shouldIRun.set()
self.sensor = Sensor3D()
self.minDist = None # i.e. unknown

def run( self ):
while self.shouldIRun.isSet():
self.sensor.save( "depth", self.sensor.readDepth() )
self.sensor.save( "pic", self.sensor.readColor() )
arr = self.sensor.readDepth()
depth = np.array( arr, dtype=np.uint16 )
depth.shape = (240, 320, 1)
centerArea = depth[120-40:120+40, 160-40:160+40]
mask = centerArea > 0
if centerArea.max() == 0:
self.minDist = 10.0
else:
self.minDist = centerArea[mask].min()/1000.0
print "Min dist", self.minDist
self.sensor.save( "logs/depth", arr )
self.sensor.save( "logs/pic", self.sensor.readColor() )

def requestStop(self):
self.shouldIRun.clear()
@@ -37,7 +49,30 @@ def followme( metalog, assertWrite, ipPair ):
if metalog is None:
scanner = ScannerThread()
scanner.start()
robot.goStraight( 1.0 )
maxSpeed = 0.1
safeDist = 1.0
index = 0
prev = None
log = open( datetime.datetime.now().strftime("logs/scanner_%y%m%d_%H%M%S.log"), "w" )
while True:
if scanner.minDist != prev:
prev = scanner.minDist
log.write("%d\n" % index )
log.write("%.3f\n" % prev )
log.flush()
index = 0
index += 1

if prev is None or prev < safeDist:
robot.setSpeedPxPa( 0, 0 )
else:
robot.setSpeedPxPa( maxSpeed, 0 )
robot.update()
robot.setSpeedPxPa( 0, 0 )
robot.update()
log.write("%d\n" % index )
log.close()

if metalog is None:
scanner.requestStop()
scanner.join()

0 comments on commit 8190af3

Please sign in to comment.
You can’t perform that action at this time.