python类logwarn()的实例源码

def handle_multi_range_message(self, multi_range_msg):
        """Handle a ROS multi-range message by updating and publishing the state.

        Args:
             multi_range_msg (uwb.msg.UWBMultiRangeWithOffsets): ROS multi-range message.
        """
        # Update tracker position based on time-of-flight measurements
        new_estimate = self.update_estimate(multi_range_msg)
        if new_estimate is None:
            rospy.logwarn('Could not compute initial estimate: address={}, remote_address={}'.format(
                multi_range_msg.address, multi_range_msg.remote_address))
        else:
            # Publish tracker message
            ros_msg = uwb.msg.UWBTracker()
            ros_msg.header.stamp = rospy.get_rostime()
            ros_msg.address = multi_range_msg.address
            ros_msg.remote_address = multi_range_msg.remote_address
            ros_msg.state = new_estimate.state
            ros_msg.covariance = np.ravel(new_estimate.covariance)
            self.uwb_pub.publish(ros_msg)

            # Publish target transform (rotation is identity)
            self.tf_broadcaster.sendTransform(
                (new_estimate.state[0], new_estimate.state[1], new_estimate.state[2]),
                tf.transformations.quaternion_from_euler(0, 0, 0),
                rospy.get_rostime(),
                self.target_frame,
                self.tracker_frame
            )

def main():
    import signal

    rospy.init_node('uwb_tracker_node')
    u = UWBTracker()

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

def read_settings(self):
        # Declination
        self._declination = math.radians(rospy.get_param('~declination_deg', 0.0))

        # Calibration offset
        self._calibration_offset = np.array(rospy.get_param('~calibration_offset', [0.0, 0.0, 0.0]))

        # Calibration compensation
        self._calibration_compensation = np.array(rospy.get_param('~calibration_compensation',
                                                                  [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]))
        # create matrix from array
        self._calibration_compensation = self._calibration_compensation.reshape(3,3)

        # Constant bearing offset
        self._bearing_offset = math.radians(rospy.get_param('~bearing_constant_offset_deg', 0.0))

        # Number of samples used to compute average
        self._number_samples_average = rospy.get_param('~number_samples_average', 10)

        # Verbose
        self._verbose = rospy.get_param('~verbose', "True")

        # Print some useful information
        rospy.logwarn(rospy.get_name() +
                      " declination: " +
                      str(math.degrees(self._declination)) + " (deg)")

        rospy.logwarn(rospy.get_name() +
                      " constant bearing offset added to final bearing: " +
                      str(math.degrees(self._bearing_offset)) + " (deg)")

        if self._verbose:
            rospy.loginfo(rospy.get_name() +
                          " calibration offset: " + str(self._calibration_offset))
            rospy.loginfo(rospy.get_name() +
                          " calibration compensation: \n" +
                          str(self._calibration_compensation))
            rospy.loginfo(rospy.get_name() +
                          " number of samples to average: " +
                          str(self._number_samples_average))

def compute_calibration(self):
        """
        Computes the calibration through the ViSP service and returns it.

        :rtype: easy_handeye.handeye_calibration.HandeyeCalibration
        """
        if len(self.samples) < HandeyeCalibrator.MIN_SAMPLES:
            rospy.logwarn("{} more samples needed! Not computing the calibration".format(
                HandeyeCalibrator.MIN_SAMPLES - len(self.samples)))
            return

        # Update data
        hand_world_samples, camera_marker_samples = self.get_visp_samples()

        if len(hand_world_samples.transforms) != len(camera_marker_samples.transforms):
            rospy.logerr("Different numbers of hand-world and camera-marker samples!")
            raise AssertionError

        rospy.loginfo("Computing from %g poses..." % len(self.samples))

        try:
            result = self.calibrate(camera_marker_samples, hand_world_samples)
            rospy.loginfo("Computed calibration: {}".format(str(result)))
            transl = result.effector_camera.translation
            rot = result.effector_camera.rotation
            result_tuple = ((transl.x, transl.y, transl.z),
                            (rot.x, rot.y, rot.z, rot.w))

            ret = HandeyeCalibration(self.eye_on_hand,
                                     self.robot_base_frame,
                                     self.robot_effector_frame,
                                     self.tracking_base_frame,
                                     result_tuple)

            return ret

        except rospy.ServiceException as ex:
            rospy.logerr("Calibration failed: " + str(ex))
            return None

