def releaseBlocks(self):
        # send message to reset hal, so it won't try to complete a previous motion
        msg = StateMsg()
        msg.state = "init"
        self.statePub.publish(msg)
        
        # open hamper
        self.motionPlanner.setHamperAngle(math.pi/2)

        # drive forward .1 meters
        self.motionPlanner.translateTo(.01)

        return
    def __init__(self):

        # variables to ensure that when time is almost out, robot dumps blocks
        self.startTime = time.time() # in seconds
        self.dumpTime = 9.5*60       # with 30 seconds left, stop whatever doing and dump blocks. Time in seconds.

        # state variables 
        self.wanderCount = 0
        self.wanderCountMax = 200*30; # ten counts per second, 30 seconds before turn
        self.blockLocation = None
        self.currentPose = pose.Pose(0., 0., 0.)
        self.motionPlanner = motionplanner.MotionPlanner()
        self.bumpFlag = 0

        
        # making subscribers and publishers
        self.odometrySub = rospy.Subscriber("/sensor/currentPose", PoseMsg, self.handleOdometryMsg)
        self.bumpSub = rospy.Subscriber('/sensor/Bump', BumpMsg, self.handleBumpMsg);
        self.blockSeenSub = rospy.Subscriber('/sensor/blockSeen', PoseMsg, self.handleKinectMsg);    
        self.guiPointPub = rospy.Publisher('/gui/Point', GUIPointMsg);
        self.guiPolyPub = rospy.Publisher('/gui/Poly', GUIPolyMsg);
        self.statePub = rospy.Publisher('command/State', StateMsg);

        # initializing node
        rospy.init_node('simpleRobotBrain')
        msg = StateMsg()
        msg.state = "init"
        self.statePub.publish(msg)
        self.motionPlanner.stopWheels() # for hal

        # loading map
#        [blockLocations, self.mapList] = mapParser.parseMap('/home/rss-student/rss-2014-team-3/src/robotbrain/src/map.txt', self.currentPose)
#        self.displayMap()

        print 'start pose', self.currentPose

        return
    def __init__(self):
        
        # defining variables related to robot's state
        self.robotState = 'wandering'               # the state of the robot; can be wandering, traveling, searching, consuming
                                               #    digesting, dispensing 
        self.searchCount = 0                        # counter used to ensure that don't spend too long searching
        self.numBlocksCollected = 0                 # number of blocks that the robot has collected so far
        self.mapList = []                           # list of obstacles
        self.blockLocations = []                    # list of locations of blocks
        self.waypointsList = []                      # list of waypoints to current destination
        self.inHamperFlag = 0                       # flag used to indicate whether a block on the conveyor belt
                                               #  has made it to the hamper
        self.wanderCount = 0                        # while wandering, count up and turn
        self.currentPose = pose.Pose(0.0,0.0,0.0);
        self.previousWaypointPose = pose.Pose(0.0, 0.0, 0.0); #used for travel
   

        # constants
        # TODO: pick values well
        self.NUM_BLOCKS_NEEDED = 9                  # number of blocks needed to complete wall
        self.MAX_SEARCH_COUNT = 500                 # arbitrary value; should be tested and set
        self.POSITION_THRESHOLD = .005              # acceptable error to reaching a position. In meters.    
        self.END_LOCATION = location.Location(0.0, 0.0);#TODO might have vision pass a goal point near a wall marker
        self.MAX_WANDER_COUNT = 30;
        self.KINECT_ERROR = .01;                    # uncertainty of the kinect in m
        self.ROBOT_RADIUS = .6                      # radius of robot in m


        # making publishers/subscribers
        self.odometrySub = rospy.Subscriber("/sensor/currentPose", PoseMsg, self.handleCurrentPoseMsg)
        self.motionPub = rospy.Publisher("command/Motors", MotionMsg);
        self.bumpSub = rospy.Subscriber('bumpData', BumpMsg, self.handleBumpMsg);
        self.blockSeenSub = rospy.Subscriber('blockSeen', PoseMsg, self.handleBlockSeenMsg);
        self.obstacleAheadSub = rospy.Subscriber('obstacleAhead', ObstacleAheadMsg, self.handleObstacleAheadMsg);
        self.statePub = rospy.Publisher("command/State", StateMsg)
        
        ##Initialization broadcast
        msg = StateMsg()
        msg.source = "robotbrain"
        msg.state = "init"
        self.statePub.publish(msg)
        

        # loading and processing map
        [self.blockLocations, self.mapList] = mapParser.parseMap('/home/rss-student/rss-2014-team-3/src/robotbrain/src/map.txt', self.currentPose);
        
        # TODO for debugging in wander
        self.blockLocations = []

        # objects
        self.motionPlanner =motionplanner.MotionPlanner();
        self.pathPlanner = pathplanner.PathPlanner(self.mapList, self.ROBOT_RADIUS);   
        #TODO: mapUpdater

        # ROSPY methods
        rospy.init_node('robotbrain')
        rospy.on_shutdown(self.onShutdown)

        # sending initial wheel 0 message to make wheel controller happy
        # (it freaks out if it gets a positive message after a long time step)
        self.motionPlanner.stopWheels()

        return
    def restartHAL(self):
        msg = StateMsg()
        msg.state = "init"
        self.statePub.publish(msg)

        return