ekf_cal_node.hpp

// Copyright 2022 Jacob Hartzer
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <https://www.gnu.org/licenses/>.
 
#ifndef APPLICATION__ROS__NODE__EKF_CAL_NODE_HPP_
#define APPLICATION__ROS__NODE__EKF_CAL_NODE_HPP_
 
 
#include <map>
#include <memory>
#include <string>
#include <vector>
 
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>
 
#include "ekf/ekf.hpp"
#include "infrastructure/data_logger.hpp"
#include "infrastructure/debug_logger.hpp"
#include "sensors/camera.hpp"
#include "sensors/gps.hpp"
#include "sensors/imu.hpp"
#include "sensors/ros/ros_camera.hpp"
#include "sensors/ros/ros_gps.hpp"
#include "sensors/ros/ros_imu.hpp"
#include "trackers/feature_tracker.hpp"
 
class EkfCalNode : public rclcpp::Node
{
public:
  EkfCalNode();
 
  void Initialize();
 
  void LoadSensors();
 
  void LoadSensorParameters(
    Sensor::Parameters & params,
    const std::string & prefix,
    const std::string & name
  );
 
  void LoadImu(const std::string & imu_name);
 
  void LoadCamera(const std::string & camera_name);
 
  void LoadGps(const std::string & gps_name);
 
  Eigen::VectorXd LoadProcessNoise();
 
  IMU::Parameters GetImuParameters(const std::string & imu_name);
 
  Camera::Parameters GetCameraParameters(const std::string & camera_name);
 
  GPS::Parameters GetGpsParameters(const std::string & gps_name);
 
  void DeclareSensors();
 
  void DeclareSensorParameters(const std::string & sensor_name);
 
  void DeclareImuParameters(const std::string & imu_name);
 
  void DeclareCameraParameters(const std::string & camera_name);
 
  void DeclareTrackerParameters(const std::string & tracker_name);
 
  void DeclareIntrinsicParameters(const std::string & intrinsics_prefix);
 
  void DeclareFiducialParameters(const std::string & fid_name);
 
  Intrinsics GetIntrinsicParameters(const std::string & intrinsics_prefix);
 
  void DeclareGpsParameters(const std::string & gps_name);
 
  FeatureTracker::Parameters GetTrackerParameters(const std::string & tracker_name);
 
  FiducialTracker::Parameters GetFiducialParameters(const std::string & fiducial_name);
 
  void ImuCallback(const sensor_msgs::msg::Imu::SharedPtr msg, unsigned int id);
 
  void CameraCallback(const sensor_msgs::msg::Image::SharedPtr msg, unsigned int id);
 
  void GpsCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg, unsigned int id);
 
  void RegisterImu(std::shared_ptr<RosIMU> imu_ptr, const std::string & topic);
 
  void RegisterCamera(std::shared_ptr<RosCamera> camera_ptr, const std::string & topic);
 
  void RegisterGps(std::shared_ptr<RosGPS> gps_ptr, const std::string & topic);
 
  void PublishState();
 
private:
  std::vector<rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr> m_imu_subs;
 
  std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> m_camera_subs;
 
  std::vector<rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr> m_gps_subs;
 
  std::vector<std::string> m_imu_list {};
  std::vector<std::string> m_camera_list {};
  std::vector<std::string> m_tracker_list {};
  std::vector<std::string> m_fiducial_list {};
  std::vector<std::string> m_gps_list {};
 
  rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr m_body_state_pub;
  rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr m_imu_state_pub;
  rclcpp::TimerBase::SharedPtr m_state_pub_timer;
 
  std::shared_ptr<EKF> m_ekf;
  std::shared_ptr<DebugLogger> m_debug_logger;
  DataLogger m_state_data_logger;
 
  std::map<unsigned int, std::shared_ptr<RosIMU>> m_map_imu{};
  std::map<unsigned int, std::shared_ptr<RosCamera>> m_map_camera{};
  std::map<unsigned int, std::shared_ptr<RosGPS>> m_map_gps{};
  std::map<unsigned int,
    std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::Image>>> m_map_image_publishers{};
 
  std::string m_log_directory{"~/log/"};
};
 
#endif  // APPLICATION__ROS__NODE__EKF_CAL_NODE_HPP_