diff --git a/cob_phidget_power_state/CHANGELOG.rst b/cob_phidget_power_state/CHANGELOG.rst deleted file mode 100644 index 93a25220b..000000000 --- a/cob_phidget_power_state/CHANGELOG.rst +++ /dev/null @@ -1,153 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package cob_phidget_power_state -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.7.16 (2024-02-20) -------------------- - -0.7.15 (2023-11-06) -------------------- - -0.7.14 (2022-11-17) -------------------- - -0.7.13 (2022-07-29) -------------------- - -0.7.12 (2022-03-15) -------------------- - -0.7.11 (2022-01-12) -------------------- - -0.7.10 (2021-12-23) -------------------- - -0.7.9 (2021-11-26) ------------------- - -0.7.8 (2021-10-19) ------------------- - -0.7.7 (2021-08-02) ------------------- - -0.7.6 (2021-05-10) ------------------- - -0.7.5 (2021-04-06) ------------------- - -0.7.4 (2020-10-14) ------------------- -* Merge pull request `#417 `_ from fmessmer/test_noetic - test noetic -* Bump CMake version to avoid CMP0048 warning -* Contributors: Felix Messmer, fmessmer - -0.7.3 (2020-03-18) ------------------- - -0.7.2 (2020-03-18) ------------------- -* Merge pull request `#409 `_ from LoyVanBeek/feature/python3_compatibility - [ci_updates] pylint + Python3 compatibility -* fix pylint errors -* Merge pull request `#408 `_ from fmessmer/ci_updates - [travis] ci updates -* catkin_lint fixes -* Contributors: Felix Messmer, fmessmer - -0.7.1 (2019-11-07) ------------------- - -0.7.0 (2019-08-06) ------------------- -* Merge pull request `#396 `_ from HannesBachter/indigo_dev - 0.6.15 -* Contributors: Felix Messmer - -0.6.15 (2019-07-17) -------------------- - -0.6.14 (2019-06-07) -------------------- - -0.6.13 (2019-03-14) -------------------- - -0.6.12 (2018-07-21) -------------------- -* update maintainer -* Contributors: fmessmer - -0.6.11 (2018-01-07) -------------------- -* Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev -* Merge pull request `#341 `_ from ipa-fxm/APACHE_license - use license apache 2.0 -* use license apache 2.0 -* Contributors: Felix Messmer, ipa-fxm, ipa-uhr-mk - -0.6.10 (2017-07-24) -------------------- - -0.6.9 (2017-07-18) ------------------- -* shut down if the voltage divider factor is undefined -* renamed max_voltage to more descriptive voltage_divider_factor -* smooth calculated voltage from phidget board by adapting average over windows of measurements -* Contributors: Benjamin Maidel, Jannik Abbenseth - -0.6.8 (2016-10-10) ------------------- -* fix current sign -* fix current sign -* remove print -* fix namespace and syntax for phidget power state -* Merge branch 'indigo_dev' into feature/phidget_power_state_params - Conflicts: - cob_phidget_power_state/scripts/power_state_phidget.py -* read power state params from param server -* increase voltage limit for empty battery -* cut percentage between 0 and 100 -* updates for changed hardware -* fixed typo -* remove find_package entries as this package only as python nodes -* Update package.xml -* updated license and version -* move power_state_phidget node to new package -* Contributors: Benjamin Maidel, Florian Weisshardt, msh - -0.6.3 (2015-06-17) ------------------- - -0.6.2 (2014-12-15) ------------------- - -0.6.1 (2014-09-17) ------------------- - -0.6.0 (2014-09-09) ------------------- - -0.5.7 (2014-08-26 09:47) ------------------------- - -0.5.6 (2014-08-26 09:42) ------------------------- - -0.5.5 (2014-08-26 08:33) ------------------------- - -0.5.4 (2014-08-25) ------------------- - -0.5.3 (2014-03-31) ------------------- - -0.5.2 (2014-03-21) ------------------- - -0.5.1 (2014-03-20 10:54) ------------------------- diff --git a/cob_phidget_power_state/CMakeLists.txt b/cob_phidget_power_state/CMakeLists.txt deleted file mode 100644 index 593f7d2d5..000000000 --- a/cob_phidget_power_state/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(cob_phidget_power_state) - -find_package(catkin REQUIRED COMPONENTS) - -catkin_package() - -catkin_install_python(PROGRAMS scripts/power_state_phidget.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/src -) diff --git a/cob_phidget_power_state/package.xml b/cob_phidget_power_state/package.xml deleted file mode 100644 index 213c021e3..000000000 --- a/cob_phidget_power_state/package.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - cob_phidget_power_state - 0.7.16 - The cob_phidget_power_state package publishes power state based on phidgets signals. - - Benjamin Maidel - Benjamin Maidel - - Apache 2.0 - - catkin - cob_msgs - cob_phidgets - rospy - - diff --git a/cob_phidget_power_state/scripts/power_state_phidget.py b/cob_phidget_power_state/scripts/power_state_phidget.py deleted file mode 100755 index d134d4a1e..000000000 --- a/cob_phidget_power_state/scripts/power_state_phidget.py +++ /dev/null @@ -1,150 +0,0 @@ -#!/usr/bin/env python -# -# Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -import numpy as np -import rospy -from cob_msgs.msg import PowerState -from cob_phidgets.msg import AnalogSensor - - -class PowerStatePhidget(): - PHIDGET_MAX_VALUE = 999 - PHIDGET_MIN_VALUE = 0 - PERIOD_RECORD_SIZE = 6 - VOLTAGE_COLLECTION_TIME = 6.0 # sec - - def __init__(self): - self.voltage = None - self.current = None - self.last_update = rospy.Time(0) - self.charging = False - try: - self.voltage_divider_factor = rospy.get_param("~voltage_divider_factor") - except KeyError: - raise KeyError("Parameter \"~voltage_divider_factor\" not found on parameter server.") - self.voltage_full = rospy.get_param("~voltage_full", 52.0) - self.voltage_empty = rospy.get_param("~voltage_empty", 38.0) - self.current_max = rospy.get_param("~current_max", 30.0) - self.current_min = rospy.get_param("~current_min", -30.0) - - self.pub_power_state = rospy.Publisher('power_state', PowerState, queue_size=1) - self.sub_analog_sensors = rospy.Subscriber("analog_sensors", AnalogSensor, self.phidget_cb) - - self.pr_next = 0 - self.period_record = [] - self.cb_avg_time = 0.1 - self.voltage_bag_maxlen = 100 - self.voltage_bag = [] - - def append_voltage_bag(self, num): - while len(self.voltage_bag) >= self.voltage_bag_maxlen: - self.voltage_bag.pop(0) - self.voltage_bag.append(num) - - def calculate_voltage(self): - if len(self.voltage_bag) > 0: - self.voltage = np.mean(self.voltage_bag) - - def phidget_cb(self, msg): - # Estimate commands frequency; we do continuously as it can be very different depending on the - # publisher type, and we don't want to impose extra constraints to keep this package flexible - if len(self.period_record) < self.PERIOD_RECORD_SIZE: - self.period_record.append((rospy.Time.now() - self.last_update).to_sec()) - else: - self.period_record[self.pr_next] = (rospy.Time.now() - self.last_update).to_sec() - - self.pr_next += 1 - self.pr_next %= len(self.period_record) - self.last_update = rospy.Time.now() - - if len(self.period_record) <= self.PERIOD_RECORD_SIZE / 2: - # wait until we have some values; make a reasonable assumption (10 Hz) meanwhile - self.cb_avg_time = 0.1 - else: - # enough; recalculate with the latest input - self.cb_avg_time = np.median(self.period_record) - - # now set the max voltage bag size - self.voltage_bag_maxlen = int(self.VOLTAGE_COLLECTION_TIME / self.cb_avg_time) - - voltage_raw = None - current_raw = None - - for i in range(0, len(msg.uri)): - if msg.uri[i] == "voltage": - voltage_raw = msg.value[i] - if msg.uri[i] == "current": - current_raw = msg.value[i] - - if voltage_raw != None: - # Calculation of real voltage - voltage = self.voltage_divider_factor * voltage_raw / self.PHIDGET_MAX_VALUE - voltage = round(voltage, 3) - self.append_voltage_bag(voltage) - - if current_raw != None: - # Calculation of real current - self.current = self.current_min + (self.current_max - self.current_min) * (current_raw - - self.PHIDGET_MIN_VALUE) / (self.PHIDGET_MAX_VALUE - self.PHIDGET_MIN_VALUE) - self.current = round(self.current, 3) - - if self.current > 0: - self.charging = True - else: - self.charging = False - - def calculate_power_consumption(self): - if not self.charging and self.voltage != None and self.current != None: - return round(self.voltage * abs(self.current), 3) - else: - return 0.0 - - def calculate_relative_remaining_capacity(self): - percentage = None - if self.voltage != None: - percentage = round((self.voltage - self.voltage_empty) * 100 / (self.voltage_full - self.voltage_empty), 3) - percentage = min(percentage, 100) - percentage = max(percentage, 0) - return percentage - else: - return 0.0 - - def publish(self): - self.calculate_voltage() - if self.voltage != None and self.current != None and (rospy.Time.now() - self.last_update) < rospy.Duration(1): - ps = PowerState() - ps.header.stamp = self.last_update - ps.voltage = self.voltage - ps.current = self.current - ps.power_consumption = self.calculate_power_consumption() - ps.relative_remaining_capacity = self.calculate_relative_remaining_capacity() - ps.charging = self.charging - self.pub_power_state.publish(ps) - -if __name__ == "__main__": - rospy.init_node("power_state_phidget") - try: - psp = PowerStatePhidget() - except KeyError as e: - rospy.logerr("Shutting down: {}".format(e)) - exit(1) - - rospy.loginfo("power state phidget running") - rate = rospy.Rate(10) - while not rospy.is_shutdown(): - psp.publish() - rate.sleep()