Anbit/guidance

.github/workflows/simulator-test.yml

@@ -22,4 +22,5 @@ jobs:
         "tests/simulator_tests/waypoint_navigation/orca_pseudoinverse.sh",
         "tests/simulator_tests/waypoint_manager_test/simulator_test.sh",
         "tests/simulator_tests/waypoint_navigation_quat/simulator_test.sh",
+        "tests/simulator_tests/los_test/simulator_test.sh"
         ]'

auv_setup/config/robots/orca.yaml

@@ -156,6 +156,7 @@
       set_killswitch: "set_killswitch"
       toggle_killswitch: "toggle_killswitch"
       get_operation_mode: "get_operation_mode"
+      los_mode: "set_los_mode"
       waypoint_addition: "waypoint_addition"
       start_mission: "start_mission"
 

auv_setup/launch/autopilot.launch.py

@@ -40,7 +40,12 @@ def launch_setup(context, *args, **kwargs):
         executable="los_guidance_node",
         name="los_guidance_node",
         namespace=namespace,
-        parameters=[drone_params, los_config],
+        parameters=[
+            drone_params,
+            {
+                "los_config_file": los_config,
+            },
+        ],
         output="screen",
     )
 

control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml

@@ -1,23 +1,23 @@
 /**:
   ros__parameters:
-    dt: 0.1
+    dt: 0.05
 
     LQR_params:
-      q_surge: 75
-      q_pitch: 175
-      q_yaw: 175
+      q_surge: 40
+      q_pitch: 80
+      q_yaw: 80
 
-      r_surge: 0.3
-      r_pitch: 0.4
-      r_yaw: 0.4
+      r_surge: 0.4
+      r_pitch: 0.6
+      r_yaw: 0.6
 
-      i_surge: 0.3
-      i_pitch: 0.4
-      i_yaw: 0.3
+      i_surge: 0.2
+      i_pitch: 0.2
+      i_yaw: 0.2
 
       i_weight: 0.5
 
-    inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729]
+    inertia_matrix: [53.7, 0.0, 0.0, 0.0, 23.1128, 0.1025, 0.0, 0.1025, 26.23998]
 
     #Clamp parameter
-    max_force: 99.5
+    max_force: 80.0

guidance/los_guidance/CMakeLists.txt

@@ -12,37 +12,51 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(vortex_msgs REQUIRED)
 find_package(vortex_utils_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(spdlog REQUIRED)
 find_package(fmt REQUIRED)
+find_package(yaml-cpp REQUIRED)
+
 
 include_directories(include)
 
 set(LIB_NAME los_guidance_lib)
 add_library(${LIB_NAME} SHARED
-  src/los_guidance.cpp
-  src/los_guidance_ros.cpp
+  src/lib/proportional_los.cpp
+  src/lib/integral_los.cpp
+  src/lib/adaptive_los.cpp
+  src/lib/vector_field_los.cpp
+  src/lib/guidance_manager.cpp
+  src/ros/los_guidance_ros.cpp
+  src/lib/utils.cpp
 )
 
 ament_target_dependencies(${LIB_NAME}
   rclcpp
   rclcpp_action
+  rclcpp_components
   geometry_msgs
+  nav_msgs
   vortex_msgs
   vortex_utils_ros
   Eigen3
   spdlog
   fmt
 )
 
-add_executable(los_guidance_node
-  src/los_guidance_node.cpp
+target_link_libraries(${LIB_NAME}
+  yaml-cpp
 )
 
-target_link_libraries(los_guidance_node ${LIB_NAME})
+rclcpp_components_register_node(${LIB_NAME}
+  PLUGIN "vortex::guidance::los::LosGuidanceNode"
+  EXECUTABLE los_guidance_node
+)
 
 install(TARGETS
   ${LIB_NAME}
@@ -52,14 +66,9 @@ install(TARGETS
   RUNTIME DESTINATION bin
 )
 
-install(TARGETS
-  los_guidance_node
-  DESTINATION lib/${PROJECT_NAME}
-)
-
 install(
   DIRECTORY include/
-  DESTINATION include
+  DESTINATION include/
 )
 
 install(DIRECTORY
@@ -68,6 +77,11 @@ install(DIRECTORY
   DESTINATION share/${PROJECT_NAME}/
 )
 
+install(PROGRAMS
+  scripts/test_scenarios.py
+  DESTINATION share/${PROJECT_NAME}/scripts
+)
+
 if(BUILD_TESTING)
   add_subdirectory(test)
 endif()