Skip to content

Commit 2d347c2

Browse files
jorgenfjjorgenfj
committed
init package structure
1 parent 4fba8ee commit 2d347c2

19 files changed

Lines changed: 1352 additions & 0 deletions

‎guidance/reference_filter_dp_quat/CMakeLists.txt‎

Lines changed: 106 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,106 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(reference_filter_dp_quat)
3+
4+
if(NOT CMAKE_CXX_STANDARD)
5+
set(CMAKE_CXX_STANDARD 20)
6+
endif()
7+
8+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
9+
add_compile_options(-Wall -Wextra -Wpedantic)
10+
endif()
11+
12+
find_package(ament_cmake REQUIRED)
13+
find_package(rclcpp REQUIRED)
14+
find_package(rclcpp_action REQUIRED)
15+
find_package(rclcpp_components REQUIRED)
16+
find_package(vortex_msgs REQUIRED)
17+
find_package(vortex_utils REQUIRED)
18+
find_package(vortex_utils_ros REQUIRED)
19+
find_package(geometry_msgs REQUIRED)
20+
find_package(nav_msgs REQUIRED)
21+
find_package(Eigen3 REQUIRED)
22+
find_package(spdlog REQUIRED)
23+
find_package(fmt REQUIRED)
24+
25+
include_directories(include)
26+
27+
set(CORE_LIB_NAME "${PROJECT_NAME}")
28+
29+
add_library(${CORE_LIB_NAME} SHARED
30+
src/lib/reference_filter.cpp
31+
src/lib/waypoint_utils.cpp
32+
src/lib/waypoint_follower.cpp
33+
)
34+
35+
target_include_directories(${CORE_LIB_NAME}
36+
PUBLIC
37+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
38+
$<INSTALL_INTERFACE:include>
39+
)
40+
41+
ament_target_dependencies(${CORE_LIB_NAME}
42+
Eigen3
43+
vortex_utils
44+
)
45+
46+
set(COMPONENT_LIB_NAME "${PROJECT_NAME}_component")
47+
48+
add_library(${COMPONENT_LIB_NAME} SHARED
49+
src/ros/reference_filter_ros.cpp
50+
)
51+
52+
ament_target_dependencies(${COMPONENT_LIB_NAME}
53+
rclcpp
54+
rclcpp_components
55+
rclcpp_action
56+
geometry_msgs
57+
nav_msgs
58+
vortex_msgs
59+
vortex_utils
60+
vortex_utils_ros
61+
spdlog
62+
fmt
63+
)
64+
65+
target_link_libraries(${COMPONENT_LIB_NAME} ${CORE_LIB_NAME})
66+
67+
rclcpp_components_register_node(
68+
${COMPONENT_LIB_NAME}
69+
PLUGIN "ReferenceFilterNode"
70+
EXECUTABLE ${PROJECT_NAME}_node
71+
)
72+
73+
install(
74+
TARGETS
75+
${CORE_LIB_NAME}
76+
${COMPONENT_LIB_NAME}
77+
EXPORT export_${PROJECT_NAME}
78+
ARCHIVE DESTINATION lib
79+
LIBRARY DESTINATION lib
80+
RUNTIME DESTINATION bin
81+
)
82+
83+
install(
84+
DIRECTORY include/
85+
DESTINATION include
86+
)
87+
88+
install(DIRECTORY
89+
launch
90+
config
91+
DESTINATION share/${PROJECT_NAME}/
92+
)
93+
94+
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
95+
ament_export_dependencies(
96+
Eigen3
97+
vortex_utils
98+
)
99+
ament_export_include_directories(include)
100+
ament_export_libraries(${CORE_LIB_NAME})
101+
102+
if(BUILD_TESTING)
103+
add_subdirectory(test)
104+
endif()
105+
106+
ament_package()

‎guidance/reference_filter_dp_quat/config/reference_filter_params.yaml‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
/**:
2+
ros__parameters:
3+
zeta: [1., 1., 1., 1., 1., 1.]
4+
omega: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2]
5+
time_step_ms: 10

‎guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/eigen_typedefs.hpp‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
/**
2+
* @file eigen_typedefs.hpp
3+
* @brief Contains Eigen typedefs used in this package.
4+
*/
5+
6+
#ifndef REFERENCE_FILTER_DP_QUAT__LIB__EIGEN_TYPEDEFS_HPP_
7+
#define REFERENCE_FILTER_DP_QUAT__LIB__EIGEN_TYPEDEFS_HPP_
8+
9+
#include <eigen3/Eigen/Dense>
10+
11+
namespace Eigen {
12+
13+
typedef Eigen::Matrix<double, 18, 18> Matrix18d;
14+
typedef Eigen::Matrix<double, 18, 6> Matrix18x6d;
15+
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
16+
typedef Eigen::Matrix<double, 6, 1> Vector6d;
17+
typedef Eigen::Matrix<double, 18, 1> Vector18d;
18+
19+
} // namespace Eigen
20+
21+
#endif // REFERENCE_FILTER_DP_QUAT__LIB__EIGEN_TYPEDEFS_HPP_

