Interface refactor

rosplane/CMakeLists.txt

@@ -49,8 +49,9 @@ install(DIRECTORY launch params DESTINATION share/${PROJECT_NAME}/)
 
 # Param Manager
 add_library(param_manager
-  include/param_manager/param_manager.hpp
-  src/param_manager/param_manager.cpp
+  include/param_manager/param_manager_interface.hpp
+  include/param_manager/param_manager_ros.hpp
+  src/param_manager/param_manager_ros.cpp
 )
 ament_target_dependencies(param_manager rclcpp)
 ament_export_targets(param_manager HAS_LIBRARY_TARGET)
@@ -63,6 +64,23 @@ install(TARGETS param_manager
   INCLUDES DESTINATION include
 )
 
+# Logger
+add_library(logger
+  include/logger/logger_interface.hpp
+  include/logger/logger_ros.hpp
+  src/logger/logger_ros.cpp
+)
+ament_target_dependencies(logger rclcpp)
+ament_export_targets(logger HAS_LIBRARY_TARGET)
+install(DIRECTORY include/logger DESTINATION include)
+install(TARGETS logger
+  EXPORT logger
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
+)
+
 ### START OF EXECUTABLES ###
 
 # Controller
@@ -72,7 +90,7 @@ add_executable(rosplane_controller
   src/controller_successive_loop.cpp
   src/controller_total_energy.cpp)
 ament_target_dependencies(rosplane_controller rosplane_msgs rosflight_msgs rclcpp rclpy Eigen3)
-target_link_libraries(rosplane_controller param_manager)
+target_link_libraries(rosplane_controller param_manager logger)
 install(TARGETS
   rosplane_controller
   DESTINATION lib/${PROJECT_NAME})
@@ -82,7 +100,7 @@ add_executable(rosplane_path_follower
   src/path_follower_example.cpp
   src/path_follower_base.cpp)
 ament_target_dependencies(rosplane_path_follower rosplane_msgs rclcpp rclpy Eigen3)
-target_link_libraries(rosplane_path_follower param_manager)
+target_link_libraries(rosplane_path_follower param_manager logger)
 install(TARGETS
   rosplane_path_follower
   DESTINATION lib/${PROJECT_NAME})
@@ -92,7 +110,7 @@ add_executable(rosplane_path_manager
   src/path_manager_base.cpp
   src/path_manager_example.cpp)
 ament_target_dependencies(rosplane_path_manager rosplane_msgs rclcpp rclpy Eigen3)
-target_link_libraries(rosplane_path_manager param_manager)
+target_link_libraries(rosplane_path_manager param_manager logger)
 install(TARGETS
   rosplane_path_manager
   DESTINATION lib/${PROJECT_NAME})
@@ -102,6 +120,7 @@ add_executable(rosplane_path_planner
   src/path_planner.cpp)
 target_link_libraries(rosplane_path_planner
   param_manager
+  logger
   ${YAML_CPP_LIBRARIES}
 )
 ament_target_dependencies(rosplane_path_planner rosplane_msgs rosflight_msgs std_srvs rclcpp rclpy Eigen3)
@@ -118,7 +137,7 @@ target_link_libraries(rosplane_estimator_node
   ${YAML_CPP_LIBRARIES}
 )
 ament_target_dependencies(rosplane_estimator_node rosplane_msgs rosflight_msgs rclcpp Eigen3)
-target_link_libraries(rosplane_estimator_node param_manager)
+target_link_libraries(rosplane_estimator_node param_manager logger)
 install(TARGETS
   rosplane_estimator_node
   DESTINATION lib/${PROJECT_NAME})

rosplane/include/controller_base.hpp

@@ -15,7 +15,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rosflight_msgs/msg/command.hpp>
 
-#include "param_manager.hpp"
+#include "param_manager_ros.hpp"
 #include "rosplane_msgs/msg/controller_commands.hpp"
 #include "rosplane_msgs/msg/controller_internals.hpp"
 #include "rosplane_msgs/msg/state.hpp"
@@ -101,7 +101,7 @@ class ControllerBase : public rclcpp::Node
   /**
    * Parameter manager object. Contains helper functions to interface parameters with ROS.
   */
