Fixed missing restoring forces calculation and build error

CMakeLists.txt

@@ -1,64 +1,56 @@
 cmake_minimum_required(VERSION 3.8)
 project(hydrodynamics)
 
-# Default to C99
-if(NOT CMAKE_C_STANDARD)
-  set(CMAKE_C_STANDARD 99)
-endif()
-
-# Default to C++ 17
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 17)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   rclcpp
-  ament_cmake
-  eigen3_cmake_module
   Eigen3
 )
 
+find_package(ament_cmake REQUIRED)
 foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
   find_package(${Dependency} REQUIRED)
 endforeach()
 
-include_directories(
-  include
-)
-
-add_library(${PROJECT_NAME} SHARED
+add_library(hydrodynamics SHARED
   src/hydrodynamics.cpp
 )
 
-target_link_libraries(${PROJECT_NAME}
-  ${rclcpp_LIBRARIES}
+target_include_directories(hydrodynamics
+  PUBLIC
+    $<INSTALL_INTERFACE:include>
+    $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+    ${EIGEN3_INCLUDE_DIR}
 )
-target_include_directories(${PROJECT_NAME} PUBLIC
-  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+target_compile_features(hydrodynamics PUBLIC cxx_std_17)
+target_link_libraries(hydrodynamics
+  PUBLIC
+    ${rclcpp_LIBRARIES}
+)
+
+ament_target_dependencies(hydrodynamics
+  PUBLIC
+    rclcpp
 )
-ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 install(
   DIRECTORY include/
-  DESTINATION include/${PROJECT_NAME}
+  DESTINATION include/hydrodynamics
 )
 
 install(
-  TARGETS ${PROJECT_NAME}
-  EXPORT "export_${PROJECT_NAME}"
+  TARGETS hydrodynamics
+  EXPORT "export_hydrodynamics"
   RUNTIME DESTINATION bin
   LIBRARY DESTINATION lib
   ARCHIVE DESTINATION lib
   INCLUDES DESTINATION include
 )
 
 if(BUILD_TESTING)
-  find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_lint_auto REQUIRED)
 
   # Run linters found in package.xml except those below
@@ -68,7 +60,7 @@ if(BUILD_TESTING)
   set(ament_cmake_flake8_FOUND TRUE)
 endif()
 
-ament_export_targets("export_${PROJECT_NAME}" HAS_LIBRARY_TARGET)
+ament_export_targets("export_hydrodynamics" HAS_LIBRARY_TARGET)
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 ament_package()

include/hydrodynamics.hpp

@@ -21,7 +21,6 @@
 #pragma once
 
 #include <Eigen/Dense>
-#include <optional>
 
 #include "eigen.hpp"
 

package.xml

@@ -15,12 +15,9 @@
   <author>Evan Palmer</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
-  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
 
   <depend>eigen</depend>
 
-  <buildtool_export_depend>eigen3_cmake_module</buildtool_export_depend>
-
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

src/hydrodynamics.cpp

@@ -148,4 +148,19 @@ RestoringForces::RestoringForces(double weight, double buoyancy, Eigen::Vector3d
 {
 }
 
+Eigen::Vector6d RestoringForces::calculateRestoringForcesVector(const Eigen::Matrix3d & rot) const
+{
+  const Eigen::Vector3d fg(0, 0, weight_);
+  const Eigen::Vector3d fb(0, 0, -buoyancy_);
+
+  Eigen::Vector6d g_rb;
+
+  g_rb.topRows(3) = rot * (fg + fb);
+  g_rb.bottomRows(3) = center_of_gravity_.cross(rot * fg) + center_of_buoyancy_.cross(rot * fb);
+
+  g_rb *= -1;
+
+  return g_rb;
+}
+
 }  // namespace hydrodynamics