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")
class RobotHardwareROS(RobotHardware): 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") ###################### # Subscriber Methods # ###################### def handleMsg_StateMsg(self,msg): #if msg.source == "robotbrain": if msg.state == "init": #self.wController = WheelController() sensordict=self.read_wheels() self.wController.reset(sensordict['left_position'],sensordict['right_position'],time.time()) def handleMsg_MotionVoltMsg(self,msg): self.command_actuators({"left_wheel":msg.leftVoltage,"right_wheel":msg.rightVoltage}) def handleMsg_MotionDistMsg(self,msg): print 'recieved msg' td = msg.translationalDist rd = msg.rotationalDist print td, rd self.wController.position(td,rd) def handleMsg_MotionMsg(self,msg): tv = msg.translationalVelocity rv = msg.rotationalVelocity # Compute dt t=time.time() if self.lastVelocityMsgTime is None: self.lastVelocityMsgTime=t dt=t-self.lastVelocityMsgTime self.lastVelocityMsgTime=t self.wController.velocity(tv,rv,dt) def handleMsg_HamperMsg(self,msg): fo = msg.fractionOpen self.command_actuators({'hopper':fo}) def handleMsg_ConveyorMsg(self,msg): frontfo = msg.frontConveyorFractionOn backfo = msg.backConveyorFractionOn self.command_actuators({'ramp_conveyer':frontfo,'back_conveyer':backfo}) ################ # Step Methods # ################ def step(self): sensordict = self.read_wheels() #print sensordict ##Encoder## msg = EncoderMsg() msg.lWheelDist = sensordict['left_position'] msg.rWheelDist = sensordict['right_position'] self.encoderPub.publish(msg) ##Ping Sensor# # TODO: get bump sensors, sonars working # send out wheel error [leftWheelErr, rightWheelErr]=self.wController.get_error(sensordict['left_position'], sensordict['right_position']) msg = WheelErrorMsg() msg.leftWheelError = leftWheelErr msg.rightWheelError = rightWheelErr self.wheelErrPub.publish(msg) ##Control## t= time.time() motors = self.wController.step(sensordict['left_position'],sensordict['right_position'],t) self.command_actuators({'left_wheel':motors[0], 'right_wheel':motors[1]}) #################### # Sensor Retrieval # #################### def get_Encoder(self): sensordict = self.read_wheels() msg = EncoderMsg() msg.lWheelDist = sensordict['left_position'] msg.rWheelDist = sensordict['right_position'] self.encoderPub.publish(msg) def get_Sonar(self): ##Straight Sonar Assumption dx = 0.25 #m dy = 0.25 #m sensordict = self.read_sonars() msg = SonarStatusMsg() msg.sonarStatus = [0.0,0.0,0.0,0.0] for i in range(4): if sensordict[self.sonarId[i]] is not None: msg.sonarStatus[i] = sensordict[self.sonarId[i]] self.sonarStatusPub.publish(msg) ##FrontLeft read_dist = sensordict[self.sonarId[0]] if read_dist != None: msg = PoseMsg() msg.xPosition = -dx - read_dist msg.yPosition = dy self.sonarPub.publish(msg) ##FrontRight read_dist = sensordict[self.sonarId[1]] if read_dist != None: msg = PoseMsg() msg.xPosition = dx + read_dist msg.yPosition = dy self.sonarPub.publish(msg) ##BackLeft read_dist = sensordict[self.sonarId[2]] if read_dist != None: msg = PoseMsg() msg.xPosition = -dx - read_dist msg.yPosition = -dy self.sonarPub.publish(msg) ##BackRight read_dist = sensordict[self.sonarId[3]] if read_dist != None: msg = PoseMsg() msg.xPosition = dx + read_dist msg.yPosition = -dy self.sonarPub.publish(msg) #msg = SonarMsg? def get_Bump(self): sensordict = self.read_touches() for i in range(len(self.bumpId)): if self.bumpId[i] in sensordict: #if sensordict[self.bumpId[i]]: # print "bump", i if (not self.bumpState[i]) and sensordict[self.bumpId[i]]: #Bump is triggered msg = BumpMsg() msg.bumpNumber = i self.bumpPub.publish(msg) self.bumpState[i] = sensordict[self.bumpId[i]] msg = BumpStatusMsg() msg.bumpStatus = self.bumpState self.bumpStatusPub.publish(msg)