Python rospy.logerr方法代码示例

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logerr [as 别名]
def callback_image(data):
    # et = time.time()
    try:
        cv_image = cv_bridge.imgmsg_to_cv2(data, "bgr8")
    except CvBridgeError as e:
        rospy.logerr('[tf-pose-estimation] Converting Image Error. ' + str(e))
        return

    acquired = tf_lock.acquire(False)
    if not acquired:
        return

    try:
        global scales
        humans = pose_estimator.inference(cv_image, scales)
    finally:
        tf_lock.release()

    msg = humans_to_msg(humans)
    msg.image_w = data.width
    msg.image_h = data.height
    msg.header = data.header

    pub_pose.publish(msg)
    # rospy.loginfo(time.time() - et) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logerr [as 别名]
def talker_ctrl():
    global rate_value
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:  
            pub_motor.publish(thruster_ctrl_msg())
            pub_rudder.publish(rudder_ctrl_msg())
            pub_result.publish(verify_result())
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logerr [as 别名]
def talker_ctrl():
    global rate_value
    global result
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:    
            pub_motor.publish(thruster_ctrl_msg())
            pub_result.publish(verify_result())
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logerr [as 别名]
def get_endeffector_pos(self):
        fkreq = SolvePositionFKRequest()
        joints = JointState()
        joints.name = self._limb_right.joint_names()
        joints.position = [self._limb_right.joint_angle(j)
                           for j in joints.name]

        # Add desired pose for forward kinematics
        fkreq.configuration.append(joints)
        fkreq.tip_names.append('right_hand')
        try:
            rospy.wait_for_service(self.name_of_service, 5)
            resp = self.fksvc(fkreq)
        except (rospy.ServiceException, rospy.ROSException), e:
            rospy.logerr("Service call failed: %s" % (e,))
            return False 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logerr [as 别名]
def convert_pose(pose, from_frame, to_frame):
    """
    Convert a pose or transform between frames using tf.
        pose            -> A geometry_msgs.msg/Pose that defines the robots position and orientation in a reference_frame
        from_frame      -> A string that defines the original reference_frame of the robot
        to_frame        -> A string that defines the desired reference_frame of the robot to convert to
    """
    global tfBuffer, listener

    # Create Listener objet to recieve and buffer transformations
    trans = None

    try:
        trans = tfBuffer.lookup_transform(to_frame, from_frame, rospy.Time(0), rospy.Duration(1.0))
    except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException), e:
        print(e)
        rospy.logerr('FAILED TO GET TRANSFORM FROM %s to %s' % (to_frame, from_frame))
        return None 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logerr [as 别名]
def clamp_float_warn(low, val, upp, name):
    """
    Clamps: low <= val <= upp
    Prints: warning if clamped, error if input is not a float
    @param low: lower bound for the input  (float)
    @param val: input (float ?)
    @param upp: upper bound for the input  (float)
    @param name: input name (str)
    @return: clamp(low,val,upp) if input is a float, None otherwise.
    """
    if not isinstance(val, float):
        rospy.logerr('Invalid input type: ' + name + ' must be a float!')
        return None
    if val < low:
        rospy.logwarn(''.join(['Clamping ' + name, ' (' + str(val) + ')',
                               ' to the lower bound: ',  str(low)]))
        return low
    if val > upp:
        rospy.logwarn(''.join(['Clamping ' + name, ' (' + str(val) + ')',
                               ' to the upper bound: ',  str(upp)]))
        return upp
    return val 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logerr [as 别名]
