def publish_servo_info():
global id, num, battery_voltage_warn_flag, battery_voltage_fatal_flag, voltage, k
multi_servo_info = Multi_servo_info()
for i in range(num):
voltage = list(voltage)
multi_servo_info.encoder_count.append(
Kondo_B3M.get_encoder_total_count(id[i]))
# ommit to get motor velocity due to low control rate
# multi_servo_info.motor_velocity.append(
# Kondo_B3M.get_servo_Velocity(id[i]))
multi_servo_info.input_voltage.append(voltage[i])
# we don't have to monitor voltage at every loop, so get sparsed at a time per 100 loops
if k % 100 == 0:
k = 0
for j in range(num):
voltage[j] = Kondo_B3M.get_servo_voltage(id[j])
if voltage[j] < BATTERY_VOLTAGE_WARN and battery_voltage_warn_flag == 0:
print("")
rospy.logwarn('battery voltage is low !')
battery_voltage_warn_flag = 1
elif voltage[j] < BATTERY_VOLTAGE_FATAL and battery_voltage_fatal_flag == 0:
print("")
rospy.logfatal('battery voltage is fatally low !')
k = k + 1
multi_servo_info_pub.publish(multi_servo_info)
del multi_servo_info
def publish_servo_info():
global id, num, voltage
global servo_drive_flag, voltage_monitor_flag
# deactivate servo_drive_flag to prevent from sending velocity command to the servos before completing this process
servo_drive_flag = 0
multi_servo_info = Multi_servo_info()
# Don't have to monitor voltage at every loop, so get sparsed at a time per a certain loops
# monitor per 100 cycles
if voltage_monitor_flag == 100:
voltage_monitor_flag = 0
for j in range(num):
voltage[j] = Drive.get_servo_voltage(id[j])
if voltage[j] < BATTERY_VOLTAGE_WARN:
print("")
rospy.logwarn('battery voltage is low !')
battery_voltage_warn_flag = 1
elif voltage[j] < BATTERY_VOLTAGE_FATAL:
print("")
rospy.logfatal('battery voltage is fatally low !')
voltage_monitor_flag = voltage_monitor_flag + 1
voltage = list(voltage)
for i in range(num):
multi_servo_info.encoder_count.append(
Drive.get_encoder_total_count(id[i]))
# if you want to ommit motor velocity(due to low control rate, for example), comment out script bellow.
multi_servo_info.motor_velocity.append(Drive.get_servo_Velocity(id[i]))
# if you want to ommit motor current(due to low control rate, for example), comment out script bellow
multi_servo_info.motor_current.append(Drive.get_servo_Current(id[i]))
multi_servo_info.input_voltage.append(voltage[i])
multi_servo_info_pub.publish(multi_servo_info)
del multi_servo_info
# activate servo_drive_flag again since this process has completed
servo_drive_flag = 1
def publish_servo_info():
global id, num, battery_voltage_warn_flag, battery_voltage_fatal_flag, voltage
multi_servo_info = Multi_servo_info()
for i in range(num):
multi_servo_info.encoder_count.append(
Kondo_B3M.get_encoder_total_count(id[i]))
multi_servo_info.input_voltage.append(
Kondo_B3M.get_servo_voltage(id[i]))
voltage = multi_servo_info.input_voltage
if voltage[i] < BATTERY_VOLTAGE_WARN and battery_voltage_warn_flag == 0:
print("")
rospy.logwarn('battery voltage is low !')
battery_voltage_warn_flag = 1
elif voltage[i] < BATTERY_VOLTAGE_FATAL and battery_voltage_fatal_flag == 0:
print("")
rospy.logfatal('battery voltage is fatally low !')
multi_servo_info.motor_velocity.append(
Kondo_B3M.get_servo_Velocity(id[i]))
multi_servo_info_pub.publish(multi_servo_info)
del multi_servo_info
def publish_servo_info():
global SERVO_ID, battery_voltage
global servo_drive_flag, voltage_monitor_count
# deactivate servo_drive_flag to prevent from sending velocity command to the servos before completing this process
servo_drive_flag = False
# set up an instance for ROS message to be published
multi_servo_info = Multi_servo_info()
# Don't have to monitor voltage at every loop, so get sparsed at a time per a certain loops
# monitor per 100 cycles
if voltage_monitor_count == 100:
voltage_monitor_count = 0 # reset count
# prepare flag paramter whether to warn voltage or not
voltage_fatal_flag = False
voltage_warn_flag = False
# convert battery_voltage paramter into "list" before refreshing it's values
battery_voltage = list(battery_voltage)
# survey and refresh voltages of all servos (frequently those are same since they're connected to same battery)
for i in range(len(SERVO_ID)):
battery_voltage[i] = Drive.get_servo_voltage(SERVO_ID[i])
if battery_voltage[
i] < BATTERY_VOLTAGE_FATAL: # if the voltage is lower than a certain fatal value
voltage_fatal_flag = True
elif battery_voltage[
i] < BATTERY_VOLTAGE_WARN: # if the voltage is lower than a certain warning value
voltage_warn_flag = True
# if more than one of values of voltage are lower than threshold, then display warning to the console.
if voltage_fatal_flag == True:
rospy.logfatal('battery voltage is fatally low !')
voltage_fatal_flag = False
elif voltage_warn_flag == True:
rospy.logwarn('battery voltage is low !')
voltage_warn_flag = False
# increment count to execute process above periodically
voltage_monitor_count = voltage_monitor_count + 1
# contain information to ROS message
for i in range(len(SERVO_ID)):
multi_servo_info.encoder_count.append(
Drive.get_encoder_total_count(SERVO_ID[i]))
# if you want to ommit motor velocity(due to low control rate, for example), comment out script bellow.
multi_servo_info.motor_velocity.append(
Drive.get_servo_Velocity(SERVO_ID[i]))
multi_servo_info.motor_current.append(
Drive.get_servo_Current(SERVO_ID[i]))
multi_servo_info.motor_position.append(
Drive.get_servo_Position(SERVO_ID[i]))
multi_servo_info.input_voltage.append(battery_voltage[i])
# publish ROS message
multi_servo_info_pub.publish(multi_servo_info)
del multi_servo_info
# activate servo_drive_flag again since this process has completed
servo_drive_flag = 1