msceqf_ros.hpp

// Copyright (C) 2023 Alessandro Fornasier.
// Control of Networked Systems, University of Klagenfurt, Austria.
//
// All rights reserved.
//
// This software is licensed under the terms of the Apache License, Version 2.0
// (the "License"); you may not use this file except in compliance with the
// License. You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations
// under the License.
//
// You can contact the authors at <alessandro.fornasier@ieee.org>
 
#ifndef MSCEQF_ROS_H
#define MSCEQF_ROS_H
 
#include <ros/ros.h>
#include <Eigen/Eigen>
#include <atomic>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/PointCloud.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Path.h>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <rosbag/bag.h>
 
#include "msceqf/msceqf.hpp"
 
class MSCEqFRos
{
 public:
  MSCEqFRos(const ros::NodeHandle &nh,
            const std::string &msceqf_config_filepath,
            const std::string &imu_topic,
            const std::string &cam_topic,
            const std::string &pose_topic,
            const std::string &path_topic,
            const std::string &image_topic,
            const std::string &extrinsics_topic,
            const std::string &intrinsics_topic,
            const std::string &origin_topic,
            const bool &record,
            const std::string &bagfile);

  void callback_image(const sensor_msgs::Image::ConstPtr &msg);

  void callback_imu(const sensor_msgs::Imu::ConstPtr &msg);
 
 private:
  void publish(const msceqf::Camera &cam);
 
  ros::NodeHandle nh_;  
 
  msceqf::MSCEqF sys_;  
 
  ros::Subscriber sub_cam_;  
  ros::Subscriber sub_imu_;  
 
  ros::Publisher pub_pose_;        
  ros::Publisher pub_image_;       
  ros::Publisher pub_path_;        
  ros::Publisher pub_extrinsics_;  
  ros::Publisher pub_intrinsics_;  
  ros::Publisher pub_origin_;      
 
  geometry_msgs::PoseWithCovarianceStamped pose_;  
  nav_msgs::Path path_;                            
  geometry_msgs::PoseStamped extrinsics_;          
  sensor_msgs::CameraInfo intrinsics_;             
  geometry_msgs::PoseStamped origin_;              
 
  std::deque<msceqf::Camera> cams_;       
  std::mutex mutex_;                      
  std::atomic<bool> processing_ = false;  
 
  bool record_;      
  rosbag::Bag bag_;  
 
  uint seq_ = 0;  
};
 
#endif  // MSCEQF_ROS_H