def GetState(self, usecached=False):
rv = SrvJointStateResponse()
if self.initialized:
if not usecached:
self.posCache = self.ss.get_pos()
#rv.header.stamp=rospy.Time.now()
rv.header.frame_id=self.name
rv.position = self._UnitsFromCount(self.posCache)
rv.velocity = self._UnitsFromCount(self.ss.get_vel())
else:
rv.header.frame_id=self.name
rv.position = 0.0
rv.velocity = 0.0
return (rv.header, rv.position, rv.velocity)
def SetVelocity_callback(self, req):
# rospy.logwarn ('(%s) setVelocity2 pos=%s, vel=%s' % (self.name, req.position, req.velocity))
rv = SrvJointStateResponse()
if self.initialized:
# if self.velLast * req.velocity <= 0.0:
if (req.velocity>0):
self.direction = POSITIVE
elif (req.velocity<0):
self.direction = NEGATIVE
else: # (req.velocity==0):
self.direction = 1-self.direction
vel = self._CountFromUnits(N.abs(req.velocity))
#rospy.logwarn('(%s) ss.set_vel_and_dir(vel=%d, dir=%d)' % (self.name, vel, self.direction))
self.ss.set_vel_and_dir(vel, self.direction)
#self.ss.set_dir_setpt (self.direction)
#self.ss.set_vel_setpt(self._CountFromUnits(N.abs(req.velocity)))
self.velLast = req.velocity
rv.header = req.header
rv.position = req.position
rv.velocity = req.velocity
else:
rv.header = None
rv.position = None
rv.velocity = None
return (rv.header, rv.position, rv.velocity)