def set_interaction_frame(self, interaction_frame = default_interaction_frame):
        """
        @param interaction_frame:
          - None:  populate with vector of default values
          - Pose: copy all the elements
        """
        if not isinstance(interaction_frame, Pose):
            rospy.logerr('interaction_frame must be of type geometry_msgs.Pose')
            return

        # check for unit quaternion
        quat_sum_square = (interaction_frame.orientation.w * interaction_frame.orientation.w
                          + interaction_frame.orientation.x * interaction_frame.orientation.x
                          + interaction_frame.orientation.y * interaction_frame.orientation.y
                          + interaction_frame.orientation.z * interaction_frame.orientation.z)
        if quat_sum_square  > 1.0 + 1e-7 or quat_sum_square < 1.0 - 1e-7:
            rospy.logerr('Invalid input to quaternion! The quaternion must be a unit quaternion!')
            return

        self._data.interaction_frame = interaction_frame 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logerr [as 别名]
def set_interaction_control_mode(self, interaction_mode = default_interaction_control_mode):
        """
        @param interaction_mode:
          - None:  set impedance mode by default
          - mode:  set each direction by the input mode
            (1: impedance, 2: force, 3: impedance with force limit, 4: force with motion limit)
          - [mode]:  copy all elements. Checks length.
        """
        # first check for valid modes
        for i in range(len(interaction_mode)):
            if (interaction_mode[i] < self.impedance_mode
                or interaction_mode[i] > self.force_limit_mode):
                rospy.logerr('Interaction mode option %d is invalid', interaction_mode[i])
                return

        if len(interaction_mode) == 1:
            self._data.interaction_control_mode = [interaction_mode[0]]*self.n_dim_cart
        elif len(interaction_mode) == self.n_dim_cart:
            self._data.interaction_control_mode = deepcopy(interaction_mode)
        else:
            rospy.logerr('The number of interaction_control_mode elements must be 1 or %d',
                         self.n_dim_cart) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logerr [as 别名]
def send_trajectory(self, wait_for_result=True, timeout=None):
        """
        Checks the trajectory message is complete.
        The message then will be packaged up with the MOTION_START
        command and sent to the motion controller.
        @param wait_for_result:
          - If true, the function will not return until the trajectory is finished
          - If false, return True immediately after sending
        @param timeout: maximum time to wait for trajectory result.
          - If timeout is reached, return None.
        @return: True if the goal finished
        @rtype: bool
        """
        if not self._traj.waypoints:
            rospy.logerr("Trajectory is empty! Cannot send.")
            return None
        self._check_options()
        self._client.send_trajectory(self.to_msg())
        return self._client.wait_for_result(timeout) if wait_for_result else True 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logerr [as 别名]
def talker_ctrl():
    global rate_value
    global currentHeading
    global windDir 
    global isTacking
    global heeling
    global spHeading

    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    # publishes to thruster and rudder topics
    #pub_sail = rospy.Publisher('angleLimits', Float64, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    pub_heading = rospy.Publisher('currentHeading', Float64, queue_size=10)
    pub_windDir = rospy.Publisher('windDirection', Float64, queue_size=10)
    pub_heeling = rospy.Publisher('heeling', Float64, queue_size=10)
    pub_spHeading = rospy.Publisher('spHeading', Float64, queue_size=10)
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:
            pub_rudder.publish(rudder_ctrl_msg())
            #pub_sail.publish(sail_ctrl())
            pub_result.publish(verify_result())
            pub_heading.publish(currentHeading)
            pub_windDir.publish(windDir)
            pub_heeling.publish(heeling)
            pub_spHeading.publish(spHeading)
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logerr [as 别名]
def talker_ctrl():
    global rate_value
    global currentHeading
    global windDir 
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    # publishes to thruster and rudder topics
    #pub_sail = rospy.Publisher('angleLimits', Float64, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    pub_heading= rospy.Publisher('currentHeading', Float64, queue_size=10)
    pub_windDir= rospy.Publisher('windDirection', Float64, queue_size=10)

    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:
            pub_rudder.publish(rudder_ctrl_msg())
            #pub_sail.publish(sail_ctrl())
            pub_result.publish(verify_result())
            pub_heading.publish(currentHeading)
            pub_windDir.publish(windDir)
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logerr [as 别名]
def _get_polly_client(self, aws_access_key_id=None, aws_secret_access_key=None, aws_session_token=None,
                          region_name=None, with_service_model_patch=False):
        """Note we get a new botocore session each time this function is called.
        This is to avoid potential problems caused by inner state of the session.
        """
        botocore_session = get_session()

        if with_service_model_patch:
            # Older versions of botocore don't have polly. We can possibly fix it by appending
            # extra path with polly service model files to the search path.
            current_dir = os.path.dirname(os.path.abspath(__file__))
            service_model_path = os.path.join(current_dir, 'data', 'models')
            botocore_session.set_config_variable('data_path', service_model_path)
            rospy.loginfo('patching service model data path: {}'.format(service_model_path))

        botocore_session.get_component('credential_provider').insert_after('boto-config', AwsIotCredentialProvider())

        botocore_session.user_agent_extra = self._generate_user_agent_suffix()

        session = Session(aws_access_key_id=aws_access_key_id, aws_secret_access_key=aws_secret_access_key,
                          aws_session_token=aws_session_token, region_name=region_name,
                          botocore_session=botocore_session)

        try:
            return session.client("polly")
        except UnknownServiceError:
            # the first time we reach here, we try to fix the problem
            if not with_service_model_patch:
                return self._get_polly_client(aws_access_key_id, aws_secret_access_key, aws_session_token, region_name,
                                              with_service_model_patch=True)
            else:
                # we have tried our best, time to panic
                rospy.logerr('Amazon Polly is not available. Please install the latest boto3.')
                raise 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logerr [as 别名]
