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
00037 import rospy
00038 import actionlib
00039 from naoqi_bridge_msgs.msg import(
00040 JointTrajectoryResult,
00041 JointTrajectoryAction,
00042 JointAnglesWithSpeed,
00043 JointAnglesWithSpeedResult,
00044 JointAnglesWithSpeedAction,
00045 BodyPoseWithSpeedAction,
00046 BodyPoseWithSpeedGoal,
00047 BodyPoseWithSpeedResult)
00048
00049 from naoqi_driver.naoqi_node import NaoqiNode
00050
00051 from std_msgs.msg import String
00052 from std_srvs.srv import Empty, EmptyResponse, Trigger, TriggerResponse
00053 from sensor_msgs.msg import JointState
00054
00055 class PoseController(NaoqiNode):
00056 def __init__(self):
00057 NaoqiNode.__init__(self, 'pose_controller')
00058
00059 self.connectNaoQi()
00060
00061 self.rate = rospy.Rate(10)
00062
00063
00064 self.collectionSize = {}
00065 for collectionName in ['Head', 'LArm', 'LLeg', 'RLeg', 'RArm', 'Body', 'BodyJoints', 'BodyActuators']:
00066 try:
00067 self.collectionSize[collectionName] = len(self.motionProxy.getJointNames(collectionName));
00068 except RuntimeError:
00069
00070 rospy.logwarn('Collection %s not found on your robot.' % collectionName)
00071
00072
00073
00074 self.poll_rate = int(rospy.get_param("~poll_rate", 0.2)*1000)
00075
00076
00077
00078 initStiffness = rospy.get_param('~init_stiffness', 0.0)
00079
00080
00081 if initStiffness > 0.0 and initStiffness <= 1.0:
00082 self.motionProxy.stiffnessInterpolation('Body', initStiffness, 0.5)
00083
00084
00085 self.getLifeStatePub = rospy.Publisher("get_life_state", String, queue_size=10)
00086
00087
00088 self.enableStiffnessSrv = rospy.Service("body_stiffness/enable", Empty, self.handleStiffnessSrv)
00089 self.disableStiffnessSrv = rospy.Service("body_stiffness/disable", Empty, self.handleStiffnessOffSrv)
00090 self.wakeUpSrv = rospy.Service("wakeup", Empty, self.handleWakeUpSrv)
00091 self.restSrv = rospy.Service("rest", Empty, self.handleRestSrv)
00092 self.enableLifeSrv = rospy.Service("life/enable", Empty, self.handleLifeSrv)
00093 self.disableLifeSrv = rospy.Service("life/disable", Empty, self.handleLifeOffSrv)
00094 self.getLifeSrv = rospy.Service("life/get_state", Trigger, self.handleGetLifeSrv)
00095
00096
00097
00098 self.jointTrajectoryServer = actionlib.SimpleActionServer("joint_trajectory", JointTrajectoryAction,
00099 execute_cb=self.executeJointTrajectoryAction,
00100 auto_start=False)
00101
00102 self.jointStiffnessServer = actionlib.SimpleActionServer("joint_stiffness_trajectory", JointTrajectoryAction,
00103 execute_cb=self.executeJointStiffnessAction,
00104 auto_start=False)
00105
00106 self.jointAnglesServer = actionlib.SimpleActionServer("joint_angles_action", JointAnglesWithSpeedAction,
00107 execute_cb=self.executeJointAnglesWithSpeedAction,
00108 auto_start=False)
00109
00110
00111 self.jointTrajectoryServer.start()
00112 self.jointStiffnessServer.start()
00113 self.jointAnglesServer.start()
00114
00115
00116 if not (self.robotPostureProxy is None):
00117 self.bodyPoseWithSpeedServer = actionlib.SimpleActionServer("body_pose_naoqi", BodyPoseWithSpeedAction,
00118 execute_cb=self.executeBodyPoseWithSpeed,
00119 auto_start=False)
00120 self.bodyPoseWithSpeedServer.start()
00121 else:
00122 rospy.logwarn("Proxy to ALRobotPosture not available, requests to body_pose_naoqi will be ignored.")
00123
00124
00125 rospy.Subscriber("joint_angles", JointAnglesWithSpeed, self.handleJointAngles, queue_size=10)
00126 rospy.Subscriber("joint_stiffness", JointState, self.handleJointStiffness, queue_size=10)
00127
00128 rospy.loginfo("nao_controller initialized")
00129
00130 def connectNaoQi(self):
00131 '''(re-) connect to NaoQI'''
00132 rospy.loginfo("Connecting to NaoQi at %s:%d", self.pip, self.pport)
00133
00134 self.motionProxy = self.get_proxy("ALMotion")
00135 if self.motionProxy is None:
00136 exit(1)
00137
00138
00139 self.robotPostureProxy = self.get_proxy("ALRobotPosture")
00140
00141
00142 self.lifeProxy = self.get_proxy("ALAutonomousLife")
00143
00144
00145 def handleJointAngles(self, msg):
00146 rospy.logdebug("Received a joint angle target")
00147 try:
00148
00149 if (msg.relative==0):
00150 self.motionProxy.setAngles(list(msg.joint_names), list(msg.joint_angles), msg.speed)
00151 else:
00152 self.motionProxy.changeAngles(list(msg.joint_names), list(msg.joint_angles), msg.speed)
00153 except RuntimeError,e:
00154 rospy.logerr("Exception caught:\n%s", e)
00155
00156 def handleJointStiffness(self, msg):
00157 rospy.logdebug("Received a joint angle stiffness")
00158 try:
00159 self.motionProxy.setStiffnesses(list(msg.name), list(msg.effort))
00160 except RuntimeError,e:
00161 rospy.logerr("Exception caught:\n%s", e)
00162
00163 def handleStiffnessSrv(self, req):
00164 try:
00165 self.motionProxy.stiffnessInterpolation("Body", 1.0, 0.5)
00166 rospy.loginfo("Body stiffness enabled")
00167 return EmptyResponse()
00168 except RuntimeError,e:
00169 rospy.logerr("Exception caught:\n%s", e)
00170 return None
00171
00172 def handleStiffnessOffSrv(self, req):
00173 try:
00174 self.motionProxy.stiffnessInterpolation("Body", 0.0, 0.5)
00175 rospy.loginfo("Body stiffness removed")
00176 return EmptyResponse()
00177 except RuntimeError,e:
00178 rospy.logerr("Exception caught:\n%s", e)
00179 return None
00180
00181 def handleWakeUpSrv(self, req):
00182 try:
00183 self.motionProxy.wakeUp()
00184 rospy.loginfo("Wake Up")
00185 return EmptyResponse()
00186 except RuntimeError,e:
00187 rospy.logerr("Exception caught:\n%s", e)
00188 return None
00189
00190 def handleRestSrv(self, req):
00191 try:
00192 self.motionProxy.rest()
00193 rospy.loginfo("Rest")
00194 return EmptyResponse()
00195 except RuntimeError,e:
00196 rospy.logerr("Exception caught:\n%s", e)
00197 return None
00198
00199 def handleLifeSrv(self, req):
00200 try:
00201 self.lifeProxy.setState("solitary")
00202 rospy.loginfo("set life state to solitary")
00203 return EmptyResponse()
00204 except RuntimeError, e:
00205 rospy.logerr("Exception while setting life state:\n%s", e)
00206 return None
00207
00208 def handleLifeOffSrv(self, req):
00209 try:
00210 self.lifeProxy.setState("disabled")
00211 rospy.loginfo("set life state to disabled")
00212 return EmptyResponse()
00213 except RuntimeError, e:
00214 rospy.logerr("Exception while disabling life state:\n%s", e)
00215 return None
00216
00217 def handleGetLifeSrv(self, req):
00218 try:
00219 res = TriggerResponse()
00220 res.success = True
00221 res.message = self.lifeProxy.getState()
00222 rospy.loginfo("current life state is " + str(res.message))
00223 return res
00224 except RuntimeError, e:
00225 rospy.logerr("Exception while getting life state:\n%s", e)
00226 return None
00227
00228 def jointTrajectoryGoalMsgToAL(self, goal):
00229 """Helper, convert action goal msg to Aldebraran-style arrays for NaoQI"""
00230 names = list(goal.trajectory.joint_names)
00231
00232
00233
00234 if len(goal.trajectory.points) == 1 and len(goal.trajectory.points[0].positions) == 1:
00235 angles = goal.trajectory.points[0].positions[0]
00236 else:
00237 angles = list(list(p.positions[i] for p in goal.trajectory.points) for i in range(0,len(goal.trajectory.points[0].positions)))
00238
00239
00240 if not isinstance(angles, float) and len(angles) > self.collectionSize["Body"]:
00241 rospy.loginfo("Stripping angles from %d down to %d", len(angles), self.collectionSize["Body"])
00242 angles.pop(6)
00243 angles.pop(7)
00244 angles.pop()
00245 angles.pop()
00246
00247 if len(names) > self.collectionSize["Body"]:
00248 rospy.loginfo("Stripping names from %d down to %d", len(names), self.collectionSize["Body"])
00249 names.pop(6)
00250 names.pop(7)
00251 names.pop()
00252 names.pop()
00253
00254 times = list(p.time_from_start.to_sec() for p in goal.trajectory.points)
00255 if len(times) == 1 and len(names) == 1:
00256 times = times[0]
00257 if (len(names) > 1):
00258 times = [times]*len(names)
00259
00260 return (names, angles, times)
00261
00262
00263 def executeJointTrajectoryAction(self, goal):
00264 rospy.loginfo("JointTrajectory action executing");
00265
00266 names, angles, times = self.jointTrajectoryGoalMsgToAL(goal)
00267
00268 rospy.logdebug("Received trajectory for joints: %s times: %s", str(names), str(times))
00269 rospy.logdebug("Trajectory angles: %s", str(angles))
00270
00271 task_id = None
00272 running = True
00273
00274 while running and not self.jointTrajectoryServer.is_preempt_requested() and not rospy.is_shutdown():
00275
00276 if task_id is None:
00277
00278 task_id = self.motionProxy.post.angleInterpolation(names, angles, times, (goal.relative==0))
00279
00280
00281 running = self.motionProxy.wait(task_id, self.poll_rate)
00282
00283
00284 if running and task_id:
00285 self.motionProxy.stop( task_id )
00286
00287 jointTrajectoryResult = JointTrajectoryResult()
00288 jointTrajectoryResult.goal_position.header.stamp = rospy.Time.now()
00289 jointTrajectoryResult.goal_position.position = self.motionProxy.getAngles(names, True)
00290 jointTrajectoryResult.goal_position.name = names
00291
00292 if not self.checkJointsLen(jointTrajectoryResult.goal_position):
00293 self.jointTrajectoryServer.set_aborted(jointTrajectoryResult)
00294 rospy.logerr("JointTrajectory action error in result: sizes mismatch")
00295
00296 elif running:
00297 self.jointTrajectoryServer.set_preempted(jointTrajectoryResult)
00298 rospy.logdebug("JointTrajectory preempted")
00299
00300 else:
00301 self.jointTrajectoryServer.set_succeeded(jointTrajectoryResult)
00302 rospy.loginfo("JointTrajectory action done")
00303
00304
00305 def executeJointStiffnessAction(self, goal):
00306 rospy.loginfo("JointStiffness action executing");
00307
00308 names, angles, times = self.jointTrajectoryGoalMsgToAL(goal)
00309
00310 rospy.logdebug("Received stiffness trajectory for joints: %s times: %s", str(names), str(times))
00311 rospy.logdebug("stiffness values: %s", str(angles))
00312
00313 task_id = None
00314 running = True
00315
00316 while running and not self.jointStiffnessServer.is_preempt_requested() and not rospy.is_shutdown():
00317
00318 if task_id is None:
00319
00320 task_id = self.motionProxy.post.stiffnessInterpolation(names, angles, times)
00321
00322
00323 running = self.motionProxy.wait(task_id, self.poll_rate)
00324
00325
00326 if running and task_id:
00327 self.motionProxy.stop( task_id )
00328
00329 jointStiffnessResult = JointTrajectoryResult()
00330 jointStiffnessResult.goal_position.header.stamp = rospy.Time.now()
00331 jointStiffnessResult.goal_position.position = self.motionProxy.getStiffnesses(names)
00332 jointStiffnessResult.goal_position.name = names
00333
00334 if not self.checkJointsLen(jointStiffnessResult.goal_position):
00335 self.jointStiffnessServer.set_aborted(jointStiffnessResult)
00336 rospy.logerr("JointStiffness action error in result: sizes mismatch")
00337
00338 elif running:
00339 self.jointStiffnessServer.set_preempted(jointStiffnessResult)
00340 rospy.logdebug("JointStiffness preempted")
00341
00342 else:
00343 self.jointStiffnessServer.set_succeeded(jointStiffnessResult)
00344 rospy.loginfo("JointStiffness action done")
00345
00346
00347 def executeJointAnglesWithSpeedAction(self, goal):
00348
00349 names = list(goal.joint_angles.joint_names)
00350 angles = list(goal.joint_angles.joint_angles)
00351 rospy.logdebug("Received JointAnglesWithSpeed for joints: %s angles: %s", str(names), str(angles))
00352
00353 if goal.joint_angles.relative == 1:
00354
00355 currentAngles = self.motionProxy.getAngles(names, True)
00356 angles = list(map(lambda x,y: x+y, angles, currentAngles))
00357
00358 task_id = None
00359 running = True
00360
00361 while running and not self.jointAnglesServer.is_preempt_requested() and not rospy.is_shutdown():
00362
00363 if task_id is None:
00364
00365 task_id = self.motionProxy.post.angleInterpolationWithSpeed(names, angles, goal.joint_angles.speed)
00366
00367
00368 running = self.motionProxy.wait(task_id, self.poll_rate)
00369
00370
00371 if running and task_id:
00372 self.motionProxy.stop( task_id )
00373
00374 jointAnglesWithSpeedResult = JointAnglesWithSpeedResult()
00375 jointAnglesWithSpeedResult.goal_position.header.stamp = rospy.Time.now()
00376 jointAnglesWithSpeedResult.goal_position.position = self.motionProxy.getAngles(names, True)
00377 jointAnglesWithSpeedResult.goal_position.name = names
00378
00379 if not self.checkJointsLen(jointAnglesWithSpeedResult.goal_position):
00380 self.jointAnglesServer.set_aborted(jointAnglesWithSpeedResult)
00381 rospy.logerr("JointAnglesWithSpeed action error in result: sizes mismatch")
00382
00383 elif running:
00384 self.jointAnglesServer.set_preempted(jointAnglesWithSpeedResult)
00385 rospy.logdebug("JointAnglesWithSpeed preempted")
00386
00387 else:
00388 self.jointAnglesServer.set_succeeded(jointAnglesWithSpeedResult)
00389 rospy.loginfo("JointAnglesWithSpeed action done")
00390
00391 def checkJointsLen(self, goal_position):
00392 if len(goal_position.name) == 1 and self.collectionSize.has_key(goal_position.name[0]):
00393 return len(goal_position.position) == self.collectionSize[goal_position.name[0]]
00394 else:
00395 return len(goal_position.position) == len(goal_position.name)
00396
00397 def executeBodyPoseWithSpeed(self, goal):
00398
00399
00400 if (goal.speed < 0.0) or (goal.speed > 1.0):
00401 bodyPoseWithSpeedResult = BodyPoseWithSpeedResult()
00402 self.bodyPoseWithSpeedServer.set_aborted(bodyPoseWithSpeedResult)
00403 rospy.logerr("Body pose setter: Not a valid speed value.")
00404 return
00405
00406 valid_postures = self.robotPostureProxy.getPostureList()
00407
00408 if goal.posture_name not in valid_postures:
00409 bodyPoseWithSpeedResult = BodyPoseWithSpeedResult()
00410 self.bodyPoseWithSpeedServer.set_aborted(bodyPoseWithSpeedResult)
00411 rospy.logerr("Body pose setter: Not a valid posture.")
00412 return
00413
00414
00415 try:
00416 self.motionProxy.stiffnessInterpolation("Body", 1.0, 0.5)
00417 rospy.loginfo("Body stiffness enabled")
00418 except RuntimeError,e:
00419 rospy.logerr("Exception caught:\n%s", e)
00420 return
00421
00422
00423 self.robotPostureProxy.goToPosture(goal.posture_name, goal.speed)
00424
00425 self.bodyPoseWithSpeedServer.set_succeeded()
00426
00427 def run(self):
00428 while self.is_looping():
00429 try:
00430 if self.getLifeStatePub.get_num_connections() > 0:
00431 get_life_state_msg = String()
00432 get_life_state_msg.data = self.lifeProxy.getState()
00433 self.getLifeStatePub.publish(get_life_state_msg)
00434
00435 except RuntimeError, e:
00436 print "Error accessing ALMotion, ALRobotPosture, ALAutonomousLife, exiting...\n"
00437 print e
00438 rospy.signal_shutdown("No NaoQI available anymore")
00439
00440 self.rate.sleep()
00441
00442 if __name__ == '__main__':
00443
00444 controller = PoseController()
00445 controller.start()
00446 rospy.loginfo("nao pose_controller running...")
00447 rospy.spin()
00448
00449 rospy.loginfo("nao pose_controller stopped.")
00450 exit(0)