python类logdebug()的实例源码

def test_publish_to_topics(self):
        topic_ending = "desired"
        rospy.logdebug("Sleeping for 5 seconds to let ROS spin up...")
        rospy.sleep(5)
        for variable, value in self.variables:
            # Publish to each variable/desired topic to see if all of the
            # actuators come on as expected.
            topic_string = variable + "/" + topic_ending
            rospy.logdebug("Testing Publishing to " + topic_string)
            pub_desired = rospy.Publisher(topic_string,
                                               Float64, queue_size=10)
            sub_desired = rospy.Subscriber(topic_string, Float64, self.callback)
            rospy.sleep(2)
            print(self.namespace + "/" + topic_string)
            for _ in range(NUM_TIMES_TO_PUBLISH):
                pub_desired.publish(value)
                rospy.sleep(1)
            rospy.sleep(2)
            pub_desired.publish(0)

def handleTargetPose(self, data, post=True):
        """handles cmd_pose requests, walks to (x,y,theta) in robot coordinate system"""

        rospy.logdebug("Walk target_pose: %f %f %f", data.x,
                data.y, data.theta)

        self.gotoStartWalkPose()

        try:
            if post:
                self.motionProxy.post.moveTo(data.x, data.y, data.theta, self.footGaitConfig)
            else:
                self.motionProxy.moveTo(data.x, data.y, data.theta, self.footGaitConfig)
            return True
        except RuntimeError,e:
            rospy.logerr("Exception caught in handleTargetPose:\n%s", e)
            return False

def handleStep(self, data):
        rospy.logdebug("Step leg: %d; target: %f %f %f", data.leg, data.pose.x,
                data.pose.y, data.pose.theta)
        try:
            if data.leg == StepTarget.right:
                leg = "RLeg"
            elif data.leg == StepTarget.left:
                leg = "LLeg"
            else:
                rospy.logerr("Received a wrong leg constant: %d, ignoring step command", data.leg)
                return
            self.motionProxy.stepTo(leg, data.pose.x, data.pose.y, data.pose.theta)
            return True
        except RuntimeError, e:
            rospy.logerr("Exception caught in handleStep:\n%s", e)
            return False

def update_with_categorical_distribution(self, recognition):
        """
        Update the recognition with a categorical distribution of the trained faces
        :param recognition: Input recognition
        :return: Output recognition with an updated categorical distribution
        """
        if self._trained_faces:
            # Try to get a representation of the detected face
            recognition_representation = None
            try:
                recognition_representation = self._get_representation(recognition.image)
            except Exception as e:
                print "Could not get representation of face image but detector found one: %s" % str(e)

            rospy.logdebug('recognition_representation: %s', recognition_representation)
            # If we have a representation, update with use of the l2 distance w.r.t. the face dict
            if recognition_representation is not None:
                recognition.l2_distances = [L2Distance(_get_min_l2_distance(
                    face.representations, recognition_representation), face.label) for face in self._trained_faces]

            # Sort l2 distances probabilities, lowest on index 0
            recognition.l2_distances = sorted(recognition.l2_distances, key=lambda x: x.distance)

        return recognition

def poll(self):
        if self.pseudo:
            self.o2 = 19.3
            return
        if self.sensor_is_connected:
            try:
                self.serial.write(('adc read {}\r'.format(self.analog_port)).encode())
                response = self.serial.read(25)
                voltage = float(response[10:-3]) * 5 / 1024
                if voltage == 0:
                    return
                self.o2 = voltage * 0.21 / 2.0 * 100 # percent
                rospy.logdebug('o2 = {}'.format(self.o2))
            except:
                rospy.logwarn("O2 SENSOR> Failed to read value during poll")
                self.o2 = None
                self.sensor_is_connected = False
        else:
            self.connect()

def waitForInitialPose(self, next_topic, timeout=None):
        counter = 0
        while not rospy.is_shutdown():
            counter = counter + 1
            if timeout and counter >= timeout:
                return False
            try:
                self.marker_lock.acquire()
                self.initialize_poses = True
                topic_suffix = next_topic.split("/")[-1]
                if self.initial_poses.has_key(topic_suffix):
                    self.pre_pose = PoseStamped(pose=self.initial_poses[topic_suffix])
                    self.initialize_poses = False
                    return True
                else:
                    rospy.logdebug(self.initial_poses.keys())
                    rospy.loginfo("Waiting for pose topic of '%s' to be initialized",
                                  topic_suffix)
                    rospy.sleep(1)
            finally:
                self.marker_lock.release()

