diff --git a/asv_setup/CMakeLists.txt b/asv_setup/CMakeLists.txt index b12ae23a..d57de63a 100755 --- a/asv_setup/CMakeLists.txt +++ b/asv_setup/CMakeLists.txt @@ -5,28 +5,20 @@ 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) - -# Install launch files. -install(DIRECTORY - launch - 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() +# Install launch files. +install(DIRECTORY + config + launch + DESTINATION share/${PROJECT_NAME}/ +) + ament_package() 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/asv_setup/launch/pc.launch.py b/asv_setup/launch/pc.launch.py new file mode 100644 index 00000000..98c62f71 --- /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.0}, + ], + 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 index efa79769..e69de29b 100755 --- a/asv_setup/launch/pc.launch.yaml +++ b/asv_setup/launch/pc.launch.yaml @@ -1,23 +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/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/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 new file mode 100644 index 00000000..e128a02f --- /dev/null +++ b/mission/internal_status/internal_status/power_sense_module_lib.py @@ -0,0 +1,52 @@ +import smbus +from MCP342x import MCP342x + +class PowerSenseModule: + + 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=1, + resolution=18) # voltage + self.channel_current = MCP342x(self.bus, + self.i2c_adress, + channel=0, + resolution=18) # current + + + # 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() 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..87ec8980 --- /dev/null +++ b/mission/internal_status/internal_status/power_sense_module_publisher.py @@ -0,0 +1,48 @@ +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) + + + 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) + + +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/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 new file mode 100644 index 00000000..ae43885f --- /dev/null +++ b/mission/internal_status/package.xml @@ -0,0 +1,26 @@ + + + + internal_status + 0.0.0 + Publisher of the PSM voltage and current data + vortex + Apache License 2.0 + + + rclpy + std_msgs + power_sense_module_lib + ros2launch + + + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + 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..8f4ee25f --- /dev/null +++ b/mission/internal_status/setup.py @@ -0,0 +1,29 @@ +import os +from glob import glob +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']), + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))) + ], + 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/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' diff --git a/mission/joystick_interface/CMakeLists.txt b/mission/joystick_interface/CMakeLists.txt index 0f5262d4..cc2e2cb4 100755 --- a/mission/joystick_interface/CMakeLists.txt +++ b/mission/joystick_interface/CMakeLists.txt @@ -10,17 +10,23 @@ 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(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) install(PROGRAMS - joystick_interface/joystick_interface.py + scripts/joystick_interface.py DESTINATION lib/${PROJECT_NAME} ) 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/README.md b/mission/joystick_interface/README.md index 61f7b0b4..fc84dc37 100755 --- 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/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 deleted file mode 100755 index 5640b02e..00000000 --- a/mission/joystick_interface/joystick_interface/joystick_interface.py +++ /dev/null @@ -1,204 +0,0 @@ -#!/usr/bin/env python3 -import rclpy -from rclpy.node import Node -from geometry_msgs.msg import Wrench -from sensor_msgs.msg import Joy -from std_msgs.msg import Bool - - -class states: - XBOX_MODE = 1 - AUTONOMOUS_MODE = 2 - NO_GO = 3 # Do nothing - - -class JoystickInterface(Node): - - def __init__(self): - super().__init__('joystick_interface_node') - self.get_logger().info("Joystick interface is up and running") - - self.last_button_press_time = 0 - self.debounce_duration = 0.25 - self.state = states.NO_GO - - self.joystick_buttons_map = [ - "A", - "B", - "X", - "Y", - "LB", - "RB", - "back", - "start", - "power", - "stick_button_left", - "stick_button_right", - ] - - 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 - "horizontal_axis_right_stick", #Rotation - "vertical_axis_right_stick", - "RT", #Positive thrust/torque multiplier - "dpad_horizontal", - "dpad_vertical", - ] - - self.joy_subscriber = self.create_subscription(Joy, "joystick/joy", - self.joystick_cb, 1) - self.wrench_publisher = self.create_publisher(Wrench, - "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) - - #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 - - #Killswitch publisher - self.software_killswitch_signal_publisher = self.create_publisher( - Bool, "softWareKillSwitch", 10) - self.software_killswitch_signal_publisher.publish( - Bool(data=False)) #Killswitch is not active - - #Operational mode 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)) - - #Controller publisher - self.enable_controller_publisher = self.create_publisher( - Bool, "controller/lqr/enable", 10) - - #does a linear conversion from trigger inputs (1 to -1) to (1 to 2) - def right_trigger_linear_converter(self, rt_input): - 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): - 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 publish_wrench_message(self, wrench): - 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 - - def transition_to_autonomous_mode(self): - # We want to publish zero force once when transitioning - 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): - 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] - - for i in range(len(msg.axes)): - axes[self.joystick_axes_map[i]] = msg.axes[i] - - xbox_control_mode_button = buttons["A"] - software_killswitch_button = buttons["B"] - software_control_mode_button = buttons["X"] - left_trigger = axes['LT'] - right_trigger = axes['RT'] - right_trigger = self.right_trigger_linear_converter(right_trigger) - left_trigger = self.left_trigger_linear_converter(left_trigger) - - surge = axes[ - "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 - yaw = axes[ - "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: - 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 - - # Toggle ks on and off - 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.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)) - # Turn off controller in sw killswitch - 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 - 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: - 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: - self.get_logger().info("autonomous mode", throttle_duration_sec=1) - - if xbox_control_mode_button: - self.transition_to_xbox_mode() - - return wrench_msg - - -def main(): - rclpy.init() - - joystick_interface = JoystickInterface() - print(joystick_interface.joystick_surge_scaling) - rclpy.spin(joystick_interface) - - joystick_interface.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() 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 index 61aac95f..e69de29b 100755 --- a/mission/joystick_interface/launch/joystick_interface.launch.yaml +++ b/mission/joystick_interface/launch/joystick_interface.launch.yaml @@ -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/package.xml b/mission/joystick_interface/package.xml index 50d62ad2..08e812e9 100755 --- 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/scripts/__init__.py b/mission/joystick_interface/scripts/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/mission/joystick_interface/scripts/joystick_interface.py b/mission/joystick_interface/scripts/joystick_interface.py new file mode 100755 index 00000000..e2123e6f --- /dev/null +++ b/mission/joystick_interface/scripts/joystick_interface.py @@ -0,0 +1,248 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Wrench +from sensor_msgs.msg import Joy +from std_msgs.msg import Bool + + +class States: + XBOX_MODE = 1 + AUTONOMOUS_MODE = 2 + NO_GO = 3 + + +class JoystickInterface(Node): + + def __init__(self): + super().__init__('joystick_interface_node') + 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.joystick_buttons_map_ = [ + "A", + "B", + "X", + "Y", + "LB", + "RB", + "back", + "start", + "power", + "stick_button_left", + "stick_button_right", + ] + + 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 + "horizontal_axis_right_stick", #Rotation + "vertical_axis_right_stick", + "RT", #Positive thrust/torque multiplier + "dpad_horizontal", + "dpad_vertical", + ] + + self.joy_subscriber_ = self.create_subscription(Joy, "joystick/joy", + self.joystick_cb, 1) + self.wrench_publisher_ = self.create_publisher(Wrench, + "thrust/wrench_input", + 1) + + 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 + 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( + Bool, "softWareKillSwitch", 10) + self.software_killswitch_signal_publisher_.publish( + Bool(data=False)) #Killswitch is not active + + #Operational mode 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)) + + #Controller 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) + + 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 + + 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: 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: 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): + """ + 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): + """ + 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) + 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 + + buttons = {} + axes = {} + + for i in range(len(msg.buttons)): + 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] + + xbox_control_mode_button = buttons["A"] + software_killswitch_button = buttons["B"] + software_control_mode_button = buttons["X"] + left_trigger = axes['LT'] + right_trigger = axes['RT'] + right_trigger = self.right_trigger_linear_converter(right_trigger) + left_trigger = self.left_trigger_linear_converter(left_trigger) + + surge = axes[ + "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 + yaw = axes[ + "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_: + 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 + + # Toggle ks on and off + 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.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)) + # Turn off controller in sw killswitch + 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 + 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: + 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: + self.get_logger().info("autonomous mode", throttle_duration_sec=1) + + if xbox_control_mode_button: + self.transition_to_xbox_mode() + + return wrench_msg + + +def main(): + rclpy.init() + joystick_interface = JoystickInterface() + rclpy.spin(joystick_interface) + joystick_interface.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/mission/joystick_interface/test/__init__.py b/mission/joystick_interface/test/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/mission/joystick_interface/joystick_interface/test/test_joystick_interface.py b/mission/joystick_interface/test/test_joystick_interface.py similarity index 89% rename from mission/joystick_interface/joystick_interface/test/test_joystick_interface.py rename to mission/joystick_interface/test/test_joystick_interface.py index 92f88915..1b492d6a 100755 --- a/mission/joystick_interface/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() 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; +} 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