Python rospy.logdebug方法代码示例

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logdebug [as 别名]
def wheelCallback(self, msg):
    ######################################################
        enc = msg.data
        if (enc < self.encoder_low_wrap and self.prev_encoder > self.encoder_high_wrap) :
            self.wheel_mult = self.wheel_mult + 1
            
        if (enc > self.encoder_high_wrap and self.prev_encoder < self.encoder_low_wrap) :
            self.wheel_mult = self.wheel_mult - 1
           
         
        self.wheel_latest = 1.0 * (enc + self.wheel_mult * (self.encoder_max - self.encoder_min)) / self.ticks_per_meter 
        self.prev_encoder = enc
        
        
#        rospy.logdebug("-D- %s wheelCallback msg.data= %0.3f wheel_latest = %0.3f mult=%0.3f" % (self.nodename, enc, self.wheel_latest, self.wheel_mult))
    
    ###################################################### 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logdebug [as 别名]
def _joy_cb(self, msg):
        """
        The joy/game-pad message callback.

        :type   msg:    Joy
        :param  msg:    The incoming joy message.

        """
        if msg.buttons[self._head_button] == 1:
            angle_deg = ((msg.axes[self._head_axes] + 1.0) / 2.0) * SUM_HEAD_ANGLE + MIN_HEAD_ANGLE
            rospy.logdebug('Send head angle: {} degrees.'.format(angle_deg))
            self._head_pub.publish(data=angle_deg)

        if msg.buttons[self._lift_button] == 1:
            lift_mm = abs(msg.axes[self._lift_axes]) * SUM_LIFT_HEIGHT + MIN_LIFT_HEIGHT
            rospy.logdebug('Send lift height: {} mm.'.format(lift_mm))
            self._lift_pub.publish(data=abs(msg.axes[self._lift_axes])) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logdebug [as 别名]
def _InitializeSpeedController(self):
		
		velocityPParam = rospy.get_param("~speedController/velocityPParam", "1")
		velocityIParam = rospy.get_param("~speedController/velocityIParam", "1")
		turnPParam = rospy.get_param("~speedController/turnPParam", "1")
		turnIParam = rospy.get_param("~speedController/turnIParam", "1")
		commandTimeout = self._GetCommandTimeoutForSpeedController()
		

		velocityPParam=velocityPParam * 1000
		velocityIParam= velocityIParam * 1000
		turnPParam = turnPParam * 1000
		turnIParam=turnIParam * 1000 


		message = 'sc %d %d %d %d %d\r' % (int(velocityPParam), int(velocityIParam), int(turnPParam), int(turnIParam), int(commandTimeout))
		#message = 'sc %f %f %f\r' % (velocityPParam, velocityIParam, turnPParam)
		rospy.logdebug("Sending differential drive gains message: " + message)
		self._WriteSerial(message)


		#speedControllerParams = (velocityPParam, velocityIParam, turnPParam, turnIParam, commandTimeout)
		#rospy.loginfo(str(speedControllerParams))
		#self._WriteSpeedControllerParams(speedControllerParams) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logdebug [as 别名]
def _InitializeBatteryMonitor(self):
	
#		rospy.logdebug("Initializing battery monitor. self._VoltageLowLowlimitvoltageTooLowLimit = " + str(self._VoltageLowLowlimit))
		rospy.logdebug("Initializing battery monitor. voltageTooLowLimit = " + str(self._VoltageLowlimit))
		
		temp_high_v = self._VoltageHighlimit * 1000
		self._VoltageHighlimit = temp_high_v
		temp_low_v = self._VoltageLowlimit * 1000
		self._VoltageLowlimit = temp_low_v 

		
		message = 'bm %d %d\r' % (self._VoltageHighlimit,self._VoltageLowlimit)
		rospy.logdebug("Sending battery monitor params message: " + message)
		self._WriteSerial(message)
		self._VoltageHighlimit = 12
		self._VoltageLowlimit = 11 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logdebug [as 别名]
def _InitializeSpeedController(self):
		velocityPParam = rospy.get_param("~speedController/velocityPParam", "0")
		velocityIParam = rospy.get_param("~speedController/velocityIParam", "0")
		turnPParam = rospy.get_param("~speedController/turnPParam", "0")
		turnIParam = rospy.get_param("~speedController/turnIParam", "0")
		commandTimeout = self._GetCommandTimeoutForSpeedController()

		message = 'sc %.2f %.2f %.2f %.2f %.2f\r' % (velocityPParam, velocityIParam, turnPParam, turnIParam, commandTimeout)
		#message = 'sc %f %f %f\r' % (velocityPParam, velocityIParam, turnPParam)
		rospy.logdebug("Sending differential drive gains message: " + message)
		self._WriteSerial(message)


		#speedControllerParams = (velocityPParam, velocityIParam, turnPParam, turnIParam, commandTimeout)
		#rospy.loginfo(str(speedControllerParams))
		#self._WriteSpeedControllerParams(speedControllerParams) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logdebug [as 别名]
def _InitializeDriveGeometry(self):
		wheelDiameter = rospy.get_param("~driveGeometry/wheelDiameter", "0.9")
		trackWidth = rospy.get_param("~driveGeometry/trackWidth", "0.1")
		countsPerRevolution = rospy.get_param("~driveGeometry/countsPerRevolution", "2000")
		

		wheelDiameter= wheelDiameter * 1000
		trackWidth=trackWidth * 1000


		#countsPerRevolution=countsperRevolution*1000;
		#wheelDiameterParts = self._GetBaseAndExponent(wheelDiameter)
		#trackWidthParts = self._GetBaseAndExponent(trackWidth)

		message = 'dg %d %d %d\r' % (int(wheelDiameter), int(trackWidth), int(countsPerRevolution))
		rospy.logdebug("Sending drive geometry params message: " + message)
		self._WriteSerial(message) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logdebug [as 别名]
