Smoke testing release - But should work :-)

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.
+
+<table>
+    <td>
+        <img src="./doc/pet_ros2_currentsensor(INA219)_wiring.png" height="350px">
+    </td>
+    <td>
+        ..
+    </td>
+  </table>
+
+# ROS2 Package/Module Behaviour
+
+# Prerequisite: Hardware
+* Single Board Computer(SBC): Raspberry Pi 3/4
+* Sensor: INA2129 Current Sensor via default I2C adr.=<code>0x4o</code>
+
+# Prerequisite: Software
+* Ubuntu 20.04 (64bit) or newer
+* Robot Operating System 2, ROS2 (Version Galathic)
+  <blockquote>...do the ROS2-installation stuff...</blockquote>
+
+## Prerequisite: I2C-interface Raspberry Pi 4 / Ubuntu
+Prepared by adding additional, i2c communication, Linux-software-packages <br/>
+`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'
+<ul><blockquote>🤔There is probably better tutorials how to do this...<br/>
+                ...but here is how I made it.<br/>
+</blockquote></ul>
+
+`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'
+    --- 
+```

package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>pet_ros2_battery_state_pkg</name>
+  <version>0.0.1</version>
+  <description>ROS2-publisher for the Current/Voltage sensor INA219. Publish measurement as ROS2-topics.</description>
+  <maintainer email="stefan.kull@gmail.com">pi</maintainer>
+  <license>MIT</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

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)

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!")

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  <https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/BatteryState.msg>
+        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()