def OdoVelocityHandler(self,channel,data):
msg = odo_dxdtheta_t.decode(data)
self.currOdoVel = (msg.dxy, msg.dtheta, msg.dt)
self.vel_accum = (self.vel_accum[0] + self.currOdoVel[0], \
self.vel_accum[1] + self.currOdoVel[1], \
self.vel_accum[2] + self.currOdoVel[2])
self.count += 1
def OdoVelocityHandler(self,channel,data):
# IMPLEMENT ME !!!
msg = odo_dxdtheta_t.decode(data)
#print "currOdoVel", self.currOdoVel
if self.currOdoVel[3]!=self.prevOdoVel[3]:
self.currOdoVel = (msg.dxy, msg.dtheta, msg.dt, msg.utime)
self.accCurrOdoVel[0] = self.accCurrOdoVel[0]+ self.currOdoVel[0]
self.accCurrOdoVel[1] = self.accCurrOdoVel[1]+ self.currOdoVel[1]
self.accCurrOdoVel[2] = self.accCurrOdoVel[2]+ self.currOdoVel[2]
self.prevOdoVel = self.currOdoVel
def OdoVelocityHandler(self, channel, data):
self.msg_counter[0] = self.msg_counter[0] + 1
msg = odo_dxdtheta_t.decode(data)
# IMPLEMENT ME
# Handle velocity message
if self.curr_time != msg.utime:
self.curr_time = msg.utime
self.vel = (msg.dxy/(msg.dt/1e6), msg.dtheta/(msg.dt))
#print msg.dt
# print self.curr_time
#print self.prev_time_vel
self.leftVelCtrl.SetUpdateRate((self.curr_time - self.prev_time_vel)/1e6)
self.rightVelCtrl.SetUpdateRate((self.curr_time - self.prev_time_vel)/1e6)
self.prev_time_vel = self.curr_time;
def OdoVelocityHandler(self,channel,data):
msg = odo_dxdtheta_t.decode(data)
self.currOdoVel = (msg.dxy, msg.dtheta, msg.dt)
self.vel_accum += self.currOdoVel
self.count += 1
def odoVelHandler(self, channel, data):
msg = odo_dxdtheta_t.decode(data)
self.currOdoVel = (msg.dxy, msg.dtheta, msg.dt)
def velocityHandler(self,channel,data):
msg = odo_dxdtheta_t.decode(data)
self.dxy = msg.dxy
self.dtheta = msg.dtheta
self.dt = msg.dt
