python类logwarn()的实例源码

def update_telemetry(navsat_msg, compass_msg):
    """Telemetry subscription callback.

    Args:
        navsat_msg: sensor_msgs/NavSatFix message.
        compass_msg: std_msgs/Float64 message in degrees.
    """
    try:
        client.post_telemetry(navsat_msg, compass_msg)
    except (ConnectionError, Timeout) as e:
        rospy.logwarn(e)
        return
    except (ValueError, HTTPError) as e:
        rospy.logerr(e)
        return
    except Exception as e:
        rospy.logfatal(e)
        return

def set_piksi_settings(self):
        save_needed = False
        for s in nested_dict_iter(self.piksi_update_settings):
            cval = self.piksi_settings[s[0][0]][s[0][1]]
            if len(cval) != 0:
                if cval != str(s[1]):
                    rospy.loginfo('Updating piksi setting %s:%s to %s.' % (s[0][0], s[0][1], s[1]))
                    self.piksi_set(s[0][0], s[0][1], s[1])
                    self.update_dr_param(s[0][0], s[0][1], s[1])
                    save_needed = True
                else:
                    rospy.loginfo('Piksi setting %s:%s already set to %s.' % (s[0][0], s[0][1], s[1]))
            else:
                rospy.logwarn('Invalid piksi setting: %s' % ':'.join(s[0]))

        if self.piksi_save_settings and save_needed:
            rospy.loginfo('Saving piksi settings to flash')
            m = MsgSettingsSave()
            self.piksi_framer(m)

def _read_unit_offsets(self):
        if not rospy.has_param('~num_of_units'):
            rospy.logwarn("No unit offset parameters found!")
        num_of_units = rospy.get_param('~num_of_units', 0)
        self._unit_offsets = np.zeros((num_of_units, 3))
        self._unit_coefficients = np.zeros((num_of_units, 2))
        for i in xrange(num_of_units):
            unit_params = rospy.get_param('~unit_{}'.format(i))
            x = unit_params['x']
            y = unit_params['y']
            z = unit_params['z']
            self._unit_offsets[i, :] = [x, y, z]
            p0 = unit_params['p0']
            p1 = unit_params['p1']
            self._unit_coefficients[i, :] = [p0, p1]
        rospy.loginfo("Unit offsets: {}".format(self._unit_offsets))
        rospy.loginfo("Unit coefficients: {}".format(self._unit_coefficients))

def send_reset_to_rovio_service_callback(self, request):
        response = std_srvs.srv.TriggerResponse()

        if self._automatic_rovio_reset_sent_once or self._send_reset_automatically:
            message = "Reset sent automatically after %d IMU messages, rosservice call refused." % \
                      (self._samples_before_reset)

            rospy.logwarn(rospy.get_name() + " " + message)
            response.success = False
            response.message = message
        elif self._num_imu_msgs_read <= 0:
            response.success = False
            response.message = "No external IMU message received, at least one single orientation is needed."
        elif self._num_gps_transform_msgs_read <= 0:
            response.success = False
            response.message = "No external GPS transform message received, at least one single position is needed."
        else: # everything's fine, send reset
            (success, message) = self.send_reset_to_rovio()
            response.success = success
            response.message = message

        return response

def wait(self):
        """
        Waits for and verifies trajectory execution result
        """
        #create a timeout for our trajectory execution
        #total time trajectory expected for trajectory execution plus a buffer
        last_time = self.goal.trajectory.points[-1].time_from_start.to_sec()
        time_buffer = rospy.get_param(self._param_ns + 'goal_time', 0.0) + 1.5
        timeout = rospy.Duration(self._slow_move_offset +
                                 last_time +
                                 time_buffer)

        finish = self._limb_client.wait_for_result(timeout)
        result = (self._limb_client.get_result().error_code == 0)

        #verify result
        if all([finish, result]):
            return True
        else:
            msg = ("Trajectory action failed or did not finish before "
                   "timeout/interrupt.")
            rospy.logwarn(msg)
            return False

def raise_event(self, perception_item, event):
        if not perception_item in self.conf_data.keys():
            rospy.logwarn('<%s> perception is not member of perception configuration...'%perception_item)
            return
        if not event in self.conf_data[perception_item]['events']:
            rospy.logwarn('<%s> event is not member of event list of perception configuration...'%event)
            return

        if not self.is_enable_perception:
            return

        msg = ForwardingEvent()
        msg.header.stamp = rospy.Time.now()
        msg.event = event
        msg.by = perception_item

        self.pub_event.publish(msg)

