def __init__(self):
RobotHardware.__init__(self)
#Subscribers
self.motionSub = rospy.Subscriber("command/Motors",MotionMsg,self.handleMsg_MotionMsg)
self.motionDistSub = rospy.Subscriber("command/MotorsDist", MotionDistMsg, self.handleMsg_MotionDistMsg)
self.motionVoltSub = rospy.Subscriber("command/MotorVolt", MotionVoltMsg, self.handleMsg_MotionVoltMsg)
self.conveyorSub = rospy.Subscriber("command/Conveyor",ConveyorMsg,self.handleMsg_ConveyorMsg)
self.hamperSub = rospy.Subscriber("command/Hamper",HamperMsg,self.handleMsg_HamperMsg)
self.stateSub = rospy.Subscriber("command/State", StateMsg, self.handleMsg_StateMsg)
#Publishers
self.encoderPub = rospy.Publisher("sensor/Encoder",EncoderMsg)
self.bumpPub = rospy.Publisher("sensor/Bump",BumpMsg)
self.sonarPub = rospy.Publisher("sensor/Sonar", PoseMsg)
self.sonarStatusPub = rospy.Publisher("sensor/SonarStatus",SonarStatusMsg)
self.bumpStatusPub = rospy.Publisher("sensor/BumpStatus",BumpStatusMsg)
self.wheelErrPub = rospy.Publisher("sensor/WheelErr", WheelErrorMsg)
self.sonarId = ["front_left_sonar","front_right_sonar","back_left_sonar","back_right_sonar"]
self.bumpId = ["block_front_touch","block_back_touch","front_left_touch","front_right_touch"]
self.bumpState = [False,False,False,False]
#Wheel Controller
self.wController = WheelController()
sensordict=self.read_wheels()
self.wController.reset(sensordict['left_position'],sensordict['right_position'],time.time())
#self.wController.velocity(0,0,time.time());
self.lastVelocityMsgTime=None
rospy.init_node("HalRos")
#!/usr/bin/python
from hal import RobotHardware
import time
r=RobotHardware()
state='init'
def bump():
ts=r.read_touches()
return (ts['front_left_touch'],ts['front_right_touch'])
try:
while True:
print state
time.sleep(0.01)
if state=='init':
r.command_actuators({'ramp_conveyer':1,'back_conveyer':1,'hopper':0,'left_wheel':0,'right_wheel':0})
state='wander'
bumps=0
gametimer=time.time()+300
elif state=='wander':
r.command_actuators({'left_wheel':.5,'right_wheel':.55})
b=bump()
if b==(1,0):
timer=time.time()+3
state='hit_left'
elif b==(0,1):
timer=time.time()+3
state='hit_right'
