Пример #1
0
    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
Пример #2
0
    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)
Пример #3
0
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)
Пример #4
0
 def on_new_telemetry(self, message):
     for servo in message.servos:
         self.voltages[servo.id] = servo.voltage
     if len(self.voltages) == SERVO_COUNT:
         voltages = self.voltages.values()
         self.voltages.clear()
         voltage = max(voltages)
         battery_state = BatteryState()
         battery_state.header.stamp = rospy.Time.now()
         battery_state.voltage = voltage
         battery_state.current = float("nan")
         battery_state.charge = float("nan")
         battery_state.capacity = float("nan")
         battery_state.design_capacity = float("nan")
         battery_state.percentage = 100 - (MAX_VOLTAGE - voltage) / (
             MAX_VOLTAGE - MIN_VOLTAGE) * 100
         battery_state.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
         battery_state.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNKNOWN
         battery_state.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LIPO
         battery_state.present = True
         battery_state.cell_voltage = [float("nan")] * 3
         battery_state.location = "Primary batter bay"
         battery_state.serial_number = "N/A"
         self.battery_publisher.publish(battery_state)
         # skip the first check so that you don't get a warning if battery is already bellow some value
         if self.first_check:
             self.first_check = False
             self.lowest_recorded_voltage = voltage
             return
         if voltage < 10.2:
             if self.last_critical_voltage_warning + self.critical_voltage_warning_period < rospy.Time.now(
             ):
                 self.speech_publisher.publish("battery_critical")
                 self.face_color_publisher.publish("flash:red")
                 self.last_critical_voltage_warning = rospy.Time.now()
         elif voltage < 11 and self.lowest_recorded_voltage >= 11:
             self.speech_publisher.publish("battery_below_11")
         elif voltage < 12 and self.lowest_recorded_voltage >= 12:
             self.speech_publisher.publish("battery_below_12")
         if voltage < self.lowest_recorded_voltage:
             self.lowest_recorded_voltage = voltage
Пример #5
0
    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)
Пример #6
0
def batRead():
    bat_dir = "/sys/devices/platform/7000c400.i2c/i2c-1/1-0042/iio_device/"
    volt0_in = open(bat_dir + "in_voltage0_input")
    curr0_in = open(bat_dir + "in_current0_input")

    rospy.init_node('BatRead')
    pub = rospy.Publisher('jetson_battery', BatteryState, queue_size=20)

    rate = rospy.Rate(5)  # 5hz
    while not rospy.is_shutdown():
        try:
            # Read voltage in mV, store in V
            voltage = float(volt0_in.read().strip()) / 1000
            volt0_in.seek(0)

            # Read voltage in mA, store in A
            current = float(curr0_in.read().strip()) / 1000
            curr0_in.seek(0)
        except (IOError, ValueError) as e:
            rospy.logerr("I/O error: {0}".format(e))
        else:
            bat_msg = BatteryState()
            bat_msg.header.stamp = rospy.Time.now()
            bat_msg.voltage = voltage
            bat_msg.current = -current
            bat_msg.charge = float('NaN')
            bat_msg.capacity = float('NaN')
            bat_msg.design_capacity = float('NaN')
            bat_msg.percentage = float('NaN')
            bat_msg.power_supply_status = bat_msg.POWER_SUPPLY_STATUS_UNKNOWN
            bat_msg.power_supply_health = bat_msg.POWER_SUPPLY_HEALTH_UNKNOWN
            bat_msg.power_supply_technology = bat_msg.POWER_SUPPLY_TECHNOLOGY_UNKNOWN
            bat_msg.present = True

            rospy.logdebug('New battery message: %s' % bat_msg)
            pub.publish(bat_msg)

        rate.sleep()
    def _hw_robot_state_cb(self, msg):
        self._get_wallbanger_state_pub.publish(msg.in_autonomous_mode)

        batteryMsg = BatteryState()

        if msg.robot_power_state == RobotState.ESTOP_ACTIVE:
            batteryMsg.voltage = float('nan')
            batteryMsg.percentage = float('nan')
        else:
            batteryMsg.voltage = msg.voltage  # volts
            batteryMsg.percentage = max(0, min(msg.voltage / 12.0,
                                               1))  # volts / volts-nominal

        batteryMsg.current = float('nan')
        batteryMsg.charge = float('nan')
        batteryMsg.capacity = float('nan')
        batteryMsg.design_capacity = 14.0  #AH (2x batteries)
        batteryMsg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_UNKNOWN
        batteryMsg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNKNOWN
        batteryMsg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_UNKNOWN
        batteryMsg.present = True

        self._battery_pub.publish(batteryMsg)

        self._watchdog_tripped_pub.publish(msg.watchdog_tripped)

        strMsg = String()
        if msg.robot_power_state == RobotState.POWER_GOOD:
            strMsg.data = "power-good"
        elif msg.robot_power_state == RobotState.POWER_LOW:
            strMsg.data = "power-low"
        elif msg.robot_power_state == RobotState.POWER_EMERGENCY:
            strMsg.data = "power-emergency"
        elif msg.robot_power_state == RobotState.ESTOP_ACTIVE:
            strMsg.data = "estop-active"

        self._robot_state_pub.publish(strMsg)
 def _status_to_battery_(status):
     """
     Converts a ds4_driver/Status to sensor_msgs/BatteryState
     Reference: https://www.psdevwiki.com/ps4/DualShock_4#Specifications
     :param status:
     :type status: Status
     :return:
     """
     msg = BatteryState()
     msg.header = status.header
     msg.percentage = status.battery_percentage
     msg.voltage = Controller.MAX_VOLTAGE * msg.percentage
     msg.current = float('NaN')
     msg.charge = float('NaN')
     msg.capacity = float('NaN')
     msg.design_capacity = 1.0
     if not status.plug_usb:
         msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING
     elif not status.battery_full_charging:
         msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING
     elif status.battery_full_charging:
         msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_FULL
     msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LION
     return msg
