python类logerr()的实例源码

def __init__(self, name):
        rospy.init_node(name)

        self.start_rotation = None

        self.WIDTH = 320
        self.HEIGHT = 240

        self.set_x_range(-30.0, 30.0)
        self.set_y_range(-30.0, 30.0)
        self.set_z_range(-30.0, 30.0)

        self.completed_image = None

        if not rospy.has_param('/ihmc_ros/robot_name'):
            rospy.logerr("Cannot run team_zarj_main.py, missing parameters!")
            rospy.logerr("Missing parameter '/ihmc_ros/robot_name'")

        else:
            self.zarj = ZarjOS()

def markerCB(self, msg):
        try:
            self.marker_lock.acquire()
            if not self.initialize_poses:
                return
            self.initial_poses = {}
            for marker in msg.markers:
                if marker.name.startswith("EE:goal_"):
                    # resolve tf
                    if marker.header.frame_id != self.frame_id:
                        ps = PoseStamped(header=marker.header, pose=marker.pose)
                        try:
                            transformed_pose = self.tf_listener.transformPose(self.frame_id, ps)
                            self.initial_poses[marker.name[3:]] = transformed_pose.pose
                        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, e):
                            rospy.logerr("tf error when resolving tf: %s" % e)
                    else:
                        self.initial_poses[marker.name[3:]] = marker.pose   #tf should be resolved
        finally:
            self.marker_lock.release()

def init_traj(self):
        try:
            # self.recorder.init_traj(itr)
            if self.use_goalimage:
                goal_img_main, goal_state = self.load_goalimage()
                goal_img_aux1 = np.zeros([64, 64, 3])
            else:
                goal_img_main = np.zeros([64, 64, 3])
                goal_img_aux1 = np.zeros([64, 64, 3])

            goal_img_main = self.bridge.cv2_to_imgmsg(goal_img_main)
            goal_img_aux1 = self.bridge.cv2_to_imgmsg(goal_img_aux1)

            rospy.wait_for_service('init_traj_visualmpc', timeout=1)
            self.init_traj_visual_func(0, 0, goal_img_main, goal_img_aux1, self.save_subdir)

        except (rospy.ServiceException, rospy.ROSException), e:
            rospy.logerr("Service call failed: %s" % (e,))
            raise ValueError('get_kinectdata service failed')

def get_interpolated_joint_angles(self):
        int_des_pos = self.calc_interpolation(self.previous_des_pos, self.des_pos, self.t_prev, self.t_next)
        # print 'interpolated: ', int_des_pos
        desired_pose = inverse_kinematics.get_pose_stamped(int_des_pos[0],
                                                           int_des_pos[1],
                                                           int_des_pos[2],
                                                           inverse_kinematics.EXAMPLE_O)
        start_joints = self.ctrl.limb.joint_angles()
        try:
            des_joint_angles = inverse_kinematics.get_joint_angles(desired_pose, seed_cmd=start_joints,
                                                                   use_advanced_options=True)
        except ValueError:
            rospy.logerr('no inverse kinematics solution found, '
                         'going to reset robot...')
            current_joints = self.ctrl.limb.joint_angles()
            self.ctrl.limb.set_joint_positions(current_joints)
            raise Traj_aborted_except('raising Traj_aborted_except')

        return des_joint_angles

def attach_springs(self):
        """
        Switches to joint torque mode and attached joint springs to current
        joint positions.
        """
        # record initial joint angles
        self._des_angles = self._limb.joint_angles()

        # set control rate
        control_rate = rospy.Rate(self._rate)

        # for safety purposes, set the control rate command timeout.
        # if the specified number of command cycles are missed, the robot
        # will timeout and disable
        self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds)

        # loop at specified rate commanding new joint torques
        while not rospy.is_shutdown():
            if not self._rs.state().enabled:
                rospy.logerr("Joint torque example failed to meet "
                             "specified control rate timeout.")
                break
            self._update_forces()
            control_rate.sleep()

def save(self, i_save, action, endeffector_pose):
        self.t_savereq = rospy.get_time()
        assert self.instance_type == 'main'

        if self.use_aux:
            # request save at auxiliary recorders
            try:
                rospy.wait_for_service('get_kinectdata', 0.1)
                resp1 = self.save_kinectdata_func(i_save)
            except (rospy.ServiceException, rospy.ROSException), e:
                rospy.logerr("Service call failed: %s" % (e,))
                raise ValueError('get_kinectdata service failed')

        if self.save_images:
            self._save_img_local(i_save)

        if self.save_actions:
            self._save_state_actions(i_save, action, endeffector_pose)

        if self.save_gif:
            highres = cv2.cvtColor(self.ltob.img_cv2, cv2.COLOR_BGR2RGB)
            print 'highres dim',highres.shape
            self.highres_imglist.append(highres)