def _node_request_handler(self, request):
        """The callback function for processing service request.

        It never raises. If anything unexpected happens, it will return a PollyResponse with details of the exception.

        :param request: an instance of PollyRequest
        :return: a PollyResponse
        """
        rospy.loginfo('Amazon Polly Request: {}'.format(request))

        try:
            response = self._dispatch(request)
            rospy.loginfo('will return {}'.format(response))
            return PollyResponse(result=response)
        except Exception as e:
            current_dir = os.path.dirname(os.path.abspath(__file__))
            exc_type = sys.exc_info()[0]

            # not using `issubclass(exc_type, ConnectionError)` for the condition below because some versions
            # of urllib3 raises exception when doing `from requests.exceptions import ConnectionError`
            error_ogg_filename = 'connerror.ogg' if 'ConnectionError' in exc_type.__name__ else 'error.ogg'

            error_details = {
                'Audio File': os.path.join(current_dir, 'data', error_ogg_filename),
                'Audio Type': 'ogg',
                'Exception': {
                    'Type': str(exc_type),
                    'Module': exc_type.__module__,
                    'Name': exc_type.__name__,
                    'Value': str(e),
                },
                'Traceback': traceback.format_exc()
            }

            error_str = json.dumps(error_details)
            rospy.logerr(error_str)
            return PollyResponse(result=error_str) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logerr [as 别名]
def do_speak(goal):
    """The action handler.

    Note that although it responds to client after the audio play is finished, a client can choose
    not to wait by not calling ``SimpleActionClient.waite_for_result()``.
    """
    rospy.loginfo('speech goal: {}'.format(goal))

    res = do_synthesize(goal)
    rospy.loginfo('synthesizer returns: {}'.format(res))

    try:
        r = json.loads(res.result)
    except Exception as e:
        s = 'Expecting JSON from synthesizer but got {}'.format(res.result)
        rospy.logerr('{}. Exception: {}'.format(s, e))
        finish_with_result(s)
        return

    result = ''

    if 'Audio File' in r:
        audio_file = r['Audio File']
        rospy.loginfo('Will play {}'.format(audio_file))
        play(audio_file)
        result = audio_file

    if 'Exception' in r:
        result = '[ERROR] {}'.format(r)
        rospy.logerr(result)

    finish_with_result(result) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logerr [as 别名]
def callback_image(self, data):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            rospy.logerr('Converting Image Error. ' + str(e))
            return

        self.frames.append((data.header.stamp, cv_image))