def pick(self, pose, direction=(0, 0, 1), distance=0.1, controller=None):
        """Go to pose + pick_direction * pick_distance, open, go to pose,
        close, go to pose + pick_direction * pick_distance.

        """
        rospy.logdebug("pick pose: %s" % pose)
        pregrasp_pose = self.translate(pose, direction, distance)
        rospy.logdebug("pregrasp_pose: %s" % pregrasp_pose)
        rospy.sleep(1)
        self.move_ik(pregrasp_pose)
        # We want to block end effector opening so that the next
        # movement happens with the gripper fully opened.
        self.gripper.open(block=True)
        self.move_ik(pose)
        if controller is not None:
            print ('controller ON!!')
            controller.enable()
            rospy.sleep(5)
            controller.disable()
        self.gripper.close(block=True)
        self.move_ik(pregrasp_pose)

def alvarcb(self, markers):
        rospy.logdebug("Detected markers!")
        # can we find the correct marker?
        for m in markers.markers:
            if m.id == self.marker_id:
                odom_meas = Odometry()
                odom_meas.header.frame_id = self.frame_id
                m.pose.header.frame_id = self.camera_frame_id
                odom_meas.child_frame_id = self.base_frame_id
                odom_meas.header.stamp = m.header.stamp
                m.pose.header.stamp = m.header.stamp
                # now we need to transform this pose measurement from the camera
                # frame into the frame that we are reporting measure odometry in
                pose_transformed = self.transform_pose(m.pose)
                if pose_transformed is not None:
                    odom_meas.pose.pose = pose_transformed.pose
                    # Now let's add our offsets:
                    odom_meas = odom_conversions.odom_add_offset(odom_meas, self.odom_offset)
                    self.meas_pub.publish(odom_meas)
                    self.send_transforms(odom_meas)
                    self.publish_path(m.pose)
        return

def login(self):
        """Authenticates with the server.

        Raises:
            Timeout: On timeout.
            HTTPError: On request failure.
            ConnectionError: On connection failure.
        """
        method = "POST"
        uri = "/api/login"
        response = self.session.request(
            method=method,
            url=self.url + uri,
            timeout=self.timeout,
            verify=self.verify,
            data=self.__credentials)
        message = self._get_response_log_message(method, uri, response)

        try:
            response.raise_for_status()
            rospy.logdebug(message)
        except requests.HTTPError:
            # For more human-readable error messages.
            raise requests.HTTPError(message, response=response)

def _joy_cb(self, msg):
        """
        The joy/game-pad message callback.

        :type   msg:    Joy
        :param  msg:    The incoming joy message.

        """
        if msg.buttons[self._head_button] == 1:
            angle_deg = ((msg.axes[self._head_axes] + 1.0) / 2.0) * SUM_HEAD_ANGLE + MIN_HEAD_ANGLE
            rospy.logdebug('Send head angle: {} degrees.'.format(angle_deg))
            self._head_pub.publish(data=angle_deg)

        if msg.buttons[self._lift_button] == 1:
            lift_mm = abs(msg.axes[self._lift_axes]) * SUM_LIFT_HEIGHT + MIN_LIFT_HEIGHT
            rospy.logdebug('Send lift height: {} mm.'.format(lift_mm))
            self._lift_pub.publish(data=abs(msg.axes[self._lift_axes]))

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

def get_current_volume(self):
        cmd = ShellCmd(self.base_get_cmd)
        while not cmd.is_done() and not rospy.is_shutdown():
            rospy.sleep(0.1)
        output = cmd.get_stdout()
        rospy.logdebug("self.base_get_cmd output: " + str(output))
        # Output looks like:
        # Simple mixer control 'Master',0
        # Capabilities: pvolume pvolume-joined pswitch pswitch-joined
        # Playback channels: Mono
        # Limits: Playback 0 - 87
        # Mono: Playback 44 [51%] [-32.25dB] [on]

        pct_idx = output.index('%')
        # At most 3 characters of 100%
        pct_text = output[pct_idx - 3:pct_idx]
        # Remove [ if it was less than 3 chars
        pct_text = pct_text.replace('[', '')
        # Remove spaces if it was less than 2 char
        pct_text = pct_text.strip()
        curr_vol = int(pct_text)
        return curr_vol

