def publish_servo_info(): global id servo_info.encoder_count = Kondo_B3M.get_encoder_total_count(id) voltage = Kondo_B3M.get_servo_voltage(id) if voltage < BATTERY_VOLTAGE_WARN and battery_voltage_warn_flag == 0: rospy.logwarn('battery voltage is low !') battery_voltage_warn_flag = 1 elif voltage < BATTERY_VOLTAGE_FATAL and battery_voltage_fatal_flag == 0: rospy.logfatal('battery voltage is low !') battery_voltage_fatal_flag = 1 servo_info.input_voltage = voltage servo_info.motor_velocity = Kondo_B3M.get_servo_Velocity(id) servo_info_pub.publish(servo_info)
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