diff --git a/mission/internal_status/CMakeLists.txt b/mission/internal_status/CMakeLists.txt deleted file mode 100644 index 6969ffc0..00000000 --- a/mission/internal_status/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(internal_status) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/mission/internal_status/internal_status/__init__.py b/mission/internal_status/internal_status/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/mission/internal_status/src/battery_monitor_lib.cpp b/mission/internal_status/internal_status/battery_monitor_lib.cpp similarity index 95% rename from mission/internal_status/src/battery_monitor_lib.cpp rename to mission/internal_status/internal_status/battery_monitor_lib.cpp index 30922db1..c7e677dd 100644 --- a/mission/internal_status/src/battery_monitor_lib.cpp +++ b/mission/internal_status/internal_status/battery_monitor_lib.cpp @@ -3,15 +3,15 @@ using namespace std; // do i need this? //#include //#include -#include "MCP342x.h" #include "smbus.h" #include +#include "MCP342x.h" -int main() { - cout << "Hello world" << endl; - cout << "smbus included" << endl; - cout << "MCP342x included" << endl; - return 0; +int main(){ + cout << "Hello world" << endl; + cout << "smbus included" << endl; + cout << "MCP342x included" << endl; + return 0; } // include doesn't work well diff --git a/mission/internal_status/src/battery_monitor_lib.py b/mission/internal_status/internal_status/power_sense_module_lib.py similarity index 98% rename from mission/internal_status/src/battery_monitor_lib.py rename to mission/internal_status/internal_status/power_sense_module_lib.py index 99daee93..aaa2fe3a 100644 --- a/mission/internal_status/src/battery_monitor_lib.py +++ b/mission/internal_status/internal_status/power_sense_module_lib.py @@ -1,8 +1,7 @@ import smbus -#import time from MCP342x import MCP342x -class BatteryMonitor: +class PowerSenseModule: def __init__(self): @@ -14,11 +13,11 @@ def __init__(self): self.bus = smbus.SMBus(1) self.channel_voltage = MCP342x(self.bus, self.i2c_adress, - channel=0, + channel=1, resolution=18) # voltage self.channel_current = MCP342x(self.bus, self.i2c_adress, - channel=1, + channel=0, resolution=18) # current #time.sleep(1) @@ -27,6 +26,7 @@ def __init__(self): self.psm_to_battery_current_scale_factor = 37.8788 # A/V self.psm_to_battery_current_offset = 0.330 # V + def get_voltage(self): # Sometimes an I/O timeout or error happens, it will run again when the error disappears try: diff --git a/mission/internal_status/internal_status/power_sense_module_publisher.py b/mission/internal_status/internal_status/power_sense_module_publisher.py new file mode 100644 index 00000000..53a442e7 --- /dev/null +++ b/mission/internal_status/internal_status/power_sense_module_publisher.py @@ -0,0 +1,49 @@ +import internal_status.power_sense_module_lib +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float32 + + +class MinimalPublisher(Node): + + + def __init__(self): + super().__init__('PSM_publisher') + self.PSM = internal_status.power_sense_module_lib.PowerSenseModule() + self.publisher_current = self.create_publisher(Float32, '/asv/power_sense_module/current', 1) + self.publisher_voltage = self.create_publisher(Float32, '/asv/power_sense_module/voltage', 1) + timer_period = 0.5 + self.timer = self.create_timer(timer_period, self.timer_callback) + self.i = 0 + + def timer_callback(self): + current = Float32() + voltage = Float32() + #call the two functions of the power_sense_module_lib to get the current and voltage values of the PSM + current.data = self.PSM.get_current() + voltage.data = self.PSM.get_voltage() + self.publisher_current.publish(current) #publish current value to the "current topic" + self.get_logger().info('Publishing PSM current: "%s"' % current.data) + self.publisher_voltage.publish(voltage) #publish voltage value to the "voltge topic" + self.get_logger().info('Publishing PSM voltage: "%s"' % voltage.data) + self.i += 1 + + +def main(args=None): + rclpy.init(args=args) + + minimal_publisher = MinimalPublisher() + + rclpy.spin(minimal_publisher) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + minimal_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() + + diff --git a/mission/internal_status/internal_status/test_lib.py b/mission/internal_status/internal_status/test_lib.py new file mode 100644 index 00000000..80a94731 --- /dev/null +++ b/mission/internal_status/internal_status/test_lib.py @@ -0,0 +1,7 @@ +import power_sense_module_lib + +bm = power_sense_module_lib.BatteryMonitor() + + +print(bm.get_current()) +print(bm.get_voltage()) \ No newline at end of file diff --git a/mission/internal_status/package.xml b/mission/internal_status/package.xml index d3a695ac..b380f4d9 100644 --- a/mission/internal_status/package.xml +++ b/mission/internal_status/package.xml @@ -3,16 +3,23 @@ internal_status 0.0.0 - TODO: Package description - rose - TODO: License declaration + Publisher of the PSM voltage and current data + vortex + Apache License 2.0 - ament_cmake - ament_lint_auto - ament_lint_common + rclpy + std_msgs + power_sense_module_lib + + + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest - ament_cmake + ament_python diff --git a/mission/internal_status/resource/internal_status b/mission/internal_status/resource/internal_status new file mode 100644 index 00000000..e69de29b diff --git a/mission/internal_status/setup.cfg b/mission/internal_status/setup.cfg new file mode 100644 index 00000000..bf1f58bc --- /dev/null +++ b/mission/internal_status/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/internal_status +[install] +install_scripts=$base/lib/internal_status diff --git a/mission/internal_status/setup.py b/mission/internal_status/setup.py new file mode 100644 index 00000000..41d9f477 --- /dev/null +++ b/mission/internal_status/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'internal_status' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='vortex', + maintainer_email='rose.j.kapps@gmail.com', + description='Publisher of the PSM voltage and current data', + license='Apache License 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'power_sense_module_publisher = internal_status.power_sense_module_publisher:main', + ], + }, +) diff --git a/mission/internal_status/src/battery_monitor.py b/mission/internal_status/src/battery_monitor.py deleted file mode 100755 index f0a1faec..00000000 --- a/mission/internal_status/src/battery_monitor.py +++ /dev/null @@ -1,164 +0,0 @@ -#!/usr/bin/python3 -import time -import rospy -import smbus - -from MCP342x import MCP342x -from std_msgs.msg import Float32 - - -class BatteryMonitor: - - def __init__(self): - rospy.init_node("battery_monitor") ###########NODE - - # Parameters - # to read voltage and current from ADC on PDB through I2C - self.i2c_adress = 0x69 ###########DRIVER - - # init of I2C bus communication - self.bus = smbus.SMBus(1) - self.channel_voltage = MCP342x(self.bus, - self.i2c_adress, - channel=0, - resolution=18) # voltage - self.channel_current = MCP342x(self.bus, - self.i2c_adress, - channel=1, - resolution=18) # current ###########DRIVER - time.sleep(1) - - # Convertion ratios taken from PSM datasheet at: https://bluerobotics.com/store/comm-control-power/control/psm-asm-r2-rp/ - self.psm_to_battery_voltage = 11.0 # V/V - self.psm_to_battery_current_scale_factor = 37.8788 # A/V - self.psm_to_battery_current_offset = 0.330 # V ##########DRIVER - - # getting params in the ROS-config file (beluga.yaml) - self.critical_level = rospy.get_param("/battery/thresholds/critical", - default=13.5) - self.warning_level = rospy.get_param("/battery/thresholds/warning", - default=14.5) ###########NODE - - # Polling intervals in seconds delay - system_interval = rospy.get_param("/battery/system/interval", 1) - logging_interval = rospy.get_param("/battery/logging/interval", 5) ###########NODE - - # Local variables - self.system_voltage = 0.0 - self.system_current = 0.0 ##########DRIVER -> WHY - - - self.system_voltage_state = ( - "No receive" # should only be "No receive", "Error", "Received" - ) - self.system_current_state = ( - "No receive" # should only be "No receive", "Error try: - ) - - # Create ROS publishers - self.system_voltage_level_pub = rospy.Publisher( - "/auv/battery_level/system", Float32, queue_size=1) - - self.system_current_level_pub = rospy.Publisher( - "/auv/current_level/system", Float32, queue_size=1) ##########NODE - - - # set up callbacks - self.log_timer = rospy.Timer( - rospy.Duration(secs=logging_interval), - self.log_cb) # for logging on ROS terminal ##########NODE - - - self.system_timer = rospy.Timer( - rospy.Duration(secs=system_interval), - self.system_cb, # will update and publish measurements to ROS ##########ROS - ) - - rospy.loginfo("BatteryMonitor initialized") - - def system_cb(self, event): ###########NODE - self.read_PSM_voltage() - self.read_PSM_current() - - self.system_voltage_level_pub.publish(self.system_voltage) - self.system_current_level_pub.publish(self.system_current) - - if self.system_voltage < self.critical_level: - rospy.logerr( - f"Critical voltage: {self.system_voltage}V! Shutting down all active nodes!" - ) - rospy.logerr(f"HAHA just kidding, let's blow up these batteries!") - # os.system("rosnode kill -a") - - def log_cb(self, event): - if self.system_voltage_state == "Received": - self.log_voltage(self.system_voltage, "system") - if self.system_voltage_state == "Error": - rospy.logwarn(f"I2C Bus IOerror") - if self.system_voltage_state == "No receive": - rospy.loginfo("No voltage recieved from system yet.") - - if self.system_current_state == "Received": - self.log_current(self.system_current, "system") - if self.system_current_state == "Error": - rospy.logwarn(f"I2C Bus IOerror") - if self.system_current_state == "No receive": - rospy.loginfo("No current recieved from system yet.") #use switch in c++ - - def log_voltage(self, voltage, title): - if voltage == 0: - rospy.loginfo("Voltage is zero. Killswitch is probably off.") - - elif voltage <= self.warning_level: - rospy.logwarn("%s voltage: %.3f V" % (title, voltage)) - - else: - rospy.loginfo("%s voltage: %.3f V" % (title, voltage)) - - def log_current(self, current, title): - if current <= 0: - rospy.loginfo("No current draw...") - else: - rospy.loginfo("%s current: %.3f A" % (title, current)) - - def read_PSM_voltage(self): - # Sometimes an I/O timeout or error happens, it will run again when the error disappears - try: #what is the function convert_and_read? - self.system_voltage = (self.channel_voltage.convert_and_read() * - self.psm_to_battery_voltage) - - if self.system_voltage_state != "Received": - self.system_voltage_state = "Received" - - except IOError: - self.I2C_error_counter_voltage += 1 - self.system_voltage_state = "Error" - rospy.logwarn(f"I2C Bus IOerror") # for debug - - def read_PSM_current(self): - try: - self.system_current = (self.channel_current.convert_and_read() - #from where comes this function -> MCP342x? - self.psm_to_battery_current_offset - ) * self.psm_to_battery_current_scale_factor - #I don't from where the formula comes - - if self.system_current_state != "Received": - self.system_current_state = "Received" - - except IOError: - self.I2C_error_counter_current += 1 #this variable isn't initialized? - self.system_current_state = "Error" - # rospy.logwarn(f"I2C Bus IOerror. Voltage error counter : {self.I2C_error_counter_current}") - - def shutdown(self): - self.system_timer.shutdown() - self.log_timer.shutdown() - self.bus.close() - - -if __name__ == "__main__": - bm = BatteryMonitor() - try: - rospy.spin() - finally: - bm.shutdown() diff --git a/mission/internal_status/src/battery_monitor_node.py b/mission/internal_status/src/battery_monitor_node.py deleted file mode 100644 index 3e5acddc..00000000 --- a/mission/internal_status/src/battery_monitor_node.py +++ /dev/null @@ -1,47 +0,0 @@ -import rclpy -from rclpy.node import Node - -#from std_msgs.msg import String -from std_msgs.msg import Float32 - -system_voltage = 0.1 -system_current = 0.2 #to remove when the lib is ready - -class MinimalPublisher(Node): - - def __init__(self): - super().__init__('minimal_publisher') - self.publisher_ = self.create_publisher(Float32, 'topic', 1) - timer_period = 0.5 # seconds - self.timer = self.create_timer(timer_period, self.timer_callback) - self.i = 0 - - def timer_callback(self): - current = Float32() - voltage = Float32() - # call the two functions of the lib to get the current and voltage values - current.data = system_current - voltage.data = system_voltage - self.publisher_.publish(current) - self.get_logger().info('Publishing: "%s"' % current.data) - self.publisher_.publish(voltage) - self.get_logger().info('Publishing: "%s"' % voltage.data) - self.i += 1 - - -def main(args=None): - rclpy.init(args=args) - - minimal_publisher = MinimalPublisher() - - rclpy.spin(minimal_publisher) - - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - minimal_publisher.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/mission/internal_status/test/test_copyright.py b/mission/internal_status/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/mission/internal_status/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/mission/internal_status/test/test_flake8.py b/mission/internal_status/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/mission/internal_status/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/mission/internal_status/test/test_pep257.py b/mission/internal_status/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/mission/internal_status/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings'