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 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 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 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 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 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(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 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 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):
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("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):
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 __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 __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 __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 _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)
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 __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 timerBatteryCB(self,event):
output = chr(0x5a) + chr(0x06) + chr(0x01) + chr(0x07) + chr(0x00) + chr(0xe4) #0xe4 is CRC-8 value
while(self.serialIDLE_flag):
time.sleep(0.01)
self.serialIDLE_flag = 3
try:
while self.serial.out_waiting:
pass
self.serial.write(output)
except:
rospy.logerr("Battery Command Send Faild")
self.serialIDLE_flag = 0
msg = BatteryState()
msg.header.stamp = self.current_time
msg.header.frame_id = self.baseId
msg.voltage = float(self.Vvoltage/1000.0)
msg.current = float(self.Icurrent/1000.0)
self.battery_pub.publish(msg)
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 _create_battery_msg(self,topic):
""" Create battery message from ROV data
Raises:
Exception: No data available
"""
# 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_topics[topic][3].publish(bat)
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 step(self):
if self._rnet._battery is not None:
bs_msg = BatteryState()
bs_msg.header.stamp = rospy.Time.now()
bs_msg.percentage = 1.0 * self._rnet._battery
self._bat_pub.publish(bs_msg)
if (rospy.Time.now() - self._last_cmd).to_sec() > self._cmd_timeout:
# zero-out velocity commands
self._cmd_vel.linear.x = 0
self._cmd_vel.angular.z = 0
self._rnet._cmd_vel = self._cmd_vel
if self._disable_chair_joy:
cf = self._joy_frame
else:
cf = self._rnet.recvfrom(timeout=0.1) #16)
if cf is not None:
cf = aid_str(cf)
# TODO : calibrate to m/s and scale accordingly
# currently, v / w are expressed in fractions where 1 = max fw, -1 = max bw
v = np.clip(self._v_scale * self._cmd_vel.linear.x, -1.0, 1.0)
w = np.clip(self._w_scale * self._cmd_vel.angular.z, -1.0, 1.0)
if cf == self._joy_frame:
# for joy : y=fw, x=turn; 0-256
cmd_y = 0x100 + int(v * 100) & 0xFF
cmd_x = 0x100 + int(-w * 100) & 0xFF
if np.abs(v) > self._min_v or np.abs(w) > self._min_w:
self._rnet.send(self._joy_frame + '#' + dec2hex(cmd_x, 2) +
dec2hex(cmd_y, 2))
else:
# below thresh, stop
self._rnet.send(self._joy_frame + '#' + dec2hex(0, 2) +
dec2hex(0, 2))
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 _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 __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 __init__(self):
super(NeatoNode, self).__init__('neato_botvac')
self.bot = BotvacDriver('/dev/ttyACM0',
callbacks=BotvacDriverCallbacks(
encoders=self.encoders_cb,
battery=self.battery_cb,
accel=self.accel_cb,
scan=self.scan_cb))
self.scan_pub = self.create_publisher(LaserScan, 'scan', 10)
self.battery_pub = self.create_publisher(BatteryState, 'battery', 2)
self.odom_pub = self.create_publisher(Odometry, 'odom', 10)
self.tf_pub = self.create_publisher(TFMessage, 'tf', 10)
self.imu_pub = self.create_publisher(Imu, 'imu', 10)
self.cmd_vel = (0, 0)
self.last_cmd_vel = self.get_clock().now()
self.cmd_vel_timeout = Duration(seconds=0.2)
self.cmd_sub = self.create_subscription(Twist, 'cmd_vel',
self.cmd_vel_cb, 10)
self.scan_msg = LaserScan(angle_min=0.0,
angle_max=deg_to_rad(359),
angle_increment=deg_to_rad(1.0),
time_increment=0.0,
scan_time=0.,
range_min=0.2,
range_max=5.0)
self.scan_msg.header.frame_id = 'laser_link'
self.battery_msg = BatteryState()
self.battery_msg.header.frame_id = 'base_link'
self.imu_msg = Imu(
orientation_covariance=[0, 0, 0, 0, 0, 0, 0, 0, 0],
angular_velocity_covariance=[-1, 0, 0, 0, 0, 0, 0, 0, 0],
linear_acceleration_covariance=[0, 0, 0, 0, 0, 0, 0, 0, 0],
)
self.imu_msg.header.frame_id = 'base_link'
self.odom_to_base_tf = TransformStamped(header=Header(frame_id='odom'),
child_frame_id='base_link')
self.odometer = Odometer(self.get_clock(), self.bot.base_width)
self.tf_timer = self.create_timer(0.025, self.pub_tf)
self.scan_timer = self.create_timer(0.2, self.bot.requestScan)
self.battery_timer = self.create_timer(1, self.bot.requestBattery)
self.encoder_timer = self.create_timer(0.1, self.bot.requestEncoders)
self.accel_timer = self.create_timer(0.1, self.bot.requestAccel)
self.send_cmd_timer = self.create_timer(0.1, self.send_cmd_vel)
def _make_state(location, voltage, current):
"""Create battery state message. Most information is unavailable."""
nan = float('NaN')
return BatteryState(
header=msg.header,
voltage=voltage,
current=current,
charge=nan,
capacity=nan,
design_capacity=nan,
percentage=nan,
power_supply_status=supply_status,
power_supply_health=BatteryState.POWER_SUPPLY_HEALTH_UNKNOWN,
power_supply_technology=BatteryState.
POWER_SUPPLY_TECHNOLOGY_LION,
present=True,
location=location)
def battery_republisher():
global MotorDriverPort
#print "Battery republisher launched"
bat_status = BatteryState()
BAT_FULL = 6*4.2
BAT_MIN = 6*3.3
bat_status.header.frame_id = 'robot'
bat_status.current = float('nan')
bat_status.charge = float('nan')
bat_status.capacity = float('nan')
bat_status.design_capacity = float('nan')
bat_status.power_supply_technology = BatteryState().POWER_SUPPLY_TECHNOLOGY_LIPO
for cell in range(0, 6):
bat_status.cell_voltage.append(float('nan'))
bat_status.location = 'Main Battery'
bat_status.serial_number = "NA"
while 1:
MotorDriverPort.write('GetBatTotal\n')
sleep(0.05)
recv_data = MotorDriverPort.readline()
print recv_data
# print recv_data
bat_status.header.stamp = rospy.Time.now()
try:
bat_status.voltage = float(recv_data)
bat_status.percentage = bat_status.voltage/BAT_FULL
bat_status.present = bat_status.voltage<BAT_FULL and bat_status.voltage>BAT_MIN
battery_pub.publish(bat_status)
except:
if DEBUG:
print "Receive Error"
else:
pass
sleep(0.01)
def publish_state(self):
state = BatteryState()
state.header.frame_id = "usv"
state.header.stamp = rospy.Time.now()
state.voltage = self.voltage
state.current = self.power_battery
state.charge = self.charging_current
#state.capacity = self.design_capacity * (self.percentage / 100.0)
state.design_capacity = self.design_capacity
state.percentage = (self.percentage/100.0)
state.power_supply_status = self.state_charging
state.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNKNOWN
state.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LIPO
state.present = True
state.cell_voltage = [self.voltage]
state.location = "Slot 1"
state.serial_number = "SUSV LIPO 3000mAH"
self.pub.publish(state)
