base.h

Go to the documentation of this file.

#ifndef BASE_H
#define BASE_H

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Image.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <image_transport/subscriber_filter.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <libhaloc/lc.h>
#include "common/pose.h"
#include "common/graph.h"
#include "common/tools.h"

using namespace std;
using namespace cv;
using namespace tools;

typedef pcl::PointXYZRGB                  PointRGB;
typedef pcl::PointCloud<PointRGB>         PointCloud;

namespace slam
{

class SlamBase
{

public:

        // Constructor
  SlamBase(ros::NodeHandle nh, ros::NodeHandle nhp);

  // Finalize stereo slam node
  void finalize();

  struct Params
  {
    // Motion parameters
    double min_displacement;         
    bool save_clouds;                
    string clouds_dir;               

    // Default settings
    Params () {
      min_displacement            = 0.2;
      save_clouds                 = false;
      clouds_dir                  = "";
    }
  };

  inline void setParams(const Params& params)
  {
    params_ = params;
  }

  inline Params params() const { return params_; }

protected:

        // Node handlers
        ros::NodeHandle nh_;
  ros::NodeHandle nh_private_;

  // Protected functions and callbacks
  void init();
  void readParameters();
  void msgsCallback(const nav_msgs::Odometry::ConstPtr& odom_msg,
                    const sensor_msgs::ImageConstPtr& l_img_msg,
                    const sensor_msgs::ImageConstPtr& r_img_msg,
                    const sensor_msgs::CameraInfoConstPtr& l_info_msg,
                    const sensor_msgs::CameraInfoConstPtr& r_info_msg);
  void msgsCallback(const nav_msgs::Odometry::ConstPtr& odom_msg,
                    const sensor_msgs::ImageConstPtr& l_img_msg,
                    const sensor_msgs::ImageConstPtr& r_img_msg,
                    const sensor_msgs::CameraInfoConstPtr& l_info_msg,
                    const sensor_msgs::CameraInfoConstPtr& r_info_msg,
                    const sensor_msgs::PointCloud2ConstPtr& cloud_msg);

private:

  // Topic properties
  image_transport::SubscriberFilter left_sub_, right_sub_;
  message_filters::Subscriber<sensor_msgs::CameraInfo> left_info_sub_, right_info_sub_;
  message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;
  message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub_;

  // Topic sync properties (no pointcloud)
  typedef message_filters::sync_policies::ExactTime<nav_msgs::Odometry,
                                                    sensor_msgs::Image,
                                                    sensor_msgs::Image,
                                                    sensor_msgs::CameraInfo,
                                                    sensor_msgs::CameraInfo> ExactPolicyNoCloud;
  typedef message_filters::Synchronizer<ExactPolicyNoCloud> ExactSyncNoCloud;
  boost::shared_ptr<ExactSyncNoCloud> exact_sync_no_cloud_;

  // Topic sync properties (with pointcloud)
  typedef message_filters::sync_policies::ExactTime<nav_msgs::Odometry,
                                                    sensor_msgs::Image,
                                                    sensor_msgs::Image,
                                                    sensor_msgs::CameraInfo,
                                                    sensor_msgs::CameraInfo,
                                                    sensor_msgs::PointCloud2> ExactPolicyCloud;
  typedef message_filters::Synchronizer<ExactPolicyCloud> ExactSyncCloud;
  boost::shared_ptr<ExactSyncCloud> exact_sync_cloud_;

  Params params_;                   
  haloc::LoopClosure lc_;           
  slam::Pose pose_;                 
  slam::Graph graph_;               
  bool first_iter_;                 
  PointCloud pcl_cloud_;            
};

} // namespace

#endif // BASE_H