def save_to_memory(self, perception_name, data={}):
        if data == {}:
            rospy.logwarn('Empty data inserted...')
            return

        for item in data.keys():
            if not item in self.conf_data[perception_name]['data'].keys():
                rospy.logwarn('[%s -- wrong data inserted [%s]...'%(perception_name, item))
                return

        srv_req = WriteDataRequest()
        srv_req.perception_name = perception_name
        srv_req.data = json.dumps(data)
        srv_req.by = rospy.get_name()

        target_memory = self.conf_data[perception_name]['target_memory']

        try:
            rospy.wait_for_service('/%s/write_data'%target_memory)
            self.dict_srv_wr[target_memory](srv_req)
        except rospy.ServiceException as e:
            pass

def read_from_memory(self, target_memory, data):
        if data == {}:
            rospy.logwarn('Empty data requested...')
            return

        req = ReadDataRequest()
        req.perception_name = data['perception_name']
        req.query = data['query']
        for item in data['data']:
            req.data.append(item)

        resonse = self.dict_srv_rd[target_memory](req)
        if not response.result:
            return {}

        results = json.loads(response.data)
        return results

def _read_unit_offsets(self):
        if not rospy.has_param('~num_of_units'):
            rospy.logwarn("No unit offset parameters found!")
        num_of_units = rospy.get_param('~num_of_units', 0)
        self._unit_offsets = np.zeros((num_of_units, 3))
        self._unit_coefficients = np.zeros((num_of_units, 2))
        for i in xrange(num_of_units):
            unit_params = rospy.get_param('~unit_{}'.format(i))
            x = unit_params['x']
            y = unit_params['y']
            z = unit_params['z']
            self._unit_offsets[i, :] = [x, y, z]
            p0 = unit_params['p0']
            p1 = unit_params['p1']
            self._unit_coefficients[i, :] = [p0, p1]
        rospy.loginfo("Unit offsets: {}".format(self._unit_offsets))
        rospy.loginfo("Unit coefficients: {}".format(self._unit_coefficients))

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 __init__(self):
        # ROS initialization:
        rospy.init_node('pose_manager')

        self.poseLibrary = dict()
        self.readInPoses()
        self.poseServer = actionlib.SimpleActionServer("body_pose", BodyPoseAction,
                                                       execute_cb=self.executeBodyPose,
                                                       auto_start=False)
        self.trajectoryClient = actionlib.SimpleActionClient("joint_trajectory", JointTrajectoryAction)
        if self.trajectoryClient.wait_for_server(rospy.Duration(3.0)):
            try:
                rospy.wait_for_service("stop_walk_srv", timeout=2.0)
                self.stopWalkSrv = rospy.ServiceProxy("stop_walk_srv", Empty)
            except:
                rospy.logwarn("stop_walk_srv not available, pose_manager will not stop the walker before executing a trajectory. "
                          +"This is normal if there is no nao_walker running.")
            self.stopWalkSrv = None
            self.poseServer.start()

            rospy.loginfo("pose_manager running, offering poses: %s", self.poseLibrary.keys());

        else:
            rospy.logfatal("Could not connect to required \"joint_trajectory\" action server, is the nao_controller node running?");
            rospy.signal_shutdown("Required component missing");

def parseXapPoses(self, xaplibrary):
        try:
            poses = xapparser.getpostures(xaplibrary)
        except RuntimeError as re:
            rospy.logwarn("Error while parsing the XAP file: %s" % str(re))
            return

        for name, pose in poses.items():

            trajectory = JointTrajectory()

            trajectory.joint_names = pose.keys()
            joint_values = pose.values()

            point = JointTrajectoryPoint()
            point.time_from_start = Duration(2.0) # hardcoded duration!
            point.positions = pose.values()
            trajectory.points = [point]

            self.poseLibrary[name] = trajectory

def __init__(self, memory_service, memory_key, latch=False, prefix='/qi/'):
        self.memory_service = memory_service
        try:
            self._pub = rospy.Publisher(prefix + memory_key,
                                        String, latch=latch,
                                        queue_size=1)
            self._sub = self.memory_service.subscriber(memory_key)
            self._sub.signal.connect(self._on_event)
            if latch:
                hist = self.memory_service.getEventHistory(memory_key)
                if len(hist) > 0:
                    try:
                        self._on_event(hist[-1][0])
                    except Exception:
                        pass
            rospy.loginfo('subscribed to %s on Qi' % memory_key)
        except Exception as e:
            rospy.logwarn("Cannot set up for %s: %s" % (memory_key, str(e)))

def abort(self, req=None):
        """
        Service function for the aborting the task '[name]_abort'.

        handles a shutdown of the task but instead of calling signal_complete,
        this method calls `signal_aborted`, which lets the task server know
        that it can proceed with launching a mayday task (as opposed to queueing
        up another general task).

        Args:
            msg (std_msgs.msg.String): the message received through the
                subscriber channel.
        """
        if not self.active:
            logwarn("Can't abort {} because task isn't active.".format(self.name))
            return False

        self.instruct()
        self.prep_shutdown(did_fail=True)
        return True