def wheelCallback(self, msg):
    ######################################################
        enc = long(msg.data)
#	rospy.logwarn(enc)
        if (enc < self.encoder_low_wrap and self.prev_encoder > self.encoder_high_wrap) :
            self.wheel_mult = self.wheel_mult + 1
            
        if (enc > self.encoder_high_wrap and self.prev_encoder < self.encoder_low_wrap) :
            self.wheel_mult = self.wheel_mult - 1
           
         
        self.wheel_latest = 1.0 * (enc + self.wheel_mult * (self.encoder_max - self.encoder_min)) / self.ticks_per_meter 
        self.prev_encoder = enc
        
        
#        rospy.logdebug("-D- %s wheelCallback msg.data= %0.3f wheel_latest = %0.3f mult=%0.3f" % (self.nodename, enc, self.wheel_latest, self.wheel_mult))
    
    ###################################################### 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logdebug [as 别名]
def calcVelocity(self):
    #####################################################
        self.dt_duration = rospy.Time.now() - self.then
        self.dt = self.dt_duration.to_sec()
        rospy.logdebug("-D- %s caclVelocity dt=%0.3f wheel_latest=%0.3f wheel_prev=%0.3f" % (self.nodename, self.dt, self.wheel_latest, self.wheel_prev))
        
        if (self.wheel_latest == self.wheel_prev):
            # we haven't received an updated wheel lately
            cur_vel = (1 / self.ticks_per_meter) / self.dt    # if we got a tick right now, this would be the velocity
            if abs(cur_vel) < self.vel_threshold: 
                # if the velocity is < threshold, consider our velocity 0
                rospy.logdebug("-D- %s below threshold cur_vel=%0.3f vel=0" % (self.nodename, cur_vel))
                self.appendVel(0)
                self.calcRollingVel()
            else:
                rospy.logdebug("-D- %s above threshold cur_vel=%0.3f" % (self.nodename, cur_vel))
                if abs(cur_vel) < self.vel:
                    rospy.logdebug("-D- %s cur_vel < self.vel" % self.nodename)
                    # we know we're slower than what we're currently publishing as a velocity
                    self.appendVel(cur_vel)
                    self.calcRollingVel()
            
        else:
            # we received a new wheel value
            cur_vel = (self.wheel_latest - self.wheel_prev) / self.dt
            self.appendVel(cur_vel)
            self.calcRollingVel()
            rospy.logdebug("-D- %s **** wheel updated vel=%0.3f **** " % (self.nodename, self.vel))
            self.wheel_prev = self.wheel_latest
            self.then = rospy.Time.now()
            
        self.pub_vel.publish(self.vel)
        
    ##################################################### 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logdebug [as 别名]
def doPid(self):
    #####################################################
        pid_dt_duration = rospy.Time.now() - self.prev_pid_time
        pid_dt = pid_dt_duration.to_sec()
        self.prev_pid_time = rospy.Time.now()
        
        self.error = self.target - self.vel
        self.integral = self.integral + (self.error * pid_dt)
        # rospy.loginfo("i = i + (e * dt):  %0.3f = %0.3f + (%0.3f * %0.3f)" % (self.integral, self.integral, self.error, pid_dt))
        self.derivative = (self.error - self.previous_error) / pid_dt
        self.previous_error = self.error
    
        self.motor = (self.Kp * self.error) + (self.Ki * self.integral) + (self.Kd * self.derivative)
    
        if self.motor > self.out_max:
            self.motor = self.out_max
            self.integral = self.integral - (self.error * pid_dt)
        if self.motor < self.out_min:
            self.motor = self.out_min
            self.integral = self.integral - (self.error * pid_dt)
      
        if (self.target == 0):
            self.motor = 0
    
        rospy.logdebug("vel:%0.2f tar:%0.2f err:%0.2f int:%0.2f der:%0.2f ## motor:%d " % 
                      (self.vel, self.target, self.error, self.integral, self.derivative, self.motor))
    
    


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

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logdebug [as 别名]
def targetCallback(self, msg):
    ######################################################
        self.target = msg.data
        self.ticks_since_target = 0
        # rospy.logdebug("-D- %s targetCallback " % (self.nodename)) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logdebug [as 别名]
def start(self):
        rospy.logdebug("Starting")
        self._SerialDataGateway.Start() 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logdebug [as 别名]
def stop(self):
        rospy.logdebug("Stopping")
        self.reset()
        self._SerialDataGateway.Stop() 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logdebug [as 别名]
def callback_scales(data):
    global scales
    scales = ast.literal_eval(data.data)
    rospy.logdebug('[tf-pose-estimation] scale changed: ' + str(scales)) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logdebug [as 别名]
def create_query_parameters(contexts=None):
    """Creates a QueryParameter with contexts. Last contexts used if
    contexts is empty. No contexts if none found.
    :param contexts: The ROS DialogflowContext message
    :type contexts: list(DialogflowContext)
    :return: A Dialogflow query parameters object.
    :rtype: QueryParameters
    """
    # Create a context list is contexts are passed
    if contexts:
        rospy.logdebug("DF_CLIENT: Using the following contexts:\n{}".format(
                        print_context_parameters(contexts)))
        contexts = contexts_msg_to_struct(contexts)
        return QueryParameters(contexts=contexts) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import logdebug [as 别名]
def _text_request_cb(self, msg):
        """ROS Callback that sends text received from a topic to Dialogflow,
        :param msg: A String message.
        :type msg: String
        """
        rospy.logdebug("DF_CLIENT: Request received")
        new_msg = DialogflowRequest(query_text=msg.data)
        df_msg = self.detect_intent_text(new_msg)