simple_limb.py 文件源码

def __init__(self,side,ik=True):
        limb.Limb.__init__(self,side)
        self.side=side

        self.DEFAULT_BAXTER_SPEED=0.3

        if not side in ["left","right"]:
            raise BaxterException,"Error non existing side: %s, please provide left or right"%side

        self.post=Post(self)
        self.__stop = False
        self.simple=self
        self._moving=False
        self.moving_lock=Lock()

        self.ik=ik        
        if self.ik:            
            self.ns = "/ExternalTools/%s/PositionKinematicsNode/IKService"%self.side    
            rospy.loginfo("Waiting for inverse kinematics service on %s..."%self.ns)
            rospy.wait_for_service(self.ns)
            self.iksvc = rospy.ServiceProxy(self.ns, SolvePositionIK)
            rospy.loginfo("Waiting for inverse kinematics service DONE")
        else:
            rospy.loginfo("Skipping inverse kinematics service loading")

        #self.simple_limb_srv = rospy.Service("/simple_limb/"+side,LimbPose,self.__cbLimbPose)

        self._pub_speed=rospy.Publisher("/robot/limb/%s/set_speed_ratio"%self.side,Float64,queue_size=1)
        while not rospy.is_shutdown() and self._pub_speed.get_num_connections() < 1:
            rospy.sleep(0.1)
        #self.setSpeed(3)


#     def __cbLimbPose(self,msg):
#         cmd = msg.command
#         if cmd == "goToPose":
#             resp = self.goToPose(msg.pose, msg.speed, msg.position_tolerance, msg.orientation_tolerance, msg.joint_tolerance, msg.speed_tolerance, msg.timeout, cartesian, msg.path_tolerance)
#         elif cmd=="goToAngles":
#             resp = self.goToAngles(msg.angles, msg.speed, msg.joint_tolerance_plan, msg.joint_tolerance, msg.speed_tolerance, msg.timeout, msg.path_tolerance)
#         return LimbPoseResponse(resp)