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)