Go to the documentation of this file.00001 import re
00002 from threading import Thread
00003 import rospy
00004 import rostopic
00005 from BAL.Handlers.keepAliveHandler import KeepAliveHandler
00006 from BAL.Header.Requests.PublishRequest import PublishRequest
00007 from BAL.Header.Requests.SetParamRequest import SetParamRequest
00008 from BAL.Header.Requests.closeMotorRequest import CloseMotorRequest
00009 from BAL.Header.Response.ParamBuildResponse import EngineCL
00010
00011 __author__ = 'tom1231'
00012 from rospy import Publisher, Subscriber
00013 from std_msgs.msg import Float32
00014 from ric_board.msg import Motor
00015 from BAL.Interfaces.Device import Device
00016
00017
00018
00019 class RiCCloseLoopMotor(Device):
00020
00021 def __init__(self, motorNum ,param,output):
00022 Device.__init__(self, param.getCloseLoopMotorName(motorNum), output)
00023 self._motorId = motorNum
00024
00025 self._kp = param.getCloseLoopMotorKp(motorNum)
00026 self._ki = param.getCloseLoopMotorKi(motorNum)
00027 self._kd = param.getCloseLoopMotorKd(motorNum)
00028
00029 self._pub = Publisher('%s/feedback' % self._name, Motor, queue_size=param.getCloseLoopMotorPubHz(motorNum))
00030 Subscriber('%s/command' % self._name, Float32, self.MotorCallback, queue_size=1)
00031 self._haveRightToPublish = False
00032
00033
00034 def publish(self, data):
00035 msg = Motor()
00036 msg.position = data[0]
00037 msg.velocity = data[1]
00038 self._pub.publish(msg)
00039
00040 def MotorCallback(self, msg):
00041 Thread(target=self.sendMsg, args=(msg,)).start()
00042
00043 def getKp(self): return self._kp
00044
00045 def getKi(self): return self._ki
00046
00047 def getKd(self): return self._kd
00048
00049 def getMotorId(self): return self._motorId
00050
00051 def getType(self): return EngineCL
00052
00053 def getName(self): return self._name
00054
00055 def sendSetParam(self, values):
00056 kp, ki, kd = values
00057
00058 if self._kp != kp:
00059 self._kp = kp
00060 self._output.write(SetParamRequest(self.getMotorId(), self.getType(), 1, kp).dataTosend())
00061
00062 if self._ki != ki:
00063 self._ki = ki
00064 self._output.write(SetParamRequest(self.getMotorId(), self.getType(), 2, ki).dataTosend())
00065
00066 if self._kd != kd:
00067 self._kd = kd
00068 self._output.write(SetParamRequest(self.getMotorId(), self.getType(), 3, kd).dataTosend())
00069
00070 def sendMsg(self, msg):
00071 req = CloseMotorRequest(self._motorId, msg.data)
00072 self._output.write(req.dataTosend())
00073
00074 def checkForSubscribers(self):
00075 try:
00076 subCheck = re.search('Subscribers:.*', rostopic.get_info_text(self._pub.name)).group(0).split(': ')[1]
00077
00078 if not self._haveRightToPublish and subCheck == '':
00079 self._output.write(PublishRequest(EngineCL, self._motorId, True).dataTosend())
00080 self._haveRightToPublish = True
00081
00082 elif self._haveRightToPublish and subCheck == 'None':
00083 self._output.write(PublishRequest(EngineCL, 0, False).dataTosend())
00084 self._haveRightToPublish = False
00085 except: pass