def wait(self):
        """
        Waits for and verifies trajectory execution result
        """
        #create a timeout for our trajectory execution
        #total time trajectory expected for trajectory execution plus a buffer
        last_time = self._r_goal.trajectory.points[-1].time_from_start.to_sec()
        time_buffer = rospy.get_param(self._param_ns + 'goal_time', 0.0) + 1.5
        timeout = rospy.Duration(self._slow_move_offset +
                                 last_time +
                                 time_buffer)

        l_finish = self._left_client.wait_for_result(timeout)
        r_finish = self._right_client.wait_for_result(timeout)
        l_result = (self._left_client.get_result().error_code == 0)
        r_result = (self._right_client.get_result().error_code == 0)

        #verify result
        if all([l_finish, r_finish, l_result, r_result]):
            return True
        else:
            msg = ("Trajectory action failed or did not finish before "
                   "timeout/interrupt.")
            rospy.logwarn(msg)
            return False

def set_position(self, position):
        """Accept position from 0 (open) to 100 (closed)."""
        position_turns = self.parse_percent_to_turn(position)
        goal = kinova_msgs.msg.SetFingersPositionGoal()
        goal.fingers.finger1 = float(position_turns[0])
        goal.fingers.finger2 = float(position_turns[1])
        if len(position)==3:
            goal.fingers.finger3 = float(position_turns[2])
        else:
            goal.fingers.finger3 = 0.0

        self.client.send_goal(goal)

        if self.client.wait_for_result(rospy.Duration(5.0)):
            return self.client.get_result()
        else:
            self.client.cancel_all_goals()
            rospy.logwarn('        the gripper action timed-out')
            return None

def execute_iteration(self, task, method, iteration, trial, num_iterations):
        rospy.logwarn("Controller starts iteration {} {}/{} trial {}".format(method, iteration, num_iterations, trial))
        rospy.wait_for_service('ergo/reset')  # Ensures Ergo keeps working or wait till it reboots

        # After resuming, we keep the same iteration
        if self.perception.has_been_pressed('buttons/help'):
            rospy.sleep(1.5)  # Wait for the robot to fully stop
            self.recorder.record(task, method, trial, iteration)
            self.perception.switch_led('button_leds/pause', True)
            recording = self.perception.record(human_demo=True, nb_points=self.params['nb_points'])
            self.torso.set_torque_max(self.torso_params['torques']['reset'])
            self.torso.reset(slow=True)
            return True
        else:
            trajectory = self.learning.produce(skill_to_demonstrate=self.demonstrate).torso_trajectory
            self.torso.set_torque_max(self.torso_params['torques']['motion'])
            self.recorder.record(task, method, trial, iteration) 
            self.perception.switch_led('button_leds/pause', True)
            self.torso.execute_trajectory(trajectory)  # TODO: blocking, non-blocking, action server?
            recording = self.perception.record(human_demo=False, nb_points=self.params['nb_points'])
            self.perception.switch_led('button_leds/pause', False)
            recording.demo.torso_demonstration = JointTrajectory()
            self.torso.set_torque_max(80)
            self.torso.reset(slow=False)
            return self.learning.perceive(recording.demo)

def cb_produce(self, request):
        with self.lock_iteration:
            # Check if we need a new learner
            self.produce_init_learner()
            focus = copy(self.focus)

        rospy.loginfo("Learning node is requesting the current state")
        state = self.get_state(GetSensorialStateRequest()).state

        demonstrate = request.skill_to_demonstrate
        if demonstrate == "":
            rospy.loginfo("Learning node is producing...")
            w = self.learning.produce(self.translator.get_context(state), focus)
        else:
            rospy.logwarn("Assessing {}...".format(demonstrate))
            context = self.translator.get_context(state)
            w = self.learning.produce(context, goal=demonstrate)

        trajectory_matrix = self.translator.w_to_trajectory(w)
        trajectory_msg = self.translator.matrix_to_trajectory_msg(trajectory_matrix)

        self.ready_for_interaction = True

        response = ProduceResponse(torso_trajectory=trajectory_msg)
        return response

def publish_obstacles(timer_event):
    """Requests and publishes obstacles.

    Args:
        timer_event: ROS TimerEvent.
    """
    try:
        moving_obstacles, stationary_obstacles = client.get_obstacles(
            frame, lifetime)
    except (ConnectionError, Timeout) as e:
        rospy.logwarn(e)
        return
    except (ValueError, HTTPError) as e:
        rospy.logerr(e)
        return
    except Exception as e:
        rospy.logfatal(e)
        return

    moving_pub.publish(moving_obstacles)
    stationary_pub.publish(stationary_obstacles)

