diff --git a/README.md b/README.md index fb1f038..f6e3d7a 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,103 @@ # pet_ros2_currentsensor_ina219_pkg ROS2-publisher for the Current/Voltage sensor INA219. Publish measurement as ROS2-topics. + +**Input:** Current&Voltage shount on a I2C-INA219 sensor-breakout-board. \ +**Output:** ROS node (ROS2) that publish topics with voltage & current values. + + + + +
+ + + .. +
+ +# ROS2 Package/Module Behaviour + +# Prerequisite: Hardware +* Single Board Computer(SBC): Raspberry Pi 3/4 +* Sensor: INA2129 Current Sensor via default I2C adr.=0x4o + +# Prerequisite: Software +* Ubuntu 20.04 (64bit) or newer +* Robot Operating System 2, ROS2 (Version Galathic) +
...do the ROS2-installation stuff...
+ +## Prerequisite: I2C-interface Raspberry Pi 4 / Ubuntu +Prepared by adding additional, i2c communication, Linux-software-packages
+`Ubuntu Shell` +``` +~$ sudo apt install i2c-tools +~$ sudo apt install python3-pip +~$ sudo pip3 install adafruit-blinka +~$ sudo pip3 install adafruit-circuitpython-ina219 +~$ sudo i2cdetect -y 1 + 0 1 2 3 4 5 6 7 8 9 a b c d e f + 00: -- -- -- -- -- -- -- -- -- -- -- -- -- + 10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- + 20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- + 30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- + 40: 40 -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- + 50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- + 60: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- + 70: -- -- -- -- -- -- -- -- +$ sudo chmod a+rw /dev/i2c-1 +``` + +## Dowload and install this ROS2 packages +Create a ROS2 workspace (in my exampel '~/ws_ros2/') \ +Dowload ROS2 package by using 'git clone' + + +`Ubuntu Shell` +``` +~$ mkdir -p ~/ws_ros2/src +~$ cd ~/ws_ros2/src +~/ws_ros2/src$ git clone https://github.com/Pet-Series/pet_ros2_currentsensor_ina219_pkg.git +~/ws_ros2/src$ cd .. +~/ws_ros2$ colcon build --symlink-install +~/ws_ros2$ source /opt/ros/galactic/setup.bash +~/ws_ros2$ source ./install/setup.bash +``` + +# ROS2 Launch sequence +`Ubuntu Shell #1` +``` +$ ros2 run pet_ros2_battery_state_pkg pet_battery_state_ina219_node + [INFO] [1649019010.401689937] [pet_current_sensor_node]: INA219 Current/Voltage sensor. Config register: + [INFO] [1649019010.404738606] [pet_current_sensor_node]: - bus_voltage_range: 0x1 + [INFO] [1649019010.407764240] [pet_current_sensor_node]: - gain: 0x3 + [INFO] [1649019010.410825520] [pet_current_sensor_node]: - bus_adc_resolution: 0x3 + [INFO] [1649019010.413920782] [pet_current_sensor_node]: - shunt_adc_resolution: 0x3 + [INFO] [1649019010.417161487] [pet_current_sensor_node]: - mode: 0x7 + [INFO] [1649019010.420058696] [pet_current_sensor_node]: .... +``` + +`Ubuntu Shell #2` +``` +$ ros2 topic echo /battery_status + header: + stamp: + sec: 1649019055 + nanosec: 523441553 + frame_id: 18650 3S x1P main battery + voltage: 11.143999981880188 + temperature: .nan + current: 0.820000000040233135 + charge: .nan + capacity: .nan + design_capacity: .nan + percentage: .nan + power_supply_status: 0 + power_supply_health: 0 + power_supply_technology: 2 + present: true + cell_voltage: [] + cell_temperature: [] + location: Think "Inside the box" + serial_number: '0000000' + --- +``` \ No newline at end of file diff --git a/doc/pet_ros2_currentsensor(INA219)_wiring.png b/doc/pet_ros2_currentsensor(INA219)_wiring.png new file mode 100644 index 0000000..e248331 Binary files /dev/null and b/doc/pet_ros2_currentsensor(INA219)_wiring.png differ diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..6a38b19 --- /dev/null +++ b/package.xml @@ -0,0 +1,18 @@ + + + + pet_ros2_battery_state_pkg + 0.0.1 + ROS2-publisher for the Current/Voltage sensor INA219. Publish measurement as ROS2-topics. + pi + MIT + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/pet_ros2_battery_state_pkg/UnitTest/INA219.py b/pet_ros2_battery_state_pkg/UnitTest/INA219.py new file mode 100755 index 0000000..e65c515 --- /dev/null +++ b/pet_ros2_battery_state_pkg/UnitTest/INA219.py @@ -0,0 +1,71 @@ +# SPDX-FileCopyrightText: 2021 ladyada for Adafruit Industries +# SPDX-License-Identifier: MIT +# +# $ sudo i2cdetect -y 1 +# 0 1 2 3 4 5 6 7 8 9 a b c d e f +# 00: -- -- -- -- -- -- -- -- +# 10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- +# 20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- +# 30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- +# 40: 40 -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- +# 50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- +# 60: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- +# 70: -- -- -- -- -- -- -- -- +# $ +# $ sudo pip3 install adafruit-circuitpython-ina219 +# $ sudo pip3 install adafruit-blinka +# +# $ sudo python3 INA219.py +# $ python3 INA219.py +# +"""Sample code and test for adafruit_ina219""" + +import time +import board +from adafruit_ina219 import ADCResolution, BusVoltageRange, INA219 + + +i2c_bus = board.I2C() + +ina219 = INA219(i2c_bus) + +print("ina219 test") + +# display some of the advanced field (just to test) +print("Config register:") +print(" bus_voltage_range: 0x%1X" % ina219.bus_voltage_range) +print(" gain: 0x%1X" % ina219.gain) +print(" bus_adc_resolution: 0x%1X" % ina219.bus_adc_resolution) +print(" shunt_adc_resolution: 0x%1X" % ina219.shunt_adc_resolution) +print(" mode: 0x%1X" % ina219.mode) +print("") + +# optional : change configuration to use 32 samples averaging for both bus voltage and shunt voltage +ina219.bus_adc_resolution = ADCResolution.ADCRES_12BIT_32S +ina219.shunt_adc_resolution = ADCResolution.ADCRES_12BIT_32S + +# optional : change voltage range to 16V +ina219.bus_voltage_range = BusVoltageRange.RANGE_16V + +# measure and display loop +while True: + bus_voltage = ina219.bus_voltage # voltage on V- (load side) + shunt_voltage = ina219.shunt_voltage # voltage between V+ and V- across the shunt + current = ina219.current # current in mA + power = ina219.power # power in watts + + # INA219 measure bus voltage on the load side. So PSU voltage = bus_voltage + shunt_voltage + print("Voltage (VIN+) : {:6.3f} V".format(bus_voltage + shunt_voltage)) + print("Voltage (VIN-) : {:6.3f} V".format(bus_voltage)) + print("Shunt Voltage : {:8.5f} V".format(shunt_voltage)) + print("Shunt Current : {:7.4f} A".format(current / 1000)) + print("Power Calc. : {:8.5f} W".format(bus_voltage * (current / 1000))) + print("Power Register : {:6.3f} W".format(power)) + print("") + + # Check internal calculations haven't overflowed (doesn't detect ADC overflows) + if ina219.overflow: + print("Internal Math Overflow Detected!") + print("") + + time.sleep(2) \ No newline at end of file diff --git a/pet_ros2_battery_state_pkg/UnitTest/blinkatest.py b/pet_ros2_battery_state_pkg/UnitTest/blinkatest.py new file mode 100755 index 0000000..4d7d061 --- /dev/null +++ b/pet_ros2_battery_state_pkg/UnitTest/blinkatest.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python3' +# coding = utf-8 +#################################################################################### +# System test for Adafruit BLINKA. +# Make sure that all Python3 package are installed and working. +# Adjust access rights for the I2C and/or GPIO-pins. +# +# ---- install +# $ sudo apt-get install python3-pip +# $ sudo apt-get install i2c-tools +# $ sudo apt-get install libgpiod-dev +# $ sudo apt-get install RPi.GPIO +# $ sudo pip3 install board +# $ sudo pip3 install smbus2 +# $ sudo apt-get install adafruit-blinka +# ($ sudo pip3 install --force-reinstall adafruit-blinka ) +# +# ---- Access to i2c and GPIO +# $ sudo chmod a+rw /dev/i2c-1 +# $ sudo groupadd i2c +# $ sudo usermod -aG i2c pi +# $ sudo usermod -a -G gpio pi +# +# ---- Run the script +# $ sudo python3 blinkatest.py +# $ python3 blinkatest.py # Might work... +# +import board +import digitalio +import busio + +print("Hello blinka!") + +# Try to create a Digital GPIO-input object. +pin = digitalio.DigitalInOut(board.D4) +print("Digital IO ok!") + +# Try to create an I2C object. +i2c = busio.I2C(board.SCL, board.SDA) +print("I2C ok!") + +# Try to create an SPI object. +spi = busio.SPI(board.SCLK, board.MOSI, board.MISO) +print("SPI ok!") + +print("done!") \ No newline at end of file diff --git a/pet_ros2_battery_state_pkg/__init__.py b/pet_ros2_battery_state_pkg/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pet_ros2_battery_state_pkg/pet_battery_state_ina219_node.py b/pet_ros2_battery_state_pkg/pet_battery_state_ina219_node.py new file mode 100755 index 0000000..71c220e --- /dev/null +++ b/pet_ros2_battery_state_pkg/pet_battery_state_ina219_node.py @@ -0,0 +1,153 @@ +#!/usr/bin/env python3' +# coding = utf-8 +######################################################################################## +## +## Maintainer: stefan.kull@gmail.com +## +## Input: INA219 Current/Voltage-sensor (a.k.a "Battery State") +## Output: ROS2 node that publish a BatteryState.msg topic +## +## Prerequisite: +## Software +## $ +## $ sudo pip3 install adafruit-circuitpython-ina219 +## +## Hardware: Power circuit via INA219 break aout board (Power at VIn+, Drain/Source at VIn- ) +## Host: Raspberry Pi 4(Ubuntu) via I2C +## +## Launch sequence: +## 1) $ ros2 run pet_ros2_currentsensor_ina219_pkg pet_current_sensor_ina219_node.py +## 2) $ ros2 topic echo /xyz +## $ ros2 topic echo /zyx +## + +# Import the ROS2-stuff +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import BatteryState + +# Import the Ubuntu/Linux-hardware stuff +import time +import board +from adafruit_ina219 import ADCResolution, BusVoltageRange, INA219 + +# Import the common Ubuntu/Linux stuff +import sys +from time import sleep +from math import modf + +#Set Button pin(GPIO no.) and ROS2-topic-name +BATTERY_STATE_TOPIC = 'battery_state' + + +class BatteryStatePublisher(Node): + ''' + ROS2 current & voltage sensor publisher node + Create a BatteryStatePublisher class, which is a subclass of the Node class. + The class publishes the battery state of an object at a specific time interval. + ''' + def __init__(self): + # Initiate the Node class's constructor and give it a name + super().__init__("pet_current_sensor_node") + + i2c_bus = board.I2C() + self.ina219 = INA219(i2c_bus) + + # optional : change configuration to use 32 samples averaging for both bus voltage and shunt voltage + self.ina219.bus_adc_resolution = ADCResolution.ADCRES_12BIT_32S + self.ina219.shunt_adc_resolution = ADCResolution.ADCRES_12BIT_32S + + # optional : change voltage range to 16V + self.ina219.bus_voltage_range = BusVoltageRange.RANGE_16V + + # display some of the advanced field (just to test) + self.get_logger().info("INA219 Current/Voltage sensor. Config register:") + self.get_logger().info(" - bus_voltage_range: 0x%1X" % self.ina219.bus_voltage_range) + self.get_logger().info(" - gain: 0x%1X" % self.ina219.gain) + self.get_logger().info(" - bus_adc_resolution: 0x%1X" % self.ina219.bus_adc_resolution) + self.get_logger().info(" - shunt_adc_resolution: 0x%1X" % self.ina219.shunt_adc_resolution) + self.get_logger().info(" - mode: 0x%1X" % self.ina219.mode) + self.get_logger().info("") + + # Create Message + current_time = modf(time.time()) + self.msg_battery = BatteryState() + self.msg_battery.header.stamp.sec = int(current_time[1]) + self.msg_battery.header.stamp.nanosec = int(current_time[0] * 1000000000) & 0xffffffff + self.msg_battery.header.frame_id = "18650 3S x1P main battery" + + self.msg_battery.voltage = float('NaN') # Voltage in Volts (Mandatory) + self.msg_battery.current = float('NaN') # Negative when discharging (A) (If unmeasured NaN) + self.msg_battery.temperature = float('NaN') # Temperature in Degrees Celsius (If unmeasured NaN) + self.msg_battery.charge = float('NaN') # Current charge in Ah (If unmeasured NaN) + self.msg_battery.capacity = float('NaN') # Capacity in Ah (last full capacity) (If unmeasured NaN) + self.msg_battery.design_capacity = float('NaN') # Capacity in Ah (design capacity) (If unmeasured NaN) + self.msg_battery.percentage = float('NaN') # Charge percentage on 0 to 1 range (If unmeasured NaN + + self.msg_battery.power_supply_status = 0 # The charging status as reported. [uint8 POWER_SUPPLY_STATUS_UNKNOWN = 0] + self.msg_battery.power_supply_health = 0 # The battery health metric. [uint8 POWER_SUPPLY_HEALTH_UNKNOWN = 0] + self.msg_battery.power_supply_technology = 2 # The battery chemistry. [uint8 POWER_SUPPLY_TECHNOLOGY_LION = 2] + self.msg_battery.present = True # True if the battery is present + + self.msg_battery.location = 'Think "Inside the box"' # The location into which the battery is inserted. (slot number or plug) + self.msg_battery.serial_number = '0000000' # The best approximation of the battery serial number + + # Create publisher(s) + self.publisher_battery_state = self.create_publisher(BatteryState, '/battery_status', 10) + + # Setup time interval in seconds... for the callback + timer_period = 5.0 + self.timer = self.create_timer(timer_period, self.get_battery_state_callback) + + # print("----------------------------------------") + # print(self.msg_battery) + # print("----------------------------------------") + + + + def get_battery_state_callback(self): + """ + Callback function. + This function gets called at the specific time interval. + """ + # Update the message header + current_time = modf(time.time()) + self.msg_battery.header.stamp.sec = int(current_time[1]) + self.msg_battery.header.stamp.nanosec = int(current_time[0] * 1000000000) & 0xffffffff + + self.msg_battery.voltage = self.ina219.bus_voltage # voltage on V- (load side) + self.msg_battery.current = self.ina219.current /1000.0 # current in mA->A + # print(self.msg_battery.voltage) + + # Publish BatteryState message + self.publisher_battery_state.publish(self.msg_battery) + + +def main(args=None): + rclpy.init(args=args) + + # Create the node + battery_state_pub = BatteryStatePublisher() + + try: + # Spin the node so the callback function is called. + # Publish any pending messages to the topics. + rclpy.spin(battery_state_pub) + + except KeyboardInterrupt: + print("**** * 💀 Ctrl-C detected...") + + finally: + print("**** 🪦 battery_state_pub ending... ") + print( str(sys.exc_info()[1]) ) # Need ´import sys´ + + # Time to clean up stuff... - Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + battery_state_pub.destroy_node() + + # Time to clean up stuff... Shutdown the ROS client library for Python + rclpy.shutdown() + +if __name__ == "__main__": + main() diff --git a/resource/pet_ros2_battery_state_pkg b/resource/pet_ros2_battery_state_pkg new file mode 100644 index 0000000..e69de29 diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..21b0546 --- /dev/null +++ b/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/pet_ros2_battery_state_pkg +[install] +install_scripts=$base/lib/pet_ros2_battery_state_pkg diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..eae7c20 --- /dev/null +++ b/setup.py @@ -0,0 +1,26 @@ +from setuptools import setup + +package_name = 'pet_ros2_battery_state_pkg' + +setup( + name=package_name, + version='0.0.1', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='SeniorKullken', + maintainer_email='stefan.kull@gmail.com', + description='ROS2-publisher for the Current/Voltage(Battery State) sensor INA219. Publish measurement as ROS2-topics.', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + "pet_battery_state_ina219_node=pet_ros2_battery_state_pkg.pet_battery_state_ina219_node:main" + ], + }, +) diff --git a/test/test_copyright.py b/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/test/test_copyright.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_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/test/test_flake8.py b/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/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/test/test_pep257.py b/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/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'