feat: implementation of a pid dp controller (#499)

* feat: adding in the setup for dp controller, dosent build, need to fix

* feat: adding in pid parts

* Develop (#481)

* chore(pylint): add .pylintrc configuration file

* refactor: fix pylint warning W1510

* refactor: fix pylint warning C0116

* refactor: fix pylint warning C0411

* refactor: fix pylint warning R1731

* refactor: fix pylint warning R1705

* refactor: fix pylint warning W1514

* refactor: fix pylint warning C0200

* refactor: fix pylint warning W0611

* refactor: fix pylint warning W0702

* refactor: fix pylint warning C0121

* refactor: fix pylint warning W0107

* refactor: fix pylint warning R1714

* refactor: fix pylint warning C0201

* refactor: fix pylint warning C0303 and W0613

* refactor: fix pylint warning W0621

* refactor: fix pylint warning R0402

* refactor: fix pylint warning W0212

* chore(pylint): update rule adjustments and formatting

* style: format code with black

* style: format imports with isort

* ci: add isort and pylint to Python pipeline

* Committing black-format changes

* chore: add pyproject.toml for project configuration

* ci: update order of pipeline jobs

* ci: update pylint job and pylint rules

* ci: update which python versions pylint test

* ci: update python pipeline to only run on pull_request

* refactor: fix pylint warning C0103

* chore: update linting and project configuration rules in .pylintrc and pyproject.toml

* Rename files to snake_case

* ci: add pipeline for grammar in comments

* refactor: fix spelling mistakes in comments

* ci: update job codespell_fix to continue on error

* refactor: fix spelling mistakes in comments

* ci: update pipeline codespell to only have one job

* refactor: fix variable naming issues and adjust imports as per review

* Committing codespell fixes

* refactor: spelling correction

* ci: update CI file and job names for consistency

* ci: update CI file for more clarity

* chore: add codespell configuration and ignore list

* chore: add type hints and return types

* ci: add mypy.ini for type checking configuration

* ci: add mypy type checking workflow

* refactor: format code with black

* refactor: format all yaml files using prettier

* ci: add pipeline for yaml formatting using prettier

* ci: update yaml formatting pipeline to run on pull request

* refactor: remove deprecated typing

* refactor: fix import sorting

* ci: remove faulty ci pipelines and combine python pipelines into single file

* fix(security): replace subprocess call with safer alternative to remove shell=true

* feat: add pre-commit hooks for black, isort, and codespell

* ci: update python pipeline to use latest versions

* refactor: apply code formatting fixes via pre-commit hooks

* ci: split python ci/cd pipelines into multiple files

* ci: add ci pipeline for checking style clang-format

* refactor: remove unused config file for clang-format

* refactor: remove duplicate file

* ci: add ci pipeline that tests that codebase can build

* refactor: update ci-build pipeline to only run on pull request

* refactor: update clang-format config

* refactor: apply clang-format

* feat: update pipeline to build and run tests

* refactor: disable linting checks when running colcon test

* refactor: update .pre-commit-config file (#480)

---------

Co-authored-by: Black Robot <black-robot@example.com>
Co-authored-by: Codespell Robot <codespell-robot@example.com>
Co-authored-by: Sondre Haugen <sondre95556888@gmail.com>

* self.reference_mode erstattes med States.REFERENCE_MODE

* Some formatting

* feat: add pid gains

* refactor: move controller to control

* refactor: change function name from J to create_J to avoid conflict with variable J

* Vinklene restarter seg ikke til null når du slipper opp kontrolleren

* feat: added a new main.cpp file with ros2 node class

* feat: add pid controller for dp

* feat: add possiblity to tune the PID during runtime by publishing Float64MultiArray

* feat: add config file for parameters

* remove unnecessary typedef

* refactor: move eta, eta_d and nu over to ros node

* feat: add launch file

* feat: add eta_dot_d as reference state

* build fix

* feat: add reference filter

* added zeta and omega as ros params

* feat: initialized action server

* feat: action server for reference filter

* feat: add odom subscriber for reference filter

* Add parameter file

* Add ssa

* Add better formating and cleaner code

* Add documentation

* feat: adding joystick input

* Add documentation and remove spamming in terminal

* fix: some issues with pid_controller_ros.hpp file

* feat: add multithreaded executor

* feat: make new goals overwrite the old goal

* feat: add rotation of velocity from body to NED

* feat: tuning reference filter

* fix: fix conversions between quat and euler using tf2

* refactor: use tf2 for conversion between euler and quat

* feat: init tuning process

* feat: added anti windup

* fix: variable issue in controller input

* refactor: Update include order in pid_controller_utils.hpp

Reorder the include statements in pid_controller_utils.hpp to follow the established convention of including standard library headers before external library headers. This improves code readability and maintainability.

Refactor the include order in pid_controller_utils.hpp to adhere to the established convention.

* feat: added in quaternions instead of euler

* feat: added quaternion normalization to fix error

* fix: changed pid params

* refactor(dp_controller): removed previous version

* docs: added documentation

* docs: added documentation

* hey

* refactor: applied formatting changes

* refactor: did some changes asked by anders

* fix(pid_controller_utils): forgot ; in one line

* feat: add methods for filling vectors

* refactor: remove redundant code

* feat: add tf2_geometry_msgs dependency

* fix: fix methods

* refactor: make precommit hook happy

* make pre-commit hook happy

* fix: fix parameter names

* feat: added struct elements for the quaternions and position

* Merge branch 'feat/dp_controller_euler' into 434-task-dp-controller

* feat: add correct qos profile for odom sug

* fix: structure fix

* fix: make pre-commit guy habby

* fix:something wrong

* fix:something wrong

* fix: fixing the control input computation

* [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci

* fix: variabel error in guidance callback

* fix: some merge errors

* [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci

* feat: added topics into config file for the euler pid controller

* [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci

* fix: Adeed kill switch and remove the msgs error

* fix: change in reference filter parameter

* [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci

* feat: Added adaptive backstepping controller

* fix: something happend

* feat: adding in the adaptive backstepping in cpp

* [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci

* fix: fixed config file and added the rest of the adaptive controller

* [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci

* fix: some errors with the commits, removed some of the code

* refactor: improve consistency

* refactor: remove callbacks for tuning params

* feat: adaptive backstepping controller works in dpp

* [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci

* fix: standard cpp version

* fix: changed and fixed som config file values

* [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci

* feat: removed the python adaptive controller, it is now in local legacy folder

* fix: containing was written wrong

* refactor: reference filter

* [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci

* refactor: remove unused odom topic declaration

* doc: add doc to pose and twist callback

* fix: cpp version

* fix: license

* fix: removed iostream library from the files

* remove iostream

* fix: removed /orca/odom, and added /dvl/pose and twist

* fix: added some changed requested

* fix: made it more readable

* feat: added Readme for the adaptive backstepping controller

* fix: some problem in the README files

* fix: tried to fix it again

* fix: tried to fix it again v2

* fix: tried to fix it again v3

* fix: tried to fix it again v4

* fix: trying to fix error in README

* fix: trying to fix error in README v2

* fix: trying to fix error in README v3

* fix: trying to fix error in README v4

* fix: trying to fix error in README v5

* fix: trying to fix error in README v6

* fix: trying to fix error in README v7

* started readme for reference filter

* [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci

* started readme for reference filter v2

* [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci

* finish readme for reference filter

* fix: eigen include

* [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci

* feat: added set_subcribers_and_publisher() function in all dp controller

* feat: added struct adapt params, feed into constructur and removed license stuff

* feat: add _very_ short READMEs for PIDs

* [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci

* refactor: update topics for reference filter

auv_setup/config/robots/orca.yaml

@@ -18,6 +18,7 @@
   ros__parameters:
     physical:
       center_of_mass: [0.0, 0.0, 0.035] # m (x,y,z)
+      mass_matrix : [ 30.0, 0.0, 0.0, 0.0, 0.0, 0.6, 0.0, 30.0, 0.0, 0.0, -0.6, 0.3, 0.0, 0.0, 30.0, 0.6, 0.3, 0.0, 0.0, 0.0, 0.6, 0.68, 0.0, 0.0, 0.0, -0.6, 0.3, 0.0, 3.32, 0.0, 0.6, 0.3, 0.0, 0.0, 0.0, 3.34]
 
     propulsion:
       dofs:
@@ -146,8 +147,9 @@
       joy: "/joy"
       pose: "/pose"
       twist: "/twist"
-      operation_mode: /operation_mode
-      killswitch: /killswitch
+      operation_mode: "/operation_mode"
+      killswitch: "/killswitch"
+      aruco_board_pose_camera: "/aruco_board_pose_camera"
       guidance:
         los: "/guidance/los"
         dp: "/guidance/dp"
@@ -160,3 +162,6 @@
         peaks: "/peaks"
         tdoa: "/time_difference_of_arrival"
         position: "/position"
+
+    action_servers:
+      reference_filter: "/reference_filter"

control/dp_adapt_backs_controller/CMakeLists.txt

@@ -0,0 +1,48 @@
+cmake_minimum_required(VERSION 3.8)
+project(dp_adapt_backs_controller)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(vortex_msgs REQUIRED)
+
+include_directories(include)
+
+add_executable(dp_adapt_backs_controller_node
+  src/dp_adapt_backs_controller.cpp
+  src/dp_adapt_backs_controller_node.cpp
+  src/dp_adapt_backs_controller_ros.cpp
+  src/dp_adapt_backs_controller_utils.cpp
+)
+
+ament_target_dependencies(dp_adapt_backs_controller_node
+  rclcpp
+  geometry_msgs
+  nav_msgs
+  Eigen3
+  tf2
+  vortex_msgs
+)
+
+install(TARGETS
+  dp_adapt_backs_controller_node
+  DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY
+  config
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()