Merge pull request #403 from vortexntnu/feature/joystick_interface_in…

…_ros2

Feature/joystick interface in ros2

.gitignore

@@ -23,6 +23,7 @@ msg/*Result.msg
 msg/_*.py
 build_isolated/
 devel_isolated/
+mission/joystick_interface_auv/test/log/
 
 # Generated by dynamic reconfigure
 *.cfgc

auv_setup/CMakeLists.txt

@@ -1,2 +1,24 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.8)
 project(auv_setup)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+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}/
+)
+
+ament_package()

auv_setup/config/robots/new_auv.yaml

@@ -0,0 +1,15 @@
+# This file defines parameters specific to Beluga
+#
+# When looking at the AUV from above, the thruster placement is:   
+#
+#        front
+#       |======|
+#  |=7↗=|      |=0↖=|
+#  |    |      |    |
+#  | 6• |      | 1• |
+#  |    |      |    |
+#  |    |      |    |
+#  | 5• |      | 2• |
+#  |    |      |    |
+#  |=4↖=|==||==|=3↗=|
+#

auv_setup/launch/topside.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_auv'),
+                         'launch/joystick_interface_auv.launch.py')))
+
+    # Return launch description
+    return LaunchDescription(
+        [set_env_var, joy_node, joystick_interface_launch])

auv_setup/package.xml

@@ -1,11 +1,18 @@
 <?xml version="1.0"?>
-<package format="2">
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
   <name>auv_setup</name>
   <version>0.0.0</version>
   <description>The auv_setup package</description>
-  <maintainer email="[email protected]">christopher</maintainer>
+  <maintainer email="[email protected]">alekskl01</maintainer>
   <license>MIT</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
 
- </package>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

mission/joystick_interface_auv/CMakeLists.txt

@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 3.8)
+project(joystick_interface_auv)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_python REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+ament_python_install_package(joystick_interface_auv)
+
+install(DIRECTORY
+  launch
+  config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(PROGRAMS
+  joystick_interface_auv/joystick_interface_auv.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_pytest REQUIRED)
+  set(_pytest_tests
+    test/test_joystick_interface_auv.py
+  )
+  foreach(_test_path ${_pytest_tests})
+    get_filename_component(_test_name ${_test_path} NAME_WE)
+    ament_add_pytest_test(${_test_name} ${_test_path}
+      APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}
+      WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+    )
+  endforeach()
+endif()
+
+ament_package()

mission/joystick_interface_auv/README.md

@@ -0,0 +1,3 @@
+## Joystick interface
+A joystick interface for manual control of AUV. A ROS2 node that subscribes on inputs from the XBOX controller and publishes the according wrench message to be used in Thruster Allocation.
+

mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml

@@ -0,0 +1,8 @@
+joystick_interface_auv:
+  ros__parameters:
+    surge_scale_factor: 60.0
+    sway_scale_factor: 60.0
+    yaw_scale_factor: 60.0
+    heave_scale_factor: 17.5
+    roll_scale_factor: 30.0
+    pitch_scale_factor: 20.0