Python源码示例：rospy.logdebug()
示例1
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))
######################################################
示例2
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]))
示例3
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)
示例4
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
示例5
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)
示例6
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))
######################################################
示例7
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))
######################################################
示例8
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)
示例9
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)
示例10
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
示例11
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))
######################################################
示例12
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))
######################################################
示例13
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)
示例14
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)
示例15
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
示例16
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)
示例17
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))
######################################################
示例18
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)
示例19
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)
#####################################################
示例20
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))
#####################################################
示例21
def targetCallback(self, msg):
######################################################
self.target = msg.data
self.ticks_since_target = 0
# rospy.logdebug("-D- %s targetCallback " % (self.nodename))
示例22
def start(self):
rospy.logdebug("Starting")
self._SerialDataGateway.Start()
示例23
def stop(self):
rospy.logdebug("Stopping")
self.reset()
self._SerialDataGateway.Stop()
示例24
def callback_scales(data):
global scales
scales = ast.literal_eval(data.data)
rospy.logdebug('[tf-pose-estimation] scale changed: ' + str(scales))
示例25
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)
示例26
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)
示例27
def _msg_request_cb(self, msg):
"""ROS Callback that sends text received from a topic to Dialogflow,
:param msg: A DialogflowRequest message.
:type msg: DialogflowRequest
"""
df_msg = self.detect_intent_text(msg)
rospy.logdebug("DF_CLIENT: Request received:\n{}".format(df_msg))
示例28
def _create_audio_output(self):
"""Creates a PyAudio output stream."""
rospy.logdebug("DF_CLIENT: Creating audio output...")
self.stream_out = self.audio.open(format=pyaudio.paInt16,
channels=1,
rate=24000,
output=True)
示例29
def Start(self):
rospy.logdebug("Starting")
self._SerialDataGateway.Start()
#######################################################################################################################
示例30
def Stop(self):
rospy.logdebug("Stopping")
self._SerialDataGateway.Stop()
#######################################################################################################################