def bs_err_inj(tb3_name):
    #Create error-injected topic
    rospy.init_node('batterystate_err_inj')

    #########################################
    #Create new message
    batterystate_msg = BatteryState()

    #Fill message with values
    batterystate_msg.header.seq = 0
    batterystate_msg.header.stamp.secs = 0
    batterystate_msg.header.stamp.nsecs = 0
    batterystate_msg.header.frame_id = ""

    batterystate_msg.voltage = 0.0
    batterystate_msg.current = 0.0
    batterystate_msg.charge = 0.0
    batterystate_msg.capacity = 0.0
    batterystate_msg.design_capacity = 0.0
    batterystate_msg.percentage = 0.0
    batterystate_msg.power_supply_status = 0
    batterystate_msg.power_supply_health = 0
    batterystate_msg.power_supply_technology = 0
    batterystate_msg.bool = 1

    batterystate_msg.cell_voltage = []

    batterystate_msg.location = ""
    batterystate_msg.serial_number = ""

    #########################################

    rate = rospy.Rate(50)

    #Publish message into new topic
    while not rospy.is_shutdown():
        my_pub = rospy.Publisher(tb3_name + 'batterystate_err_inj',
                                 BatteryState,
                                 queue_size=10)
        my_sub = rospy.Subscriber(tb3_name + 'battery_state', BatteryState,
                                  listener)

        #########################################
        #INJECT ERRORS HERE
        batterystate_msg.header.seq = actual_seq
        batterystate_msg.header.stamp.secs = actual_secs
        batterystate_msg.header.stamp.nsecs = actual_nsecs
        batterystate_msg.header.frame_id = actual_frameid

        batterystate_msg.voltage = actual_voltage
        batterystate_msg.current = actual_current
        batterystate_msg.charge = actual_charge
        batterystate_msg.capacity = actual_capacity
        batterystate_msg.design_capacity = actual_designcapacity
        batterystate_msg.percentage = actual_percentage

        batterystate_msg.power_supply_status = actual_powersupplystatus
        batterystate_msg.power_supply_health = actual_powersupplyhealth
        batterystate_msg.power_supply_technology = actual_powersupplytechnology

        batterystate_msg.present = actual_present
        batterystate_msg.cell_voltage = actual_cellvoltage
        batterystate_msg.location = actual_location
        batterystate_msg.serial_number = actual_serialnumber

        #########################################

        my_pub.publish(batterystate_msg)
        rate.sleep()

    rospy.spin()