-  ParamManager params_;
+  ParamManagerROS params_;
 
   /**
    * Interface for control algorithm.

rosplane/include/estimator_continuous_discrete.hpp

@@ -1,25 +1,22 @@
 #ifndef ESTIMATOR_CONTINUOUS_DISCRETE_H
 #define ESTIMATOR_CONTINUOUS_DISCRETE_H
 
-#include <math.h>
-
 #include <Eigen/Geometry>
-#include <yaml-cpp/yaml.h>
 
 #include "estimator_ekf.hpp"
-#include "estimator_ros.hpp"
+#include "param_manager/param_manager_interface.hpp"
+#include "logger/logger_interface.hpp"
 
 namespace rosplane
 {
 
 class EstimatorContinuousDiscrete : public EstimatorEKF
 {
 public:
-  EstimatorContinuousDiscrete();
-  EstimatorContinuousDiscrete(bool use_params);
+  EstimatorContinuousDiscrete(ParamMangerInterface * params, LoggerInterface * logger);
 
 private:
-  virtual void estimate(const Input & input, Output & output);
+  void estimate(const Input & input, Output & output) override;
 
   double lpf_a_;
   float alpha_;
@@ -138,6 +135,9 @@ class EstimatorContinuousDiscrete : public EstimatorEKF
    * @brief Initializes the state covariance matrix with the ROS2 parameters
    */
   void initialize_state_covariances();
+
+  ParamMangerInterface * params_;
+  LoggerInterface * logger_;
 };
 
 } // namespace rosplane

rosplane/include/estimator_ekf.hpp

@@ -8,17 +8,17 @@
 #include <Eigen/Geometry>
 #include <yaml-cpp/yaml.h>
 
-#include "estimator_ros.hpp"
+#include "estimator_interface.hpp"
+#include "param_manager/param_manager_interface.hpp"
 
 namespace rosplane
 {
 
-class EstimatorEKF : public EstimatorROS
+class EstimatorEKF : public EstimatorInterface
 {
 public:
-  EstimatorEKF();
+  EstimatorEKF(ParamMangerInterface * params);
 
-protected:
   std::tuple<Eigen::MatrixXf, Eigen::VectorXf> measurement_update(
     Eigen::VectorXf x, Eigen::VectorXf inputs,
     std::function<Eigen::VectorXf(const Eigen::VectorXf, const Eigen::VectorXf)> measurement_model,
@@ -41,7 +41,8 @@ class EstimatorEKF : public EstimatorROS
                             Eigen::VectorXf x, Eigen::MatrixXf P);
 
 private:
-  virtual void estimate(const Input & input, Output & output) override = 0;
+  ParamMangerInterface * params_;
+
 };
 
 } // namespace rosplane

rosplane/include/estimator_interface.hpp

@@ -0,0 +1,55 @@
+#ifndef ESTIMATOR_INTERFACE_HPP
+#define ESTIMATOR_INTERFACE_HPP
+
+namespace rosplane
+{
+class EstimatorInterface
+{
+public:
+  virtual ~EstimatorInterface() = default;
+
+  struct Input
+  {
+    float gyro_x;
+    float gyro_y;
+    float gyro_z;
+    float accel_x;
+    float accel_y;
+    float accel_z;
+    float static_pres;
+    float diff_pres;
+    bool gps_new;
+    float gps_n;
+    float gps_e;
+    float gps_h;
+    float gps_Vg;
+    float gps_course;
+    bool status_armed;
+    bool armed_init;
+  };
+
+  struct Output
+  {
+    float pn;
+    float pe;
+    float h;
+    float va;
+    float alpha;
+    float beta;
+    float phi;
+    float theta;
+    float psi;
+    float chi;
+    float p;
+    float q;
+    float r;
+    float Vg;
+    float wn;
+    float we;
+  };
+
+  virtual void estimate(const Input & input, Output & output) = 0;
+};
+} // namespace rosplane
+
+#endif //ESTIMATOR_INTERFACE_HPP

rosplane/include/estimator_ros.hpp

@@ -21,14 +21,13 @@
 #include <rosflight_msgs/msg/status.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 #include <sensor_msgs/msg/nav_sat_fix.hpp>
