Python rospy.loginfo方法代码示例

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import loginfo [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 loginfo [as 别名]
def spinOnce(self):
    #############################################################
    
        # dx = (l + r) / 2
        # dr = (r - l) / w
            
        self.right = 1.0 * self.dx + self.dr * self.w / 2 
        self.left = 1.0 * self.dx - self.dr * self.w / 2
        # rospy.loginfo("publishing: (%d, %d)", left, right) 
                
        self.pub_lmotor.publish(self.left)
        self.pub_rmotor.publish(self.right)
            
        self.ticks_since_target += 1

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

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import loginfo [as 别名]
def start(self, node_name='polly_node', service_name='polly'):
        """The entry point of a ROS service node.

        Details of the service API can be found in Polly.srv.

        :param node_name: name of ROS node
        :param service_name:  name of ROS service
        :return: it doesn't return
        """
        rospy.init_node(node_name)

        service = rospy.Service(service_name, Polly, self._node_request_handler)

        rospy.loginfo('polly running: {}'.format(service.uri))

        rospy.spin() 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import loginfo [as 别名]
def __init__(self):
    #############################################################
        rospy.init_node("twist_to_motors")
        nodename = rospy.get_name()
        rospy.loginfo("%s started" % nodename)
    
        self.w = rospy.get_param("~base_width", 0.2)
    
        self.pub_lmotor = rospy.Publisher('lwheel_vtarget', Float32, queue_size=10)
        self.pub_rmotor = rospy.Publisher('rwheel_vtarget', Float32, queue_size=10)
        rospy.Subscriber('twist', Twist, self.twistCallback)
    
    
        self.rate = rospy.get_param("~rate", 50)
        self.timeout_ticks = rospy.get_param("~timeout_ticks", 2)
        self.left = 0
        self.right = 0
        
    ############################################################# 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import loginfo [as 别名]
def initUI(self):      
    #####################################################################    
        
        img_path = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) + "/../images/crosshair.jpg"
        rospy.loginfo("initUI img_path: %s" % img_path)

        self.statusBar()
        
        self.setStyleSheet("QMainWindow { border-image: url(%s); }" % img_path)
        
                
        self.setGeometry(0, 600, 200, 200)
        self.setWindowTitle('Virtual Joystick')
        self.show()
        self.timer = QtCore.QBasicTimer()
        
        self.statusBar().showMessage('started')
        
    ##################################################################### 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import loginfo [as 别名]
def pubTwist(self):
    #######################################################
        # rospy.loginfo("publishing twist from (%0.3f,%0.3f)" %(self.x,self.y))
        self.twist = Twist()
        self.twist.linear.x = (1-self.y) * (x_max - x_min) + x_min
        self.twist.linear.y = 0
        self.twist.linear.z = 0
        self.twist.angular.x = 0
        self.twist.angular.y = 0
        self.twist.angular.z = (1-self.x) * (r_max - r_min) + r_min
        
        if self.twist.linear.x > x_max:
            self.twist.linear.x = x_max
        if self.twist.linear.x < x_min:
            self.twist.linear.x = x_min
        if self.twist.angular.z > r_max:
            self.twist.angular.z = r_max
        if self.twist.angular.z < r_min:
            self.twist.angular.z = r_min
        
        self.pub_twist.publish( self.twist )
        
##########################################################################
########################################################################## 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import loginfo [as 别名]
def __init__(self):
    ###############################################
        rospy.init_node("wheel_loopback");
        self.nodename = rospy.get_name()
        rospy.loginfo("%s started" % self.nodename)
        
        self.rate = rospy.get_param("~rate", 200)
        self.timeout_secs = rospy.get_param("~timeout_secs", 0.5)
        self.ticks_meter = float(rospy.get_param('~ticks_meter', 50))
        self.velocity_scale = float(rospy.get_param('~velocity_scale', 255))
        self.latest_motor = 0
        
        self.wheel = 0
        
        rospy.Subscriber('motor', Int16, self.motor_callback)
        
        self.pub_wheel = rospy.Publisher('wheel', Int16, queue_size=10)
        
    ############################################### 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import loginfo [as 别名]
def spin(self):
    ###############################################
        r = rospy.Rate
        self.secs_since_target = self.timeout_secs
        self.then = rospy.Time.now()
        self.latest_msg_time = rospy.Time.now()
        rospy.loginfo("-D- spinning")
        
        ###### main loop #########
        while not rospy.is_shutdown():
            while not rospy.is_shutdown() and self.secs_since_target < self.timeout_secs:
                self.spinOnce()
                r.sleep
                self.secs_since_target = rospy.Time.now() - self.latest_msg_time
                self.secs_since_target = self.secs_since_target.to_sec()
                #rospy.loginfo("  inside: secs_since_target: %0.3f" % self.secs_since_target)
                
            # it's been more than timeout_ticks since we recieved a message
            self.secs_since_target = rospy.Time.now() - self.latest_msg_time
            self.secs_since_target = self.secs_since_target.to_sec()
            # rospy.loginfo("  outside: secs_since_target: %0.3f" % self.secs_since_target)
            self.velocity = 0
            r.sleep
        
    ############################################### 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import loginfo [as 别名]
def scaleWheel():
    rospy.init_node("wheel_scaler")
    rospy.loginfo("wheel_scaler started")
    
    scale = rospy.get_param('distance_scale', 1)
    rospy.loginfo("wheel_scaler scale: %0.2f", scale)
    
    rospy.Subscriber("lwheel", Int16, lwheelCallback)
    rospy.Subscriber("rwheel", Int16, rwheelCallback)
    
    lscaled_pub = rospy.Publisher("lwheel_scaled", Int16, queue_size=10)
    rscaled_pub = rospy.Publisher("rwheel_scaled", Int16, queue_size=10) 
    
    ### testing sleep CPU usage
    r = rospy.Rate(50) 
    while not rospy.is_shutdown:
        r.sleep()
        
    rospy.spin() 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import loginfo [as 别名]