def compute_calibration(self, _):
        self.last_calibration = self.calibrator.compute_calibration()
        # TODO: avoid confusion class/msg, change class into HandeyeCalibrationConversions
        ret = hec.srv.ComputeCalibrationResponse()
        if self.last_calibration is None:
            rospy.logwarn('No valid calibration computed, returning null')
            return ret
        ret.calibration.eye_on_hand = self.last_calibration.eye_on_hand
        ret.calibration.transform = self.last_calibration.transformation
        return ret

def __init__(self, arm, lights=True):
        """
        @type arm: str
        @param arm: arm of gripper to control
        @type lights: bool
        @param lights: if lights should activate on cuff grasp
        """
        self._arm = arm
        # inputs
        self._cuff = Cuff(limb=arm)
        # connect callback fns to signals
        self._lights = None
        if lights:
            self._lights = Lights()
            self._cuff.register_callback(self._light_action,
                                         '{0}_cuff'.format(arm))
        try:
            self._gripper = Gripper(arm)
            if not (self._gripper.is_calibrated() or
                    self._gripper.calibrate() == True):
                rospy.logerr("({0}_gripper) calibration failed.".format(
                               self._gripper.name))
                raise
            self._cuff.register_callback(self._close_action,
                                         '{0}_button_upper'.format(arm))
            self._cuff.register_callback(self._open_action,
                                         '{0}_button_lower'.format(arm))
            rospy.loginfo("{0} Cuff Control initialized...".format(
                          self._gripper.name))
        except:
            self._gripper = None
            msg = ("{0} Gripper is not connected to the robot."
                   " Running cuff-light connection only.").format(arm.capitalize())
            rospy.logwarn(msg)

def _command_joints(self, joint_names, point, start_time, dimensions_dict):
        if (self._limb.has_collided() or not self.robot_is_enabled()
             or not self._alive or self._cuff.cuff_button()):
           rospy.logerr("{0}: Robot arm in Error state. Stopping execution.".format(
                            self._action_name))
           self._limb.exit_control_mode()
           self._result.error_code = self._result.PATH_TOLERANCE_VIOLATED
           self._server.set_aborted(self._result)
           return False
        elif self._server.is_preempt_requested():
           rospy.logwarn("{0}: Trajectory execution Preempted. Stopping execution.".format(
                            self._action_name))
           self._limb.exit_control_mode()
           self._server.set_preempted()
           return False
        velocities = []
        deltas = self._get_current_error(joint_names, point.positions)
        for delta in deltas:
            if ((math.fabs(delta[1]) >= self._path_thresh[delta[0]]
                  and self._path_thresh[delta[0]] >= 0.0)):
                rospy.logerr("%s: Exceeded Error Threshold on %s: %s" %
                             (self._action_name, delta[0], str(delta[1]),))
                self._result.error_code = self._result.PATH_TOLERANCE_VIOLATED
                self._server.set_aborted(self._result)
                self._limb.exit_control_mode()
                return False
            if self._mode == 'velocity':
                velocities.append(self._pid[delta[0]].compute_output(delta[1]))
        if self._mode == 'velocity':
            self._limb.set_joint_velocities(dict(zip(joint_names, velocities)))
        if self._mode == 'position':
            self._limb.set_joint_positions(dict(zip(joint_names, point.positions)))
        if self._mode == 'position_w_id':
            self._limb.set_joint_trajectory(joint_names,
                                            point.positions,
                                            point.velocities,
                                            point.accelerations)
        return True