def purge(self):
        rospy.logdebug("### ENTRY  purge  ###")
        # wait for old transition to finish before processing new one
        self._wait_for_transition_is_done()

        if not self.atf_started:
            raise ATFTestblockError("Calling purge for testblock '%s' before ATF has been started." % self.name)
        if self.get_state() in self.m.endStates:
            raise ATFTestblockError("Calling purge for testblock '%s' while testblock already stopped." % self.name)

        # set new transition trigger
        t = TestblockTrigger()
        t.stamp = rospy.Time.now()
        t.name = self.name
        t.trigger = TestblockTrigger.PURGE
        self.trigger = t
        rospy.logdebug("### EXIT purge  ###")

def start(self):
        rospy.logdebug("### ENTRY  start  ###")
        # wait for old transition to finish before processing new one
        self._wait_for_transition_is_done()

        if not self.atf_started:
            raise ATFTestblockError("Calling start for testblock '%s' before ATF has been started." % self.name)
        if self.get_state() in self.m.endStates:
            raise ATFTestblockError("Calling start for testblock '%s' while testblock already stopped." % self.name)

        # set new transition trigger
        t = TestblockTrigger()
        t.stamp = rospy.Time.now()
        t.name = self.name
        t.trigger = TestblockTrigger.START
        self.trigger = t
        rospy.logdebug(" start call with trigger : '%s'", self.trigger.trigger)
        rospy.logdebug("### EXIT  start  ###")

def pause(self):
        rospy.logdebug("### ENTRY  pause  ###")
        # wait for old transition to finish before processing new one
        self._wait_for_transition_is_done()

        if not self.atf_started:
            raise ATFTestblockError("Calling pause for testblock '%s' before ATF has been started." % self.name)
        if self.get_state() in self.m.endStates:
            raise ATFTestblockError("Calling pause for testblock '%s' while testblock already stopped." % self.name)

        # set new transition trigger
        t = TestblockTrigger()
        t.stamp = rospy.Time.now()
        t.name = self.name
        t.trigger = TestblockTrigger.PAUSE
        self.trigger = t
        rospy.logdebug("### EXIT  pause  ###")

def _purged_state(self):
        rospy.logdebug("*** ENTRY _purged_state ***")
        self._wait_while_transition_is_active()
        #self.recorder_handle.record_trigger(self.trigger)
        if self.trigger.trigger == TestblockTrigger.START:
            self._start()
            new_state = TestblockState.ACTIVE
        elif self.trigger.trigger == TestblockTrigger.STOP:
            rospy.logdebug("Stopping testblock is called from _purged_state")
            self._stop()
            new_state = TestblockState.SUCCEEDED
        else:
            message = "testblock '%s': invalid transition '%s' from state '%s'" % (self.trigger.name, str(self.trigger.trigger), self.get_state())
            rospy.logerr(message)
            new_state = TestblockState.ERROR
            self.exception = message
            raise ATFTestblockError(message)
        rospy.logdebug(" _purged_state trigger : '%s'", self.trigger.trigger)
        self.trigger = None
        rospy.logdebug(" _purged_state after trigger = None : '%s'", self.trigger)
        rospy.logdebug("*** EXIT _purged_state ***")
        return new_state

def _active_state(self):
        rospy.logdebug("*** ENTRY _active_state ***")
        self._wait_while_transition_is_active()
        #self.recorder_handle.record_trigger(self.trigger)
        if self.trigger.trigger == TestblockTrigger.PURGE:
            self._purge()
            new_state = TestblockState.PURGED
        elif self.trigger.trigger == TestblockTrigger.PAUSE:
            self._pause()
            new_state = TestblockState.PAUSED
        elif self.trigger.trigger == TestblockTrigger.STOP:
            rospy.logdebug("Stopping testblock is called from _active_state")
            self._stop()
            new_state = TestblockState.SUCCEEDED
        else:
            message = "testblock '%s': invalid transition '%s' from state '%s'" % (self.trigger.name, str(self.trigger.trigger), self.get_state())
            rospy.logerr(message)
            new_state = TestblockState.ACTIVE
            self.exception = message
            raise ATFTestblockError(message)
        rospy.logdebug(" _active_state trigger - _active_state : '%s'", self.trigger.trigger)
        self.trigger = None
        rospy.logdebug(" _active_state after trigger = None : '%s'", self.trigger)
        rospy.logdebug("*** EXIT _active_state ***")
        return new_state