‎guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/reference_filter.hpp‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
#ifndef REFERENCE_FILTER_DP_QUAT__LIB__REFERENCE_FILTER_HPP_
2+
#define REFERENCE_FILTER_DP_QUAT__LIB__REFERENCE_FILTER_HPP_
3+
4+
#include "reference_filter_dp_quat/lib/eigen_typedefs.hpp"
5+
6+
namespace vortex::guidance {
7+
8+
struct ReferenceFilterParams {
9+
Eigen::Vector6d omega = Eigen::Vector6d::Zero();
10+
Eigen::Vector6d zeta = Eigen::Vector6d::Zero();
11+
};
12+
13+
/**
14+
* @brief Stateless third-order reference filter.
15+
*
16+
* Computes the state derivative x_dot = Ad * x + Bd * r, where the state
17+
* x = [eta, eta_dot, eta_ddot] is 18-dimensional and r is the 6D reference.
18+
*/
19+
class ReferenceFilter {
20+
public:
21+
explicit ReferenceFilter(const ReferenceFilterParams& params);
22+
23+
// @brief Calculate the state derivative
24+
// @param x The state vector 18x1
25+
// @param r The reference vector 6x1
26+
// @return The state derivative 18x1
27+
// REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen
28+
// 2021 p. 337 eq: 12.11
29+
Eigen::Vector18d calculate_x_dot(const Eigen::Vector18d& x,
30+
const Eigen::Vector6d& r);
31+
32+
// @brief Calculate the state transition matrix
33+
// REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen
34+
// 2021 p. 337 eq: 12.12
35+
void calculate_Ad(const Eigen::Vector6d& omega,
36+
const Eigen::Vector6d& zeta);
37+
38+
// @brief Calculate the input matrix
39+
// REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen
40+
// 2021 p. 337 eq: 12.12
41+
void calculate_Bd(const Eigen::Vector6d& omega);
42+
43+
private:
44+
Eigen::Matrix18d Ad_ = Eigen::Matrix18d::Zero();
45+
Eigen::Matrix18x6d Bd_ = Eigen::Matrix18x6d::Zero();
46+
};
47+
48+
} // namespace vortex::guidance
49+
50+
#endif // REFERENCE_FILTER_DP_QUAT__LIB__REFERENCE_FILTER_HPP_

‎guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp‎

Lines changed: 83 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,83 @@
1+
#ifndef REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_FOLLOWER_HPP_
2+
#define REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_FOLLOWER_HPP_
3+
4+
#include <mutex>
5+
#include <vortex/utils/types.hpp>
6+
#include "reference_filter_dp_quat/lib/eigen_typedefs.hpp"
7+
#include "reference_filter_dp_quat/lib/reference_filter.hpp"
8+
#include "reference_filter_dp_quat/lib/waypoint_types.hpp"
9+
10+
namespace vortex::guidance {
11+
12+
using vortex::utils::types::PoseEuler;
13+
using vortex::utils::types::Twist;
14+
15+
/**
16+
* @brief Manages reference filter state and waypoint following logic.
17+
*
18+
* Thread-safe: all public methods acquire a mutex.
19+
*/
20+
class WaypointFollower {
21+
public:
22+
WaypointFollower(const ReferenceFilterParams& params, double dt_seconds);
23+
24+
/**
25+
* @brief Initialize the follower with the current vehicle state and target.
26+
* @param pose Current vehicle pose.
27+
* @param twist Current vehicle twist (body frame).
28+
* @param waypoint Target waypoint with mode.
29+
* @param convergence_threshold Max error norm to consider target reached.
30+
*/
31+
void start(const PoseEuler& pose,
32+
const Twist& twist,
33+
const Waypoint& waypoint,
34+
double convergence_threshold);
35+
36+
/**
37+
* @brief Advance the filter by one time step.
38+
* @return The updated filter state.
39+
*/
40+
Eigen::Vector18d step();
41+
42+
/**
43+
* @brief Check if the measured pose has converged to the reference goal.
44+
* @param measured_pose Current measured pose.
45+
* @return True if the error norm is within the convergence threshold.
46+
*/
47+
bool within_convergance(const Eigen::Vector6d& measured_pose) const;
48+
49+
/**
50+
* @brief Update the reference goal pose mid-sequence.
51+
* @param reference_goal_pose The new reference pose.
52+
*/
53+
void set_reference(const PoseEuler& reference_goal_pose);
54+
55+
/**
56+
* @brief Snap the position component of the filter state to the reference.
57+
*
58+
* Useful after convergence to eliminate any remaining steady-state offset
59+
*/
60+
void snap_state_to_reference();
61+
62+
/// @brief Get the current 18D filter state.
63+
Eigen::Vector18d state() const;
64+
65+
/// @brief Get the current 6D reference goal pose.
66+
Eigen::Vector6d reference() const;
67+
68+
private:
69+
Eigen::Vector18d compute_initial_state(const PoseEuler& pose,
70+
const Twist& twist);
71+
72+
mutable std::mutex mutex_;
73+
ReferenceFilter filter_;
74+
double dt_seconds_{0.01};
75+
Eigen::Vector18d state_ = Eigen::Vector18d::Zero();
76+
Eigen::Vector6d reference_goal_ = Eigen::Vector6d::Zero();
77+
WaypointMode waypoint_mode_{WaypointMode::FULL_POSE};
78+
double convergence_threshold_{0.1};
79+
};
80+
81+
} // namespace vortex::guidance
82+
83+
#endif // REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_FOLLOWER_HPP_

