From ad0eea81ccce65df9d40ad682ec7bcb3c5ade67a Mon Sep 17 00:00:00 2001 From: Martin Huynh Date: Fri, 13 Oct 2023 16:04:15 +0200 Subject: [PATCH 01/16] Freya tf tree (#95) * First try TF tree converted to yaml, not working * Renamed launch files to YAML, WORKING ROS2 * Newest measurements from mech, Z-axis is DOWN * Removed if(BUILD_TESTING) from CMakeLists * Removed unnecessary comments from CMakeLists --------- Co-authored-by: Aleksander Klund <99870311+alekskl01@users.noreply.github.com> --- asv_setup/CMakeLists.txt | 15 --------------- asv_setup/launch/pc.launch.yaml | 1 + asv_setup/launch/tf.launch.yaml | 12 ++++++++++++ 3 files changed, 13 insertions(+), 15 deletions(-) create mode 100644 asv_setup/launch/tf.launch.yaml diff --git a/asv_setup/CMakeLists.txt b/asv_setup/CMakeLists.txt index b12ae23a..8a67f88d 100644 --- a/asv_setup/CMakeLists.txt +++ b/asv_setup/CMakeLists.txt @@ -7,9 +7,6 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) # Install launch files. install(DIRECTORY @@ -17,16 +14,4 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) -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/asv_setup/launch/pc.launch.yaml b/asv_setup/launch/pc.launch.yaml index efa79769..680bcb3a 100644 --- a/asv_setup/launch/pc.launch.yaml +++ b/asv_setup/launch/pc.launch.yaml @@ -21,3 +21,4 @@ launch: - include: file: "src/vortex-asv/mission/joystick_interface/launch/joystick_interface.launch.yaml" + diff --git a/asv_setup/launch/tf.launch.yaml b/asv_setup/launch/tf.launch.yaml new file mode 100644 index 00000000..2da7b95f --- /dev/null +++ b/asv_setup/launch/tf.launch.yaml @@ -0,0 +1,12 @@ +launch: + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_lidar + args: --frame-id base_link --child-frame-id os_lidar --x -0.0145 --y 0 --z -0.392425 --roll 0 --pitch 0 --yaw 0 + + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_zed2_camera_center + args: --frame-id base_link --child-frame-id zed2i_camera_center --x 0.3005 --y 0 --z -0.22036 --roll 0 --pitch 0 --yaw 0 \ No newline at end of file From c768defc30afb3dd9f2d03a0eb7a155e601d5f69 Mon Sep 17 00:00:00 2001 From: Aleksander Klund <99870311+alekskl01@users.noreply.github.com> Date: Sat, 14 Oct 2023 17:40:38 +0200 Subject: [PATCH 02/16] Bug/static tf (#102) * fixed rotation of sensor frames * fixed static_tf qos * removed old tf launch file --- asv_setup/launch/tf.launch.py | 65 +++++++++++++++++++++++++++++++++ asv_setup/launch/tf.launch.yaml | 12 ------ 2 files changed, 65 insertions(+), 12 deletions(-) create mode 100644 asv_setup/launch/tf.launch.py delete mode 100644 asv_setup/launch/tf.launch.yaml diff --git a/asv_setup/launch/tf.launch.py b/asv_setup/launch/tf.launch.py new file mode 100644 index 00000000..452a59f2 --- /dev/null +++ b/asv_setup/launch/tf.launch.py @@ -0,0 +1,65 @@ +import math + +from launch import LaunchDescription +from launch_ros.actions import Node +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy, QoSHistoryPolicy +from launch.actions import ExecuteProcess + + +def generate_launch_description(): + # command line arguments to get desired QoS settings + echo_tf_static = ExecuteProcess( + cmd=['ros2', 'topic', 'echo', '--qos-durability', 'transient_local', '--qos-reliability', 'reliable', '/tf_static'], + output='screen' + ) + + # base_link (NED) to base_link (SEU) tf. + tf_base_link_ned_to_base_link_seu = Node( + package='tf2_ros', + name='base_link_ned_to_base_link_seu', + executable='static_transform_publisher', + arguments=['--x' , '0', + '--y' , '0', + '--z' , '0', + '--roll' , '0', + '--pitch' , str(math.pi), + '--yaw' , '0', + '--frame-id' , 'base_link', + '--child-frame-id' , 'base_link_SEU'], + ) + + tf_base_link_to_lidar = Node( + package='tf2_ros', + name='base_link_to_lidar', + executable='static_transform_publisher', + arguments=['--x' , '-0.0145', + '--y' , '0', + '--z' , '-0.392425', + '--roll' , str(math.pi), + '--pitch' , '0', + '--yaw' , '0', + '--frame-id' , 'base_link', + '--child-frame-id' , 'os_lidar'], + ) + + # base_link to zed2i_camera_center tf. + tf_base_link_to_zed2_camera_center = Node( + package='tf2_ros', + name='base_link_to_zed2_camera_center', + executable='static_transform_publisher', + arguments=['--x' , '0.3005', + '--y' , '0', + '--z' , '-0.22036', + '--roll' , str(math.pi), + '--pitch' , '0', + '--yaw' , '0', + '--frame-id' , 'base_link', + '--child-frame-id' , 'zed2i_camera_center'], + ) + + return LaunchDescription([ + tf_base_link_ned_to_base_link_seu, + tf_base_link_to_lidar, + tf_base_link_to_zed2_camera_center, + echo_tf_static + ]) diff --git a/asv_setup/launch/tf.launch.yaml b/asv_setup/launch/tf.launch.yaml deleted file mode 100644 index 2da7b95f..00000000 --- a/asv_setup/launch/tf.launch.yaml +++ /dev/null @@ -1,12 +0,0 @@ -launch: - - node: - pkg: tf2_ros - exec: static_transform_publisher - name: base_link_to_lidar - args: --frame-id base_link --child-frame-id os_lidar --x -0.0145 --y 0 --z -0.392425 --roll 0 --pitch 0 --yaw 0 - - - node: - pkg: tf2_ros - exec: static_transform_publisher - name: base_link_to_zed2_camera_center - args: --frame-id base_link --child-frame-id zed2i_camera_center --x 0.3005 --y 0 --z -0.22036 --roll 0 --pitch 0 --yaw 0 \ No newline at end of file From 432f7a58ad7ea5f268c3fe178751e69c771fed8c Mon Sep 17 00:00:00 2001 From: Aleksander Klund <99870311+alekskl01@users.noreply.github.com> Date: Sun, 15 Oct 2023 17:57:05 +0200 Subject: [PATCH 03/16] Enhancement/create py launchfiles (#103) * fixed rotation of sensor frames * fixed static_tf qos * created py launchfiles for joystick_interface and pc.launch + minor restructuring * small fix --- asv_setup/launch/pc.launch.py | 42 +++++++++++++++++++ asv_setup/launch/pc.launch.yaml | 24 ----------- mission/joystick_interface/CMakeLists.txt | 8 +++- ...ams.yaml => param_joystick_interface.yaml} | 0 .../joystick_interface/joystick_interface.py | 6 +-- .../launch/joystick_interface.launch.py | 21 ++++++++++ .../launch/joystick_interface.launch.yaml | 13 ------ .../{joystick_interface => }/test/__init__.py | 0 .../test/test_joystick_interface.py | 0 9 files changed, 73 insertions(+), 41 deletions(-) create mode 100644 asv_setup/launch/pc.launch.py delete mode 100644 asv_setup/launch/pc.launch.yaml rename mission/joystick_interface/config/{params.yaml => param_joystick_interface.yaml} (100%) create mode 100644 mission/joystick_interface/launch/joystick_interface.launch.py delete mode 100644 mission/joystick_interface/launch/joystick_interface.launch.yaml rename mission/joystick_interface/{joystick_interface => }/test/__init__.py (100%) rename mission/joystick_interface/{joystick_interface => }/test/test_joystick_interface.py (100%) diff --git a/asv_setup/launch/pc.launch.py b/asv_setup/launch/pc.launch.py new file mode 100644 index 00000000..1248ae25 --- /dev/null +++ b/asv_setup/launch/pc.launch.py @@ -0,0 +1,42 @@ +import os +from launch import LaunchDescription +from launch.actions import SetEnvironmentVariable, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + # Set environment variable + set_env_var = SetEnvironmentVariable( + name='ROSCONSOLE_FORMAT', + value='[${severity}] [${time}] [${node}]: ${message}' + ) + + # Joystick node + joy_node = Node( + package='joy', + executable='joy_node', + name='joystick_driver', + output='screen', + parameters=[ + {'_deadzone': 0.15}, + {'_autorepeat_rate': 100}, + ], + remappings=[ + ('/joy', '/joystick/joy'), + ], + ) + + # Joystick interface launch + joystick_interface_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('joystick_interface'), 'launch/joystick_interface.launch.py') + ) + ) + + # Return launch description + return LaunchDescription([ + set_env_var, + joy_node, + joystick_interface_launch + ]) diff --git a/asv_setup/launch/pc.launch.yaml b/asv_setup/launch/pc.launch.yaml deleted file mode 100644 index 680bcb3a..00000000 --- a/asv_setup/launch/pc.launch.yaml +++ /dev/null @@ -1,24 +0,0 @@ -launch: - - set_env: - name: ROSCONSOLE_FORMAT - value: "[${severity}] [${time}] [${node}]: ${message}" - - - group: - - node: - pkg: joy - exec: joy_node - name: joystick_driver - output: screen - param: - - name: _deadzone - value: 0.15 - - name: _autorepeat_rate - value: 100 - remap: - - - from: "/joy" - to: "/joystick/joy" - - - include: - file: "src/vortex-asv/mission/joystick_interface/launch/joystick_interface.launch.yaml" - diff --git a/mission/joystick_interface/CMakeLists.txt b/mission/joystick_interface/CMakeLists.txt index 0f5262d4..e58d5013 100644 --- a/mission/joystick_interface/CMakeLists.txt +++ b/mission/joystick_interface/CMakeLists.txt @@ -12,6 +12,12 @@ find_package(geometry_msgs REQUIRED) ament_python_install_package(${PROJECT_NAME}) +# Install launch files. +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) + install(PROGRAMS joystick_interface/joystick_interface.py DESTINATION lib/${PROJECT_NAME} @@ -20,7 +26,7 @@ install(PROGRAMS if(BUILD_TESTING) find_package(ament_cmake_pytest REQUIRED) set(_pytest_tests - joystick_interface/test/test_joystick_interface.py + test/test_joystick_interface.py ) foreach(_test_path ${_pytest_tests}) get_filename_component(_test_name ${_test_path} NAME_WE) diff --git a/mission/joystick_interface/config/params.yaml b/mission/joystick_interface/config/param_joystick_interface.yaml similarity index 100% rename from mission/joystick_interface/config/params.yaml rename to mission/joystick_interface/config/param_joystick_interface.yaml diff --git a/mission/joystick_interface/joystick_interface/joystick_interface.py b/mission/joystick_interface/joystick_interface/joystick_interface.py index 5640b02e..d76f4555 100755 --- a/mission/joystick_interface/joystick_interface/joystick_interface.py +++ b/mission/joystick_interface/joystick_interface/joystick_interface.py @@ -53,9 +53,9 @@ def __init__(self): "thrust/wrench_input", 1) - self.declare_parameter('surge_scale_factor', 100.0) - self.declare_parameter('sway_scale_factor', 100.0) - self.declare_parameter('yaw_scale_factor', 100.0) + self.declare_parameter('surge_scale_factor', 50.0) + self.declare_parameter('sway_scale_factor', 50.0) + self.declare_parameter('yaw_scale_factor', 50.0) #Gets the scaling factors from the yaml file self.joystick_surge_scaling = self.get_parameter('surge_scale_factor').value diff --git a/mission/joystick_interface/launch/joystick_interface.launch.py b/mission/joystick_interface/launch/joystick_interface.launch.py new file mode 100644 index 00000000..53d7c6a8 --- /dev/null +++ b/mission/joystick_interface/launch/joystick_interface.launch.py @@ -0,0 +1,21 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + + joystick_interface_node = Node( + package='joystick_interface', + executable='joystick_interface.py', + name='joystick_interface', + output='screen', + parameters=[os.path.join(get_package_share_directory('joystick_interface'), 'config/param_joystick_interface.yaml')] + ) + + return LaunchDescription([ + joystick_interface_node + ]) + diff --git a/mission/joystick_interface/launch/joystick_interface.launch.yaml b/mission/joystick_interface/launch/joystick_interface.launch.yaml deleted file mode 100644 index 61aac95f..00000000 --- a/mission/joystick_interface/launch/joystick_interface.launch.yaml +++ /dev/null @@ -1,13 +0,0 @@ -launch: -- node: - pkg: joystick_interface - exec: joystick_interface.py - name: joystick_interface - output: screen - param: - - from: src/vortex-asv/mission/joystick_interface/config/params.yaml - - - - - diff --git a/mission/joystick_interface/joystick_interface/test/__init__.py b/mission/joystick_interface/test/__init__.py similarity index 100% rename from mission/joystick_interface/joystick_interface/test/__init__.py rename to mission/joystick_interface/test/__init__.py diff --git a/mission/joystick_interface/joystick_interface/test/test_joystick_interface.py b/mission/joystick_interface/test/test_joystick_interface.py similarity index 100% rename from mission/joystick_interface/joystick_interface/test/test_joystick_interface.py rename to mission/joystick_interface/test/test_joystick_interface.py From 017195827adf20c2c8284c20482d418cc6586e8f Mon Sep 17 00:00:00 2001 From: RoseKapps Date: Sun, 22 Oct 2023 13:36:36 +0200 Subject: [PATCH 04/16] just testing smbus file --- mission/internal_status/CMakeLists.txt | 26 +++ mission/internal_status/package.xml | 18 ++ .../internal_status/src/battery_monitor.py | 175 ++++++++++++++++++ .../src/battery_monitor_lib.cpp | 128 +++++++++++++ mission/internal_status/src/smbus.h | 59 ++++++ 5 files changed, 406 insertions(+) create mode 100644 mission/internal_status/CMakeLists.txt create mode 100644 mission/internal_status/package.xml create mode 100755 mission/internal_status/src/battery_monitor.py create mode 100644 mission/internal_status/src/battery_monitor_lib.cpp create mode 100644 mission/internal_status/src/smbus.h diff --git a/mission/internal_status/CMakeLists.txt b/mission/internal_status/CMakeLists.txt new file mode 100644 index 00000000..6969ffc0 --- /dev/null +++ b/mission/internal_status/CMakeLists.txt @@ -0,0 +1,26 @@ +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/package.xml b/mission/internal_status/package.xml new file mode 100644 index 00000000..d3a695ac --- /dev/null +++ b/mission/internal_status/package.xml @@ -0,0 +1,18 @@ + + + + internal_status + 0.0.0 + TODO: Package description + rose + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/mission/internal_status/src/battery_monitor.py b/mission/internal_status/src/battery_monitor.py new file mode 100755 index 00000000..0f2ece97 --- /dev/null +++ b/mission/internal_status/src/battery_monitor.py @@ -0,0 +1,175 @@ +#!/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: + + self.system_current = (self.channel_current.convert_and_read() - + self.psm_to_battery_current_offset + ) * self.psm_to_battery_current_scale_factor + + if self.system_current_state != "Received": + self.system_current_state = "Received" + + except IOError: + self.I2C_error_counter_current += 1 + self.system_current_state = "Error"", "Received" + ) + + # 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_lib.cpp b/mission/internal_status/src/battery_monitor_lib.cpp new file mode 100644 index 00000000..02ce0eb0 --- /dev/null +++ b/mission/internal_status/src/battery_monitor_lib.cpp @@ -0,0 +1,128 @@ +#include +using namespace std; //do i need this? + +#include +#include "i2c/smbus.h" +//#include + +// include doesn't work well + +//how to know the types of the values? + +// + +/* +class BatteryMonitor{ + +public: + BatteryMonitor() //default constructor to initialize the class + { + //***************I2C protocole******************************* + i2c_adress = 0x69; + bus = smbus.SMBus(1); + channel_voltage = MCP342x(bus,i2c_adress,channel=0,resolution=18); + channel_current = MCP342x(bus,i2c_adress,channel=1,resolution=18); + //TIME SLEEP? -> only for rospy? + + //**********convertion ratio taken from PSM datasheet************** + psm_to_battery_voltage = 11.0; + psm_to_battery_current_scale_factor =37.8788; + psm_to_battery_current_offset = 0.330; + + + system_voltage = 0.0; + system_current = 0.0; + + system_voltage_state = "No receive"; + system_current_state = "No receive"; + + I2C_error_counter_current = 0; + I2C_error_counter_voltage = 0; + + cout << "Iniatialization done" << endl; + } + + float get_PSM_current(){ + + try { + system_current = ((channel_current.convert_and_read() + - psm_to_battery_current_offset) + * psm_to_battery_current_scale_factor); + + if (system_current_state != "Received") { + system_current_state = "Received"; + } + } + + catch (const std::ios_base::failure& e) { + I2C_error_counter_current += 1; + system_current_state = "Error"; + cerr << "Error: " << e.what() << endl; + } + + return system_current; + } + + float get_PSM_voltage(){ + + try { + system_voltage = (channel_voltage.convert_and_read() + * psm_to_battery_voltage); + + if (system_voltage_state != "Received") { + system_voltage_state = "Received"; + } + } + + catch (const std::ios_base::failure& e) { + I2C_error_counter_voltage += 1; + system_current_state = "Error"; + cerr << "Error: " << e.what() << endl; + } + + return system_voltage; + } + + void shutdown(){ + + bus.close(); + } + +private: + int16_t i2c_adress; + int bus; + float channel_voltage; //what's the type of these value? + float channel_current; //what's the type of these value? + float psm_to_battery_voltage; + float psm_to_battery_current_scale_factor; + float psm_to_battery_current_offset; + float system_voltage; + float system_current; + string system_voltage_state; + string system_current_state; + int I2C_error_counter_current; + int I2C_error_counter_voltage; + +}; + +*/ + +/* TRY CATCH BLOCK + + +try { + system_current = (channel_current.convert_and_read() + - psm_to_battery_current_offset) + * psm_to_battery_current_scale_factor; + + if (system_current_state != "Received") { + system_current_state = "Received"; + } +} +catch (const std::ios_base::failure& e) { + I2C_error_counter_current += 1; + system_current_state = "Error"; +} + + +*/ \ No newline at end of file diff --git a/mission/internal_status/src/smbus.h b/mission/internal_status/src/smbus.h new file mode 100644 index 00000000..3eb7d2f1 --- /dev/null +++ b/mission/internal_status/src/smbus.h @@ -0,0 +1,59 @@ +/* + smbus.h - SMBus level access helper functions + + Copyright (C) 1995-97 Simon G. Vogl + Copyright (C) 1998-99 Frodo Looijaard + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + MA 02110-1301 USA. +*/ + +#ifndef LIB_I2C_SMBUS_H +#define LIB_I2C_SMBUS_H + +#include +#include + +extern __s32 i2c_smbus_access(int file, char read_write, __u8 command, + int size, union i2c_smbus_data *data); + +extern __s32 i2c_smbus_write_quick(int file, __u8 value); +extern __s32 i2c_smbus_read_byte(int file); +extern __s32 i2c_smbus_write_byte(int file, __u8 value); +extern __s32 i2c_smbus_read_byte_data(int file, __u8 command); +extern __s32 i2c_smbus_write_byte_data(int file, __u8 command, __u8 value); +extern __s32 i2c_smbus_read_word_data(int file, __u8 command); +extern __s32 i2c_smbus_write_word_data(int file, __u8 command, __u16 value); +extern __s32 i2c_smbus_process_call(int file, __u8 command, __u16 value); + +/* Returns the number of read bytes */ +extern __s32 i2c_smbus_read_block_data(int file, __u8 command, __u8 *values); +extern __s32 i2c_smbus_write_block_data(int file, __u8 command, __u8 length, + const __u8 *values); + +/* Returns the number of read bytes */ +/* Until kernel 2.6.22, the length is hardcoded to 32 bytes. If you + ask for less than 32 bytes, your code will only work with kernels + 2.6.23 and later. */ +extern __s32 i2c_smbus_read_i2c_block_data(int file, __u8 command, __u8 length, + __u8 *values); +extern __s32 i2c_smbus_write_i2c_block_data(int file, __u8 command, __u8 length, + const __u8 *values); + +/* Returns the number of read bytes */ +extern __s32 i2c_smbus_block_process_call(int file, __u8 command, __u8 length, + __u8 *values); + +#endif /* LIB_I2C_SMBUS_H */ \ No newline at end of file From d888a5cd8565d2af8fc29917fed3cb5ef774408b Mon Sep 17 00:00:00 2001 From: Clang Robot Date: Sun, 22 Oct 2023 11:37:31 +0000 Subject: [PATCH 05/16] Committing clang-format changes --- .../src/battery_monitor_lib.cpp | 22 +++++++++---------- mission/internal_status/src/smbus.h | 14 ++++++------ 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/mission/internal_status/src/battery_monitor_lib.cpp b/mission/internal_status/src/battery_monitor_lib.cpp index 02ce0eb0..a21c347b 100644 --- a/mission/internal_status/src/battery_monitor_lib.cpp +++ b/mission/internal_status/src/battery_monitor_lib.cpp @@ -1,13 +1,13 @@ #include -using namespace std; //do i need this? +using namespace std; // do i need this? -#include #include "i2c/smbus.h" +#include //#include // include doesn't work well -//how to know the types of the values? +// how to know the types of the values? // @@ -16,14 +16,14 @@ class BatteryMonitor{ public: BatteryMonitor() //default constructor to initialize the class - { + { //***************I2C protocole******************************* i2c_adress = 0x69; bus = smbus.SMBus(1); channel_voltage = MCP342x(bus,i2c_adress,channel=0,resolution=18); channel_current = MCP342x(bus,i2c_adress,channel=1,resolution=18); //TIME SLEEP? -> only for rospy? - + //**********convertion ratio taken from PSM datasheet************** psm_to_battery_voltage = 11.0; psm_to_battery_current_scale_factor =37.8788; @@ -45,8 +45,8 @@ class BatteryMonitor{ float get_PSM_current(){ try { - system_current = ((channel_current.convert_and_read() - - psm_to_battery_current_offset) + system_current = ((channel_current.convert_and_read() + - psm_to_battery_current_offset) * psm_to_battery_current_scale_factor); if (system_current_state != "Received") { @@ -59,7 +59,7 @@ class BatteryMonitor{ system_current_state = "Error"; cerr << "Error: " << e.what() << endl; } - + return system_current; } @@ -68,7 +68,7 @@ class BatteryMonitor{ try { system_voltage = (channel_voltage.convert_and_read() * psm_to_battery_voltage); - + if (system_voltage_state != "Received") { system_voltage_state = "Received"; } @@ -111,8 +111,8 @@ class BatteryMonitor{ try { - system_current = (channel_current.convert_and_read() - - psm_to_battery_current_offset) + system_current = (channel_current.convert_and_read() + - psm_to_battery_current_offset) * psm_to_battery_current_scale_factor; if (system_current_state != "Received") { diff --git a/mission/internal_status/src/smbus.h b/mission/internal_status/src/smbus.h index 3eb7d2f1..a7ffd3b3 100644 --- a/mission/internal_status/src/smbus.h +++ b/mission/internal_status/src/smbus.h @@ -23,11 +23,11 @@ #ifndef LIB_I2C_SMBUS_H #define LIB_I2C_SMBUS_H -#include #include +#include -extern __s32 i2c_smbus_access(int file, char read_write, __u8 command, - int size, union i2c_smbus_data *data); +extern __s32 i2c_smbus_access(int file, char read_write, __u8 command, int size, + union i2c_smbus_data *data); extern __s32 i2c_smbus_write_quick(int file, __u8 value); extern __s32 i2c_smbus_read_byte(int file); @@ -41,19 +41,19 @@ extern __s32 i2c_smbus_process_call(int file, __u8 command, __u16 value); /* Returns the number of read bytes */ extern __s32 i2c_smbus_read_block_data(int file, __u8 command, __u8 *values); extern __s32 i2c_smbus_write_block_data(int file, __u8 command, __u8 length, - const __u8 *values); + const __u8 *values); /* Returns the number of read bytes */ /* Until kernel 2.6.22, the length is hardcoded to 32 bytes. If you ask for less than 32 bytes, your code will only work with kernels 2.6.23 and later. */ extern __s32 i2c_smbus_read_i2c_block_data(int file, __u8 command, __u8 length, - __u8 *values); + __u8 *values); extern __s32 i2c_smbus_write_i2c_block_data(int file, __u8 command, __u8 length, - const __u8 *values); + const __u8 *values); /* Returns the number of read bytes */ extern __s32 i2c_smbus_block_process_call(int file, __u8 command, __u8 length, - __u8 *values); + __u8 *values); #endif /* LIB_I2C_SMBUS_H */ \ No newline at end of file From 5a847f30bea4243b3c96c10f155ebcd89d886f4a Mon Sep 17 00:00:00 2001 From: PizzaAllTheWay Date: Sun, 22 Oct 2023 17:28:16 +0200 Subject: [PATCH 06/16] now lib in python --- mission/internal_status/src/MCP342X.cpp | 226 ++++++++++++++++++ mission/internal_status/src/MCP342X.h | 134 +++++++++++ mission/internal_status/src/Martynas_test.cpp | 9 + .../internal_status/src/battery_monitor.py | 11 - .../src/battery_monitor_lib.cpp | 13 +- .../src/battery_monitor_lib.py | 52 ++++ 6 files changed, 432 insertions(+), 13 deletions(-) create mode 100755 mission/internal_status/src/MCP342X.cpp create mode 100755 mission/internal_status/src/MCP342X.h create mode 100755 mission/internal_status/src/Martynas_test.cpp create mode 100644 mission/internal_status/src/battery_monitor_lib.py diff --git a/mission/internal_status/src/MCP342X.cpp b/mission/internal_status/src/MCP342X.cpp new file mode 100755 index 00000000..8a45e15c --- /dev/null +++ b/mission/internal_status/src/MCP342X.cpp @@ -0,0 +1,226 @@ +/**************************************************************************/ +/*! + @file MCP342X.cpp + @author C. Schnarel + @license BSD (see license.txt) + + This is part of an Arduino library to interface with the Microchip + MCP47X6 series of Analog-to-Digital converters which are connected + via the I2C bus. + + MCP342X I2C device class + Based on Microchip datasheets for the following part numbers + MCP3421, MCP3422, MCP3423, MCP3424, MCP3425, MCP3426, MCP3427, MCP3428 + These parts share a common programming interface + + (c) Copyright 2013 by Chip Schnarel + Updates should (hopefully) always be available at + https://github.com/uchip/MCP342X + + @section HISTORY + + 2013-Dec-24 - First release, C. Schnarel +*/ +/**************************************************************************/ + +#include "MCP342X.h" + +/* static float stepSizeTbl[] = { + 0.001, // 12-bit, 1X Gain + 0.0005, // 12-bit, 2X Gain + 0.00025, // 12-bit, 4X Gain + 0.000125, // 12-bit, 8X Gain + 0.00025, // 14-bit, 1X Gain + 0.000125, // 14-bit, 2X Gain + 0.0000625, // 14-bit, 4X Gain + 0.00003125, // 14-bit, 8X Gain + 0.0000625, // 16-bit, 1X Gain + 0.00003125, // 16-bit, 2X Gain + 0.000015625, // 16-bit, 4X Gain + 0.0000078125, // 16-bit, 8X Gain + 0.000015625, // 18-bit, 1X Gain + 0.0000078125, // 18-bit, 2X Gain + 0.00000390625, // 18-bit, 4X Gain + 0.000001953125 // 18-bit, 8X Gain + }; +*/ + +/****************************************** + * Default constructor, uses default I2C address. + * @see MCP342X_DEFAULT_ADDRESS + */ +MCP342X::MCP342X() { + devAddr = MCP342X_DEFAULT_ADDRESS; +} + +/****************************************** + * Specific address constructor. + * @param address I2C address + * @see MCP342X_DEFAULT_ADDRESS + * @see MCP342X_A0GND_A1GND, etc. + */ +MCP342X::MCP342X(uint8_t address) { + devAddr = address; +} + +/****************************************** + * Verify the I2C connection. + * Make sure the device is connected and responds as expected. + * @return True if connection is valid, false otherwise + */ +bool MCP342X::testConnection() { + Wire.beginTransmission(devAddr); + return (Wire.endTransmission() == 0); +} + + +/****************************************** + * Set the configuration shadow register + */ +void MCP342X::configure(uint8_t configData) { + configRegShdw = configData; +} + +/****************************************** + * Get the configuration shadow register + */ +uint8_t MCP342X::getConfigRegShdw(void) { + return configRegShdw; +} + +/****************************************** + * Get the step size based on the configuration shadow register + */ +/*float MCP342X::getStepSize(void) { + uint8_t select = configRegShdw & (MCP342X_SIZE_MASK | MCP342X_GAIN_MASK); + return stepSizeTbl[select]; +}*/ + +/****************************************** + * Start a conversion using configuration settings from + * the shadow configuration register + */ +bool MCP342X::startConversion(void) { + Wire.beginTransmission(devAddr); + Wire.write(configRegShdw | MCP342X_RDY); + return (Wire.endTransmission() == 0); +} + + +/****************************************** + * Start a conversion using configuration settings from + * the shadow configuration register substituting the + * supplied channel + */ +bool MCP342X::startConversion(uint8_t channel) { + Wire.beginTransmission(devAddr); + configRegShdw = ((configRegShdw & ~MCP342X_CHANNEL_MASK) | + (channel & MCP342X_CHANNEL_MASK)); + Wire.write(configRegShdw | MCP342X_RDY); + return (Wire.endTransmission() == 0); +} + + +/****************************************** + * Read the conversion value (12, 14 or 16 bit) + * Spins reading status until ready then + * fills in the supplied location with the 16-bit (two byte) + * conversion value and returns the status byte + * Note: status of -1 "0xFF' implies read error + */ +uint8_t MCP342X::getResult(int16_t *dataPtr) { + uint8_t adcStatus; + if((configRegShdw & MCP342X_SIZE_MASK) == MCP342X_SIZE_18BIT) { + return 0xFF; + } + + do { + if(Wire.requestFrom(devAddr, (uint8_t) 3) == 3) { + ((char*)dataPtr)[1] = Wire.read(); + ((char*)dataPtr)[0] = Wire.read(); + adcStatus = Wire.read(); + } + else return 0xFF; + } while((adcStatus & MCP342X_RDY) != 0x00); + return adcStatus; +} + + +/****************************************** + * Check to see if the conversion value (12, 14 or 16 bit) + * is available. If so, then + * fill in the supplied location with the 16-bit (two byte) + * conversion value and status the config byte + * Note: status of -1 "0xFF' implies read error + */ +uint8_t MCP342X::checkforResult(int16_t *dataPtr) { + uint8_t adcStatus; + if((configRegShdw & MCP342X_SIZE_MASK) == MCP342X_SIZE_18BIT) { + return 0xFF; + } + + if(Wire.requestFrom(devAddr, (uint8_t) 3) == 3) { + ((char*)dataPtr)[1] = Wire.read(); + ((char*)dataPtr)[0] = Wire.read(); + adcStatus = Wire.read(); + } + else return 0xFF; + + return adcStatus; +} + + +/****************************************** + * Read the conversion value (18 bit) + * Spins reading status until ready then + * fills in the supplied location (32 bit) with + * the 24-bit (three byte) conversion value + * and returns the status byte + * Note: status of -1 "0xFF' implies read error + */ +uint8_t MCP342X::getResult(int32_t *dataPtr) { + uint8_t adcStatus; + if((configRegShdw & MCP342X_SIZE_MASK) != MCP342X_SIZE_18BIT) { + return 0xFF; + } + + do { + if(Wire.requestFrom((uint8_t) devAddr, (uint8_t) 4) == 4) { + ((char*)dataPtr)[3] = Wire.read(); + ((char*)dataPtr)[2] = Wire.read(); + ((char*)dataPtr)[1] = Wire.read(); + adcStatus = Wire.read(); + } + else return 0xFF; + } while((adcStatus & MCP342X_RDY) != 0x00); + *dataPtr = (*dataPtr)>>8; + return adcStatus; +} + + +/****************************************** + * Check to see if the conversion value (18 bit) + * is available. If so, then + * fill in the supplied location (32 bit) with + * the 24-bit (three byte) conversion value + * and return the status byte + * Note: status of -1 "0xFF' implies read error + */ +uint8_t MCP342X::checkforResult(int32_t *dataPtr) { + uint8_t adcStatus; + if((configRegShdw & MCP342X_SIZE_MASK) != MCP342X_SIZE_18BIT) { + return 0xFF; + } + + if(Wire.requestFrom((uint8_t) devAddr, (uint8_t) 4) == 4) { + ((char*)dataPtr)[3] = Wire.read(); + ((char*)dataPtr)[2] = Wire.read(); + ((char*)dataPtr)[1] = Wire.read(); + adcStatus = Wire.read(); + } + else return 0xFF; + + *dataPtr = (*dataPtr)>>8; + return adcStatus; +} + diff --git a/mission/internal_status/src/MCP342X.h b/mission/internal_status/src/MCP342X.h new file mode 100755 index 00000000..e14092a9 --- /dev/null +++ b/mission/internal_status/src/MCP342X.h @@ -0,0 +1,134 @@ +/**************************************************************************/ +/*! + @file MCP342X.h + @author C. Schnarel + @license BSD (see license.txt) + + This is part of an Arduino library to interface with the Microchip + MCP47X6 series of Analog-to-Digital converters which are connected + via the I2C bus. + + MCP342X I2C device class header file + Based on Microchip datasheets for the following part numbers + MCP3421, MCP3422, MCP3423, MCP3424, MCP3425, MCP3426, MCP3427, MCP3428 + These parts share a common programming interface + + (c) Copyright 2013 by Chip Schnarel + Updates should (hopefully) always be available at + https://github.com/uchip/MCP342X + + @section HISTORY + + 2013-Dec-24 - First release, C. Schnarel +*/ +/**************************************************************************/ + +#ifndef _MCP342X_H_ +#define _MCP342X_H_ + +/* +#ifdef ARDUINO + #if ARDUINO < 100 + #include "WProgram.h" + #else + #include "Arduino.h" + #endif +#else + #include "ArduinoWrapper.h" +#endif +*/ + +#include +#include + +// #include + +#include + +// I2C Address of device +// MCP3421, MCP3425 & MCP3426 are factory programed for any of 0x68 thru 0x6F +#define MCP342X_DEFAULT_ADDRESS 0x68 + +// MCP3422, MCP3423, MCP3424, MCP3427 & MCP3428 addresses are controlled by address lines A0 and A1 +// each address line can be low (GND), high (VCC) or floating (FLT) +#define MCP342X_A0GND_A1GND 0x68 +#define MCP342X_A0GND_A1FLT 0x69 +#define MCP342X_A0GND_A1VCC 0x6A +#define MCP342X_A0FLT_A1GND 0x6B +#define MCP342X_A0VCC_A1GND 0x6C +#define MCP342X_A0VCC_A1FLT 0x6D +#define MCP342X_A0VCC_A1VCC 0x6E +#define MCP342X_A0FLT_A1VCC 0x6F + + +// Conversion mode definitions +#define MCP342X_MODE_ONESHOT 0x00 +#define MCP342X_MODE_CONTINUOUS 0x10 + + +// Channel definitions +// MCP3421 & MCP3425 have only the one channel and ignore this param +// MCP3422, MCP3423, MCP3426 & MCP3427 have two channels and treat 3 & 4 as repeats of 1 & 2 respectively +// MCP3424 & MCP3428 have all four channels +#define MCP342X_CHANNEL_1 0x00 +#define MCP342X_CHANNEL_2 0x20 +#define MCP342X_CHANNEL_3 0x40 +#define MCP342X_CHANNEL_4 0x60 +#define MCP342X_CHANNEL_MASK 0x60 + + +// Sample size definitions - these also affect the sampling rate +// 12-bit has a max sample rate of 240sps +// 14-bit has a max sample rate of 60sps +// 16-bit has a max sample rate of 15sps +// 18-bit has a max sample rate of 3.75sps (MCP3421, MCP3422, MCP3423, MCP3424 only) +#define MCP342X_SIZE_12BIT 0x00 +#define MCP342X_SIZE_14BIT 0x04 +#define MCP342X_SIZE_16BIT 0x08 +#define MCP342X_SIZE_18BIT 0x0C +#define MCP342X_SIZE_MASK 0x0C + + +// Programmable Gain definitions +#define MCP342X_GAIN_1X 0x00 +#define MCP342X_GAIN_2X 0x01 +#define MCP342X_GAIN_4X 0x02 +#define MCP342X_GAIN_8X 0x03 +#define MCP342X_GAIN_MASK 0x03 + + +// /RDY bit definition +#define MCP342X_RDY 0x80 + + +class MCP342X { + public: + MCP342X(); + MCP342X(uint8_t address); + + bool testConnection(void); + + // Set/Get the configuration bits for the ADC + void configure(uint8_t config); + uint8_t getConfigRegShdw(void); + //float getStepSize(); // returns step size based on configRegShdw + + // Start a conversion + bool startConversion(void); + bool startConversion(uint8_t channel); + + // Read the ADC result + uint8_t getResult(int16_t *data); + uint8_t getResult(int32_t *data); + + // Non-blocking Read the ADC result + uint8_t checkforResult(int16_t *data); + uint8_t checkforResult(int32_t *data); + + private: + uint8_t devAddr; + uint8_t configRegShdw; + //float stepSizeTbl[]; +}; + +#endif /* _MCP342X_H_ */ \ No newline at end of file diff --git a/mission/internal_status/src/Martynas_test.cpp b/mission/internal_status/src/Martynas_test.cpp new file mode 100755 index 00000000..ef36ff18 --- /dev/null +++ b/mission/internal_status/src/Martynas_test.cpp @@ -0,0 +1,9 @@ +#include +#include +#include +#include "MCP342X.h" + +int main(){ + std::cout << "Hello world" << std::endl; + return 0; +} \ No newline at end of file diff --git a/mission/internal_status/src/battery_monitor.py b/mission/internal_status/src/battery_monitor.py index 0f2ece97..f0a1faec 100755 --- a/mission/internal_status/src/battery_monitor.py +++ b/mission/internal_status/src/battery_monitor.py @@ -53,17 +53,6 @@ def __init__(self): ) self.system_current_state = ( "No receive" # should only be "No receive", "Error try: - - self.system_current = (self.channel_current.convert_and_read() - - self.psm_to_battery_current_offset - ) * self.psm_to_battery_current_scale_factor - - if self.system_current_state != "Received": - self.system_current_state = "Received" - - except IOError: - self.I2C_error_counter_current += 1 - self.system_current_state = "Error"", "Received" ) # Create ROS publishers diff --git a/mission/internal_status/src/battery_monitor_lib.cpp b/mission/internal_status/src/battery_monitor_lib.cpp index a21c347b..c7e677dd 100644 --- a/mission/internal_status/src/battery_monitor_lib.cpp +++ b/mission/internal_status/src/battery_monitor_lib.cpp @@ -1,9 +1,18 @@ #include using namespace std; // do i need this? -#include "i2c/smbus.h" +//#include +//#include +#include "smbus.h" #include -//#include +#include "MCP342x.h" + +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/src/battery_monitor_lib.py new file mode 100644 index 00000000..99daee93 --- /dev/null +++ b/mission/internal_status/src/battery_monitor_lib.py @@ -0,0 +1,52 @@ +import smbus +#import time +from MCP342x import MCP342x + +class BatteryMonitor: + + def __init__(self): + + # Parameters + # to read voltage and current from ADC on PDB through I2C + self.i2c_adress = 0x69 + + # 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 + #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 + + def get_voltage(self): + # Sometimes an I/O timeout or error happens, it will run again when the error disappears + try: + system_voltage = (self.channel_voltage.convert_and_read() * + self.psm_to_battery_voltage) + return system_voltage + + except IOError: + return + + def get_current(self): + try: + system_current = (self.channel_current.convert_and_read() - + self.psm_to_battery_current_offset + ) * self.psm_to_battery_current_scale_factor + + return system_current + + except IOError: + return + + def shutdown(self): + self.bus.close() From fc7c35ddc8613d8a72f1063ed5cfc5eb98d29b03 Mon Sep 17 00:00:00 2001 From: RoseKapps Date: Wed, 25 Oct 2023 19:20:16 +0200 Subject: [PATCH 07/16] new lib and node --- key | 7 + key.pub | 1 + mission/internal_status/src/MCP342X.cpp | 226 ------------------ mission/internal_status/src/MCP342X.h | 134 ----------- mission/internal_status/src/Martynas_test.cpp | 9 - .../src/battery_monitor_node.py | 47 ++++ mission/internal_status/src/smbus.h | 59 ----- 7 files changed, 55 insertions(+), 428 deletions(-) create mode 100644 key create mode 100644 key.pub delete mode 100755 mission/internal_status/src/MCP342X.cpp delete mode 100755 mission/internal_status/src/MCP342X.h delete mode 100755 mission/internal_status/src/Martynas_test.cpp create mode 100644 mission/internal_status/src/battery_monitor_node.py delete mode 100644 mission/internal_status/src/smbus.h diff --git a/key b/key new file mode 100644 index 00000000..6b6a057f --- /dev/null +++ b/key @@ -0,0 +1,7 @@ +-----BEGIN OPENSSH PRIVATE KEY----- +b3BlbnNzaC1rZXktdjEAAAAABG5vbmUAAAAEbm9uZQAAAAAAAAABAAAAMwAAAAtzc2gtZW +QyNTUxOQAAACCFDsFdDE0QTwWks7dIgYJ43Q0Qs2Wi8IntJ8r02AKEBAAAAKAzONmnMzjZ +pwAAAAtzc2gtZWQyNTUxOQAAACCFDsFdDE0QTwWks7dIgYJ43Q0Qs2Wi8IntJ8r02AKEBA +AAAEBlXUTHFpp+Vhorjlf4Bc/LTZjfg67TqT5JYA38Jp9WE4UOwV0MTRBPBaSzt0iBgnjd +DRCzZaLwie0nyvTYAoQEAAAAFnJvc2Uuai5rYXBwc0BnbWFpbC5jb20BAgMEBQYH +-----END OPENSSH PRIVATE KEY----- diff --git a/key.pub b/key.pub new file mode 100644 index 00000000..cc873bd3 --- /dev/null +++ b/key.pub @@ -0,0 +1 @@ +ssh-ed25519 AAAAC3NzaC1lZDI1NTE5AAAAIIUOwV0MTRBPBaSzt0iBgnjdDRCzZaLwie0nyvTYAoQE rose.j.kapps@gmail.com diff --git a/mission/internal_status/src/MCP342X.cpp b/mission/internal_status/src/MCP342X.cpp deleted file mode 100755 index 8a45e15c..00000000 --- a/mission/internal_status/src/MCP342X.cpp +++ /dev/null @@ -1,226 +0,0 @@ -/**************************************************************************/ -/*! - @file MCP342X.cpp - @author C. Schnarel - @license BSD (see license.txt) - - This is part of an Arduino library to interface with the Microchip - MCP47X6 series of Analog-to-Digital converters which are connected - via the I2C bus. - - MCP342X I2C device class - Based on Microchip datasheets for the following part numbers - MCP3421, MCP3422, MCP3423, MCP3424, MCP3425, MCP3426, MCP3427, MCP3428 - These parts share a common programming interface - - (c) Copyright 2013 by Chip Schnarel - Updates should (hopefully) always be available at - https://github.com/uchip/MCP342X - - @section HISTORY - - 2013-Dec-24 - First release, C. Schnarel -*/ -/**************************************************************************/ - -#include "MCP342X.h" - -/* static float stepSizeTbl[] = { - 0.001, // 12-bit, 1X Gain - 0.0005, // 12-bit, 2X Gain - 0.00025, // 12-bit, 4X Gain - 0.000125, // 12-bit, 8X Gain - 0.00025, // 14-bit, 1X Gain - 0.000125, // 14-bit, 2X Gain - 0.0000625, // 14-bit, 4X Gain - 0.00003125, // 14-bit, 8X Gain - 0.0000625, // 16-bit, 1X Gain - 0.00003125, // 16-bit, 2X Gain - 0.000015625, // 16-bit, 4X Gain - 0.0000078125, // 16-bit, 8X Gain - 0.000015625, // 18-bit, 1X Gain - 0.0000078125, // 18-bit, 2X Gain - 0.00000390625, // 18-bit, 4X Gain - 0.000001953125 // 18-bit, 8X Gain - }; -*/ - -/****************************************** - * Default constructor, uses default I2C address. - * @see MCP342X_DEFAULT_ADDRESS - */ -MCP342X::MCP342X() { - devAddr = MCP342X_DEFAULT_ADDRESS; -} - -/****************************************** - * Specific address constructor. - * @param address I2C address - * @see MCP342X_DEFAULT_ADDRESS - * @see MCP342X_A0GND_A1GND, etc. - */ -MCP342X::MCP342X(uint8_t address) { - devAddr = address; -} - -/****************************************** - * Verify the I2C connection. - * Make sure the device is connected and responds as expected. - * @return True if connection is valid, false otherwise - */ -bool MCP342X::testConnection() { - Wire.beginTransmission(devAddr); - return (Wire.endTransmission() == 0); -} - - -/****************************************** - * Set the configuration shadow register - */ -void MCP342X::configure(uint8_t configData) { - configRegShdw = configData; -} - -/****************************************** - * Get the configuration shadow register - */ -uint8_t MCP342X::getConfigRegShdw(void) { - return configRegShdw; -} - -/****************************************** - * Get the step size based on the configuration shadow register - */ -/*float MCP342X::getStepSize(void) { - uint8_t select = configRegShdw & (MCP342X_SIZE_MASK | MCP342X_GAIN_MASK); - return stepSizeTbl[select]; -}*/ - -/****************************************** - * Start a conversion using configuration settings from - * the shadow configuration register - */ -bool MCP342X::startConversion(void) { - Wire.beginTransmission(devAddr); - Wire.write(configRegShdw | MCP342X_RDY); - return (Wire.endTransmission() == 0); -} - - -/****************************************** - * Start a conversion using configuration settings from - * the shadow configuration register substituting the - * supplied channel - */ -bool MCP342X::startConversion(uint8_t channel) { - Wire.beginTransmission(devAddr); - configRegShdw = ((configRegShdw & ~MCP342X_CHANNEL_MASK) | - (channel & MCP342X_CHANNEL_MASK)); - Wire.write(configRegShdw | MCP342X_RDY); - return (Wire.endTransmission() == 0); -} - - -/****************************************** - * Read the conversion value (12, 14 or 16 bit) - * Spins reading status until ready then - * fills in the supplied location with the 16-bit (two byte) - * conversion value and returns the status byte - * Note: status of -1 "0xFF' implies read error - */ -uint8_t MCP342X::getResult(int16_t *dataPtr) { - uint8_t adcStatus; - if((configRegShdw & MCP342X_SIZE_MASK) == MCP342X_SIZE_18BIT) { - return 0xFF; - } - - do { - if(Wire.requestFrom(devAddr, (uint8_t) 3) == 3) { - ((char*)dataPtr)[1] = Wire.read(); - ((char*)dataPtr)[0] = Wire.read(); - adcStatus = Wire.read(); - } - else return 0xFF; - } while((adcStatus & MCP342X_RDY) != 0x00); - return adcStatus; -} - - -/****************************************** - * Check to see if the conversion value (12, 14 or 16 bit) - * is available. If so, then - * fill in the supplied location with the 16-bit (two byte) - * conversion value and status the config byte - * Note: status of -1 "0xFF' implies read error - */ -uint8_t MCP342X::checkforResult(int16_t *dataPtr) { - uint8_t adcStatus; - if((configRegShdw & MCP342X_SIZE_MASK) == MCP342X_SIZE_18BIT) { - return 0xFF; - } - - if(Wire.requestFrom(devAddr, (uint8_t) 3) == 3) { - ((char*)dataPtr)[1] = Wire.read(); - ((char*)dataPtr)[0] = Wire.read(); - adcStatus = Wire.read(); - } - else return 0xFF; - - return adcStatus; -} - - -/****************************************** - * Read the conversion value (18 bit) - * Spins reading status until ready then - * fills in the supplied location (32 bit) with - * the 24-bit (three byte) conversion value - * and returns the status byte - * Note: status of -1 "0xFF' implies read error - */ -uint8_t MCP342X::getResult(int32_t *dataPtr) { - uint8_t adcStatus; - if((configRegShdw & MCP342X_SIZE_MASK) != MCP342X_SIZE_18BIT) { - return 0xFF; - } - - do { - if(Wire.requestFrom((uint8_t) devAddr, (uint8_t) 4) == 4) { - ((char*)dataPtr)[3] = Wire.read(); - ((char*)dataPtr)[2] = Wire.read(); - ((char*)dataPtr)[1] = Wire.read(); - adcStatus = Wire.read(); - } - else return 0xFF; - } while((adcStatus & MCP342X_RDY) != 0x00); - *dataPtr = (*dataPtr)>>8; - return adcStatus; -} - - -/****************************************** - * Check to see if the conversion value (18 bit) - * is available. If so, then - * fill in the supplied location (32 bit) with - * the 24-bit (three byte) conversion value - * and return the status byte - * Note: status of -1 "0xFF' implies read error - */ -uint8_t MCP342X::checkforResult(int32_t *dataPtr) { - uint8_t adcStatus; - if((configRegShdw & MCP342X_SIZE_MASK) != MCP342X_SIZE_18BIT) { - return 0xFF; - } - - if(Wire.requestFrom((uint8_t) devAddr, (uint8_t) 4) == 4) { - ((char*)dataPtr)[3] = Wire.read(); - ((char*)dataPtr)[2] = Wire.read(); - ((char*)dataPtr)[1] = Wire.read(); - adcStatus = Wire.read(); - } - else return 0xFF; - - *dataPtr = (*dataPtr)>>8; - return adcStatus; -} - diff --git a/mission/internal_status/src/MCP342X.h b/mission/internal_status/src/MCP342X.h deleted file mode 100755 index e14092a9..00000000 --- a/mission/internal_status/src/MCP342X.h +++ /dev/null @@ -1,134 +0,0 @@ -/**************************************************************************/ -/*! - @file MCP342X.h - @author C. Schnarel - @license BSD (see license.txt) - - This is part of an Arduino library to interface with the Microchip - MCP47X6 series of Analog-to-Digital converters which are connected - via the I2C bus. - - MCP342X I2C device class header file - Based on Microchip datasheets for the following part numbers - MCP3421, MCP3422, MCP3423, MCP3424, MCP3425, MCP3426, MCP3427, MCP3428 - These parts share a common programming interface - - (c) Copyright 2013 by Chip Schnarel - Updates should (hopefully) always be available at - https://github.com/uchip/MCP342X - - @section HISTORY - - 2013-Dec-24 - First release, C. Schnarel -*/ -/**************************************************************************/ - -#ifndef _MCP342X_H_ -#define _MCP342X_H_ - -/* -#ifdef ARDUINO - #if ARDUINO < 100 - #include "WProgram.h" - #else - #include "Arduino.h" - #endif -#else - #include "ArduinoWrapper.h" -#endif -*/ - -#include -#include - -// #include - -#include - -// I2C Address of device -// MCP3421, MCP3425 & MCP3426 are factory programed for any of 0x68 thru 0x6F -#define MCP342X_DEFAULT_ADDRESS 0x68 - -// MCP3422, MCP3423, MCP3424, MCP3427 & MCP3428 addresses are controlled by address lines A0 and A1 -// each address line can be low (GND), high (VCC) or floating (FLT) -#define MCP342X_A0GND_A1GND 0x68 -#define MCP342X_A0GND_A1FLT 0x69 -#define MCP342X_A0GND_A1VCC 0x6A -#define MCP342X_A0FLT_A1GND 0x6B -#define MCP342X_A0VCC_A1GND 0x6C -#define MCP342X_A0VCC_A1FLT 0x6D -#define MCP342X_A0VCC_A1VCC 0x6E -#define MCP342X_A0FLT_A1VCC 0x6F - - -// Conversion mode definitions -#define MCP342X_MODE_ONESHOT 0x00 -#define MCP342X_MODE_CONTINUOUS 0x10 - - -// Channel definitions -// MCP3421 & MCP3425 have only the one channel and ignore this param -// MCP3422, MCP3423, MCP3426 & MCP3427 have two channels and treat 3 & 4 as repeats of 1 & 2 respectively -// MCP3424 & MCP3428 have all four channels -#define MCP342X_CHANNEL_1 0x00 -#define MCP342X_CHANNEL_2 0x20 -#define MCP342X_CHANNEL_3 0x40 -#define MCP342X_CHANNEL_4 0x60 -#define MCP342X_CHANNEL_MASK 0x60 - - -// Sample size definitions - these also affect the sampling rate -// 12-bit has a max sample rate of 240sps -// 14-bit has a max sample rate of 60sps -// 16-bit has a max sample rate of 15sps -// 18-bit has a max sample rate of 3.75sps (MCP3421, MCP3422, MCP3423, MCP3424 only) -#define MCP342X_SIZE_12BIT 0x00 -#define MCP342X_SIZE_14BIT 0x04 -#define MCP342X_SIZE_16BIT 0x08 -#define MCP342X_SIZE_18BIT 0x0C -#define MCP342X_SIZE_MASK 0x0C - - -// Programmable Gain definitions -#define MCP342X_GAIN_1X 0x00 -#define MCP342X_GAIN_2X 0x01 -#define MCP342X_GAIN_4X 0x02 -#define MCP342X_GAIN_8X 0x03 -#define MCP342X_GAIN_MASK 0x03 - - -// /RDY bit definition -#define MCP342X_RDY 0x80 - - -class MCP342X { - public: - MCP342X(); - MCP342X(uint8_t address); - - bool testConnection(void); - - // Set/Get the configuration bits for the ADC - void configure(uint8_t config); - uint8_t getConfigRegShdw(void); - //float getStepSize(); // returns step size based on configRegShdw - - // Start a conversion - bool startConversion(void); - bool startConversion(uint8_t channel); - - // Read the ADC result - uint8_t getResult(int16_t *data); - uint8_t getResult(int32_t *data); - - // Non-blocking Read the ADC result - uint8_t checkforResult(int16_t *data); - uint8_t checkforResult(int32_t *data); - - private: - uint8_t devAddr; - uint8_t configRegShdw; - //float stepSizeTbl[]; -}; - -#endif /* _MCP342X_H_ */ \ No newline at end of file diff --git a/mission/internal_status/src/Martynas_test.cpp b/mission/internal_status/src/Martynas_test.cpp deleted file mode 100755 index ef36ff18..00000000 --- a/mission/internal_status/src/Martynas_test.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include -#include -#include -#include "MCP342X.h" - -int main(){ - std::cout << "Hello world" << std::endl; - return 0; -} \ No newline at end of file diff --git a/mission/internal_status/src/battery_monitor_node.py b/mission/internal_status/src/battery_monitor_node.py new file mode 100644 index 00000000..3e5acddc --- /dev/null +++ b/mission/internal_status/src/battery_monitor_node.py @@ -0,0 +1,47 @@ +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/src/smbus.h b/mission/internal_status/src/smbus.h deleted file mode 100644 index a7ffd3b3..00000000 --- a/mission/internal_status/src/smbus.h +++ /dev/null @@ -1,59 +0,0 @@ -/* - smbus.h - SMBus level access helper functions - - Copyright (C) 1995-97 Simon G. Vogl - Copyright (C) 1998-99 Frodo Looijaard - - This program is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; either version 2 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program; if not, write to the Free Software - Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, - MA 02110-1301 USA. -*/ - -#ifndef LIB_I2C_SMBUS_H -#define LIB_I2C_SMBUS_H - -#include -#include - -extern __s32 i2c_smbus_access(int file, char read_write, __u8 command, int size, - union i2c_smbus_data *data); - -extern __s32 i2c_smbus_write_quick(int file, __u8 value); -extern __s32 i2c_smbus_read_byte(int file); -extern __s32 i2c_smbus_write_byte(int file, __u8 value); -extern __s32 i2c_smbus_read_byte_data(int file, __u8 command); -extern __s32 i2c_smbus_write_byte_data(int file, __u8 command, __u8 value); -extern __s32 i2c_smbus_read_word_data(int file, __u8 command); -extern __s32 i2c_smbus_write_word_data(int file, __u8 command, __u16 value); -extern __s32 i2c_smbus_process_call(int file, __u8 command, __u16 value); - -/* Returns the number of read bytes */ -extern __s32 i2c_smbus_read_block_data(int file, __u8 command, __u8 *values); -extern __s32 i2c_smbus_write_block_data(int file, __u8 command, __u8 length, - const __u8 *values); - -/* Returns the number of read bytes */ -/* Until kernel 2.6.22, the length is hardcoded to 32 bytes. If you - ask for less than 32 bytes, your code will only work with kernels - 2.6.23 and later. */ -extern __s32 i2c_smbus_read_i2c_block_data(int file, __u8 command, __u8 length, - __u8 *values); -extern __s32 i2c_smbus_write_i2c_block_data(int file, __u8 command, __u8 length, - const __u8 *values); - -/* Returns the number of read bytes */ -extern __s32 i2c_smbus_block_process_call(int file, __u8 command, __u8 length, - __u8 *values); - -#endif /* LIB_I2C_SMBUS_H */ \ No newline at end of file From 533f9240e3003f3ffc0cc5f8a94174703b2eb86b Mon Sep 17 00:00:00 2001 From: Clang Robot Date: Wed, 25 Oct 2023 17:57:33 +0000 Subject: [PATCH 08/16] Committing clang-format changes --- mission/internal_status/src/battery_monitor_lib.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/mission/internal_status/src/battery_monitor_lib.cpp b/mission/internal_status/src/battery_monitor_lib.cpp index c7e677dd..30922db1 100644 --- a/mission/internal_status/src/battery_monitor_lib.cpp +++ b/mission/internal_status/src/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 From 859c696850ae4b75b3c3f4360201b069b6dcdbd1 Mon Sep 17 00:00:00 2001 From: RoseKapps Date: Sun, 29 Oct 2023 19:12:37 +0100 Subject: [PATCH 09/16] lib and node work --- mission/internal_status/CMakeLists.txt | 26 --- .../internal_status/__init__.py | 0 .../battery_monitor_lib.cpp | 12 +- .../power_sense_module_lib.py} | 8 +- .../power_sense_module_publisher.py | 49 ++++++ .../internal_status/test_lib.py | 7 + mission/internal_status/package.xml | 21 ++- .../internal_status/resource/internal_status | 0 mission/internal_status/setup.cfg | 4 + mission/internal_status/setup.py | 26 +++ .../internal_status/src/battery_monitor.py | 164 ------------------ .../src/battery_monitor_node.py | 47 ----- .../internal_status/test/test_copyright.py | 25 +++ mission/internal_status/test/test_flake8.py | 25 +++ mission/internal_status/test/test_pep257.py | 23 +++ 15 files changed, 183 insertions(+), 254 deletions(-) delete mode 100644 mission/internal_status/CMakeLists.txt create mode 100644 mission/internal_status/internal_status/__init__.py rename mission/internal_status/{src => internal_status}/battery_monitor_lib.cpp (95%) rename mission/internal_status/{src/battery_monitor_lib.py => internal_status/power_sense_module_lib.py} (98%) create mode 100644 mission/internal_status/internal_status/power_sense_module_publisher.py create mode 100644 mission/internal_status/internal_status/test_lib.py create mode 100644 mission/internal_status/resource/internal_status create mode 100644 mission/internal_status/setup.cfg create mode 100644 mission/internal_status/setup.py delete mode 100755 mission/internal_status/src/battery_monitor.py delete mode 100644 mission/internal_status/src/battery_monitor_node.py create mode 100644 mission/internal_status/test/test_copyright.py create mode 100644 mission/internal_status/test/test_flake8.py create mode 100644 mission/internal_status/test/test_pep257.py 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' From 00b5051d5de51ae8975ce89ab1b419295b452e5e Mon Sep 17 00:00:00 2001 From: Clang Robot Date: Sun, 29 Oct 2023 18:13:08 +0000 Subject: [PATCH 10/16] Committing clang-format changes --- .../internal_status/battery_monitor_lib.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/mission/internal_status/internal_status/battery_monitor_lib.cpp b/mission/internal_status/internal_status/battery_monitor_lib.cpp index c7e677dd..30922db1 100644 --- a/mission/internal_status/internal_status/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 From 12cfeabd3e8b6dd5c0c1495d1ba8bb5978c89a1d Mon Sep 17 00:00:00 2001 From: RoseKapps Date: Wed, 1 Nov 2023 20:36:27 +0100 Subject: [PATCH 11/16] lib, publisher and launch file clean --- .../internal_status/__init__.py | 0 .../internal_status/battery_monitor_lib.cpp | 137 ------------------ .../internal_status/test_lib.py | 7 - .../launch/internal_status_launch.py | 10 ++ mission/internal_status/package.xml | 1 + mission/internal_status/setup.py | 3 + 6 files changed, 14 insertions(+), 144 deletions(-) delete mode 100644 mission/internal_status/internal_status/__init__.py delete mode 100644 mission/internal_status/internal_status/battery_monitor_lib.cpp delete mode 100644 mission/internal_status/internal_status/test_lib.py create mode 100644 mission/internal_status/launch/internal_status_launch.py diff --git a/mission/internal_status/internal_status/__init__.py b/mission/internal_status/internal_status/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/mission/internal_status/internal_status/battery_monitor_lib.cpp b/mission/internal_status/internal_status/battery_monitor_lib.cpp deleted file mode 100644 index 30922db1..00000000 --- a/mission/internal_status/internal_status/battery_monitor_lib.cpp +++ /dev/null @@ -1,137 +0,0 @@ -#include -using namespace std; // do i need this? - -//#include -//#include -#include "MCP342x.h" -#include "smbus.h" -#include - -int main() { - cout << "Hello world" << endl; - cout << "smbus included" << endl; - cout << "MCP342x included" << endl; - return 0; -} - -// include doesn't work well - -// how to know the types of the values? - -// - -/* -class BatteryMonitor{ - -public: - BatteryMonitor() //default constructor to initialize the class - { - //***************I2C protocole******************************* - i2c_adress = 0x69; - bus = smbus.SMBus(1); - channel_voltage = MCP342x(bus,i2c_adress,channel=0,resolution=18); - channel_current = MCP342x(bus,i2c_adress,channel=1,resolution=18); - //TIME SLEEP? -> only for rospy? - - //**********convertion ratio taken from PSM datasheet************** - psm_to_battery_voltage = 11.0; - psm_to_battery_current_scale_factor =37.8788; - psm_to_battery_current_offset = 0.330; - - - system_voltage = 0.0; - system_current = 0.0; - - system_voltage_state = "No receive"; - system_current_state = "No receive"; - - I2C_error_counter_current = 0; - I2C_error_counter_voltage = 0; - - cout << "Iniatialization done" << endl; - } - - float get_PSM_current(){ - - try { - system_current = ((channel_current.convert_and_read() - - psm_to_battery_current_offset) - * psm_to_battery_current_scale_factor); - - if (system_current_state != "Received") { - system_current_state = "Received"; - } - } - - catch (const std::ios_base::failure& e) { - I2C_error_counter_current += 1; - system_current_state = "Error"; - cerr << "Error: " << e.what() << endl; - } - - return system_current; - } - - float get_PSM_voltage(){ - - try { - system_voltage = (channel_voltage.convert_and_read() - * psm_to_battery_voltage); - - if (system_voltage_state != "Received") { - system_voltage_state = "Received"; - } - } - - catch (const std::ios_base::failure& e) { - I2C_error_counter_voltage += 1; - system_current_state = "Error"; - cerr << "Error: " << e.what() << endl; - } - - return system_voltage; - } - - void shutdown(){ - - bus.close(); - } - -private: - int16_t i2c_adress; - int bus; - float channel_voltage; //what's the type of these value? - float channel_current; //what's the type of these value? - float psm_to_battery_voltage; - float psm_to_battery_current_scale_factor; - float psm_to_battery_current_offset; - float system_voltage; - float system_current; - string system_voltage_state; - string system_current_state; - int I2C_error_counter_current; - int I2C_error_counter_voltage; - -}; - -*/ - -/* TRY CATCH BLOCK - - -try { - system_current = (channel_current.convert_and_read() - - psm_to_battery_current_offset) - * psm_to_battery_current_scale_factor; - - if (system_current_state != "Received") { - system_current_state = "Received"; - } -} -catch (const std::ios_base::failure& e) { - I2C_error_counter_current += 1; - system_current_state = "Error"; -} - - -*/ \ No newline at end of file diff --git a/mission/internal_status/internal_status/test_lib.py b/mission/internal_status/internal_status/test_lib.py deleted file mode 100644 index 80a94731..00000000 --- a/mission/internal_status/internal_status/test_lib.py +++ /dev/null @@ -1,7 +0,0 @@ -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/launch/internal_status_launch.py b/mission/internal_status/launch/internal_status_launch.py new file mode 100644 index 00000000..18c26015 --- /dev/null +++ b/mission/internal_status/launch/internal_status_launch.py @@ -0,0 +1,10 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package = 'internal_status', + executable = 'power_sense_module_publisher' + ) + ]) \ No newline at end of file diff --git a/mission/internal_status/package.xml b/mission/internal_status/package.xml index b380f4d9..ae43885f 100644 --- a/mission/internal_status/package.xml +++ b/mission/internal_status/package.xml @@ -11,6 +11,7 @@ rclpy std_msgs power_sense_module_lib + ros2launch diff --git a/mission/internal_status/setup.py b/mission/internal_status/setup.py index 41d9f477..8f4ee25f 100644 --- a/mission/internal_status/setup.py +++ b/mission/internal_status/setup.py @@ -1,3 +1,5 @@ +import os +from glob import glob from setuptools import find_packages, setup package_name = 'internal_status' @@ -10,6 +12,7 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))) ], install_requires=['setuptools'], zip_safe=True, From 33f564cf0f03c23e7b5a238d7af3fd95effddbe6 Mon Sep 17 00:00:00 2001 From: RoseKapps Date: Sun, 5 Nov 2023 13:36:34 +0100 Subject: [PATCH 12/16] some unusefull stuff deleted and readme file done (hope it's ok) --- mission/internal_status/README.md | 22 +++++++++++++++++++ .../internal_status/power_sense_module_lib.py | 2 +- .../power_sense_module_publisher.py | 3 +-- 3 files changed, 24 insertions(+), 3 deletions(-) create mode 100644 mission/internal_status/README.md diff --git a/mission/internal_status/README.md b/mission/internal_status/README.md new file mode 100644 index 00000000..491c159d --- /dev/null +++ b/mission/internal_status/README.md @@ -0,0 +1,22 @@ +# Internal Status + +This package contains a library which allows us to get current and voltage values from the Power Sense Module (PSM), and a ros node which publishes this data to the whole ROS environment. + +## PSM lib + +The ASV as a PSM. It takes voltage and current data going into internal electrical instruments on board. This library is a driver, which allows us to get the voltage (in V) and current (in A) values from PSM through I2C. +The I2C address is 0X69. +* Typical values for voltage: around 21V +* Typical values for current: around 0.6A + + +## PSM node + +This ros node just publishes the current and voltage data we get from the lib to the entire ROS environment. +### Publishes to +* /asv/power_sense_module/current for the current data [A] +* /asv/power_sense_module/voltage for the voltage data [V] + + + + diff --git a/mission/internal_status/internal_status/power_sense_module_lib.py b/mission/internal_status/internal_status/power_sense_module_lib.py index aaa2fe3a..e128a02f 100644 --- a/mission/internal_status/internal_status/power_sense_module_lib.py +++ b/mission/internal_status/internal_status/power_sense_module_lib.py @@ -19,7 +19,7 @@ def __init__(self): self.i2c_adress, channel=0, resolution=18) # current - #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 diff --git a/mission/internal_status/internal_status/power_sense_module_publisher.py b/mission/internal_status/internal_status/power_sense_module_publisher.py index 53a442e7..87ec8980 100644 --- a/mission/internal_status/internal_status/power_sense_module_publisher.py +++ b/mission/internal_status/internal_status/power_sense_module_publisher.py @@ -14,7 +14,7 @@ def __init__(self): 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() @@ -26,7 +26,6 @@ def timer_callback(self): 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): From d57337945314db0cf19b3f1a3002efa469203cc0 Mon Sep 17 00:00:00 2001 From: Martin Huynh Date: Tue, 7 Nov 2023 16:40:45 +0100 Subject: [PATCH 13/16] Thrust allocation ros2 (#99) * Created Wrench publisher, force = (1,1,1) * Impl. thrust allocation, listening wrench_input * Committing clang-format changes * Committing clang-format changes * Added matrix and wrench health cheks * Committing clang-format changes * Cleanup, add launch file, thrust saturation * Committing clang-format changes * Cleanup: removed unnecessary functions and variables, added pass by reference, use ROS2 timer_callback, added doxygen documentation * Committing clang-format changes * Add runtime_error for calculateRightPseudoinverse and renamed Allocator to ThrusterAllocator * Removed TODO comment * Committing clang-format changes * Comments/doxygen cleanup, fixed healthyWrench() logic * Committing clang-format changes * Renamed node to thruster_allocator_node * Added thruster allocator tests (not working) * Committing clang-format changes * Updated freya.yaml with wildcards * Update thruster_allocator_launch.py so that it takes in params from freya.yaml * Update CMakeLists.txt with installation of config * Added get ros parameters from YAML wildcards * Committing clang-format changes * Fix: No raw loops * Committing clang-format changes * Removed non-working tests * rearranged doxygen comments * fixed include statements and removed non-existent tests from cmake * Committing clang-format changes --------- Co-authored-by: Clang Robot Co-authored-by: Aldokan Co-authored-by: alekskl01 Co-authored-by: Aleksander Klund <99870311+alekskl01@users.noreply.github.com> --- asv_setup/CMakeLists.txt | 9 +- asv_setup/config/robots/freya.yaml | 52 ++++++++ motion/thruster_allocator/CMakeLists.txt | 50 ++++++++ motion/thruster_allocator/README.md | 5 + .../thruster_allocator/allocator_ros.hpp | 62 +++++++++ .../thruster_allocator/allocator_utils.hpp | 118 ++++++++++++++++++ .../pseudoinverse_allocator.hpp | 39 ++++++ .../launch/thruster_allocator.launch.py | 16 +++ motion/thruster_allocator/package.xml | 24 ++++ .../thruster_allocator/src/allocator_ros.cpp | 95 ++++++++++++++ .../src/pseudoinverse_allocator.cpp | 10 ++ .../src/thruster_allocator_node.cpp | 11 ++ 12 files changed, 490 insertions(+), 1 deletion(-) create mode 100644 asv_setup/config/robots/freya.yaml create mode 100644 motion/thruster_allocator/CMakeLists.txt create mode 100644 motion/thruster_allocator/README.md create mode 100644 motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp create mode 100644 motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp create mode 100644 motion/thruster_allocator/include/thruster_allocator/pseudoinverse_allocator.hpp create mode 100644 motion/thruster_allocator/launch/thruster_allocator.launch.py create mode 100644 motion/thruster_allocator/package.xml create mode 100644 motion/thruster_allocator/src/allocator_ros.cpp create mode 100644 motion/thruster_allocator/src/pseudoinverse_allocator.cpp create mode 100644 motion/thruster_allocator/src/thruster_allocator_node.cpp diff --git a/asv_setup/CMakeLists.txt b/asv_setup/CMakeLists.txt index 8a67f88d..d57de63a 100644 --- a/asv_setup/CMakeLists.txt +++ b/asv_setup/CMakeLists.txt @@ -5,11 +5,18 @@ 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) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + # Install launch files. install(DIRECTORY + config launch DESTINATION share/${PROJECT_NAME}/ ) diff --git a/asv_setup/config/robots/freya.yaml b/asv_setup/config/robots/freya.yaml new file mode 100644 index 00000000..7ec64e1e --- /dev/null +++ b/asv_setup/config/robots/freya.yaml @@ -0,0 +1,52 @@ +# This file defines parameters specific to Freya +# +# When looking at the ASV from above, the thruster placement is: +# /\ /\ +# / \ front / \ +# / \ / \ +# |=3↗=| |=0↖=| +# | | | | +# | | | | +# | |======| | +# | | | | +# | | | | +# | | | | +# |=2↖=|==||==|=1↗=| +# +/**: + ros__parameters: + physical: + mass_kg: 0 + displacement_m3: 0 + buoyancy: 0 # kg + center_of_mass: [0, 0, 0] # mm (x,y,z) + center_of_buoyancy: [0, 0, 0] # mm (x,y,z) + + + propulsion: + dofs: + num: 3 + which: + surge: true + sway: true + yaw: true + thrusters: + num: 4 + min: -100 + max: 100 + configuration_matrix: #NED + [0.70711, 0.70711, 0.70711, 0.70711, + -0.70711, 0.70711, -0.70711, 0.70711, + 0.27738, 0.27738, -0.27738, -0.27738] + + rate_of_change: + max: 0 # Maximum rate of change in newton per second for a thruster + thruster_to_pin_map: [1, 3, 2, 0] # I.e. if thruster_to_pin = [1, 3, 2, 0] then thruster 0 is pin 1 etc.. + direction: [1, 1, 1, 1] # Disclose during thruster mapping + offset: [0, 0, 0, 0] # Disclose during thruster mapping + command: + wrench: + max: [150.0, 150.0, 150.0, 150.0, 150.0, 150.0] + scaling: [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + + computer: "pc-debug" diff --git a/motion/thruster_allocator/CMakeLists.txt b/motion/thruster_allocator/CMakeLists.txt new file mode 100644 index 00000000..de9e0920 --- /dev/null +++ b/motion/thruster_allocator/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.8) +project(thruster_allocator) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +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) +find_package(rclcpp REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) + +include_directories(include) + +add_executable(${PROJECT_NAME}_node + src/thruster_allocator_node.cpp + src/allocator_ros.cpp + src/pseudoinverse_allocator.cpp + ) + +ament_target_dependencies(${PROJECT_NAME}_node + rclcpp + geometry_msgs + vortex_msgs + Eigen3 + ) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(TARGETS + ${PROJECT_NAME}_node + DESTINATION lib/${PROJECT_NAME}) + +# Install launch files. +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/motion/thruster_allocator/README.md b/motion/thruster_allocator/README.md new file mode 100644 index 00000000..5cbc8f0a --- /dev/null +++ b/motion/thruster_allocator/README.md @@ -0,0 +1,5 @@ +## Thruster allocator + +This package takes a thrust vector and calculates the corresponding thrust required from each individual thruster. The resulting calculation is published as a vortex_msgs ThrusterForces. + +The calculation itself is based on the 'unweighted pseudoinverse-based allocator' as described in Fossen (2011): Handbook of Marine Craft Hydrodynamics and Motion Control (chapter 12.3.2). \ No newline at end of file diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp new file mode 100644 index 00000000..22f2d5cf --- /dev/null +++ b/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp @@ -0,0 +1,62 @@ +/** + * @file allocator_ros.hpp + * @brief ThrusterAllocator class, which + * allocates thrust to the ASV's thrusters based on the desired body frame + * forces. + */ + +#ifndef VORTEX_ALLOCATOR_ALLOCATOR_ROS_HPP +#define VORTEX_ALLOCATOR_ALLOCATOR_ROS_HPP + +#include "thruster_allocator/allocator_utils.hpp" +#include "thruster_allocator/pseudoinverse_allocator.hpp" +#include +#include + +#include +#include + +using namespace std::chrono_literals; + +class ThrusterAllocator : public rclcpp::Node { +public: + explicit ThrusterAllocator(); + + /** + * @brief Calculates the allocated + * thrust based on the body frame forces. It then saturates the output vector + * between min and max values and publishes the thruster forces to the topic + * "thrust/thruster_forces". + */ + void timer_callback(); + +private: + Eigen::MatrixXd thrust_configuration; + /** + * @brief Callback function for the wrench input subscription. Extracts the + * surge, sway and yaw values from the received wrench msg + * and stores them in the body_frame_forces_ Eigen vector. + * @param msg The received geometry_msgs::msg::Wrench message. + */ + void wrench_callback(const geometry_msgs::msg::Wrench &msg); + + /** + * @brief Checks if the given Eigen vector contains any NaN or Inf values + * @param v The Eigen vector to check. + * @return True if the vector is healthy, false otherwise. + */ + bool healthyWrench(const Eigen::VectorXd &v) const; + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Subscription::SharedPtr subscription_; + rclcpp::TimerBase::SharedPtr timer_; + size_t count_; + int num_dof_; + int num_thrusters_; + int min_thrust_; + int max_thrust_; + Eigen::Vector3d body_frame_forces_; + std::vector direction_; + PseudoinverseAllocator pseudoinverse_allocator_; +}; + +#endif // VORTEX_ALLOCATOR_ALLOCATOR_ROS_HPP diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp new file mode 100644 index 00000000..5c861b0b --- /dev/null +++ b/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp @@ -0,0 +1,118 @@ +/** + * @file allocator_utils.hpp + * @brief This file contains utility functions for the thruster allocator + * module. + */ + +#ifndef VORTEX_ALLOCATOR_ALLOCATOR_UTILS_HPP +#define VORTEX_ALLOCATOR_ALLOCATOR_UTILS_HPP + +#include +#include +#include +#include + +#include + +/** + * @brief Check if the matrix has any NaN or INF elements. + * + * @tparam Derived The type of the matrix. + * @param M The matrix to check. + * @return true if the matrix has any NaN or INF elements, false otherwise. + */ +template +inline bool isInvalidMatrix(const Eigen::MatrixBase &M) { + bool has_nan = !(M.array() == M.array()).all(); + bool has_inf = M.array().isInf().any(); + return has_nan || has_inf; +} + +/** + * @brief Returns a string stream containing the matrix with the given name. + * + * @param name The name of the matrix. + * @param M The matrix to print. + * @return std::stringstream The string stream containing the matrix. + */ +inline std::stringstream printMatrix(std::string name, + const Eigen::MatrixXd &M) { + std::stringstream ss; + ss << std::endl << name << " = " << std::endl << M; + return ss; +} + +/** + * @brief Calculates the right pseudoinverse of the given matrix. + * + * @param M The matrix to calculate the pseudoinverse of. + * @param M_pinv The resulting pseudoinverse matrix. + * @throws char* if the pseudoinverse is invalid. + */ +inline void calculateRightPseudoinverse(const Eigen::MatrixXd &M, + Eigen::MatrixXd &M_pinv) { + Eigen::MatrixXd pseudoinverse = M.transpose() * (M * M.transpose()).inverse(); + // pseudoinverse.completeOrthogonalDecomposition().pseudoInverse(); + if (isInvalidMatrix(pseudoinverse)) { + throw std::runtime_error("Invalid Psuedoinverse Calculated"); + } + M_pinv = pseudoinverse; +} + +/** + * @brief Saturates the values of a given Eigen vector between a minimum and + * maximum value. + * + * @param vec The Eigen vector to be saturated. + * @param min The minimum value to saturate the vector values to. + * @param max The maximum value to saturate the vector values to. + * @return True if all vector values are within the given range, false + * otherwise. + */ +inline bool saturateVectorValues(Eigen::VectorXd &vec, double min, double max) { + bool vector_in_range = std::all_of(vec.begin(), vec.end(), [&](double &val) { + if (val > max) { + val = max; + return false; + } else if (val < min) { + val = min; + return false; + } + return true; + }); + return vector_in_range; +} + +/** + * @brief Converts an Eigen VectorXd to a vortex_msgs::msg::ThrusterForces + * message. + * + * @param u The Eigen VectorXd to be converted. + * @param msg The vortex_msgs::msg::ThrusterForces message to store the + * converted values. + */ +inline void arrayEigenToMsg(const Eigen::VectorXd &u, + vortex_msgs::msg::ThrusterForces &msg) { + int r = u.size(); + std::vector u_vec(r); + std::copy_n(u.begin(), r, u_vec.begin()); + msg.thrust = u_vec; +} + +/** + * @brief Converts a 1D array of doubles to a 2D Eigen matrix. + * + * @param matrix The 1D array of doubles to be converted. + * @param rows The number of rows in the resulting Eigen matrix. + * @param cols The number of columns in the resulting Eigen matrix. + * @return The resulting Eigen matrix. + */ +inline Eigen::MatrixXd +doubleArrayToEigenMatrix(const std::vector &matrix, int rows, + int cols) { + return Eigen::Map>(matrix.data(), rows, + cols); +} + +#endif // VORTEX_ALLOCATOR_ALLOCATOR_UTILS_HPP diff --git a/motion/thruster_allocator/include/thruster_allocator/pseudoinverse_allocator.hpp b/motion/thruster_allocator/include/thruster_allocator/pseudoinverse_allocator.hpp new file mode 100644 index 00000000..0d94596e --- /dev/null +++ b/motion/thruster_allocator/include/thruster_allocator/pseudoinverse_allocator.hpp @@ -0,0 +1,39 @@ +/** + * @file pseudoinverse_allocator.hpp + * @brief Contains the PseudoinverseAllocator class, which implements the + * unweighted pseudoinverse-based allocator described in e.g. Fossen 2011 + * Handbook of Marine Craft Hydrodynamics and Motion Control (chapter 12.3.2). + */ + +#ifndef VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_HPP +#define VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_HPP + +#include + +/** + * @brief The PseudoinverseAllocator class calculates the allocated thrust given + * the input torques using the pseudoinverse allocator. + */ +class PseudoinverseAllocator { +public: + /** + * @brief Constructor for the PseudoinverseAllocator class. + * + * @param T_pinv The pseudoinverse of the thruster configuration matrix. + */ + explicit PseudoinverseAllocator(const Eigen::MatrixXd &T_pinv); + + /** + * @brief Calculates the allocated thrust given the input torques using the + * pseudoinverse allocator. + * + * @param tau The input torques as a vector. + * @return The allocated thrust as a vector. + */ + Eigen::VectorXd calculateAllocatedThrust(const Eigen::VectorXd &tau); + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Eigen::MatrixXd T_pinv; +}; + +#endif // VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_HPP \ No newline at end of file diff --git a/motion/thruster_allocator/launch/thruster_allocator.launch.py b/motion/thruster_allocator/launch/thruster_allocator.launch.py new file mode 100644 index 00000000..3e08ca13 --- /dev/null +++ b/motion/thruster_allocator/launch/thruster_allocator.launch.py @@ -0,0 +1,16 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + trushter_allocator_node = Node( + package='thruster_allocator', + executable='thruster_allocator_node', + name='thruster_allocator_node', + parameters=[os.path.join(get_package_share_directory('asv_setup'),'config','robots','freya.yaml')], + output='screen', + ) + return LaunchDescription([ + trushter_allocator_node + ]) diff --git a/motion/thruster_allocator/package.xml b/motion/thruster_allocator/package.xml new file mode 100644 index 00000000..6f3f3a38 --- /dev/null +++ b/motion/thruster_allocator/package.xml @@ -0,0 +1,24 @@ + + + + thruster_allocator + 0.0.0 + Thurster allocator for ASV + martin + MIT + + rclcpp + geometry_msgs + vortex_msgs + eigen + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + ament_cmake + + + ament_cmake + + diff --git a/motion/thruster_allocator/src/allocator_ros.cpp b/motion/thruster_allocator/src/allocator_ros.cpp new file mode 100644 index 00000000..0bf1ba68 --- /dev/null +++ b/motion/thruster_allocator/src/allocator_ros.cpp @@ -0,0 +1,95 @@ +#include "thruster_allocator/allocator_ros.hpp" +#include "thruster_allocator/allocator_utils.hpp" +#include "thruster_allocator/pseudoinverse_allocator.hpp" +#include + +#include +#include + +using namespace std::chrono_literals; + +ThrusterAllocator::ThrusterAllocator() + : Node("thruster_allocator_node"), + pseudoinverse_allocator_(Eigen::MatrixXd::Zero(3, 4)) { + declare_parameter("propulsion.dofs.num", 3); + declare_parameter("propulsion.thrusters.num", 4); + declare_parameter("propulsion.thrusters.min", -100); + declare_parameter("propulsion.thrusters.max", 100); + declare_parameter("propulsion.thrusters.direction", std::vector{0}); + declare_parameter("propulsion.thrusters.configuration_matrix", + std::vector{0}); + + num_dof_ = get_parameter("propulsion.dofs.num").as_int(); + num_thrusters_ = get_parameter("propulsion.thrusters.num").as_int(); + min_thrust_ = get_parameter("propulsion.thrusters.min").as_int(); + max_thrust_ = get_parameter("propulsion.thrusters.max").as_int(); + direction_ = + get_parameter("propulsion.thrusters.direction").as_integer_array(); + thrust_configuration = doubleArrayToEigenMatrix( + get_parameter("propulsion.thrusters.configuration_matrix") + .as_double_array(), + num_dof_, num_thrusters_); + + subscription_ = this->create_subscription( + "thrust/wrench_input", 1, + std::bind(&ThrusterAllocator::wrench_callback, this, + std::placeholders::_1)); + + publisher_ = this->create_publisher( + "thrust/thruster_forces", 1); + + timer_ = this->create_wall_timer( + 100ms, std::bind(&ThrusterAllocator::timer_callback, this)); + + Eigen::MatrixXd thrust_configuration_pseudoinverse; + calculateRightPseudoinverse(thrust_configuration, + thrust_configuration_pseudoinverse); + + pseudoinverse_allocator_.T_pinv = thrust_configuration_pseudoinverse; + + body_frame_forces_.setZero(); +} + +void ThrusterAllocator::timer_callback() { + Eigen::VectorXd thruster_forces = + pseudoinverse_allocator_.calculateAllocatedThrust(body_frame_forces_); + + if (isInvalidMatrix(thruster_forces)) { + RCLCPP_ERROR(get_logger(), "ThrusterForces vector invalid"); + return; + } + + if (!saturateVectorValues(thruster_forces, min_thrust_, max_thrust_)) { + RCLCPP_WARN(get_logger(), "Thruster forces vector required saturation."); + } + + vortex_msgs::msg::ThrusterForces msg_out; + arrayEigenToMsg(thruster_forces, msg_out); + std::transform(msg_out.thrust.begin(), msg_out.thrust.end(), + direction_.begin(), msg_out.thrust.begin(), + std::multiplies<>()); + publisher_->publish(msg_out); +} + +void ThrusterAllocator::wrench_callback(const geometry_msgs::msg::Wrench &msg) { + Eigen::Vector3d msg_vector; + msg_vector(0) = msg.force.x; // surge + msg_vector(1) = msg.force.y; // sway + msg_vector(2) = msg.torque.z; // yaw + if (!healthyWrench(msg_vector)) { + RCLCPP_ERROR(get_logger(), "ASV wrench vector invalid, ignoring."); + return; + } + std::swap(msg_vector, body_frame_forces_); +} + +bool ThrusterAllocator::healthyWrench(const Eigen::VectorXd &v) const { + if (isInvalidMatrix(v)) + return false; + + bool within_max_thrust = std::none_of(v.begin(), v.end(), [this](double val) { + return std::abs(val) > max_thrust_; + }); + + return within_max_thrust; +} diff --git a/motion/thruster_allocator/src/pseudoinverse_allocator.cpp b/motion/thruster_allocator/src/pseudoinverse_allocator.cpp new file mode 100644 index 00000000..8b553763 --- /dev/null +++ b/motion/thruster_allocator/src/pseudoinverse_allocator.cpp @@ -0,0 +1,10 @@ +#include "thruster_allocator/pseudoinverse_allocator.hpp" + +PseudoinverseAllocator::PseudoinverseAllocator(const Eigen::MatrixXd &T_pinv) + : T_pinv(T_pinv) {} + +Eigen::VectorXd +PseudoinverseAllocator::calculateAllocatedThrust(const Eigen::VectorXd &tau) { + Eigen::VectorXd u = T_pinv * tau; + return u; +} \ No newline at end of file diff --git a/motion/thruster_allocator/src/thruster_allocator_node.cpp b/motion/thruster_allocator/src/thruster_allocator_node.cpp new file mode 100644 index 00000000..77c604b7 --- /dev/null +++ b/motion/thruster_allocator/src/thruster_allocator_node.cpp @@ -0,0 +1,11 @@ +#include "thruster_allocator/allocator_ros.hpp" +#include "thruster_allocator/allocator_utils.hpp" + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto allocator = std::make_shared(); + RCLCPP_INFO(allocator->get_logger(), "Thruster allocator initiated"); + rclcpp::spin(allocator); + rclcpp::shutdown(); + return 0; +} From 29d46bece2bf19cc25be89d1cc6ea11fd192f912 Mon Sep 17 00:00:00 2001 From: Aldokan Date: Sun, 12 Nov 2023 13:37:32 +0100 Subject: [PATCH 14/16] Enhancement/joystick interface cleanup (#110) * Cleanup and added docstrings to functions * Fixed deadzone on joy driver * Fixed bugs in tests * Updated Readme * updated readme again * removed comment, and type specifiers paranthesis in docstrings --------- Co-authored-by: Aldokan --- asv_setup/launch/pc.launch.py | 4 +- mission/joystick_interface/CMakeLists.txt | 6 +- mission/joystick_interface/README.md | 4 +- mission/joystick_interface/package.xml | 1 + .../__init__.py | 0 .../joystick_interface.py | 162 +++++++++++------- .../test/test_joystick_interface.py | 14 +- 7 files changed, 118 insertions(+), 73 deletions(-) rename mission/joystick_interface/{joystick_interface => scripts}/__init__.py (100%) rename mission/joystick_interface/{joystick_interface => scripts}/joystick_interface.py (50%) diff --git a/asv_setup/launch/pc.launch.py b/asv_setup/launch/pc.launch.py index 1248ae25..98c62f71 100644 --- a/asv_setup/launch/pc.launch.py +++ b/asv_setup/launch/pc.launch.py @@ -19,8 +19,8 @@ def generate_launch_description(): name='joystick_driver', output='screen', parameters=[ - {'_deadzone': 0.15}, - {'_autorepeat_rate': 100}, + {'deadzone': 0.15}, + {'autorepeat_rate': 100.0}, ], remappings=[ ('/joy', '/joystick/joy'), diff --git a/mission/joystick_interface/CMakeLists.txt b/mission/joystick_interface/CMakeLists.txt index e58d5013..cc2e2cb4 100644 --- a/mission/joystick_interface/CMakeLists.txt +++ b/mission/joystick_interface/CMakeLists.txt @@ -10,16 +10,16 @@ find_package(rclpy REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) -ament_python_install_package(${PROJECT_NAME}) +ament_python_install_package(scripts) -# Install launch files. install(DIRECTORY launch + config DESTINATION share/${PROJECT_NAME} ) install(PROGRAMS - joystick_interface/joystick_interface.py + scripts/joystick_interface.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/mission/joystick_interface/README.md b/mission/joystick_interface/README.md index 61f7b0b4..fc84dc37 100644 --- a/mission/joystick_interface/README.md +++ b/mission/joystick_interface/README.md @@ -1,2 +1,2 @@ -In the launch files we need to make it able to find the yaml files without having an explicit path such as this: "src/vortex-asv/mission/joystick_interface/launch/joystick_interface_launch.yaml" -Perhaps change to python launch +## Joystick interface +A joystick interface for manual control of ASV Freya. A ROS2 node that subscribes on inputs from the XBOX controller and publishes the according wrench message to be used in Thruster Allocation. \ No newline at end of file diff --git a/mission/joystick_interface/package.xml b/mission/joystick_interface/package.xml index 50d62ad2..08e812e9 100644 --- a/mission/joystick_interface/package.xml +++ b/mission/joystick_interface/package.xml @@ -14,6 +14,7 @@ rclpy geometry_msgs sensor_msgs + ros2launch ament_lint_auto ament_lint_common diff --git a/mission/joystick_interface/joystick_interface/__init__.py b/mission/joystick_interface/scripts/__init__.py similarity index 100% rename from mission/joystick_interface/joystick_interface/__init__.py rename to mission/joystick_interface/scripts/__init__.py diff --git a/mission/joystick_interface/joystick_interface/joystick_interface.py b/mission/joystick_interface/scripts/joystick_interface.py similarity index 50% rename from mission/joystick_interface/joystick_interface/joystick_interface.py rename to mission/joystick_interface/scripts/joystick_interface.py index d76f4555..e2123e6f 100755 --- a/mission/joystick_interface/joystick_interface/joystick_interface.py +++ b/mission/joystick_interface/scripts/joystick_interface.py @@ -6,23 +6,23 @@ from std_msgs.msg import Bool -class states: +class States: XBOX_MODE = 1 AUTONOMOUS_MODE = 2 - NO_GO = 3 # Do nothing + NO_GO = 3 class JoystickInterface(Node): def __init__(self): super().__init__('joystick_interface_node') - self.get_logger().info("Joystick interface is up and running") + self.get_logger().info("Joystick interface is up and running. \n When the XBOX controller is connected, press the killswitch button once to enter XBOX mode.") - self.last_button_press_time = 0 - self.debounce_duration = 0.25 - self.state = states.NO_GO + self.last_button_press_time_ = 0 + self.debounce_duration_ = 0.25 + self.state_ = States.NO_GO - self.joystick_buttons_map = [ + self.joystick_buttons_map_ = [ "A", "B", "X", @@ -36,7 +36,7 @@ def __init__(self): "stick_button_right", ] - self.joystick_axes_map = [ + self.joystick_axes_map_ = [ "horizontal_axis_left_stick", #Translation (Left and Right) "vertical_axis_left_stick", #Translation (Forwards and Backwards) "LT", #Negative thrust/torque multiplier @@ -47,9 +47,9 @@ def __init__(self): "dpad_vertical", ] - self.joy_subscriber = self.create_subscription(Joy, "joystick/joy", + self.joy_subscriber_ = self.create_subscription(Joy, "joystick/joy", self.joystick_cb, 1) - self.wrench_publisher = self.create_publisher(Wrench, + self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1) @@ -58,73 +58,120 @@ def __init__(self): self.declare_parameter('yaw_scale_factor', 50.0) #Gets the scaling factors from the yaml file - self.joystick_surge_scaling = self.get_parameter('surge_scale_factor').value - self.joystick_sway_scaling = self.get_parameter('sway_scale_factor').value - self.joystick_yaw_scaling = self.get_parameter('yaw_scale_factor').value + self.joystick_surge_scaling_ = self.get_parameter('surge_scale_factor').value + self.joystick_sway_scaling_ = self.get_parameter('sway_scale_factor').value + self.joystick_yaw_scaling_ = self.get_parameter('yaw_scale_factor').value #Killswitch publisher - self.software_killswitch_signal_publisher = self.create_publisher( + self.software_killswitch_signal_publisher_ = self.create_publisher( Bool, "softWareKillSwitch", 10) - self.software_killswitch_signal_publisher.publish( + self.software_killswitch_signal_publisher_.publish( Bool(data=False)) #Killswitch is not active #Operational mode publisher - self.operational_mode_signal_publisher = self.create_publisher( + self.operational_mode_signal_publisher_ = self.create_publisher( Bool, "softWareOperationMode", 10) + # Signal that we are not in autonomous mode - self.operational_mode_signal_publisher.publish(Bool(data=True)) + self.operational_mode_signal_publisher_.publish(Bool(data=True)) #Controller publisher - self.enable_controller_publisher = self.create_publisher( + self.enable_controller_publisher_ = self.create_publisher( Bool, "controller/lqr/enable", 10) + + def right_trigger_linear_converter(self, rt_input: float) -> float: + """ + Does a linear conversion from the right trigger input range of (1 to -1) to (1 to 2) - #does a linear conversion from trigger inputs (1 to -1) to (1 to 2) - def right_trigger_linear_converter(self, rt_input): + Args: + rt_input: The input value of the right trigger, ranging from -1 to 1. + + Returns: + float: The output value, ranging from 1 to 2. + """ output_value = (rt_input + 1) * (-0.5) + 2 return output_value - #does a linear conversion from trigger input (1 to -1) to (1 to 0.5) - def left_trigger_linear_converter(self, lt_input): + def left_trigger_linear_converter(self, lt_input: float ) -> float: + """ + Does a linear conversion from the left trigger input range of (1 to -1) to (1 to 0.5) + + Args: + lt_input: The input value of the left trigger, ranging from -1 to 1. + + Returns: + float: The output value, ranging from 1 to 2. + """ ouput_value = lt_input * 0.25 + 0.75 return ouput_value - def create_2d_wrench_message(self, x, y, yaw): - wrench_msg = Wrench() - wrench_msg.force.x = x - wrench_msg.force.y = y - wrench_msg.torque.z = yaw - return wrench_msg + def create_2d_wrench_message(self, x: float, y: float, yaw: float) -> Wrench: + """ + Creates a 2D wrench message with the given x, y, and yaw values. + + Args: + x: The x component of the force vector. + y: The y component of the force vector. + yaw: The z component of the torque vector. + + Returns: + Wrench: A 2D wrench message with the given values. + """ + wrench_msg = Wrench() + wrench_msg.force.x = x + wrench_msg.force.y = y + wrench_msg.torque.z = yaw + return wrench_msg - def publish_wrench_message(self, wrench): - self.wrench_publisher.publish(wrench) + def publish_wrench_message(self, wrench: Wrench): + """ + Publishes a Wrench message to the wrench_publisher_ topic. + + Args: + wrench: The Wrench message to be published. + """ + self.wrench_publisher_.publish(wrench) def transition_to_xbox_mode(self): - # We want to turn off controller when moving to xbox mode - self.enable_controller_publisher.publish(Bool(data=False)) - # signal that we enter xbox mode - self.operational_mode_signal_publisher.publish(Bool(data=True)) - self.state = states.XBOX_MODE + """ + Turns off the controller and signals that the operational mode has switched to Xbox mode. + """ + self.enable_controller_publisher_.publish(Bool(data=False)) + self.operational_mode_signal_publisher_.publish(Bool(data=True)) + self.state_ = States.XBOX_MODE def transition_to_autonomous_mode(self): - # We want to publish zero force once when transitioning + """ + Publishes a zero force wrench message and signals that the system is turning on autonomous mode. + """ wrench_msg = self.create_2d_wrench_message(0.0, 0.0, 0.0) self.publish_wrench_message(wrench_msg) - # signal that we are turning on autonomous mode - self.operational_mode_signal_publisher.publish(Bool(data=False)) - self.state = states.AUTONOMOUS_MODE - - def joystick_cb(self, msg): + self.operational_mode_signal_publisher_.publish(Bool(data=False)) + self.state_ = States.AUTONOMOUS_MODE + + def joystick_cb(self, msg : Joy) -> Wrench: + """ + Callback function that receives joy messages and converts them into + wrench messages to be sent to the thruster allocation node. + Handles software killswitch and control mode buttons, + and transitions between different states of operation. + + Args: + msg: A ROS message containing the joy input data. + + Returns: + A ROS message containing the wrench data that was sent to the thruster allocation node. + """ current_time = self.get_clock().now().to_msg()._sec - #Input from controller to joystick_interface buttons = {} axes = {} for i in range(len(msg.buttons)): - buttons[self.joystick_buttons_map[i]] = msg.buttons[i] + buttons[self.joystick_buttons_map_[i]] = msg.buttons[i] for i in range(len(msg.axes)): - axes[self.joystick_axes_map[i]] = msg.axes[i] + axes[self.joystick_axes_map_[i]] = msg.axes[i] xbox_control_mode_button = buttons["A"] software_killswitch_button = buttons["B"] @@ -135,52 +182,52 @@ def joystick_cb(self, msg): left_trigger = self.left_trigger_linear_converter(left_trigger) surge = axes[ - "vertical_axis_left_stick"] * self.joystick_surge_scaling * left_trigger * right_trigger + "vertical_axis_left_stick"] * self.joystick_surge_scaling_ * left_trigger * right_trigger sway = axes[ - "horizontal_axis_left_stick"] * self.joystick_sway_scaling * left_trigger * right_trigger + "horizontal_axis_left_stick"] * self.joystick_sway_scaling_ * left_trigger * right_trigger yaw = axes[ - "horizontal_axis_right_stick"] * self.joystick_yaw_scaling * left_trigger * right_trigger + "horizontal_axis_right_stick"] * self.joystick_yaw_scaling_ * left_trigger * right_trigger # Debounce for the buttons - if current_time - self.last_button_press_time < self.debounce_duration: + if current_time - self.last_button_press_time_ < self.debounce_duration_: software_control_mode_button = False xbox_control_mode_button = False software_killswitch_button = False # If any button is pressed, update the last button press time if software_control_mode_button or xbox_control_mode_button or software_killswitch_button: - self.last_button_press_time = current_time + self.last_button_press_time_ = current_time # Toggle ks on and off - if self.state == states.NO_GO and software_killswitch_button: + if self.state_ == States.NO_GO and software_killswitch_button: # signal that killswitch is not blocking - self.software_killswitch_signal_publisher.publish(Bool(data=True)) + self.software_killswitch_signal_publisher_.publish(Bool(data=True)) self.transition_to_xbox_mode() return if software_killswitch_button: self.get_logger().info("SW killswitch", throttle_duration_sec=1) # signal that killswitch is blocking - self.software_killswitch_signal_publisher.publish(Bool(data=False)) + self.software_killswitch_signal_publisher_.publish(Bool(data=False)) # Turn off controller in sw killswitch - self.enable_controller_publisher.publish(Bool(data=False)) + self.enable_controller_publisher_.publish(Bool(data=False)) # Publish a zero wrench message when sw killing wrench_msg = self.create_2d_wrench_message(0.0, 0.0, 0.0) self.publish_wrench_message(wrench_msg) - self.state = states.NO_GO + self.state_ = States.NO_GO return wrench_msg #Msg published from joystick_interface to thrust allocation wrench_msg = self.create_2d_wrench_message(surge, sway, yaw) - if self.state == states.XBOX_MODE: + if self.state_ == States.XBOX_MODE: self.get_logger().info("XBOX mode", throttle_duration_sec=1) self.publish_wrench_message(wrench_msg) if software_control_mode_button: self.transition_to_autonomous_mode() - if self.state == states.AUTONOMOUS_MODE: + if self.state_ == States.AUTONOMOUS_MODE: self.get_logger().info("autonomous mode", throttle_duration_sec=1) if xbox_control_mode_button: @@ -191,11 +238,8 @@ def joystick_cb(self, msg): def main(): rclpy.init() - joystick_interface = JoystickInterface() - print(joystick_interface.joystick_surge_scaling) rclpy.spin(joystick_interface) - joystick_interface.destroy_node() rclpy.shutdown() diff --git a/mission/joystick_interface/test/test_joystick_interface.py b/mission/joystick_interface/test/test_joystick_interface.py index 92f88915..1b492d6a 100644 --- a/mission/joystick_interface/test/test_joystick_interface.py +++ b/mission/joystick_interface/test/test_joystick_interface.py @@ -1,5 +1,5 @@ from joystick_interface.joystick_interface import JoystickInterface -from joystick_interface.joystick_interface import states +from joystick_interface.joystick_interface import States import rclpy from sensor_msgs.msg import Joy from sensor_msgs.msg import Joy @@ -41,8 +41,8 @@ def test_input_from_controller_into_wrench_msg(self): joy_msg.axes = [-1.0, -1.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0] joy_msg.buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] wrench_msg = JoystickInterface().joystick_cb(joy_msg) - assert wrench_msg.force.x == -100.0 - assert wrench_msg.force.y == -100.0 + assert wrench_msg.force.x == -50.0 + assert wrench_msg.force.y == -50.0 assert wrench_msg.torque.z == 0.0 rclpy.shutdown() @@ -50,7 +50,7 @@ def test_input_from_controller_into_wrench_msg(self): def test_killswitch_button(self): rclpy.init() joystick = JoystickInterface() - joystick.state = states.XBOX_MODE + joystick.state_ = States.XBOX_MODE joy_msg = Joy() joy_msg.axes = [-1.0, -1.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0] joy_msg.buttons = [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0] @@ -64,12 +64,12 @@ def test_killswitch_button(self): def test_moving_in_of_xbox_mode(self): rclpy.init() joystick = JoystickInterface() - joystick.state = states.XBOX_MODE + joystick.state_ = States.XBOX_MODE joy_msg = Joy() joy_msg.axes = [-1.0, -1.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0] joy_msg.buttons = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] wrench_msg = joystick.joystick_cb(joy_msg) - assert wrench_msg.force.x == -100.0 - assert wrench_msg.force.y == -100.0 + assert wrench_msg.force.x == -50.0 + assert wrench_msg.force.y == -50.0 assert wrench_msg.torque.z == 0.0 rclpy.shutdown() From c7892aa709854c17d418f5e631cc862af5f36abb Mon Sep 17 00:00:00 2001 From: Martynas Date: Sun, 12 Nov 2023 14:00:44 +0100 Subject: [PATCH 15/16] Deleted the github key for security reasons O_O --- key | 7 ------- key.pub | 1 - 2 files changed, 8 deletions(-) delete mode 100644 key delete mode 100644 key.pub diff --git a/key b/key deleted file mode 100644 index 6b6a057f..00000000 --- a/key +++ /dev/null @@ -1,7 +0,0 @@ ------BEGIN OPENSSH PRIVATE KEY----- -b3BlbnNzaC1rZXktdjEAAAAABG5vbmUAAAAEbm9uZQAAAAAAAAABAAAAMwAAAAtzc2gtZW -QyNTUxOQAAACCFDsFdDE0QTwWks7dIgYJ43Q0Qs2Wi8IntJ8r02AKEBAAAAKAzONmnMzjZ -pwAAAAtzc2gtZWQyNTUxOQAAACCFDsFdDE0QTwWks7dIgYJ43Q0Qs2Wi8IntJ8r02AKEBA -AAAEBlXUTHFpp+Vhorjlf4Bc/LTZjfg67TqT5JYA38Jp9WE4UOwV0MTRBPBaSzt0iBgnjd -DRCzZaLwie0nyvTYAoQEAAAAFnJvc2Uuai5rYXBwc0BnbWFpbC5jb20BAgMEBQYH ------END OPENSSH PRIVATE KEY----- diff --git a/key.pub b/key.pub deleted file mode 100644 index cc873bd3..00000000 --- a/key.pub +++ /dev/null @@ -1 +0,0 @@ -ssh-ed25519 AAAAC3NzaC1lZDI1NTE5AAAAIIUOwV0MTRBPBaSzt0iBgnjdDRCzZaLwie0nyvTYAoQE rose.j.kapps@gmail.com From 2dfa73403e0cba9e1f79f8f2bb66f3c9062d5ce2 Mon Sep 17 00:00:00 2001 From: Aleksander Klund <99870311+alekskl01@users.noreply.github.com> Date: Sun, 12 Nov 2023 14:07:22 +0100 Subject: [PATCH 16/16] 98 task thruster interface (#111) * moved thruster interface to ROS2, needs cleanup and proper tests * Committing clang-format changes * added config directory to install in CMake * Committing clang-format changes * changed subscriber and publisher name * Committing clang-format changes * Limited pwm more to 1400-1600 * Fixed thrusters jesjes :) * Added launch file * Added node startup log message * Committing clang-format changes * removed non-functioning test file. * small fix --------- Co-authored-by: Clang Robot Co-authored-by: PizzaAllTheWay Co-authored-by: RoseKapps Co-authored-by: Martin Huynh --- motion/thruster_interface/CMakeLists.txt | 65 ++++++ .../config/ThrustMe_P1000_force_mapping.csv | 190 ++++++++++++++++++ .../thruster_interface/thruster_interface.hpp | 31 +++ .../thruster_interface_node.hpp | 31 +++ .../launch/thruster_interface.launch.py | 16 ++ motion/thruster_interface/package.xml | 24 +++ .../src/thruster_interface.cpp | 114 +++++++++++ .../src/thruster_interface_node.cpp | 55 +++++ 8 files changed, 526 insertions(+) create mode 100644 motion/thruster_interface/CMakeLists.txt create mode 100644 motion/thruster_interface/config/ThrustMe_P1000_force_mapping.csv create mode 100644 motion/thruster_interface/include/thruster_interface/thruster_interface.hpp create mode 100644 motion/thruster_interface/include/thruster_interface/thruster_interface_node.hpp create mode 100644 motion/thruster_interface/launch/thruster_interface.launch.py create mode 100644 motion/thruster_interface/package.xml create mode 100644 motion/thruster_interface/src/thruster_interface.cpp create mode 100644 motion/thruster_interface/src/thruster_interface_node.cpp diff --git a/motion/thruster_interface/CMakeLists.txt b/motion/thruster_interface/CMakeLists.txt new file mode 100644 index 00000000..c89c2538 --- /dev/null +++ b/motion/thruster_interface/CMakeLists.txt @@ -0,0 +1,65 @@ +cmake_minimum_required(VERSION 3.8) +project(thruster_interface) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +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) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(vortex_msgs REQUIRED) + +include_directories(include) + + +add_executable(${PROJECT_NAME}_node + src/thruster_interface.cpp + src/thruster_interface_node.cpp +) + +ament_target_dependencies(${PROJECT_NAME}_node + rclcpp + std_msgs + vortex_msgs +) + +install( + DIRECTORY include/ + DESTINATION include +) + +# Install launch files. +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +install(TARGETS + ${PROJECT_NAME}_node + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(${PROJECT_NAME}_test + src/thruster_interface.cpp + ) + target_include_directories(${PROJECT_NAME}_test PUBLIC + $ + $ + ) + ament_target_dependencies(${PROJECT_NAME}_test + std_msgs + ) +endif() + +ament_package() diff --git a/motion/thruster_interface/config/ThrustMe_P1000_force_mapping.csv b/motion/thruster_interface/config/ThrustMe_P1000_force_mapping.csv new file mode 100644 index 00000000..bac8cf21 --- /dev/null +++ b/motion/thruster_interface/config/ThrustMe_P1000_force_mapping.csv @@ -0,0 +1,190 @@ +"Force(g)","PWM(micros)" +-12989.13 1100 +-12792.53 1104.49 +-12779.61 1108.99 +-12635.04 1113.48 +-12547.48 1117.98 +-12554.38 1122.47 +-12528.53 1126.97 +-12484.77 1131.46 +-12327.03 1135.96 +-12337.55 1140.45 +-12202.25 1144.94 +-12147.19 1149.44 +-12097.76 1153.93 +-11947.61 1158.43 +-11948.59 1162.92 +-11878.31 1167.42 +-11685.45 1171.91 +-11630.87 1176.4 +-11660.74 1180.9 +-11514.6 1185.39 +-11370.6 1189.89 +-11293.55 1194.38 +-11221.2 1198.88 +-11213.27 1203.37 +-11064.32 1207.87 +-11049.23 1212.36 +-10945.55 1216.85 +-10837.99 1221.35 +-10678.96 1225.84 +-10754.31 1230.34 +-10588.09 1234.83 +-10503.28 1239.33 +-10344.9 1243.82 +-10226.64 1248.31 +-10190.24 1252.81 +-10081.85 1257.3 +-9959.01 1261.8 +-9948.11 1266.29 +-9759.36 1270.79 +-9753.3 1275.28 +-9619.75 1279.78 +-9525.81 1284.27 +-9362.72 1288.76 +-9333.51 1293.26 +-9187.35 1297.75 +-9078.53 1302.25 +-8851.82 1306.74 +-8732.14 1311.24 +-8632.45 1315.73 +-8490.62 1320.22 +-8411.14 1324.72 +-8312.52 1329.21 +-8047.98 1333.71 +-7948.19 1338.2 +-7934.35 1342.7 +-7780.13 1347.19 +-7709.89 1351.69 +-7375.4 1356.18 +-7364.05 1360.67 +-7233.1 1365.17 +-6972.58 1369.66 +-6845.09 1374.16 +-6759.8 1378.65 +-6633.6 1383.15 +-6337.56 1387.64 +-6224.88 1392.13 +-6132.13 1396.63 +-5939.88 1401.12 +-5682.91 1405.62 +-5527.56 1410.11 +-5258.96 1414.61 +-5074.12 1419.1 +-4823.63 1423.6 +-4685.91 1428.09 +-4500.56 1432.58 +-4308.91 1437.08 +-4012.16 1441.57 +-3866.78 1446.07 +-3608.86 1450.56 +-3235.68 1455.06 +-2964.98 1459.55 +-2782.91 1464.04 +-2522.36 1468.54 +-2112.45 1473.03 +-1955.09 1477.53 +-1551.28 1482.02 +-1327.8 1486.52 +-913.3 1491.01 +-506.29 1495.51 +0 1500 +614.46 1504.04 +967.25 1508.08 +1501.92 1512.12 +1965.98 1516.16 +2315.18 1520.2 +2789.34 1524.24 +3218.91 1528.28 +3442.74 1532.32 +3763.95 1536.36 +4118.76 1540.4 +4473.32 1544.44 +4733.06 1548.48 +5052.11 1552.53 +5306.67 1556.57 +5514.31 1560.61 +5789.63 1564.65 +6061.47 1568.69 +6337.72 1572.73 +6516.65 1576.77 +6718.8 1580.81 +6901.18 1584.85 +7112.16 1588.89 +7352 1592.93 +7634.54 1596.97 +7720.88 1601.01 +7973.84 1605.05 +8136.83 1609.09 +8386.24 1613.13 +8569.13 1617.17 +8745.66 1621.21 +8921.09 1625.25 +9081.5 1629.29 +9306.64 1633.33 +9397.61 1637.37 +9521.35 1641.41 +9746.14 1645.45 +9890.84 1649.49 +9952.96 1653.54 +10218.41 1657.58 +10275.6 1661.62 +10446.95 1665.66 +10584.48 1669.7 +10754.31 1673.74 +10864.61 1677.78 +10959.78 1681.82 +11109.23 1685.86 +11210.52 1689.9 +11367.21 1693.94 +11528.98 1697.98 +11604.96 1702.02 +11700.44 1706.06 +11756.9 1710.1 +11942.16 1714.14 +12045.06 1718.18 +12179.5 1722.22 +12383.12 1726.26 +12317.95 1730.3 +12531.05 1734.34 +12581.61 1738.38 +12716.21 1742.42 +12825.03 1746.46 +12832.83 1750.51 +13053.31 1754.55 +13181.88 1758.59 +13207.91 1762.63 +13247.19 1766.67 +13414.53 1770.71 +13466.28 1774.75 +13573.65 1778.79 +13696.17 1782.83 +13803.27 1786.87 +13847.3 1790.91 +14029.66 1794.95 +14067.08 1798.99 +14156.74 1803.03 +14283.08 1807.07 +14300.44 1811.11 +14443.34 1815.15 +14541.8 1819.19 +14560.88 1823.23 +14584.52 1827.27 +14665.63 1831.31 +14718.97 1835.35 +14901.01 1839.39 +14973.02 1843.43 +15054.56 1847.47 +15149.7 1851.52 +15239.98 1855.56 +15272.75 1859.6 +15281.37 1863.64 +15384.03 1867.68 +15419.57 1871.72 +15593.75 1875.76 +15602.55 1879.8 +15724.38 1883.84 +15696.74 1887.88 +15764.41 1891.92 +15910.14 1895.96 +15958.35 1900 \ No newline at end of file diff --git a/motion/thruster_interface/include/thruster_interface/thruster_interface.hpp b/motion/thruster_interface/include/thruster_interface/thruster_interface.hpp new file mode 100644 index 00000000..8367eb68 --- /dev/null +++ b/motion/thruster_interface/include/thruster_interface/thruster_interface.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class ThrusterInterface { +private: + std::map pwm_table; + const int I2C_BUS = 1; + const int I2C_ADDRESS = 0x21; + const char *I2C_DEVICE = "/dev/i2c-1"; + + std::vector pwm_to_bytes(const std::vector &pwm_values); + +public: + ThrusterInterface(std::string mapping_file); + void publish_thrust_to_escs(std::vector forces); + float interpolate(float force); +}; diff --git a/motion/thruster_interface/include/thruster_interface/thruster_interface_node.hpp b/motion/thruster_interface/include/thruster_interface/thruster_interface_node.hpp new file mode 100644 index 00000000..6e4f0d65 --- /dev/null +++ b/motion/thruster_interface/include/thruster_interface/thruster_interface_node.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include +#include +#include +#include + +#include +using std::placeholders::_1; + +class ThrusterInterfaceROS : public rclcpp::Node { +private: + std::string mapping_file = + ament_index_cpp::get_package_share_directory("thruster_interface") + + "/config/ThrustMe_P1000_force_mapping.csv"; + ThrusterInterface thrusterInterface{mapping_file}; + + rclcpp::Subscription::SharedPtr + thruster_forces_sub_; + rclcpp::Publisher::SharedPtr pwm_pub_; + +public: + ThrusterInterfaceROS() : Node("thruster_interface") { + pwm_pub_ = this->create_publisher("pwm", 10); + thruster_forces_sub_ = + this->create_subscription( + "thrust/thruster_forces", 10, + std::bind(&ThrusterInterfaceROS::thrust_callback, this, _1)); + } + void thrust_callback(const vortex_msgs::msg::ThrusterForces::SharedPtr msg); +}; diff --git a/motion/thruster_interface/launch/thruster_interface.launch.py b/motion/thruster_interface/launch/thruster_interface.launch.py new file mode 100644 index 00000000..c8e88718 --- /dev/null +++ b/motion/thruster_interface/launch/thruster_interface.launch.py @@ -0,0 +1,16 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + thruster_interface_node = Node( + package='thruster_interface', + executable='thruster_interface_node', + name='thruster_interface_node', + parameters=[], + output='screen', + ) + return LaunchDescription([ + thruster_interface_node + ]) \ No newline at end of file diff --git a/motion/thruster_interface/package.xml b/motion/thruster_interface/package.xml new file mode 100644 index 00000000..af20df9f --- /dev/null +++ b/motion/thruster_interface/package.xml @@ -0,0 +1,24 @@ + + + + thruster_interface + 0.0.0 + Interface for transforming thrust to thruster rpm + alekskl01 + MIT + + ament_cmake + + rclcpp + std_msgs + vortex_msgs + ament_index_cpp + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/motion/thruster_interface/src/thruster_interface.cpp b/motion/thruster_interface/src/thruster_interface.cpp new file mode 100644 index 00000000..3e1741d0 --- /dev/null +++ b/motion/thruster_interface/src/thruster_interface.cpp @@ -0,0 +1,114 @@ +#include + +ThrusterInterface::ThrusterInterface(std::string mapping_file) { + // Open the data file + std::ifstream data(mapping_file); + + if (!data.is_open()) { + std::cerr << "Unable to open file\n"; + exit(1); + } + + std::string line; + // Ignore the header line + std::getline(data, line); + + while (std::getline(data, line)) { + std::istringstream stream(line); + + std::string force_str, pwm_str; + + std::getline(stream, force_str, '\t'); + std::getline(stream, pwm_str); + + double force = std::stod(force_str); + double pwm = std::stod(pwm_str); + + pwm_table[force] = pwm; + } +} + +float ThrusterInterface::interpolate(float force) { + auto upper_bound = pwm_table.upper_bound(force); + if (upper_bound == pwm_table.end()) { + --upper_bound; + } + auto lower_bound = std::prev(upper_bound); + + if (lower_bound == pwm_table.end()) { + return upper_bound->second; + } + + float force1 = lower_bound->first; + float force2 = upper_bound->first; + + float pwm1 = lower_bound->second; + float pwm2 = upper_bound->second; + + if (force1 == force2) { + return pwm1; + } + + int pwm_signal = std::round( + pwm1 + ((force - force1) * (pwm2 - pwm1)) / (force2 - force1) + 0.5); + + int clipped_pwm_signal = + std::min(std::max(pwm_signal, 1400), 1600); // min 1100, max 1900 + + return clipped_pwm_signal; +} + +std::vector +ThrusterInterface::pwm_to_bytes(const std::vector &pwm_values) { + + std::vector bytes; + for (const auto &val : pwm_values) { + // Ensure the value is in the correct range and cast to an integer + int pwm_int = static_cast( + std::min(std::max(val, 1400), 1600)); // min 1100, max 1900 + + // Split the integer into most significant byte (MSB) and least significant + // byte (LSB) + uint8_t msb = (pwm_int >> 8) & 0xFF; + uint8_t lsb = pwm_int & 0xFF; + + // Add MSB and LSB to the bytes vector + bytes.push_back(msb); + bytes.push_back(lsb); + } + return bytes; +} + +void ThrusterInterface::publish_thrust_to_escs(std::vector forces) { + std::vector pwm_values; + for (auto force : forces) { + pwm_values.push_back(interpolate(force)); + } + + std::vector pwm_bytes = pwm_to_bytes(pwm_values); + int data_size = pwm_bytes.size(); + + // DEBUG + // for (auto pwm : pwm_values) { + // std::cout << pwm << " "; + //} + // std::cout << std::endl; + + int file = open(I2C_DEVICE, O_RDWR); + if (file < 0) { + std::cerr << "Error opening device\n"; + exit(1); + } + + if (ioctl(file, I2C_SLAVE, I2C_ADDRESS) < 0) { + std::cerr << "Error setting I2C address\n"; + exit(1); + } + + // Send the I2C message + if (write(file, pwm_bytes.data(), data_size) != data_size) { + std::cerr << "Error sending data, ignoring message...\n"; + } + + close(file); +} diff --git a/motion/thruster_interface/src/thruster_interface_node.cpp b/motion/thruster_interface/src/thruster_interface_node.cpp new file mode 100644 index 00000000..ffcceea2 --- /dev/null +++ b/motion/thruster_interface/src/thruster_interface_node.cpp @@ -0,0 +1,55 @@ +#include + +void ThrusterInterfaceROS::thrust_callback( + const vortex_msgs::msg::ThrusterForces::SharedPtr msg) { + // Convert from Newton to grams + double newton_to_gram_conversion_factor = 101.97162; + + std::vector forces_in_grams = { + msg->thrust[0] * newton_to_gram_conversion_factor, + msg->thrust[1] * newton_to_gram_conversion_factor, + msg->thrust[2] * newton_to_gram_conversion_factor, + msg->thrust[3] * newton_to_gram_conversion_factor}; + + std::vector pwm_values; + for (auto force : forces_in_grams) { + pwm_values.push_back(thrusterInterface.interpolate(force)); + } + + vortex_msgs::msg::Pwm pwm_msg; + // TODO: Get mapping and offsets from rosparam + // Give thrust to thruster 0: publish on pin = thruster_to_pin_map[0] + std::vector thruster_to_pin_map = {1, 3, 2, 0}; + std::vector thruster_direction_map = {1, 1, 1, -1}; + std::vector pwm_offsets = {100, 100, 100, 100}; + + // Iterates through thruster 0 to 3, where 0 is front right, iterated + // clockwise + for (int i = 0; i < 4; i++) { + + int center_pwm_value = 1500; + int offset_from_center_value = + pwm_values[i] - center_pwm_value + pwm_offsets[i]; + int pwm_value_correct_direction = + center_pwm_value + thruster_direction_map[i] * offset_from_center_value; + + int pwm_clamped = std::min(std::max(pwm_value_correct_direction, 1400), + 1600); // min 1100, max 1900 + pwm_msg.positive_width_us.push_back(pwm_clamped); + pwm_msg.pins.push_back(thruster_to_pin_map[i]); + } + + pwm_pub_->publish(pwm_msg); + + thrusterInterface.publish_thrust_to_escs(forces_in_grams); +} + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + auto thruster_interface_node = std::make_shared(); + RCLCPP_INFO(thruster_interface_node->get_logger(), + "Starting thruster_interface_node"); + rclcpp::spin(thruster_interface_node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file