def _paused_state(self):
        rospy.logdebug("*** ENTRY _paused_state ***")
        self._wait_while_transition_is_active()
        #self.recorder_handle.record_trigger(self.trigger)
        if self.trigger.trigger == TestblockTrigger.PURGE:
            self._purge()
            new_state = TestblockState.PURGED
        elif self.trigger.trigger == TestblockTrigger.START:
            self._start()
            new_state = TestblockState.ACTIVE
        elif self.trigger.trigger == TestblockTrigger.STOP:
            rospy.logdebug("Stopping testblock is called from _paused_state")
            self._stop()
            new_state = TestblockState.SUCCEEDED
        else:
            message = "testblock '%s': invalid transition '%s' from state '%s'" % (self.trigger.name, str(self.trigger.trigger), self.get_state())
            rospy.logerr(message)
            new_state = TestblockState.ERROR
            self.exception = message
            raise ATFTestblockError(message)
        rospy.logdebug(" _paused_state trigger - _active_state : '%s'", self.trigger.trigger)
        self.trigger = None
        rospy.logdebug(" _paused_state after trigger = None : '%s'", self.trigger)
        rospy.logdebug("*** EXIT _paused_state ***")
        return new_state

def shutdown(self):
        """
        This needs to be called whenever this class terminates.
        This closes all the instances on all trees.
        Also unregisters ROS' subscriber, stops timer.
        """
        rospy.logdebug('RobotMonitorWidget in shutdown')

        names = self._inspectors.keys()
        for name in names:
            self._inspectors[name].close()

        if self._timeline:
            self._timeline.shutdown()

        self._timer.stop()
        del self._timer

def check_vitals(self, stat):
        try:
            status = roboclaw.ReadError(self.address)[1]
        except OSError as e:
            rospy.logwarn("Diagnostics OSError: %d", e.errno)
            rospy.logdebug(e)
            return
        state, message = self.ERRORS[status]
        stat.summary(state, message)
        try:
            stat.add("Main Batt V:", float(roboclaw.ReadMainBatteryVoltage(self.address)[1] / 10))
            stat.add("Logic Batt V:", float(roboclaw.ReadLogicBatteryVoltage(self.address)[1] / 10))
            stat.add("Temp1 C:", float(roboclaw.ReadTemp(self.address)[1] / 10))
            stat.add("Temp2 C:", float(roboclaw.ReadTemp2(self.address)[1] / 10))
        except OSError as e:
            rospy.logwarn("Diagnostics OSError: %d", e.errno)
            rospy.logdebug(e)
        return stat

    # TODO: need clean shutdown so motors stop even if new msgs are arriving

def check_vitals(self, stat):
        try:
            status = roboclaw.ReadError(self.address)[1]
        except OSError as e:
            rospy.logwarn("Diagnostics OSError: %d", e.errno)
            rospy.logdebug(e)
            return
        state, message = self.ERRORS[status]
        stat.summary(state, message)
        try:
            stat.add("Main Batt V:", float(roboclaw.ReadMainBatteryVoltage(self.address)[1] / 10))
            stat.add("Logic Batt V:", float(roboclaw.ReadLogicBatteryVoltage(self.address)[1] / 10))
            stat.add("Temp1 C:", float(roboclaw.ReadTemp(self.address)[1] / 10))
            stat.add("Temp2 C:", float(roboclaw.ReadTemp2(self.address)[1] / 10))
        except OSError as e:
            rospy.logwarn("Diagnostics OSError: %d", e.errno)
            rospy.logdebug(e)
        return stat

    # TODO: need clean shutdown so motors stop even if new msgs are arriving

