nao_walker_v2.py 文件源码

def handleCmdVel(self, data):
        rospy.logdebug("Walk cmd_vel: %f %f %f, frequency %f", data.linear.x, data.linear.y, data.angular.z, self.stepFrequency)
        if data.linear.x != 0 or data.linear.y != 0 or data.angular.z != 0:
            self.gotoStartWalkPose()
        try:
            eps = 1e-3 # maybe 0,0,0 is a special command in motionProxy...
            if abs(data.linear.x)<eps and abs(data.linear.y)<eps and abs(data.angular.z)<eps:
                self.motionProxy.moveToward(0,0,0,[["Frequency",0.5]])
            else:
                if data.linear.x>=eps and abs(data.linear.y)<eps and abs(data.angular.z)<eps:
                    self.motionProxy.angleInterpolationWithSpeed("HeadPitch", self.headPitchAngle, 0.5)
                    self.motionProxy.moveToward(data.linear.x, data.linear.y, data.angular.z, [["Frequency", self.stepFrequency]] + self.footGaitConfig)
                    rospy.loginfo('case forward')
                elif data.linear.x<=-eps and abs(data.linear.y)<eps and abs(data.angular.z)<eps:
                    self.motionProxy.angleInterpolationWithSpeed("HeadPitch", self.headPitchAngleBack, 0.5)
                    self.motionProxy.moveToward(data.linear.x, data.linear.y, data.angular.z, [["Frequency", self.stepFrequencyBack]] + self.footGaitConfigBack)
                    rospy.loginfo('case backward')
                elif abs(data.linear.x)<eps and abs(data.linear.y)<eps and data.angular.z>=-eps:
                    self.motionProxy.angleInterpolationWithSpeed("HeadPitch", self.headPitchAngleLeft, 0.5)
                    self.motionProxy.moveToward(data.linear.x, data.linear.y, data.angular.z, [["Frequency", self.stepFrequencyLeft]] + self.footGaitConfigLeft)
                    rospy.loginfo('case left')
                elif abs(data.linear.x)<eps and abs(data.linear.y)<eps and data.angular.z<=eps:
                    self.motionProxy.angleInterpolationWithSpeed("HeadPitch", self.headPitchAngleRight, 0.5)
                    self.motionProxy.moveToward(data.linear.x, data.linear.y, data.angular.z, [["Frequency", self.stepFrequencyRight]] + self.footGaitConfigRight)
                    rospy.loginfo('case right')
                else:
                    self.motionProxy.angleInterpolationWithSpeed("HeadPitch", self.headPitchAngle, 0.5)
                    self.motionProxy.moveToward(data.linear.x, data.linear.y, data.angular.z, [["Frequency", self.stepFrequency]] + self.footGaitConfig)
                    rospy.loginfo('case else')
        except RuntimeError,e:
            # We have to assume there's no NaoQI running anymore => exit!
            rospy.logerr("Exception caught in handleCmdVel:\n%s", e)
            rospy.signal_shutdown("No NaoQI available anymore")