‎guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_types.hpp‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
#ifndef REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_TYPES_HPP_
2+
#define REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_TYPES_HPP_
3+
4+
#include <cstdint>
5+
#include <vortex/utils/types.hpp>
6+
7+
namespace vortex::guidance {
8+
9+
using vortex::utils::types::PoseEuler;
10+
11+
/**
12+
* @brief Determines which degrees of freedom the reference filter controls.
13+
*
14+
* The mode affects both the reference goal computation (via apply_mode_logic)
15+
* and the convergence check (via has_converged).
16+
*/
17+
enum class WaypointMode : uint8_t {
18+
FULL_POSE = 0, ///< Control all 6 DOF.
19+
ONLY_POSITION = 1, ///< Control x, y, z; hold current orientation.
20+
FORWARD_HEADING = 2, ///< Control x, y, z with yaw toward target.
21+
ONLY_ORIENTATION = 3, ///< Control roll, pitch, yaw; hold current position.
22+
};
23+
24+
/**
25+
* @brief A target pose with an associated waypoint mode.
26+
*/
27+
struct Waypoint {
28+
PoseEuler pose{};
29+
WaypointMode mode = WaypointMode::FULL_POSE;
30+
};
31+
32+
} // namespace vortex::guidance
33+
34+
#endif // REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_TYPES_HPP_

‎guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_utils.hpp‎

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
#ifndef REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_UTILS_HPP_
2+
#define REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_UTILS_HPP_
3+
4+
#include "reference_filter_dp_quat/lib/eigen_typedefs.hpp"
5+
#include "reference_filter_dp_quat/lib/waypoint_types.hpp"
6+
7+
namespace vortex::guidance {
8+
9+
/**
10+
* @brief Apply waypoint mode logic to a reference pose.
11+
*
12+
* For modes that don't control all DOFs, the uncontrolled components are
13+
* replaced with values from @p current_state so the filter holds them steady.
14+
*
15+
* @param r_in The raw reference pose (6D).
16+
* @param mode The waypoint mode.
17+
* @param current_state The current filter state pose (6D).
18+
* @return The adjusted reference pose.
19+
*/
20+
Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& r_in,
21+
WaypointMode mode,
22+
const Eigen::Vector6d& current_state);
23+
24+
/**
25+
* @brief Check whether the measured pose has converged to the reference.
26+
*
27+
* Only the DOFs relevant to the waypoint mode are included in the error norm.
28+
*
29+
* @param measured_pose The current measured pose (6D).
30+
* @param reference The reference goal pose (6D).
31+
* @param mode The waypoint mode.
32+
* @param convergence_threshold The maximum allowed error norm.
33+
* @return True if the error is below the threshold.
34+
*/
35+
bool has_converged(const Eigen::Vector6d& measured_pose,
36+
const Eigen::Vector6d& reference,
37+
WaypointMode mode,
38+
double convergence_threshold);
39+
40+
} // namespace vortex::guidance
41+
42+
#endif // REFERENCE_FILTER_DP_QUAT__LIB__WAYPOINT_UTILS_HPP_

0 commit comments

Comments
 (0)