def update_filter(self, timestep, estimate, ranges):
        """Update position filter.

        Args:
             timestep (float): Time elapsed since last update.
             estimate (StateEstimate): Position estimate to update.
             ranges (list of floats): Range measurements.

        Returns:
            new_estimate (StateEstimate): Updated position estimate.
            outlier_flag (bool): Flag indicating whether the measurement was rejected as an outlier.
        """
        num_of_units = len(ranges)
        x = estimate.state
        P = estimate.covariance
        # Compute process matrix and covariance matrices
        F, Q, R = self._compute_process_and_covariance_matrices(timestep)
        # rospy.logdebug('F: {}'.format(F))
        # rospy.logdebug('Q: {}'.format(Q))
        # rospy.logdebug('R: {}'.format(R))
        # Prediction
        x = np.dot(F, x)
        P = np.dot(F, np.dot(P, F.T)) + Q
        # Update
        n = np.copy(x)
        H = np.zeros((num_of_units, x.size))
        z = np.zeros((num_of_units, 1))
        h = np.zeros((num_of_units, 1))
        for i in xrange(self.ikf_iterations):
            n, K, outlier_flag = self._ikf_iteration(x, n, ranges, h, H, z, estimate, R)
        if outlier_flag:
            new_estimate = estimate
        else:
            new_state = n
            new_covariance = np.dot((np.eye(6) - np.dot(K, H)), P)
            new_estimate = UWBTracker.StateEstimate(new_state, new_covariance)
        return new_estimate, outlier_flag

def Start(self):
        rospy.logdebug("Starting")
        self._SerialDataGateway.Start()

#######################################################################################################################

def Stop(self):
        rospy.logdebug("Stopping")
        self._SerialDataGateway.Stop()



#######################################################################################################################

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")

def _open_action(self, value):
        if value and self._gripper.is_ready():
            rospy.logdebug("gripper open triggered")
            self._gripper.open()
            if self._lights:
                self._set_lights('red', False)
                self._set_lights('green', True)

def _close_action(self, value):
        if value and self._gripper.is_ready():
            rospy.logdebug("gripper close triggered")
            self._gripper.close()
            if self._lights:
                self._set_lights('green', False)
                self._set_lights('red', True)

def _light_action(self, value):
        if value:
            rospy.logdebug("cuff grasp triggered")
        else:
            rospy.logdebug("cuff release triggered")
        if self._lights:
            self._set_lights('red', False)
            self._set_lights('green', False)
            self._set_lights('blue', value)

def display_image(self, image_path, display_in_loop=False, display_rate=1.0):
        """
        Displays image(s) to robot's head

        @type image_path: list
        @param image_path: the relative or absolute file path to the image file(s)

        @type display_in_loop: bool
        @param display_in_loop: Set True to loop through all image files infinitely

        @type display_rate: float
        @param display_rate: the rate to publish image into head
        """
        rospy.logdebug("Display images in loop:'{0}', frequency: '{1}'".format(display_in_loop, display_rate))

        image_msg = []
        image_list = image_path if isinstance(image_path, list) else [image_path]
        for one_path in image_list:
            cv_img = self._setup_image(one_path)
            if cv_img:
                image_msg.append(cv_img)

        if not image_msg:
            rospy.logerr("Image message list is empty")
        else:
            r = rospy.Rate(display_rate)
            while not rospy.is_shutdown():
                for one_msg in image_msg:
                    self._image_pub.publish(one_msg)
                    r.sleep()
                if not display_in_loop:
                    break

def __init__(self, path_root, config_msg_type, status_msg_type):
        self._path = path_root
        self.config_mutex = Lock()
        self.state_mutex = Lock()
        self.cmd_times = []
        self.ports = dict()
        self.signals = dict()
        self.state = None
        self.config = None
        self.state_changed = intera_dataflow.Signal()
        self.config_changed = intera_dataflow.Signal()

        self._config_sub = rospy.Subscriber(self._path + "/config",
                                            config_msg_type,
                                            self.handle_config)
        self._state_sub = rospy.Subscriber(self._path + "/state",
                                           status_msg_type,
                                           self.handle_state)
        self._command_pub = rospy.Publisher(self._path + "/command",
                                            IOComponentCommand, queue_size=10)

        # Wait for the state to be populated
        intera_dataflow.wait_for(
                          lambda: not self.state is None,
                          timeout=5.0,
                          timeout_msg=("Failed to get state.")
                          )

        rospy.logdebug("Making new IOInterface on %s" % (self._path,))