def vel_ctrl(self):
        self.lin_vel = self.target_vel.linear.x - self.usv_vel.twist.twist.linear.x
        self.lin_vel = self.lin_vel * self.kp_lin + self.I_lin(self.lin_vel)

        #if self.target_vel.angular.z != self.target_vel_ant.angular.z:
        #    self.I_ant_ang = 0

        if self.target_vel.angular.z == 0:
            self.vel_left = self.lin_vel
            self.vel_right = self.lin_vel
        else: 
            self.ang_vel = self.target_vel.angular.z - self.usv_vel.twist.twist.angular.z
            self.erro = self.ang_vel
            self.ang_vel = self.ang_vel * self.kp_ang + self.I_ang(self.ang_vel)
            self.vel_left = self.lin_vel - self.ang_vel
            self.vel_right = self.lin_vel + self.ang_vel
                   
        self.vel_left = self.sat_thruster(self.vel_left)
        self.vel_right = self.sat_thruster(self.vel_right)
        msg = "atual: {0}; desejada: {1}; erro: {2}; vel_left: {3}; vel_right: {4}; erro_bruto: {5}" .format(self.usv_vel.twist.twist.angular.z, self.target_vel.angular.z, self.ang_vel, self.vel_left, self.vel_right, self.erro)
        rospy.loginfo(msg)

        return [self.vel_left, self.vel_right] 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import loginfo [as 别名]
def loadMap():
	global array, width, height, originX, originY, mymap, minSpeed, maxSpeed
	mynorm = matplotlib.colors.Normalize(vmin=-0.1,vmax=0.1);

	mymap.header.frame_id="odom";
	mymap.info.resolution = resolution;
	mymap.info.width = width;
	mymap.info.height = height;
	mymap.info.origin.position.x = originX;
	mymap.info.origin.position.y = originY;
	mymap.info.origin.position.z = 0;
	mymap.info.origin.orientation.x = 0;
	mymap.info.origin.orientation.y = 0;
	mymap.info.origin.orientation.z = 0;
	mymap.info.origin.orientation.w = 1;
	rospy.loginfo ("################### Preparing map. Size (%d, %d) origin(%d, %d)", width, height, originX, originY)
	for x in xrange(height-1, -1, -1):
		for y in range(0, width):
			mymap.data.append(0);			
	rospy.loginfo("#################### Map loaded!") 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import loginfo [as 别名]
def _call_engine(self, **kw):
        """Call engine to do the job.

        If no output path is found from input, the audio file will be put into /tmp and the file name will have
        a prefix of the md5 hash of the text.

        :param kw: what AmazonPolly needs to synthesize
        :return: response from AmazonPolly
        """
        if 'output_path' not in kw:
            tmp_filename = hashlib.md5(kw['text']).hexdigest()
            tmp_filepath = os.path.join(os.sep, 'tmp', 'voice_{}_{}'.format(tmp_filename, str(time.time())))
            kw['output_path'] = os.path.abspath(tmp_filepath)
        rospy.loginfo('audio will be saved as {}'.format(kw['output_path']))

        return self.engine(**kw) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import loginfo [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 SynthesizerResponse with the exception.

        :param request: an instance of SynthesizerRequest
        :return: a SynthesizerResponse
        """
        rospy.loginfo(request)
        try:
            kws = self._parse_request_or_raise(request)
            res = self._call_engine(**kws).result

            return SynthesizerResponse(res)
        except Exception as e:
            return SynthesizerResponse('Exception: {}'.format(e)) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import loginfo [as 别名]
def __init__(self):
        rospy.loginfo("Tianbot Racecar JoyTeleop Initializing...")
        self._twist = Twist()
        self._twist.linear.x = 1500
        self._twist.angular.z = 90
        self._deadman_pressed = False
        self._zero_twist_published = False

        self._cmd_vel_pub = rospy.Publisher('~/car/cmd_vel', Twist, queue_size=5)
        self._joy_sub = rospy.Subscriber('/joy', Joy, self.joy_callback)
        self._timer = rospy.Timer(rospy.Duration(0.05), self.joystick_controller)
        self._axis_throttle = 1

        _joy_mode = rospy.get_param("~joy_mode", "D").lower()
        if _joy_mode == "d":
            self._axis_servo = 2
        if _joy_mode == "x":
            self._axis_servo = 3

        self._throttle_scale = rospy.get_param("~throttle_scale", 0.5)
        self._servo_scale = rospy.get_param("~servo_scale", 1) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import loginfo [as 别名]
def __init__(self, goalListX, goalListY, retry, map_frame):
        self.sub = rospy.Subscriber('move_base/result', MoveBaseActionResult, self.statusCB, queue_size=10)
        self.pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10)   
        # params & variables
        self.goalListX = goalListX
        self.goalListY = goalListY
        self.retry = retry
        self.goalId = 0
        self.goalMsg = PoseStamped()
        self.goalMsg.header.frame_id = map_frame
        self.goalMsg.pose.orientation.z = 0.0
        self.goalMsg.pose.orientation.w = 1.0
        # Publish the first goal
        time.sleep(1)
        self.goalMsg.header.stamp = rospy.Time.now()
        self.goalMsg.pose.position.x = self.goalListX[self.goalId]
        self.goalMsg.pose.position.y = self.goalListY[self.goalId]
        self.pub.publish(self.goalMsg) 
        rospy.loginfo("Initial goal published! Goal ID is: %d", self.goalId) 
        self.goalId = self.goalId + 1