lib and node work

mission/internal_status/src/battery_monitor_lib.cpp → mission/internal_status/internal_status/battery_monitor_lib.cpp

@@ -3,15 +3,15 @@ using namespace std; // do i need this?
 
 //#include <linus/i2c-dev.h>
 //#include <sys/ioctl.h>
-#include "MCP342x.h"
 #include "smbus.h"
 #include <time.h>
+#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

mission/internal_status/src/battery_monitor_lib.py → 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:  

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()
+
+

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())

mission/internal_status/package.xml

@@ -3,16 +3,23 @@
 <package format="3">
   <name>internal_status</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="[email protected]">rose</maintainer>
-  <license>TODO: License declaration</license>
+  <description>Publisher of the PSM voltage and current data</description>
+  <maintainer email="[email protected]">vortex</maintainer>
+  <license>Apache License 2.0</license>
 
-  <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>power_sense_module_lib</exec_depend>
+
+
+
+  <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_cmake</build_type>
+    <build_type>ament_python</build_type>
   </export>
 </package>

mission/internal_status/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/internal_status
+[install]
+install_scripts=$base/lib/internal_status

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='[email protected]',
+    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', 
+        ],
+    },
+)