simple_limb.py 文件源码

def goToAngles(self,angles,speed=DEFAULT_SPEED,joint_tolerance_plan=DEFAULT_JOINT_TOLERANCE_PLAN,joint_tolerance=DEFAULT_JOINT_TOLERANCE,speed_tolerance=DEFAULT_SPEED_TOLERANCE,timeout=DEFAULT_TIMEOUT,path_tolerance=0,stop_condition=None):
        """ joint_tolerance_plan,speed_tolerance are ignored, """
        with self.moving_lock:
            self._moving=True
        if type(angles) is not dict:
#             print "converting to dict"
            d=getDictFromAngles(angles)            
        else:
#             print "not converting to dict",angles
            d=dict(angles)        
            angles=getAnglesFromDict(angles)
        rospy.logdebug("SimpleLimb %s goToAngles %s speed %f joint_tolerance %f speed_tolerance %f timeout %f"%(self.side,getStrFromAngles(angles),speed,joint_tolerance,speed_tolerance,timeout))
        self.setSpeed(speed)
        try:
            ret=self.move_to_joint_positions(d,joint_tolerance,speed_tolerance,timeout,stop_condition=stop_condition)
        except Exception,e:
            rospy.logwarn( "Timeout PID: "+str(e))
            ret=False

        with self.moving_lock:
            self._moving=False

        diff=getAnglesDiff(angles,self.getAngles())
        rospy.logdebug("SimpleLimb %s goToAngles distance to target: %s"%(self.side,str(diff)))
        return ret