00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 import rospy
00037
00038 from naoqi_driver.naoqi_node import NaoqiNode
00039
00040 from std_msgs.msg import String
00041 from geometry_msgs.msg import Twist
00042 from geometry_msgs.msg import Pose2D
00043
00044 from std_srvs.srv import Empty, EmptyResponse
00045 from naoqi_bridge_msgs.srv import CmdPoseService, CmdVelService, CmdPoseServiceResponse, CmdVelServiceResponse, SetArmsEnabled, SetArmsEnabledResponse
00046 from humanoid_nav_msgs.msg import StepTarget
00047 from humanoid_nav_msgs.srv import StepTargetService, StepTargetServiceResponse
00048
00049 from nao_apps import startWalkPose
00050
00051 class NaoWalker(NaoqiNode):
00052 def __init__(self):
00053 NaoqiNode.__init__(self, 'nao_walker')
00054
00055 self.connectNaoQi()
00056
00057
00058 self.stepFrequency = rospy.get_param('~step_frequency', 0.5)
00059
00060 self.useStartWalkPose = rospy.get_param('~use_walk_pose', False)
00061 self.needsStartWalkPose = True
00062
00063
00064 self.maxHeadSpeed = rospy.get_param('~max_head_speed', 0.2)
00065
00066
00067 initStiffness = rospy.get_param('~init_stiffness', 0.0)
00068
00069 self.useFootGaitConfig = rospy.get_param('~use_foot_gait_config', False)
00070 rospy.loginfo("useFootGaitConfig = %d" % self.useFootGaitConfig)
00071 if self.useFootGaitConfig:
00072 self.footGaitConfig = rospy.get_param('~foot_gait_config', self.motionProxy.getFootGaitConfig("Default"))
00073 else:
00074 self.footGaitConfig = self.motionProxy.getFootGaitConfig("Default")
00075
00076
00077 if initStiffness > 0.0 and initStiffness <= 1.0:
00078 self.motionProxy.stiffnessInterpolation('Body', initStiffness, 0.5)
00079
00080 try:
00081 enableFootContactProtection = rospy.get_param('~enable_foot_contact_protection')
00082 self.motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", enableFootContactProtection]])
00083 if enableFootContactProtection:
00084 rospy.loginfo("Enabled foot contact protection")
00085 else:
00086 rospy.loginfo("Disabled foot contact protection")
00087 except KeyError:
00088
00089 pass
00090
00091
00092 rospy.Subscriber("cmd_vel", Twist, self.handleCmdVel, queue_size=1)
00093 rospy.Subscriber("cmd_pose", Pose2D, self.handleTargetPose, queue_size=1)
00094 rospy.Subscriber("cmd_step", StepTarget, self.handleStep, queue_size=50)
00095
00096
00097 self.pub = rospy.Publisher("speech", String, latch = True)
00098
00099
00100 self.cmdPoseSrv = rospy.Service("cmd_pose_srv", CmdPoseService, self.handleTargetPoseService)
00101 self.cmdVelSrv = rospy.Service("cmd_vel_srv", CmdVelService, self.handleCmdVelService)
00102 self.stepToSrv = rospy.Service("cmd_step_srv", StepTargetService, self.handleStepSrv)
00103 self.stopWalkSrv = rospy.Service("stop_walk_srv", Empty, self.handleStopWalkSrv)
00104 self.needsStartWalkPoseSrv = rospy.Service("needs_start_walk_pose_srv", Empty, self.handleNeedsStartWalkPoseSrv)
00105 self.readFootGaitConfigSrv = rospy.Service("read_foot_gait_config_srv", Empty, self.handleReadFootGaitConfigSrv)
00106 self.setArmsEnabledSrv = rospy.Service("enable_arms_walking_srv", SetArmsEnabled, self.handleSetArmsEnabledSrv)
00107
00108 self.say("Walker online")
00109
00110 rospy.loginfo("nao_walker initialized")
00111
00112 def connectNaoQi(self):
00113 '''(re-) connect to NaoQI'''
00114 rospy.loginfo("Connecting to NaoQi at %s:%d", self.pip, self.pport)
00115
00116 self.motionProxy = self.get_proxy("ALMotion")
00117 if self.motionProxy is None:
00118 exit(1)
00119
00120 def stopWalk(self):
00121 """ Stops the current walking bahavior and blocks until the clearing is complete. """
00122 try:
00123 self.motionProxy.setWalkTargetVelocity(0.0, 0.0, 0.0, self.stepFrequency)
00124 self.motionProxy.waitUntilWalkIsFinished()
00125
00126
00127 except RuntimeError,e:
00128 print "An error has been caught"
00129 print e
00130 return False
00131
00132 return True
00133
00134
00135 def say(self, text):
00136 self.pub.publish(text)
00137
00138 def handleCmdVel(self, data):
00139 rospy.logdebug("Walk cmd_vel: %f %f %f, frequency %f", data.linear.x, data.linear.y, data.angular.z, self.stepFrequency)
00140 if data.linear.x != 0 or data.linear.y != 0 or data.angular.z != 0:
00141 self.gotoStartWalkPose()
00142 try:
00143 eps = 1e-3
00144 if abs(data.linear.x)<eps and abs(data.linear.y)<eps and abs(data.angular.z)<eps:
00145 self.motionProxy.setWalkTargetVelocity(0,0,0,0.5)
00146 else:
00147 self.motionProxy.setWalkTargetVelocity(data.linear.x, data.linear.y, data.angular.z, self.stepFrequency, self.footGaitConfig)
00148 except RuntimeError,e:
00149
00150 rospy.logerr("Exception caught in handleCmdVel:\n%s", e)
00151 rospy.signal_shutdown("No NaoQI available anymore")
00152
00153
00154
00155 def handleCmdVelService(self, req):
00156 self.handleCmdVel(req.twist)
00157 return CmdVelServiceResponse()
00158
00159 def handleTargetPose(self, data, post=True):
00160 """handles cmd_pose requests, walks to (x,y,theta) in robot coordinate system"""
00161
00162 rospy.logdebug("Walk target_pose: %f %f %f", data.x,
00163 data.y, data.theta)
00164
00165 self.gotoStartWalkPose()
00166
00167 try:
00168 if post:
00169 self.motionProxy.post.walkTo(data.x, data.y, data.theta, self.footGaitConfig)
00170 else:
00171 self.motionProxy.walkTo(data.x, data.y, data.theta, self.footGaitConfig)
00172 return True
00173 except RuntimeError,e:
00174 rospy.logerr("Exception caught in handleTargetPose:\n%s", e)
00175 return False
00176
00177
00178 def handleStep(self, data):
00179 rospy.logdebug("Step leg: %d; target: %f %f %f", data.leg, data.pose.x,
00180 data.pose.y, data.pose.theta)
00181 try:
00182 if data.leg == StepTarget.right:
00183 leg = "RLeg"
00184 elif data.leg == StepTarget.left:
00185 leg = "LLeg"
00186 else:
00187 rospy.logerr("Received a wrong leg constant: %d, ignoring step command", data.leg)
00188 return
00189 self.motionProxy.stepTo(leg, data.pose.x, data.pose.y, data.pose.theta)
00190 return True
00191 except RuntimeError, e:
00192 rospy.logerr("Exception caught in handleStep:\n%s", e)
00193 return False
00194
00195 def handleStepSrv(self, req):
00196 if self.handleStep(req.step):
00197 return StepTargetServiceResponse()
00198 else:
00199 return None
00200
00201 def handleTargetPoseService(self, req):
00202 """ do NOT use post"""
00203 if self.handleTargetPose(req.pose, False):
00204 return CmdPoseServiceResponse()
00205 else:
00206 return None
00207
00208 def handleStopWalkSrv(self, req):
00209 if self.stopWalk():
00210 return EmptyResponse()
00211 else:
00212 return None
00213
00214 def gotoStartWalkPose(self):
00215 if self.useStartWalkPose and self.needsStartWalkPose:
00216 startWalkPose(self.motionProxy)
00217 self.needsStartWalkPose = False
00218
00219 def handleNeedsStartWalkPoseSrv(self, data):
00220 self.needsStartWalkPose = True
00221 return EmptyResponse()
00222
00223 def handleReadFootGaitConfigSrv(self, data):
00224 self.useFootGaitConfig = rospy.get_param('~use_foot_gait_config', False)
00225 rospy.loginfo("useFootGaitConfig = %d" % self.useFootGaitConfig)
00226 if self.useFootGaitConfig:
00227 self.footGaitConfig = rospy.get_param('~foot_gait_config', self.motionProxy.getFootGaitConfig("Default"))
00228 else:
00229 self.footGaitConfig = self.motionProxy.getFootGaitConfig("Default")
00230 return EmptyResponse()
00231
00232 def handleSetArmsEnabledSrv(self, req):
00233 self.motionProxy.setWalkArmsEnable(req.left_arm, req.right_arm)
00234 rospy.loginfo("Arms enabled during walk: left(%s) right(%s)" % (req.left_arm, req.right_arm))
00235 return SetArmsEnabledResponse()
00236
00237
00238 if __name__ == '__main__':
00239 walker = NaoWalker()
00240 rospy.loginfo("nao_walker running...")
00241 rospy.spin()
00242 rospy.loginfo("nao_walker stopping...")
00243 walker.stopWalk()
00244
00245 rospy.loginfo("nao_walker stopped.")
00246 exit(0)