Henrik/velocity/merge

.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,73 @@
+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_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(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(ct_optcon REQUIRED)
+find_package(ct_core REQUIRED)
+find_package(casadi REQUIRED)
+
+include_directories(
+  include
+)
+
+#target_include_directories(velocity_controller_node PUBLIC
+#  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+#  $<INSTALL_INTERFACE:include>
+#)
+
+add_executable(velocity_controller_node
+  src/velocity_controller.cpp
+  src/PID_setup.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
+  nav_msgs
+  vortex_utils
+)
+#target_include_directories(velocity_controller_node PRIVATE casadi Eigen3)
+target_link_libraries(velocity_controller_node Eigen3::Eigen casadi::casadi ct_optcon ct_core)
+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()