def version_check(self):
        """
        Verifies the version of the software running on the robot is
        compatible with this local version of the Intera SDK.

        Currently uses the variables in intera_interface.settings and
        can be overridden for all default examples by setting CHECK_VERSION
        to False.

        @rtype: bool
        @return: Returns True if SDK version is compatible with robot Version, False otherwise
        """
        param_name = "/manifest/robot_software/version/HLR_VERSION_STRING"
        sdk_version = settings.SDK_VERSION

        # get local lock for rosparam threading bug
        with self.__class__.param_lock:
            robot_version = rospy.get_param(param_name, None)
        if not robot_version:
            rospy.logwarn("RobotEnable: Failed to retrieve robot version "
                          "from rosparam: %s\n"
                          "Verify robot state and connectivity "
                          "(i.e. ROS_MASTER_URI)", param_name)
            return False
        else:
            # parse out first 3 digits of robot version tag
            pattern = ("^([0-9]+)\.([0-9]+)\.([0-9]+)")
            match = re.search(pattern, robot_version)
            if not match:
                rospy.logwarn("RobotEnable: Invalid robot version: %s",
                              robot_version)
                return False
            robot_version = match.string[match.start(1):match.end(3)]
            if robot_version not in settings.VERSIONS_SDK2ROBOT[sdk_version]:
                errstr_version = """RobotEnable: Software Version Mismatch.
Robot Software version (%s) does not match local SDK version (%s). Please
Update your Robot Software. \
See: http://sdk.rethinkrobotics.com/intera/Software_Update"""
                rospy.logerr(errstr_version, robot_version, sdk_version)
                return False
        return True

def publish_command(self, op, args, timeout=2.0):
        """
        publish on the command topic
        return true if the command is acknowleged within the timeout
        """
        cmd_time = rospy.Time.now()
        self.cmd_times.append(cmd_time)
        self.cmd_times = self.cmd_times[-100:]
        cmd_msg = IOComponentCommand(
            time=cmd_time,
            op=op,
            args=json.dumps(args))
        rospy.logdebug("publish_command %s %s" % (cmd_msg.op, cmd_msg.args))
        if timeout != None:
            timeout_time = rospy.Time.now() + rospy.Duration(timeout)
            while not rospy.is_shutdown():
                self._command_pub.publish(cmd_msg)
                if self.is_state_valid():
                    if cmd_time in self.state.commands:
                        rospy.logdebug("command %s acknowleged" % (cmd_msg.op,))
                        return True
                rospy.sleep(0.1)
                if timeout_time < rospy.Time.now():
                    rospy.logwarn("Timed out waiting for command acknowlegment...")
                    break
            return False
        return True

def preempt_callback(self):
        rospy.logwarn('\033[94m[%s]\033[0m rendering preempted.'%rospy.get_name())
        for k in self.is_rendering.keys():
            if self.is_rendering[k]:
                self.render_client[k].cancel_all_goals()

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

def handle_multi_range_message(self, multi_range_msg):
        """Handle a ROS multi-range message by updating and publishing the state.

        Args:
             multi_range_msg (uwb.msg.UWBMultiRangeWithOffsets): ROS multi-range message.
        """
        # Update tracker position based on time-of-flight measurements
        new_estimate = self.update_estimate(multi_range_msg)
        if new_estimate is None:
            rospy.logwarn('Could not compute initial estimate: address={}, remote_address={}'.format(
                multi_range_msg.address, multi_range_msg.remote_address))
        else:
            # Publish tracker message
            ros_msg = uwb.msg.UWBTracker()
            ros_msg.header.stamp = rospy.get_rostime()
            ros_msg.address = multi_range_msg.address
            ros_msg.remote_address = multi_range_msg.remote_address
            ros_msg.state = new_estimate.state
            ros_msg.covariance = np.ravel(new_estimate.covariance)
            self.uwb_pub.publish(ros_msg)

            # Publish target transform (rotation is identity)
            self.tf_broadcaster.sendTransform(
                (new_estimate.state[0], new_estimate.state[1], new_estimate.state[2]),
                tf.transformations.quaternion_from_euler(0, 0, 0),
                rospy.get_rostime(),
                self.target_frame,
                self.tracker_frame
            )

def main():
    import signal

    rospy.init_node('uwb_tracker_node')
    u = UWBTracker()

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

def new_goal_callback(self, goal_pose):
        if not self.is_working:
            self.is_working = True
            new_goal = State.from_pose(goal_pose.pose)
            if self.map is not None and self.map.is_allowed(new_goal, self.robot):
                self.goal = new_goal
                rospy.loginfo("New goal was set")
                if self.ready_to_plan():
                    self.replan()
            else:
                rospy.logwarn("New goal is bad or no map available")

            self.is_working = False