def ik_quaternion(self, quaternion_pose):
        node = ("ExternalTools/{}/PositionKinematicsNode/"
                "IKService".format(self.limb_name))
        ik_service = rospy.ServiceProxy(node, SolvePositionIK)
        ik_request = SolvePositionIKRequest()
        ik_request.pose_stamp.append(quaternion_pose)
        try:
            rospy.loginfo('ik: waiting for IK service...')
            rospy.wait_for_service(node, 5.0)
            ik_response = ik_service(ik_request)
        except (rospy.ServiceException, rospy.ROSException) as err:
            rospy.logerr("ik_move: service request failed: %r" % err)
        else:
            if ik_response.isValid[0]:
                rospy.loginfo("ik_move: valid joint configuration found")
                limb_joints = dict(zip(ik_response.joints[0].name,
                                       ik_response.joints[0].position))
                return limb_joints
            else:
                rospy.logerr('ik_move: no valid configuration found')
                return None

def attach_springs(self):
        self._start_angles = self._get_cmd_positions()
        control_rate = rospy.Rate(self._rate)

        # for safety purposes, set the control rate command timeout.
        # if the specified number of command cycles are missed, the robot
        # will timeout and disable
        self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds)

        error = [self._limb.joint_angles()[key] - self._start_angles[key] for key in self._limb.joint_angles().keys()]
        self.sum_sqr_error = sum([error[i]**2 for i in range(len(error))])
        print ('-------new set of joint position---------')
        print (self.sum_sqr_error)
        tolerance = 0.020
        # loop at specified rate commanding new joint torques
        while self.sum_sqr_error>tolerance and not rospy.is_shutdown():

            if not self._rs.state().enabled:
                    rospy.logerr("Joint torque example failed to meet "
                            "specified control rate timeout.")
                    break
            self._update_forces()
            control_rate.sleep()

def __init__(self):
        token = rospy.get_param('/telegram/token', None)
        if token is None:
            rospy.logerr("No token found in /telegram/token param server.")
            exit(0)
        else:
            rospy.loginfo("Got telegram bot token from param server.")

        # Set CvBridge
        self.bridge = CvBridge()

        # Create the EventHandler and pass it your bot's token.
        updater = Updater(token)

        # Get the dispatcher to register handlers
        dp = updater.dispatcher

        # on noncommand i.e message - echo the message on Telegram
        dp.add_handler(MessageHandler(Filters.text, self.pub_received))

        # log all errors
        dp.add_error_handler(self.error)

        # Start the Bot
        updater.start_polling()

def __init__(self):
        self.joy_pub = rospy.Publisher('sensors/joystick/1', Joy, queue_size=1)
        self.joy_pub2 = rospy.Publisher('sensors/joystick/2', Joy, queue_size=1)

        self.rospack = RosPack()

        self.rate = rospy.Rate(20)

        count = len(devices.gamepads)

        if count < 2:
            rospy.logerr("Sensors: Expecting 2 joysticks but found only {}, exiting".format(count))
            sys.exit(-1)

        gamepads = sorted(devices.gamepads, key=lambda pad: int(str(pad.name)[-1]))
        rospy.loginfo(gamepads)
        self.joysticks = [JoystickThread(pad) for pad in gamepads]
        [joystick.start() for joystick in self.joysticks]

def _read(self, max_attempts=600):
        got_image = False
        if self._camera is not None and self._camera.isOpened():
            got_image, image = self._camera.read()

        if not got_image:
            if not self._error:
                if max_attempts > 0:
                    rospy.sleep(0.1)
                    self._open()
                    return self._read(max_attempts-1)
                rospy.logerr("Reached maximum camera reconnection attempts, abandoning!")
                self._error = True
                return False, None
            return False, None
        return True, image

