def callback_gps(gps):
global x_charge
global y_charge
global z_charge
global battery_constant
global battery_percentage
global old_location_x
global old_location_y
global old_location_z
time_begin = rospy.Time(1075)
time_now = rospy.get_rostime()
if time_now<time_begin:
print('not_yet_started')
old_location_x=gps.pose.position.x
old_location_y=gps.pose.position.y
old_location_z=gps.pose.position.z
battery = BatteryState()
battery_percentage=100
battery.percentage = battery_percentage
battery_pub.publish(battery)
if time_now>time_begin:
print('started')
new_location_x = gps.pose.position.x
new_location_y = gps.pose.position.y
new_location_z = gps.pose.position.z
percentage_loss=battery_constant*(math.pow((new_location_x-old_location_x), 2) + math.pow((new_location_y-old_location_y), 2)+ math.pow((new_location_z-old_location_z), 2))
print("percentage lossp", percentage_loss)
battery_percentage=battery_percentage-percentage_loss
charge_diff=(math.pow((new_location_x-x_charge), 2) + math.pow((new_location_y-y_charge), 2)+ math.pow((new_location_z-z_charge), 2))
if battery_percentage < 0.1:
battery_percentage = 0
print("battery drained")
if charge_diff<0.5:
battery_percentage=battery_percentage+0.1
if battery_percentage>100:
battery_percentage=100
print("fully charged")
battery = BatteryState()
battery.percentage = battery_percentage
battery_pub.publish(battery)
old_location_x=new_location_x
old_location_y=new_location_y
old_location_z=new_location_z
print('battery_percentage', battery_percentage)
def __init__(self):
#############################################################################
rospy.init_node("status_manager")
self.nodename = rospy.get_name()
rospy.loginfo("-I- %s started" % self.nodename)
self.state_pub_ = rospy.Publisher("rtcrobot/state", RobotState, queue_size=10)
self.__mode = RobotMode.NAVIGATION
#status starting
self.__state = RobotState()
self.__state.code = RobotState.STARTING
self.__state_old = self.__state
self.bat__ = BatteryState()
self.dock__ = DockState()
self.__moving = False
#Subcriber
self.mode_sub_ = rospy.Subscriber('robot_mode', RobotMode, self.mode_callback)
self.bat_sub_ = rospy.Subscriber('rtcrobot/battery', BatteryState, self.bat_callback)
self.dock_sub_ = rospy.Subscriber('rtcrobot/dockstate', DockState, self.dock_callback)
self.mb_feedback_sub_ = rospy.Subscriber('move_base/feedback', MoveBaseActionFeedback, self.movebasefeedback_callback)
self.mb_result_sub_ = rospy.Subscriber('move_base/result', MoveBaseActionResult, self.movebaseresulfcallback)
def __init__(self):
self._mutex = threading.Lock()
self._last_info_update = 0
self._last_state_update = 0
self._msg = BatteryState()
self._power_pub = rospy.Publisher('laptop_charge',
BatteryState,
latch=True,
queue_size=5)
self._diag_pub = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=5)
# Battery info
self._batt_acpi_path = rospy.get_param('~acpi_path',
"/sys/class/power_supply/BAT0")
self._batt_design_capacity = 0
self._batt_last_full_capacity = 0
self._last_info_update = 0
self._batt_info_rate = 1 / 60.0
self._batt_info_thread = threading.Thread(target=self._check_batt_info)
self._batt_info_thread.daemon = True
self._batt_info_thread.start()
# Battery state
self._batt_state_rate = 1 / 5.0
self._batt_state_thread = threading.Thread(
target=self._check_batt_state)
self._batt_state_thread.daemon = True
self._batt_state_thread.start()
def __init__(self, filename=None):
self.ser = serial.Serial(SERIAL_PORT, 9600)
if filename:
self.log_file = open("logs/%s_log.csv" % filename, 'w')
else:
self.log_file = open(
"logs/%s_log.csv" %
datetime.datetime.now().strftime('%m%d%H%M%S'), 'w')
self.log_file.write(
"timestamp,vel_x,vel_y,vel_z,acc_x,acc_y,acc_z,roll,pitch,yaw,rc0,rc1,rc2,rc3,vol,cur_raw,cur,power,act_vx,act_vy,act_vz,mode\n"
)
mavros.set_namespace()
rospy.init_node('aero_energy_logger')
self.cur_state = State()
self.velocity = TwistStamped()
self.rc_in = RCIn()
self.imu = Imu()
self.manual_control = ManualControl()
self.battery_state = BatteryState()
self.pose = PoseStamped()
self.cur_val_list = [0] * FIELD_NUM
print "initialize subscribers..."
self.init_subscribers()
print "initialize services..."
self.init_services()
print "initialize publishers..."
self.init_publishers()
self.start_time = time.time()
def batt_cb(self, msg):
"""
Battery callback specific for one vehicle
"""
self.mutex.acquire()
if self.battery == BatteryState():
self.battery = deepcopy(msg)
self.modified_batt = deepcopy(msg)
elif msg.percentage > 0.0:
idx = msg.header.seq % len(self._delta)
self._delta[idx] = (msg.percentage - self.battery.percentage)
new_perc = self.modified_batt.percentage + (self._delta[idx] *
self.multiplier)
self.battery = deepcopy(msg)
self.modified_batt = deepcopy(msg)
self.modified_batt.percentage = new_perc
elif msg.voltage <= self.MAX_VOLTAGE:
delta = np.min([
-0.0001,
np.random.normal(loc=np.mean(self._delta),
scale=np.std(self._delta))
])
new_perc = self.modified_batt.percentage + (delta *
self.multiplier)
self.modified_batt = deepcopy(msg)
self.modified_batt.percentage = new_perc
self.mutex.release()
def __init__(self, mav_name, mav_type="mavros"):
#rospy.init_node("MAV_" + mav_name)
self.rate = rospy.Rate(20)
self.drone_pose = PoseStamped()
self.goal_pose = PoseStamped()
self.goal_vel = TwistStamped()
self.drone_state = State()
self.battery = BatteryState()
############## Publishers ###############
self.local_position_pub = rospy.Publisher(
CONFIG[mav_type + "_local_position_pub"],
PoseStamped,
queue_size=20)
self.velocity_pub = rospy.Publisher(CONFIG[mav_type + "_velocity_pub"],
TwistStamped,
queue_size=5)
self.mavlink_pub = rospy.Publisher('/mavlink/to',
Mavlink,
queue_size=1)
########## Subscribers ##################
self.local_atual = rospy.Subscriber(CONFIG[mav_type + "_local_atual"],
PoseStamped, self.local_callback)
self.state_sub = rospy.Subscriber(CONFIG[mav_type + "_state_sub"],
State, self.state_callback)
self.battery_sub = rospy.Subscriber("/mavros/battery", BatteryState,
self.battery_callback)
############# Services ##################
self.arm = rospy.ServiceProxy(CONFIG[mav_type + "_arm"],
mavros_msgs.srv.CommandBool)
self.set_mode = rospy.ServiceProxy(CONFIG[mav_type + "_set_mode"],
mavros_msgs.srv.SetMode)
def looping(self): rate = rospy.Rate(50) while not rospy.is_shutdown(): # Generate Commands LEDs = np.array([0, 0, 0, 0, 0, 0, 1, 1]) self.command = np.array([self.throttle, self.steering]) current, batteryVoltage, encoderCounts = self.myCar.read_write_std(self.command, LEDs) battery_state = BatteryState() battery_state.header.stamp = rospy.Time.now() battery_state.header.frame_id = 'battery_voltage' battery_state.voltage = batteryVoltage self.battery_pub_.publish(battery_state) longitudinal_car_speed = basic_speed_estimation(encoderCounts) velocity_state = Vector3Stamped() velocity_state.header.stamp = rospy.Time.now() velocity_state.header.frame_id = 'car_velocity' velocity_state.vector.x = float(np.cos(self.command[1]) * longitudinal_car_speed) velocity_state.vector.y = float(np.sin(self.command[1]) * longitudinal_car_speed) self.carvel_pub_.publish(velocity_state) rate.sleep() self.myCar.terminate()
def __init__(self):
ACTION_NAME = "robot_mission"
sv = rospy.Service('/robot_services/mission/load',CallMission, self.sv_loadcallback)
rospy.loginfo("Connecting to %s..." % ACTION_NAME)
self.client = actionlib.SimpleActionClient(ACTION_NAME, MissionAction)
self.client.wait_for_server()
sv = rospy.Service('/robot_services/mission/continue',Empty, self.sv_continuecallback)
sv = rospy.Service('/robot_services/mission/pause',Empty, self.sv_pausecallback)
sv = rospy.Service('/robot_services/mission/cancel',Empty, self.sv_cancelcallback)
self.mssv_pub = rospy.Publisher('robot_mission/cancel', GoalID,queue_size=10)
self.missionstate_pub = rospy.Publisher('rtcrobot/mission/state', MissionState,queue_size=10)
self.robotstate_sub = rospy.Subscriber("rtcrobot/state", RobotState, self.stateCallback)
self.bat_sub_ = rospy.Subscriber('rtcrobot/battery', BatteryState, self.bat_callback)
self.robotpose_sub_ = rospy.Subscriber('rtcrobot/pose', PoseStamped, self.robotpose_callback)
currentmap_sub = rospy.Subscriber("rtcrobot/currentmap", String, self.currentmapCallback)
self.numcommand = len(self.lst_command_)
self.command_idx = 0
self.trigersendgoal = False
self.actionrunning = False
self.battery = BatteryState()
self.robotpose = PoseStamped()
self.__currentmap = ''
self.__robotstate = RobotState()
self.__enableFindCharger = True
self.__percentautochargeenable = 0.0
self.__percentautochargeend = 0.0
self.missionstate_ = MissionState()
def __init__(self):
rospy.init_node("mav_control_node")
rospy.Subscriber("/mavros/global_position/global",NavSatFix,self.gps_callback)
rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.pose_callback)
rospy.Subscriber('mavros/state',State,self.arm_callback)
rospy.Subscriber('mavros/battery',BatteryState,self.battery_callback)
rospy.Subscriber('mavros/global_position/raw/satellites',UInt32,self.sat_num_callback)
rospy.Subscriber('mavros/global_position/rel_alt',Float64,self.rel_alt_callback)
rospy.Subscriber('mavros/imu/data',Imu,self.imu_callback)
rospy.Subscriber("/mavros/rc/in", RCIn, self.rc_callback)
rospy.Subscriber('mavros/vfr_hud',VFR_HUD,self.hud_callback)
self.gps = NavSatFix()
self.pose = PoseStamped()
self.arm_status = State()
self.battery_status = BatteryState()
self.sat_num = UInt32()
self.rel_alt = Float64()
self.imu = Imu()
self.rc = RCIn()
self.hud = VFR_HUD()
self.timestamp = rospy.Time()
self.cmd_pos_pub = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=1)
self.cmd_vel_pub = rospy.Publisher("/mavros/setpoint_velocity/cmd_vel_unstamped", Twist, queue_size=1)
self.rc_override = rospy.Publisher("/mavros/rc/override", OverrideRCIn, queue_size=1)
self.mode_service = rospy.ServiceProxy("/mavros/set_mode", SetMode)
self.arm_service = rospy.ServiceProxy("/mavros/cmd/arming", CommandBool)
self.takeoff_service = rospy.ServiceProxy("/mavros/cmd/takeoff", CommandTOL)
self.stream_service = rospy.ServiceProxy("/mavros/set_stream_rate", StreamRate)
self.home_service = rospy.ServiceProxy("/mavros/cmd/set_home", CommandHome)
def main(self):
battery = BatteryState()
battery.power_supply_health = battery.POWER_SUPPLY_HEALTH_GOOD
while not rospy.is_shutdown():
#self.pub_current_floor.publish(self.current_floor)
#self.pub_target_floor.publish(self.target_floor)
self.pub_battery_status.publish(battery)
self.rate.sleep()
def __init__(self,battname):
#init Dict to hold steering state for each servo
self.bs = BatteryState()
self.pack=battname
self.topic=battname+"/battery/info"
rospy.init_node("battstat")
rospy.Subscriber(self.topic,BatteryState,self.batt_state_read_cb)
def PublishStatus(self):
battery_state = BatteryState()
# Read the battery voltage
try:
battery_state.voltage = self.__thunderborg.GetBatteryReading()
self.__status_pub.publish(battery_state)
except:
rospy.logwarn("Thunderborg node: Failed to read battery voltage")
def callback(self, msg):
rpi_battery_msg = BatteryState()
voltages = [msg.cell1, msg.cell2, msg.cell3, msg.cell4]
rpi_battery_msg.header.stamp = rospy.Time.now()
rpi_battery_msg.voltage = sum(voltages)
rpi_battery_msg.cell_voltage = voltages
rpi_battery_msg.present = True
self.bridge_pub.publish(rpi_battery_msg)
def __init__(self, name, multiplier=0.33):
self._delta = [0.0 for i in range(30)]
self.vehicle_name = name
self.multiplier = multiplier
self.battery = BatteryState()
self.modified_batt = BatteryState()
self.batt_pub = rospy.Publisher('/%s/mavros/modified_battery' % name,
BatteryState,
queue_size=3)
rospy.Subscriber('/%s/mavros/battery' % name,
BatteryState,
self.batt_cb,
queue_size=1)
rospy.Subscriber('/rosplan_plan_dispatcher/action_dispatch',
ActionDispatch,
self.dispatch_cb,
queue_size=10)
rospy.Timer(rospy.Duration(0.1), self.battery_update)
def __init__(self):
# get parameters
self._loop_rate = rospy.get_param("~battery_pub_rate_in_hz", 1) # how many seconds to publish battery state
self._battery_dir = rospy.get_param("~battery_dir", "/sys/class/power_supply/BAT0")
# create default battery message
self._battery_msg = BatteryState(power_supply_technology=BatteryState.POWER_SUPPLY_TECHNOLOGY_LION, power_supply_health=BatteryState.POWER_SUPPLY_HEALTH_UNKNOWN)
# create battery state publisher
self._battery_state_pub = rospy.Publisher("computer_battery_state", BatteryState, queue_size=1)
def _check_battery_state(_battery_acpi_path):
"""
@return BatteryState
"""
rv = BatteryState()
if _battery_acpi_path.startswith('/proc'):
if os.access(_battery_acpi_path, os.F_OK):
o = slerp(_battery_acpi_path + '/state')
else:
raise Exception(_battery_acpi_path + ' does not exist')
batt_info = yaml.load(o)
state = batt_info.get('charging state', 'discharging')
rv.power_supply_status = state_to_val.get(state, 0)
rv.current = _strip_A(batt_info.get('present rate', '-1 mA'))
if rv.power_supply_status == BatteryState.POWER_SUPPLY_STATUS_DISCHARGING:
rv.current = math.copysign(
rv.current, -1) # Need to set discharging rate to negative
rv.charge = _strip_Ah(batt_info.get('remaining capacity',
'-1 mAh')) # /energy_now
rv.voltage = _strip_V(batt_info.get('present voltage',
'-1 mV')) # /voltage_now
rv.present = batt_info.get('present', False) # /present
rv.header.stamp = rospy.get_rostime()
else:
# Charging state; make lowercase and remove trailing eol
state = _read_string(_battery_acpi_path + '/status',
'discharging').lower().rstrip()
rv.power_supply_status = state_to_val.get(state, 0)
if os.path.exists(_battery_acpi_path + '/power_now'):
rv.current = _read_number(_battery_acpi_path + '/power_now')/10e5 / \
_read_number(_battery_acpi_path + '/voltage_now')
else:
rv.current = _read_number(_battery_acpi_path +
'/current_now') / 10e5
if rv.power_supply_status == BatteryState.POWER_SUPPLY_STATUS_DISCHARGING:
rv.current = math.copysign(
rv.current, -1) # Need to set discharging rate to negative
if os.path.exists(_battery_acpi_path + '/energy_now'):
rv.charge = _read_number(_battery_acpi_path + '/energy_now') / 10e5
else:
rv.charge = _read_number(_battery_acpi_path + '/charge_now') / 10e5
rv.voltage = _read_number(_battery_acpi_path + '/voltage_now') / 10e5
rv.present = _read_number(_battery_acpi_path + '/present') == 1
rv.header.stamp = rospy.get_rostime()
return rv
def talker():
pub = rospy.Publisher("test_topic_ufro", BatteryState, queue_size=10)
rospy.init_node('publisher_ufro', anonymous=True)
rate = rospy.Rate(1) # 10hz
while not rospy.is_shutdown():
battery = BatteryState()
battery.voltage = random.random() * 10
rospy.loginfo(battery.voltage)
pub.publish(battery)
rate.sleep()
def publish_battery_state_msg(self):
battery_state_msg = BatteryState()
battery_state_msg.header.stamp = rospy.get_rostime()
battery_state_msg.voltage = self.battery_mv / 1000.0
# estimate remaining battery life
# 1.2v * 6 batteries = 7.2 v
# 6v is depletion point
battery_state_msg.percentage = (battery_state_msg.voltage - 6.0) / 1.2
battery_state_msg.present = True if self.battery_mv > 0 else False
self.battery_pub.publish(battery_state_msg)
def _publish_battery(self):
battery = BatteryState()
battery.header.stamp = rospy.Time.now()
battery.voltage = self._cozmo.battery_voltage
battery.present = True
if self._cozmo.is_on_charger: # is_charging always return False
battery.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING
else:
battery.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING
self._battery_pub.publish(battery)
def get_battery_state(self, battery):
"""Get the current state of a battery."""
battery_config = BATTERIES[battery]
battery_state = BatteryState()
battery_voltage = PiPuckBatteryServer.get_battery_voltage(battery_config["path"])
self._battery_history[battery].insert(0, battery_voltage)
self._battery_history[battery] = self._battery_history[battery][:HISTORY_MAX]
split_point = len(self._battery_history[battery]) // 2
head_half = self._battery_history[battery][:split_point]
tail_half = self._battery_history[battery][split_point:]
try:
battery_delta = (sum(head_half) / len(head_half)) - (sum(tail_half) / len(tail_half))
except ZeroDivisionError:
battery_delta = 0.0
battery_state.voltage = battery_voltage
battery_state.present = True
battery_state.design_capacity = battery_config["design_capacity"]
battery_state.power_supply_technology = battery_config["power_supply_technology"]
average_battery_voltage = sum(self._battery_history[battery]) / len(
self._battery_history[battery])
if average_battery_voltage >= battery_config["max_voltage"] * CHARGED_VOLTAGE_MARGIN:
battery_state.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_FULL
elif battery_delta < 0:
battery_state.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
elif battery_delta > 0:
battery_state.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING
else:
battery_state.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING
if average_battery_voltage >= battery_config["max_voltage"] * OVER_VOLTAGE_MARGIN:
battery_state.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_OVERVOLTAGE
elif average_battery_voltage <= battery_config["min_voltage"]:
# It is unclear whether this means "out of charge" or "will never charge again", we
# assume here that it means "out of charge".
battery_state.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_DEAD
else:
battery_state.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_GOOD
battery_state.percentage = clamp(
(average_battery_voltage - battery_config["min_voltage"]) /
(battery_config["max_voltage"] - battery_config["min_voltage"]), 1.0, 0.0)
battery_state.current = NAN
battery_state.charge = NAN
battery_state.capacity = NAN
battery_state.location = battery_config["location"]
return battery_state
def test__on_receive_battery_state_under_threshold(self, mocked_rospy,
mocked_mqtt):
mocked_rospy.get_param.return_value = utils.get_attrs_params()
mocked_mqtt_client = mocked_mqtt.Client.return_value
with freezegun.freeze_time('2018-01-02T03:04:05.123456+09:00'):
bridge = AttrsBridge().connect()
with freezegun.freeze_time('2018-01-02T03:04:05.123457+09:00'):
bridge._on_receive_battery_state(BatteryState())
mocked_mqtt_client.publish.assert_not_called()
def publish(self):
battery_msg = BatteryState()
battery_msg.percentage = 1.0
pose_msg = PoseStamped()
pose_msg.header.frame_id = "map"
pose_msg.pose.position.x = self.id
pose_msg.pose.position.y = self.id
pose_msg.pose.position.z = 0
self.battery_pub.publish(battery_msg)
self.pose_pub.publish(pose_msg)
self.state_pub.publish(self.state_msg)
def main():
rospy.init_node("adc_reader")
battery_type=rospy.get_param("~type","Pb")
battery_volt=rospy.get_param("~V",12.0)
battery_design_capacity=rospy.get_param('~C',7) # capacity of battery in Ah
reading_pub = rospy.Publisher("energy/adc_raw",Readings,queue_size=10)
battery_pub = rospy.Publisher("energy/battery",BatteryState,queue_size=10)
hz = rospy.Rate(10)
rospy.loginfo('Reading ADS1x15 values, press Ctrl-C to quit...')
# Print nice channel column headers.
filters = []
cutoffs = rospy.get_param('~cutoffs',[4,4,4,4])
for i in range(4):
filters.append(lowpass.LowPass(10,cutoffs[i],10,order=3))
rospy.logdebug('| {0:>6} | {1:>6} | {2:>6} | {3:>6} |'.format(*range(4)))
rospy.logdebug('-' * 37)
battery_msg = BatteryState()
reading_msg = Readings()
try:
while not rospy.is_shutdown():
# Read all the ADC channel values in a list.
raw = np.zeros(4)
for i in range(4):
# Read the specified ADC channel using the previously set gain value.
#raw[i] = filters[i].update(adc.read_adc(i, gain=GAIN))
raw[i] = adc.read_adc(i, gain=GAIN)
# Note you can also pass in an optional data_rate parameter that controls
# the ADC conversion time (in samples/second). Each chip has a different
# set of allowed data rate values, see datasheet Table 9 config register
# DR bit values.
#values[i] = adc.read_adc(i, gain=GAIN, data_rate=128)
# Each value will be a 12 or 16 bit signed integer value depending on the
# ADC (ADS1015 = 12-bit, ADS1115 = 16-bit).
values = FACTORS*(raw)+ZEROS
for i in range(4):
values[i] = filters[i].update(values[i])
# Print the ADC values.
rospy.logdebug('| {0:>6} | {1:>6} | {2:>6} | {3:>6} |'.format(*values))
battery_msg.header.stamp = rospy.Time.now()
battery_msg.header.frame_id = 'adc'
battery_msg.voltage = values[3]
battery_msg.current = values[0]
battery_msg.design_capacity = battery_design_capacity
battery_msg.present = True
battery_pub.publish(battery_msg)
reading_msg.header = battery_msg.header
reading_msg.data = raw
reading_pub.publish(reading_msg)
hz.sleep()
except rospy.ROSInterruptException:
print("Exiting")
def __init__(self):
rospy.init_node('dji_sdk_ros_python')
self.attitude = QuaternionStamped()
self.flight_status = UInt8()
self.battery_state = BatteryState()
self.velocity = Vector3Stamped()
self.gps_health = UInt8()
self.gps_position = NavSatFix()
self.local_position = PointStamped()
self.init_services()
self.init_subscribers()
self.init_publishers()
def timerBatCB(self, event):
try:
data = self.data
if len(data) == 8:
voltage = float( int(data[6].encode('hex'),16)*256 \
+ int(data[7].encode('hex'),16) ) / 100.0
else:
print("Received wrong data")
bat = BatteryState()
bat.voltage = voltage
self.bat_pub.publish(bat)
except:
print('publish battery sate error')
def update_state(self):
voltage_dec_, current_dec_, charge_dec_, percentage_dec_, temperature_dec_, power_supply_status_dec_, cell_voltage_dec_ = self.read_bms(
)
battery_msg = BatteryState()
# Power supply status constants
# uint8 POWER_SUPPLY_STATUS_UNKNOWN = 0
# uint8 POWER_SUPPLY_STATUS_CHARGING = 1
# uint8 POWER_SUPPLY_STATUS_DISCHARGING = 2
# uint8 POWER_SUPPLY_STATUS_NOT_CHARGING = 3
# uint8 POWER_SUPPLY_STATUS_FULL = 4
# Power supply health constants
# uint8 POWER_SUPPLY_HEALTH_UNKNOWN = 0
# uint8 POWER_SUPPLY_HEALTH_GOOD = 1
# uint8 POWER_SUPPLY_HEALTH_OVERHEAT = 2
# uint8 POWER_SUPPLY_HEALTH_DEAD = 3
# uint8 POWER_SUPPLY_HEALTH_OVERVOLTAGE = 4
# uint8 POWER_SUPPLY_HEALTH_UNSPEC_FAILURE = 5
# uint8 POWER_SUPPLY_HEALTH_COLD = 6
# uint8 POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE = 7
# uint8 POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE = 8
# Power supply technology (chemistry) constants
# uint8 POWER_SUPPLY_TECHNOLOGY_UNKNOWN = 0
# uint8 POWER_SUPPLY_TECHNOLOGY_NIMH = 1
# uint8 POWER_SUPPLY_TECHNOLOGY_LION = 2
# uint8 POWER_SUPPLY_TECHNOLOGY_LIPO = 3
# uint8 POWER_SUPPLY_TECHNOLOGY_LIFE = 4
# uint8 POWER_SUPPLY_TECHNOLOGY_NICD = 5
# uint8 POWER_SUPPLY_TECHNOLOGY_LIMN = 6
# Populate battery parameters.
battery_msg.voltage = voltage_dec_ # Voltage in Volts (Mandatory)
battery_msg.current = current_dec_ # Negative when discharging (A) (If unmeasured NaN)
battery_msg.charge = charge_dec_ # Current charge in Ah (If unmeasured NaN)
battery_msg.capacity = 150 # Capacity in Ah (last full capacity) (If unmeasured NaN)
battery_msg.design_capacity = 150 # Capacity in Ah (design capacity) (If unmeasured NaN)
battery_msg.percentage = percentage_dec_ # Charge percentage on 0 to 1 range (If unmeasured NaN)
battery_msg.power_supply_status = int(
power_supply_status_dec_
) # The charging status as reported. Values defined above
battery_msg.power_supply_health = 0 # The battery health metric. Values defined above
battery_msg.power_supply_technology = battery_msg.POWER_SUPPLY_TECHNOLOGY_LIFE # The battery chemistry. Values defined above
battery_msg.present = True # True if the battery is present
battery_msg.cell_voltage = cell_voltage_dec_
self.pub_batt_state.publish(battery_msg)
def __init__(self, mav_name):
self.rate = rospy.Rate(60)
self.desired_state = ""
self.drone_pose = PoseStamped()
self.goal_pose = PoseStamped()
self.pose_target = PositionTarget()
self.goal_vel = TwistStamped()
self.drone_state = State()
self.battery = BatteryState()
self.global_pose = NavSatFix()
self.gps_target = GeoPoseStamped()
############# Services ##################
self.arm = rospy.ServiceProxy(mavros_arm, CommandBool)
self.set_mode_srv = rospy.ServiceProxy(mavros_set_mode, SetMode)
############### Publishers ##############
self.local_position_pub = rospy.Publisher(mavros_local_position_pub, PoseStamped, queue_size = 20)
self.velocity_pub = rospy.Publisher(mavros_velocity_pub, TwistStamped, queue_size=5)
self.target_pub = rospy.Publisher(mavros_pose_target_sub, PositionTarget, queue_size=5)
self.global_position_pub = rospy.Publisher(mavros_set_global_pub, GeoPoseStamped, queue_size= 20)
########## Subscribers ##################
self.local_atual = rospy.Subscriber(mavros_local_atual, PoseStamped, self.local_callback)
self.state_sub = rospy.Subscriber(mavros_state_sub, State, self.state_callback, queue_size=10)
self.battery_sub = rospy.Subscriber(mavros_battery_sub, BatteryState, self.battery_callback)
self.global_position_sub = rospy.Subscriber(mavros_global_position_sub, NavSatFix, self.global_callback)
self.extended_state_sub = rospy.Subscriber(extended_state_sub, ExtendedState, self.extended_state_callback, queue_size=2)
self.LAND_STATE = ExtendedState.LANDED_STATE_UNDEFINED # landing state
'''
LANDED_STATE_UNDEFINED = 0
LANDED_STATE_ON_GROUND = 1
LANDED_STATE_IN_AIR = 2
LANDED_STATE_TAKEOFF = 3
LANDED_STATE_LANDING = 4
'''
service_timeout = 30
rospy.loginfo("waiting for ROS services")
try:
rospy.wait_for_service('mavros/param/get', service_timeout)
rospy.wait_for_service('mavros/cmd/arming', service_timeout)
rospy.wait_for_service('mavros/set_mode', service_timeout)
rospy.loginfo("ROS services are up")
except rospy.ROSException:
rospy.logerr("failed to connect to services")
def __init__(self):
self.parser = re.compile(r'V(\d+(\.\d*)?|\.\d+)')
""" Extracts voltage reading from the Arduino serial message.
:returns: voltage data string reported, may be empty.
"""
self.pub = rospy.Publisher('battery0', BatteryState, queue_size=1)
# initialize constant fields in battery message
self.battery = BatteryState(
current=float('nan'),
charge=float('nan'),
capacity=float('nan'),
design_capacity=float('nan'),
percentage=float('nan'),
power_supply_status=BatteryState.POWER_SUPPLY_STATUS_DISCHARGING,
present=1)
def __init__(self):
self.robot_name = rospy.get_param("~robot_name")
self.lcg_joint_min = rospy.get_param("~lcg_joint_min", -0.01)
self.lcg_joint_max = rospy.get_param("~lcg_joint_max", 0.01)
self.vbs_vol_min = rospy.get_param("~vbs_vol_min", -0.5)
self.vbs_vol_max = rospy.get_param("~vbs_vol_max", 0.5)
self.last_thruster_msg_time = 0.
self.last_thruster_msg = Setpoints(Header(), [0., 0.])
self.joint_states = rospy.Publisher('desired_joint_states',
JointState,
queue_size=10)
self.thrusters = rospy.Publisher('thruster_setpoints',
Setpoints,
queue_size=10)
self.vbs_pub = rospy.Publisher('vbs/setpoint', Float64, queue_size=10)
self.vbs_fb_pub = rospy.Publisher('core/vbs_fb',
PercentStamped,
queue_size=10)
self.lcg_fb_pub = rospy.Publisher('core/lcg_fb',
PercentStamped,
queue_size=10)
self.battery_pub = rospy.Publisher('core/battery_fb',
BatteryState,
queue_size=10)
self.press_pub = rospy.Publisher('core/depth20_pressure',
FluidPressure,
queue_size=10)
rospy.Subscriber("core/rpm_cmd", ThrusterRPMs, self.thruster_callback)
rospy.Subscriber("core/thrust_vector_cmd", ThrusterAngles,
self.angles_callback)
rospy.Subscriber("core/lcg_cmd", PercentStamped, self.lcg_callback)
rospy.Subscriber("core/vbs_cmd", PercentStamped, self.vbs_callback)
rospy.Subscriber("vbs/volume_centered", Float64, self.vbs_vol_callback)
rospy.Subscriber("joint_states", JointState, self.joint_state_callback)
rospy.Subscriber("core/depth20_pressure_sim", FluidPressure,
self.press_callback)
self.battery_msg = BatteryState()
self.battery_msg.voltage = 12.5
self.battery_msg.percentage = 81.
self.battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_GOOD
self.battery_msg.header = Header()
rospy.Timer(rospy.Duration(1), self.battery_callback)
rospy.Timer(rospy.Duration(0.1), self.publish_thruster_callback)
def _create_battery_msg(self):
# Check if data is available
if 'SYS_STATUS' not in self.get_data():
raise Exception('no SYS_STATUS data')
if 'BATTERY_STATUS' not in self.get_data():
raise Exception('no BATTERY_STATUS data')
bat = BatteryState()
self._create_header(bat)
#http://docs.ros.org/jade/api/sensor_msgs/html/msg/BatteryState.html
bat.voltage = self.get_data()['SYS_STATUS']['voltage_battery'] / 1000
bat.current = self.get_data()['SYS_STATUS']['current_battery'] / 100
bat.percentage = self.get_data(
)['BATTERY_STATUS']['battery_remaining'] / 100
self.pub.set_data('/battery', bat)