Пример #10
0
    def publish_state(self):

        current_time = rospy.Time.now()
        self.encoder_left = self.motor_driver.encoder1_get()
        self.encoder_right = self.motor_driver.encoder2_get()
        self.battery_voltage = self.motor_driver.bat_voltage_get()

        msg_battery = BatteryState()
        msg_battery.voltage = self.battery_voltage
        msg_battery.power_supply_status = 2
        msg_battery.power_supply_health = 1
        msg_battery.power_supply_technology = 0
        msg_battery.present = True

        msg_left = JointState()
        msg_left.header.frame_id = self.joint_frames[0]  # + "_hinge"
        msg_left.header.stamp = current_time
        msg_left.header.seq = self.seq

        msg_right = JointState()
        msg_right.header.frame_id = self.joint_frames[1]  # + "_hinge"
        msg_right.header.stamp = current_time
        msg_right.header.seq = self.seq

        self.encoder_left = self.encoder_left
        self.encoder_right = self.encoder_right
        # print("Left",left,"Right",right)
        msg_left.name.append(self.joint_frames[0] +
                             "_hinge")  # 360 ticks/ per wheel turn
        msg_left.position.append(
            self.encoder_left)  # 360 ticks/ per wheel turn

        msg_right.name.append(self.joint_frames[1] + "_hinge")
        msg_right.position.append(
            self.encoder_right)  # 360 ticks/ per wheel turn

        self.pub_joint_right.publish(msg_right)
        self.pub_joint_left.publish(msg_left)
        self.pub_battery_state.publish(msg_battery)

        # extract the wheel velocities from the tick signals count
        #
        if (current_time - self.past_time).to_sec() != 0 and (
                self.encoder_left != self._PreviousLeftEncoderCounts
                or self.encoder_right != self._PreviousRightEncoderCounts):
            msg_odom = Odometry()
            # --- Get the distance delta since the last period ---
            deltaLeft = (self.encoder_left - self._PreviousLeftEncoderCounts
                         ) * self.distance_per_count
            deltaRight = (self.encoder_right - self._PreviousRightEncoderCounts
                          ) * self.distance_per_count
            # --- Update the local position and orientation ---
            self.local_pose["x"] = (
                deltaLeft + deltaRight) / 2.0  # distance in X direction
            self.local_pose["y"] = 0.0
            # distance in Y direction
            self.local_pose["th"] = (
                deltaRight - deltaLeft
            ) / self.length_between_two_wheels  # Change in orientation
            if -360 > self.local_pose["th"] or self.local_pose["th"] > 360:
                return
            # self.yaw += self.local_pose["th"]

            # --- Update the velocity ---
            leftDistance = (deltaLeft - self.pos_left_past)
            rightDistance = (deltaRight - self.pos_right_past)
            delta_distance = (leftDistance + rightDistance) / 2.0
            delta_theta = (rightDistance - leftDistance
                           ) / self.length_between_two_wheels  # in radians

            self.local_vel["x_lin"] = delta_distance / (
                current_time - self.past_time).to_sec()  # Linear x velocity
            self.local_vel["y_lin"] = 0.0
            self.local_vel["y_ang"] = (
                delta_theta / (current_time - self.past_time).to_sec()
            )  # In radians per/sec

            odom_quat = tf.transformations.quaternion_from_euler(
                0, 0, self.local_pose["th"])
            # first, we'll publish the transform over tf
            # send the transform
            self.odom_broadcaster.sendTransform(
                (self.local_pose["x"], self.local_pose["y"], 0.), odom_quat,
                current_time, self.base_link_topic[1:], self.odom_topic[1:])
            msg_odom.header.stamp = current_time
            msg_odom.header.frame_id = self.odom_topic[1:]
            msg_odom.header.seq = self.seq
            # set the position
            msg_odom.pose.pose = Pose(
                Point(self.local_pose["x"], self.local_pose["y"], 0.),
                Quaternion(*odom_quat))

            # set the velocity
            msg_odom.child_frame_id = self.base_link_topic[1:]
            msg_odom.twist.twist = Twist(
                Vector3(self.local_vel["x_lin"], self.local_vel["y_lin"], 0),
                Vector3(0, 0, self.local_vel["y_ang"]))

            # publish the message
            self.odom_pub.publish(msg_odom)
            self.seq += 1

            # ---  Save the last position values ---
            self.pos_left_past = deltaLeft
            self.pos_right_past = deltaRight

            self.past_time = current_time
            self._PreviousLeftEncoderCounts = self.encoder_left
            self._PreviousRightEncoderCounts = self.encoder_right
