slam_solver.hpp

Go to the documentation of this file.

#pragma once
#include <memory>
 
#include "common_lib/competition_logic/mission_logic.hpp"
#include "common_lib/structures/cone.hpp"
#include "common_lib/structures/pose.hpp"
#include "common_lib/structures/position.hpp"
#include "common_lib/structures/velocities.hpp"
#include "motion_lib/v2p_models/base_v2p_motion_model.hpp"
#include "perception_sensor_lib/data_association/base_data_association.hpp"
#include "perception_sensor_lib/landmark_filter/base_landmark_filter.hpp"
#include "perception_sensor_lib/loop_closure/loop_closure.hpp"
#include "rclcpp/rclcpp.hpp"
#include "slam_config/general_config.hpp"
#include "track_loader/track_loader.hpp"
 
class SLAMSolver {
protected:
  SLAMParameters _params_;
  std::shared_ptr<DataAssociationModel> _data_association_;  //< Data association pointer
  std::shared_ptr<V2PMotionModel> _motion_model_;            //< Motion model pointer
  std::shared_ptr<LandmarkFilter> _landmark_filter_;         //< Landmark filter pointer
  common_lib::competition_logic::Mission _mission_ = common_lib::competition_logic::Mission::NONE;
  std::shared_ptr<std::vector<double>>
      _execution_times_;                        //< Execution times: 0 -> total motion; 1 -> total
                                                // observation; the rest are solver specific
  std::shared_ptr<LoopClosure> _loop_closure_;  //< Loop closure object pointer
 
  rclcpp::Time _last_pose_update_ = rclcpp::Time(0);
  rclcpp::Time _last_observation_update_ = rclcpp::Time(0);
 
  bool _received_first_velocities_ =
      false;  //< Flag to check if the first velocities have been received
 
  int lap_counter_ = 0;  //< Lap counter for the graph SLAM solver
 
public:
  SLAMSolver(const SLAMParameters& params, std::shared_ptr<DataAssociationModel> data_association,
             std::shared_ptr<V2PMotionModel> motion_model,
             std::shared_ptr<LandmarkFilter> landmark_filter,
             std::shared_ptr<std::vector<double>> execution_times,
             std::shared_ptr<LoopClosure> loop_closure);
 
  virtual ~SLAMSolver() = default;
 
  virtual void add_observations(const std::vector<common_lib::structures::Cone>& cones,
                                rclcpp::Time cones_timestamp) = 0;
 
  virtual void add_velocities(const common_lib::structures::Velocities& velocities) = 0;
 
  virtual void load_initial_state(const Eigen::VectorXd& map, const Eigen::Vector3d& pose) = 0;
 
  virtual std::vector<common_lib::structures::Cone> get_map_estimate() = 0;
 
  virtual common_lib::structures::Pose get_pose_estimate() = 0;
 
  virtual Eigen::MatrixXd get_covariance() = 0;
 
  virtual int get_lap_counter() = 0;
 
  virtual Eigen::VectorXi get_associations() const = 0;
 
  virtual Eigen::VectorXd get_observations_global() const = 0;
 
  virtual Eigen::VectorXd get_map_coordinates() const = 0;
 
  void set_mission(common_lib::competition_logic::Mission mission);
};