def _init_pubsub(self):
        self.joint_states_pub = rospy.Publisher('joint_states', JointState)
        self.odom_pub = rospy.Publisher('odom', Odometry)

        self.sensor_state_pub = rospy.Publisher('~sensor_state', TurtlebotSensorState)
        self.operating_mode_srv = rospy.Service('~set_operation_mode', SetTurtlebotMode, self.set_operation_mode)
        self.digital_output_srv = rospy.Service('~set_digital_outputs', SetDigitalOutputs, self.set_digital_outputs)

        if self.drive_mode == 'twist':
            self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Twist, self.cmd_vel)
            self.drive_cmd = self.robot.direct_drive
        elif self.drive_mode == 'drive':
            self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Drive, self.cmd_vel)
            self.drive_cmd = self.robot.drive
        elif self.drive_mode == 'turtle':
            self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Turtle, self.cmd_vel)
            self.drive_cmd = self.robot.direct_drive
        else:
            rospy.logerr("unknown drive mode :%s"%(self.drive_mode))

        self.transform_broadcaster = None
        if self.publish_tf:
            self.transform_broadcaster = tf.TransformBroadcaster()

def set_operation_mode(self,req):
        if not self.robot.sci:
            raise Exception("Robot not connected, SCI not available")

        self.operate_mode = req.mode

        if req.mode == 1: #passive
            self._robot_run_passive()
        elif req.mode == 2: #safe
            self._robot_run_safe()
        elif req.mode == 3: #full
            self._robot_run_full()
        else:
            rospy.logerr("Requested an invalid mode.")
            return SetTurtlebotModeResponse(False)
        return SetTurtlebotModeResponse(True)

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 delete_object(self, req):
        """Handles DeleteObject service requests.

        Args:
            req: DeleteObjectRequest message.

        Returns:
            DeleteObjectResponse.
        """
        response = interop.srv.DeleteObjectResponse()

        try:
            self.objects_dir.delete_object(req.id)
        except (KeyError, OSError) as e:
            rospy.logerr("Could not delete object: {}".format(e))
            response.success = False
        except Exception as e:
            rospy.logfatal(e)
            response.success = False
        else:
            response.success = True

        return response

def delete_object_image(self, req):
        """Handles DeleteObjectImage service requests.

        Args:
            req: DeleteObjectImageRequest message.

        Returns:
            DeleteObjectImageResponse.
        """
        response = interop.srv.DeleteObjectImageResponse()

        try:
            self.objects_dir.delete_object_image(req.id)
        except (KeyError, IOError) as e:
            rospy.logerr("Could not delete object image: {}".format(e))
            response.success = False
        except Exception as e:
            rospy.logfatal(e)
            response.success = False
        else:
            response.success = True

        return response

def symlink_objects_path_to_latest(objects_path):
    """Symlinks 'latest' to the given directory.

    Args:
        objects_path: Path to objects directory.
    """
    objects_root = os.path.dirname(objects_path)
    path_to_symlink = os.path.join(objects_root, "latest")

    try:
        os.symlink(objects_path, path_to_symlink)
    except OSError as e:
        # Replace the old symlink if an old symlink with the same
        # name exists.
        if e.errno == errno.EEXIST:
            os.remove(path_to_symlink)
            os.symlink(objects_path, path_to_symlink)
        else:
            rospy.logerr(
                "Could not create symlink to the latest objects directory")

def callback_cmd_vel(message):
    lfile = '/dev/rtmotor_raw_l0'
    rfile = '/dev/rtmotor_raw_r0'

    #for forwarding
    forward_hz = 80000.0*message.linear.x/(9*math.pi)
    #for rotation
    rot_hz = 400.0*message.angular.z/math.pi
    try:
        lf = open(lfile,'w')
        rf = open(rfile,'w')
        lf.write(str(int(round(forward_hz - rot_hz))) + '\n')
        rf.write(str(int(round(forward_hz + rot_hz))) + '\n')
    except:
        rospy.logerr("cannot write to rtmotor_raw_*")

    lf.close()
    rf.close()

def __init__(self, limb):
        ns = 'robot/limb/' + limb + '/'
        self._client = actionlib.SimpleActionClient(
            ns + "follow_joint_trajectory",
            FollowJointTrajectoryAction,
        )
        self._goal = FollowJointTrajectoryGoal()
        self._goal_time_tolerance = rospy.Time(0.1)
        self._goal.goal_time_tolerance = self._goal_time_tolerance
        server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0))
        if not server_up:
            rospy.logerr("Timed out waiting for Joint Trajectory"
                         " Action Server to connect. Start the action server"
                         " before running example.")
            rospy.signal_shutdown("Timed out waiting for Action Server")
            sys.exit(1)
        self.clear(limb)