def get_active_mission(req):
    """ Service to update mission information with current active mission.

    Args:
        req: Request of type Trigger.

    Returns:
        TriggerResponse with true, false for success, failure.
    """
    with lock:
        global msgs
        try:
            msgs = client.get_active_mission(frame)
        except (ConnectionError, Timeout) as e:
            rospy.logwarn(e)
            return False, str(e)
        except (ValueError, HTTPError) as e:
            rospy.logerr(e)
            return False, str(e)

    rospy.loginfo("Using active mission")
    return True, "Success"

def _send_data(self, channel, data, string_pattern):
        str_pat = 'I'
        if string_pattern != 's':
            str_pat += string_pattern
            packer = struct.Struct(str_pat)
            sent_vect = [channel] + data
            packed_data = packer.pack(*sent_vect)
        else:
            packer = struct.Struct(str_pat)
            sent_vect = [channel]
            packed_data = packer.pack(*sent_vect)
            packed_data += data
        sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  # UDP
        try:
            sock.sendto(packed_data, (self.ip, self.port))
        except socket.gaierror:
            rospy.logwarn("Host not connected")

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 device_serial_nr_cb(self, data):
        snr = data.data

        if self.serial_nr is not None:
            if not snr == self.serial_nr:
                rospy.logwarn('Got new serial Nr but already have one. Restart to calibrate a new cameara.')
            return

        # if the serial nr is available, the width and height are too
        self.res_height = rospy.get_param('/duo_node/resolution_height')
        self.res_width = rospy.get_param('/duo_node/resolution_width')

        self.left_image_label.setFixedSize(QSize(self.res_width, self.res_height))
        self.right_image_label.setFixedSize(QSize(self.res_width, self.res_height))

        rospy.loginfo('Got new serial Nr: {}'.format(snr))
        self.status_bar_update.emit('Device serial Nr.: {}'.format(snr))
        self.serial_nr = snr

def parse_parameter(self, testblock_name, params):
        """
        Method that returns the metric method with the given parameter.
        :param params: Parameter
        """
        metrics = []
        if type(params) is not list:
            rospy.logerr("metric config not a list")
            return False

        for metric in params:
            # check for optional parameters
            try:
                groundtruth = metric["groundtruth"]
                groundtruth_epsilon = metric["groundtruth_epsilon"]
            except (TypeError, KeyError):
                rospy.logwarn("No groundtruth parameters given, skipping groundtruth evaluation for metric 'path_length' in testblock '%s'", testblock_name)
                groundtruth = None
                groundtruth_epsilon = None
            metrics.append(CalculatePathLength(metric["root_frame"], metric["measured_frame"], groundtruth, groundtruth_epsilon))
        return metrics

def parse_parameter(self, testblock_name, params):
        """
        Method that returns the metric method with the given parameter.
        :param params: Parameter
        """
        metrics = []
        if type(params) is not list:
            rospy.logerr("metric config not a list")
            return False

        for metric in params:
            # check for optional parameters
            try:
                groundtruth = metric["groundtruth"]
                groundtruth_epsilon = metric["groundtruth_epsilon"]
            except (TypeError, KeyError):
                rospy.logwarn("No groundtruth parameters given, skipping groundtruth evaluation for metric 'publish_rate' in testblock '%s'", testblock_name)
                groundtruth = None
                groundtruth_epsilon = None
            metrics.append(CalculatePublishRate(metric["topic"], groundtruth, groundtruth_epsilon))
        return metrics

def dashwarn(msg, obj, title='Warning'):
    """
    Logs a message with ``rospy.logwarn`` and displays a ``QMessageBox`` to the user

    :param msg: Message to display.
    :type msg: str
    :param obj: Parent object for the ``QMessageBox``
    :type obj: QObject
    :param title: An optional title for the `QMessageBox``
    :type title: str
    """
    rospy.logwarn(msg)

    box = QMessageBox()
    box.setText(msg)
    box.setWindowTitle(title)
    box.show()

    obj._message_box = box

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 callback_external(self, msg, **metadata):
        if self.debug:
            rospy.loginfo("Received external SBP msg.")

        if self.piksi_framer:
            self.piksi_framer(msg, **metadata)
        else:
            rospy.logwarn("Received external SBP msg, but Piksi not connected.")

def main():
    import signal

    rospy.init_node('uwb_multi_range_node')
    u = UWBMultiRange()

    def sigint_handler(sig, _):
        if sig == signal.SIGINT:
            u.stop()
    signal.signal(signal.SIGINT, sigint_handler)

    try:
        u.exec_()
    except (rospy.ROSInterruptException, select.error):
        rospy.logwarn("Interrupted... Stopping.")