Henrik/velocity control

.gitignore

@@ -1,9 +1,9 @@
 # Created by https://www.gitignore.io/api/ros
 # Edit at https://www.gitignore.io/?templates=ros
-
+ 
 # CMake
 cmake-build-debug/
-
+ 
 ### ROS ###
 install/
 log/
@@ -21,41 +21,39 @@ msg/*Result.msg
 msg/_*.py
 build_isolated/
 devel_isolated/
-*.cache/
-
+
 # Generated by dynamic reconfigure
 *.cfgc
 /cfg/cpp/
 /cfg/*.py
-
+ 
 # Ignore generated docs
 *.dox
 *.wikidoc
-
+ 
 # IDE stuff
 .project
 .cproject
 .vscode
 .DS_Store
 .idea
-
+ 
 # qcreator stuff
 CMakeLists.txt.user
-
+ 
 srv/_*.py
 *.pcd
 *.pyc
 qtcreator-*
 *.user
-*hintmux.sh
-
+
 /planning/cfg
 /planning/docs
 /planning/src
-
+ 
 *~
-
+ 
 # Emacs
 .#*
-
-# End of https://www.gitignore.io/api/ros
+ .cache
+# End of https://www.gitignore.io/api/ros

auv_setup/config/robots/orca.yaml

@@ -155,6 +155,7 @@
       set_killswitch: "set_killswitch"
       toggle_killswitch: "toggle_killswitch"
       get_operation_mode: "get_operation_mode"
+      los_mode: "set_los_mode"
       waypoint_addition: "waypoint_addition"
 
     fsm:

auv_setup/launch/autopilot.launch.py

@@ -9,44 +9,65 @@
 )
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
-
-
+from auv_setup.launch_arg_common import (
+    declare_drone_and_namespace_args,
+    resolve_drone_and_namespace,
+)
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
 def launch_setup(context, *args, **kwargs):
-    drone = LaunchConfiguration("drone").perform(context)
-
-    los_guidance_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(
-                get_package_share_directory("los_guidance"),
-                "launch",
-                "los_guidance.launch.py",
-            )
-        ),
-        launch_arguments={"drone": drone}.items(),
+    drone, namespace = resolve_drone_and_namespace(context)
+    drone_params = os.path.join(
+        get_package_share_directory("auv_setup"),
+        "config",
+        "robots",
+        f"{drone}.yaml",
     )
-
-    autopilot_controller_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(
-                get_package_share_directory("velocity_controller_lqr"),
-                "launch",
-                "velocity_controller_lqr.launch.py",
-            )
-        ),
-        launch_arguments={"drone": drone}.items(),
+    VC_params = os.path.join(
+        get_package_share_directory("velocity_controller"),
+        "config",
+        "parameters.yaml",
     )
+    adapt_params = os.path.join(
+        get_package_share_directory("los_guidance"),
+        "config",
+        "guidance_params.yaml",
+    )
+    container=ComposableNodeContainer(
+        name="autopilot_container",
+        namespace=namespace,
+        package="rclcpp_components",
+        executable="component_container_mt",
+        composable_node_descriptions=[
+            ComposableNode(
+                package="velocity_controller",
+                plugin="velocity_controller_node",
+                name="velocity_controller_node",
+                namespace=namespace,
+                parameters=[VC_params,drone_params],
+                extra_arguments=[{"use_intra_process_comms":True}],
+            ),
+            ComposableNode(
+                package="los_guidance",
+                plugin="los_guidance_node",
+                name="los_guidance_node",
+                namespace=namespace,
+                parameters=[adapt_params,drone_params],
+                extra_arguments=[{"use_intra_process_comms":True}],
+            ),
 
-    return [los_guidance_launch, autopilot_controller_launch]
+        ],
+        output="screen",
+        arguments=["--ros-args","--log-level","error"],
+    )
+    return [container]
+
 
 
-def generate_launch_description() -> LaunchDescription:
+def generate_launch_description():
     return LaunchDescription(
-        [
-            DeclareLaunchArgument(
-                "drone",
-                default_value="orca",
-                description="Drone name / namespace",
-            ),
+        declare_drone_and_namespace_args()
+        + [
             OpaqueFunction(function=launch_setup),
         ]
     )

control/velocity_controller/CMakeLists.txt

@@ -0,0 +1,113 @@
+cmake_minimum_required(VERSION 3.8)
+project(velocity_controller)
+
+set(CMAKE_CXX_STANDARD 20)
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+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(rclcpp_lifecycle REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(vortex_msgs REQUIRED)
+find_package(vortex_utils REQUIRED)
+find_package(Eigen3 REQUIRED)
+#find_package(CasADi REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(ct_optcon REQUIRED)
+find_package(ct_core REQUIRED)
+find_package(casadi REQUIRED)
+
+set(ACADOS_ROOT "$ENV{ACADOS_SOURCE_DIR}" CACHE PATH "acados root")
+include_directories(
+  ${ACADOS_ROOT}/include
+  ${ACADOS_ROOT}/include/acados
+  ${ACADOS_ROOT}/include/acados/ocp_nlp
+  ${ACADOS_ROOT}/include/acados_c
+  ${ACADOS_ROOT}/include/blasfeo/include
+  ${ACADOS_ROOT}/include/hpipm/include
+  ${ACADOS_ROOT}/include/qpOASES_e
+  ${CMAKE_CURRENT_SOURCE_DIR}/scripts/build_auv_solver
+
+  include
+
+)
+link_directories(${ACADOS_ROOT}/lib)
+
+
+# After setting ACADOS_ROOT etc.
+file(GLOB_RECURSE GEN_SRCS
+  ${CMAKE_CURRENT_SOURCE_DIR}/scripts/build_auv_solver/*.c
+  ${CMAKE_CURRENT_SOURCE_DIR}/scripts/build_auv_solver/*/*.c
+)
+
+
+
+add_library(auv_nmpc STATIC
+  src/NMPC_acados.cpp
+  ${GEN_SRCS}
+)
+
+
+add_executable(velocity_controller_node
+  src/velocity_controller.cpp
+  src/PID_setup.cpp
+  src/LQR_setup.cpp
+  src/utilities.cpp
+  src/ct_instantiations.cpp
+  src/NMPC_setup.cpp
+  #src/NMPC_acados.cpp
+)
+
+
+
+#add_executable(test_LQR_node
+#  src/LQR_test.cpp
+#  src/LQR_setup.cpp
+#  src/utilities.cpp
+#  src/ct_instantiations.cpp
+#  )
+ament_target_dependencies(velocity_controller_node
+  rclcpp
+  rclcpp_lifecycle
+  lifecycle_msgs
+  std_msgs
+  vortex_msgs
+  geometry_msgs
+ # CasADi
+  nav_msgs
+  vortex_utils
+)
+#target_include_directories(velocity_controller_node PRIVATE casadi Eigen3)
+target_link_libraries(auv_nmpc acados hpipm blasfeo qpOASES_e m)
+target_link_libraries(velocity_controller_node Eigen3::Eigen casadi::casadi ct_optcon ct_core auv_nmpc)
+install(TARGETS
+  velocity_controller_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(DIRECTORY
+  launch
+  config 
+  DESTINATION share/${PROJECT_NAME}/
+)
+if(BUILD_TESTING)
+  add_subdirectory(tests) 
+endif()
+
+
+
+
+ament_package()

control/velocity_controller/config/nautilus_params.yaml

@@ -0,0 +1,33 @@
+/**:
+  ros__parameters:
+
+    PID_params:
+      surge: [300.0,10.0,5.0]
+      pitch: [60.0,8.0,12.0]
+      yaw: [10.0,1.0,5.0]
+
+    LQR_params:
+      Q: [200.0,32.84,32.84,15.0,15.0,100.0,32.84,32.84]
+      R: [0.02,3.1,3.10]
+    NMPCA_params:
+      Q: [1.0,1.0,1.0,5.0,5.0] # u,q,r,theta,psi
+      R: [0.1,0.1,0.1] # u_surge, u_theta, u_psi
+      N: 20
+    NMPC_params:
+      Q: [100.0,0.0,0.0,0.0,1.0,1.0,0.0,5.0,50.0] # u,q,r,theta,psi
+      R: [0.1,0.1,0.1] # u_surge, u_theta, u_psi
+      N: 20
+    #inertia_matrix: [ 30.0, 0.0, 0.0, 0.0, 0.0, 0.60, 0.0, 30.0, 0.0, 0.0, -0.60, 0.30, 0.0, 0.0, 30.0, 0.60, 0.30, 0.0, 0.0, 0.0, 0.60, 0.68, 0.0, 0.0, 0.0, -0.60, 0.30, 0.0, 3.32, 0.0, 0.60, 0.30, 0.0, 0.0, 0.0, 3.34]
+    dampening_matrix_low:  [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0,  0.0,0.0,46.0,0.0,0.0,0.0,  0.0,0.0,0.0,46.0,0.0,0.0,  0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0]
+    dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0,  0.0,0.0,1.0,0.0,0.0,0.0,  0.0,0.0,0.0,1.0,0.0,0.0,  0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0]
+
+    publish_rate: 100 #ms
+    #Clamp parameter
+    max_force: 99.5 
+    controller_type: 2 #1 PID 2 LQR 3 NMPC 4NMPC fast
+
+    #Q: [300.0,0.01,0.01,0.01,32.84,32.84,32.84,32.84,32.84] # u,v,w,p,q,r,phi,theta,psi
+    # R: [0.02,3.1,3.10] # u_surge, u_theta, u_psi
+
+    #Fixes: reduce oscillations, follows angles close to wrap around, model restoring forces in pitch, test different references, try to find out why the fuck it went backwards?,
+

control/velocity_controller/config/orca_params.yaml

@@ -0,0 +1,34 @@
+/**:
+  ros__parameters:
+
+    PID_params:
+      surge: [300.0,10.0,5.0]
+      pitch: [60.0,8.0,12.0]
+      yaw: [10.0,1.0,5.0]
+
+    LQR_params:
+      #Q: [200.0,62.84,62.84,15.0,15.0,10.0,3.84,3.84]
+      #R: [8.0,100.0,100.0]
+      Q: [200.0,32.84,32.84,15.0,15.0,100.0,32.84,32.84]
+      R: [0.02,3.1,3.10]
+    NMPCA_params:
+      Q: [1.0,1.0,1.0,5.0,5.0] # u,q,r,theta,psi
+      R: [0.1,0.1,0.1] # u_surge, u_theta, u_psi
+      N: 20
+    NMPC_params:
+      Q: [100.0,0.0,0.0,0.0,1.0,1.0,0.0,5.0,50.0] # u,q,r,theta,psi
+      R: [0.1,0.1,0.1] # u_surge, u_theta, u_psi
+      N: 20
+    dampening_matrix_low:  [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0,  0.0,0.0,46.0,0.0,0.0,0.0,  0.0,0.0,0.0,46.0,0.0,0.0,  0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0]
+    dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0,  0.0,0.0,1.0,0.0,0.0,0.0,  0.0,0.0,0.0,1.0,0.0,0.0,  0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0]
+
+    publish_rate: 100 #ms
+    #Clamp parameter
+    max_force: 99.5 
+    controller_type: 2 #1 PID 2 LQR 3 NMPC 4NMPC fast
+
+    #Q: [300.0,0.01,0.01,0.01,32.84,32.84,32.84,32.84,32.84] # u,v,w,p,q,r,phi,theta,psi
+    # R: [0.02,3.1,3.10] # u_surge, u_theta, u_psi
+
+    #Fixes: reduce oscillations, follows angles close to wrap around, model restoring forces in pitch, test different references, try to find out why the fuck it went backwards?,
+