Пример #11
0
    def spin(self):
        encoders = [0, 0]

        self.x = 0  # position in xy plane
        self.y = 0
        self.th = 0
        then = rospy.Time.now()

        # things that don't ever change
        scan_link = rospy.get_param('~frame_id', 'base_laser_link')
        scan = LaserScan(header=rospy.Header(frame_id=scan_link))

        scan.angle_min = 0.0
        scan.angle_max = 359.0 * pi / 180.0
        scan.angle_increment = pi / 180.0
        scan.range_min = 0.020
        scan.range_max = 5.0

        odom = Odometry(header=rospy.Header(frame_id="odom"),
                        child_frame_id='base_footprint')

        # main loop of driver
        r = rospy.Rate(20)
        cycle_count = 0
        self.bumperEngaged = None

        while not rospy.is_shutdown():

            # Emergency shutdown checks.
            if int(self.chargerValues["FuelPercent"]) < 10:
                rospy.logerr(
                    "Neato battery is less than 10%. Terminating Node")
                rospy.signal_shutdown(
                    "Neato battery is less than 10%. Terminating Node")
                break
            if self.chargerValues["BatteryFailure"] == "1":
                rospy.logerr("Neato battery failure. Terminating Node")
                rospy.signal_shutdown(
                    "Neato battery failure. Terminating Node")
                break
            if self.chargerValues["EmptyFuel"] == "1":
                rospy.logerr("Neato battery is empty. Terminating Node")
                break

            # get motor encoder values
            left, right = self.getMotors()

            if not self.lifted and cycle_count % 2 == 0:

                # bumper engaged procedure
                # left or right bumpers
                if self.moving_forward and self.bumperEngaged == 0:
                    # left bump
                    self.setMotors(-100, -110, MAX_SPEED / 2)

                if self.moving_forward and self.bumperEngaged == 1:
                    # right bump
                    self.setMotors(-110, -100, MAX_SPEED / 2)

                # all other bumpers
                elif self.moving_forward and self.bumperEngaged > 1:
                    self.setMotors(-100, -100, MAX_SPEED / 2)

                # undock proceedure
                if self.cmd_vel[0] and self.chargerValues["ChargingActive"]:
                    self.setMotors(-400, -400, MAX_SPEED / 2)

                else:
                    # send updated movement commands
                    self.setMotors(
                        self.cmd_vel[0], self.cmd_vel[1],
                        max(abs(self.cmd_vel[0]), abs(self.cmd_vel[1])))

            self.old_vel = self.cmd_vel

            # prepare laser scan
            scan.header.stamp = rospy.Time.now()

            self.getldsscan()
            scan.ranges, scan.intensities = self.getScanRanges()

            # now update position information
            dt = (scan.header.stamp - then).to_sec()
            then = scan.header.stamp

            d_left = (left - encoders[0]) / 1000.0
            d_right = (right - encoders[1]) / 1000.0
            encoders = [left, right]

            dx = (d_left + d_right) / 2
            dth = (d_right - d_left) / (self.base_width / 1000.0)

            x = cos(dth) * dx
            y = -sin(dth) * dx
            self.x += cos(self.th) * x - sin(self.th) * y
            self.y += sin(self.th) * x + cos(self.th) * y
            self.th += dth

            # prepare tf from base_link to odom
            quaternion = Quaternion()
            quaternion.z = sin(self.th / 2.0)
            quaternion.w = cos(self.th / 2.0)

            # prepare odometry
            odom.header.stamp = rospy.Time.now()
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.twist.twist.linear.x = dx / dt
            odom.twist.twist.angular.z = dth / dt

            # read sensors and data
            # Neato cannot handle reads of all sensors every cycle.
            # use cycle_count to rate limit the reads or
            # you will get errors like:
            # navigation costmap2DROS transform timeout.
            # Could not get robot pose.

            if cycle_count % 2 == 0:
                self.getDigitalSensors()

                for i, b in enumerate(
                    ("LSIDEBIT", "RSIDEBIT", "LFRONTBIT", "RFRONTBIT")):

                    engaged = None
                    engaged = self.digitalSensors[b]  # Bumper Switches
                    self.bumperHandler(b, engaged, i)

            if cycle_count == 2:
                self.getAnalogSensors()

                for i, b in enumerate(("LeftDropInMM", "RightDropInMM",
                                       "LeftMagSensor", "RightMagSensor")):

                    engaged = None
                    if i < 2:
                        # Optical Sensors (no drop: ~0-60)
                        engaged = (self.analogSensors[b] > 100)
                    else:
                        # Mag Sensors (no mag: ~ +/-20)
                        engaged = (abs(self.analogSensors[b]) > 20)

                    self.bumperHandler(b, engaged, i)

            if cycle_count == 1:
                self.getButtons()

                # region Publish Button Events

                for i, b in enumerate(
                    ("BTN_SOFT_KEY", "BTN_SCROLL_UP", "BTN_START", "BTN_BACK",
                     "BTN_SCROLL_DOWN")):
                    engaged = (self.buttons[b] == 1)
                    if engaged != self.state[b]:
                        buttonEvent = ButtonEvent()
                        buttonEvent.button = i
                        buttonEvent.engaged = engaged
                        self.buttonEventPub.publish(buttonEvent)

                    self.state[b] = engaged

                # endregion Publish Button Info

            if cycle_count == 3:
                self.getCharger()

                # region Publish Battery Info
                # pulls data from analogSensors and charger info to publish battery state
                battery = BatteryState()
                # http://docs.ros.org/en/api/sensor_msgs/html/msg/BatteryState.html

                power_supply_health = 1  # POWER_SUPPLY_HEALTH_GOOD
                if self.chargerValues["BatteryOverTemp"]:
                    power_supply_health = 2  # POWER_SUPPLY_HEALTH_OVERHEAT
                elif self.chargerValues["EmptyFuel"]:
                    power_supply_health = 3  # POWER_SUPPLY_HEALTH_DEAD
                elif self.chargerValues["BatteryFailure"]:
                    power_supply_health = 5  # POWER_SUPPLY_HEALTH_UNSPEC_FAILURE

                power_supply_status = 3  # POWER_SUPPLY_STATUS_NOT_CHARGING
                if self.chargerValues["ChargingActive"]:
                    power_supply_status = 1  # POWER_SUPPLY_STATUS_CHARGING
                elif (self.chargerValues["FuelPercent"] == 100):
                    power_supply_status = 4  # POWER_SUPPLY_STATUS_FULL

                battery.voltage = self.analogSensors[
                    "BatteryVoltageInmV"] // 1000
                # battery.temperature = self.analogSensors["BatteryTemp0InC"]
                battery.current = self.analogSensors["CurrentInmA"] // 1000
                # battery.charge
                # battery.capacity
                # battery.design_capacity
                battery.percentage = self.chargerValues["FuelPercent"]
                battery.power_supply_status = power_supply_status
                battery.power_supply_health = power_supply_health
                battery.power_supply_technology = 1  # POWER_SUPPLY_TECHNOLOGY_NIMH
                battery.present = self.chargerValues['FuelPercent'] > 0
                # battery.cell_voltage
                # battery.cell_temperature
                # battery.location
                # battery.serial_number
                self.batteryPub.publish(battery)

                # endregion Publish Battery Info

            self.publishSensors()
            # region publish lidar and odom
            self.odomBroadcaster.sendTransform(
                (self.x, self.y, 0),
                (quaternion.x, quaternion.y, quaternion.z, quaternion.w), then,
                "base_footprint", "odom")
            self.scanPub.publish(scan)
            self.odomPub.publish(odom)

            # endregion publish lidar and odom

            # wait, then do it again
            r.sleep()

            cycle_count = cycle_count + 1
            if cycle_count == 4:
                cycle_count = 0

        # shut down
        self.setLed(LED.BacklightOff)
        self.setLed(LED.ButtonOff)
        self.setLdsRotation("Off")
        self.testmode("Off")