def new_start_callback(self, start_pose):
        if not self.is_working:
            self.is_working = True
            new_start = State.from_pose(start_pose.pose.pose)
            if self.map is not None and self.map.is_allowed(new_start, self.robot):
                self.start = new_start
                rospy.loginfo("New start was set")
                if self.ready_to_plan():
                    self.replan()
            else:
                rospy.logwarn("New start is bad or no map available")
            self.is_working = False

def handle_process(self, proc, err):
        """
        Takes a running subprocess.Popen object `proc`, rosdebugs everything it
        prints to stdout, roswarns everything it prints to stderr, and raises
        `err` if it fails
        """
        poll = select.poll()
        poll.register(proc.stdout)
        poll.register(proc.stderr)
        while proc.poll() is None and not rospy.is_shutdown():
            res = poll.poll(1)
            for fd, evt in res:
                if not (evt & select.POLLIN):
                    continue
                if fd == proc.stdout.fileno():
                    line = proc.stdout.readline().strip()
                    if line:
                        rospy.logdebug(line)
                elif fd == proc.stderr.fileno():
                    line = proc.stderr.readline().strip()
                    if line:
                        rospy.logwarn(line)
        if proc.poll():
            proc.terminate()
            proc.wait()
            raise RuntimeError("Process interrupted by ROS shutdown")
        if proc.returncode:
            raise err

def connect_serial():
    timeout_s = 2 / serial_rate_hz # serial port timeout is 2x loop rate
    baud_rate = rospy.get_param("~baud_rate", 115200)

    # Initialize the serial connection
    path = "/dev/serial/by-id"
    port = None
    close_serial()
    while port is None:
        try:
            if not os.path.exists(path):
              raise Exception("No serial device found on system in {}".format(path))

            ports = [port for port in os.listdir(path) if "arduino" in port.lower()]
            if len(ports) == 0:
              raise Exception("No arduino device found on system in {}".format(path))
            port = ports[0]
            serial_connection = serial.Serial(os.path.join(path, port),
                                              baud_rate,
                                              dsrdtr = True, # Causes an Arduino soft reset
                                              timeout = timeout_s,
                                              writeTimeout = timeout_s)
            return serial_connection
        except Exception as e:
            rospy.logwarn(e)
            rospy.sleep(0.2) #seconds

def connect(self):
        if self.pseudo:
            rospy.loginfo('Connected to pseudo sensor')
            return
        try:
            self.serial = serial.Serial(self.serial_port, 19200, timeout=1)
            rospy.logdebug("self.serial.isOpen() = {}".format(self.serial.isOpen()))
            if not self.sensor_is_connected:
                self.sensor_is_connected = True
                rospy.loginfo('Connected to sensor')
        except:
            if self.sensor_is_connected:
                self.sensor_is_connected = False
                rospy.logwarn('Unable to connect to sensor')

def refresh(self):
        try:
            self.event_queue = pygame.event.get()
            self.screen.fill(self.black)
            self.blitSensorValues()
            self.blitDesiredUI()
            pygame.display.update()
        except Exception as e:
            rospy.logwarn("Refresh crashed. Error: {}".format(e))

def connect(self):
        try:
            if self.device_id is None:
                # Try to find pH device id automatically
                devices = list_devices.get_ftdi_device_list()

                # Check if any atlas devices are connected to device
                if len(devices) == 0:
                  raise IOError("No atlas device found on system")

                for device in devices:
                    # Extract id
                    device_id = (device.split("FTDI:FT230X Basic UART:"))[1]

                    # Check if device is ph sensor
                    with AtlasDevice(device_id) as atlas_device:
                        atlas_device.send_cmd("i") # get device information
                        time.sleep(1)
                        lines = atlas_device.read_lines()
                        if len(lines) == 0:
                            continue
                        if "EC" in lines[0]:
                            self.device_id = device_id
                            rospy.loginfo("Automatically found device id: {}".format(device_id))

            self.device = AtlasDevice(self.device_id)
            self.device.send_cmd("C,0") # turn off continuous mode
            time.sleep(1)
            self.device.flush()
            rospy.loginfo("Connected to AtlasEc")

        except Exception as e:
            rospy.logwarn("Failed to connect to AtlasEc. Error: {}".format(e))
            pass