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