Пример #12
0
    # print out pure ADC voltage
    # rospy.loginfo("Battery: %.1f Volt" % voltage)

    # completing the ROS message
    # @SA http://docs.ros.org/jade/api/sensor_msgs/html/msg/BatteryState.html
    battery_msg.charge = 0.0
    battery_msg.capacity = 0.0
    battery_msg.design_capacity = 2.2  # 2.2 Ah
    battery_msg.percentage = 0.0  # 0 to 1!
    #    battery_msg.power_supply_status = POWER_SUPPLY_STATUS_DISCHARGING
    #    battery_msg.power_supply_health = POWER_SUPPLY_HEALTH_GOOD
    #    battery_msg.power_supply_technology = POWER_SUPPLY_TECHNOLOGY_LIPO
    battery_msg.power_supply_status = 2
    battery_msg.power_supply_health = 1
    battery_msg.power_supply_technology = 3
    battery_msg.present = True
    battery_msg.cell_voltage = [float(0)]
    battery_msg.location = "1"  # The location into which the battery is inserted. (slot number or plug)
    battery_msg.serial_number = "1"

    # this is the battery voltage
    # @TODO strange, this assignment as to be exactly here...
    battery_msg.voltage = float(voltage)

    # publish voltage
    pubBattery.publish(battery_msg)

    # Sleep for a second until the next reading.
    rospy.sleep(sleepTime)