-#include <yaml-cpp/yaml.h>
 
-#include "param_manager.hpp"
+#include "estimator_interface.hpp"
+#include "param_manager_ros.hpp"
 #include "rosplane_msgs/msg/state.hpp"
 
 #define EARTH_RADIUS 6378145.0f
 
-using std::placeholders::_1;
 using namespace std::chrono_literals;
 
 namespace rosplane
@@ -37,61 +36,19 @@ namespace rosplane
 class EstimatorROS : public rclcpp::Node
 {
 public:
-  EstimatorROS();
-
-protected:
-  struct Input
-  {
-    float gyro_x;
-    float gyro_y;
-    float gyro_z;
-    float accel_x;
-    float accel_y;
-    float accel_z;
-    float static_pres;
-    float diff_pres;
-    bool gps_new;
-    float gps_n;
-    float gps_e;
-    float gps_h;
-    float gps_Vg;
-    float gps_course;
-    bool status_armed;
-    bool armed_init;
-  };
-
-  struct Output
-  {
-    float pn;
-    float pe;
-    float h;
-    float va;
-    float alpha;
-    float beta;
-    float phi;
-    float theta;
-    float psi;
-    float chi;
-    float p;
-    float q;
-    float r;
-    float Vg;
-    float wn;
-    float we;
-  };
-
-  bool baro_init_; /**< Initial barometric pressure */
-
-  virtual void estimate(const Input & input, Output & output) = 0;
-
-  ParamManager params_;
+  EstimatorROS(bool use_seeding_params);
+
+private:
+  EstimatorInterface * estimator_;
+  ParamManagerROS params_;
+
+  bool baro_init_;
   bool gps_init_;
-  double init_lat_ = 0.0; /**< Initial latitude in degrees */
-  double init_lon_ = 0.0; /**< Initial longitude in degrees */
-  float init_alt_ = 0.0;  /**< Initial altitude in meters above MSL  */
+  float init_lat_; /**< Initial latitude in degrees */
+  float init_lon_; /**< Initial longitude in degrees */
+  float init_alt_;  /**< Initial altitude in meters above MSL  */
   float init_static_;     /**< Initial static pressure (mbar)  */
 
-private:
   rclcpp::Publisher<rosplane_msgs::msg::State>::SharedPtr vehicle_state_pub_;
   rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gnss_fix_sub_;
   rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr
@@ -158,7 +115,7 @@ class EstimatorROS : public rclcpp::Node
   rcl_interfaces::msg::SetParametersResult
   parametersCallback(const std::vector<rclcpp::Parameter> & parameters);
 
-  Input input_;
+  EstimatorInterface::Input input_;
 };
 
 } // namespace rosplane

rosplane/include/logger/logger_interface.hpp

@@ -0,0 +1,21 @@
+#ifndef LOGGER_INTERFACE_HPP
+#define LOGGER_INTERFACE_HPP
+
+#include <string>
+
+namespace rosplane
+{
+class LoggerInterface
+{
+public:
+  virtual ~LoggerInterface() = default;
+
+  virtual void debug(const std::string & message) = 0;
+  virtual void info(const std::string & message) = 0;
+  virtual void warn(const std::string & message) = 0;
+  virtual void error(const std::string & message) = 0;
+  virtual void fatal(const std::string & message) = 0;
+};
+}
+
+#endif //LOGGER_INTERFACE_HPP

rosplane/include/logger/logger_ros.hpp

@@ -0,0 +1,26 @@
+#ifndef LOGGER_ROS_HPP
+#define LOGGER_ROS_HPP
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "logger_interface.hpp"
+
+namespace rosplane
+{
+class LoggerROS : public LoggerInterface
+{
+public:
+  LoggerROS(rclcpp::Node & node);
+
+  void debug(const std::string& message) override;
+  void info(const std::string& message) override;
+  void warn(const std::string& message) override;
+  void error(const std::string& message) override;
+  void fatal(const std::string& message) override;
+
+private:
+  rclcpp::Node & node_;
+};
+} // namespace rosplane
+
+#endif //LOGGER_ROS_HPP