示例#1
0
    def SignalInput_callback (self, srvSignalReq):
        rv = SrvSignalResponse()

        if (self.angposMech is not None):
            Jinv = self.JacobianInv(self.angposMech)
            
            (self.stateRef, isInArena) = self.TransformStateToFrame('Stage', srvSignalReq.state)
            self.angposRef = self.Get1FromPt(self.stateRef.pose.position)
            self.angvelRef = Jinv.dot(np.array([[self.stateRef.velocity.linear.x],[self.stateRef.velocity.linear.y]]))

        rv.success = True #isInArena
        
        return rv