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)