navigator.h

Go to the documentation of this file.

#ifndef CCNY_RGBD_KEYFRAME_MAPPER_H
#define CCNY_RGBD_KEYFRAME_MAPPER_H

#include <iostream>
#include <fstream>
#include <ros/ros.h>
#include <ros/publisher.h>
#include <pcl/point_cloud.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl/filters/passthrough.h>
#include <tf/transform_listener.h>
#include <visualization_msgs/Marker.h>
#include <boost/regex.hpp>
#include <boost/filesystem.hpp>
#include <octomap/octomap.h>
#include <octomap/OcTree.h>
#include <octomap/ColorOcTree.h>
#include <rgbdtools/rgbdtools.h>

#include "ccny_rgbd/types.h"
#include "ccny_rgbd/util.h"
#include "ccny_rgbd/GenerateGraph.h"
#include "ccny_rgbd/SolveGraph.h"
#include "ccny_rgbd/AddManualKeyframe.h"
#include "ccny_rgbd/PublishKeyframe.h"
#include "ccny_rgbd/PublishKeyframes.h"
#include "ccny_rgbd/Save.h"
#include "ccny_rgbd/Load.h"

namespace ccny_rgbd {

class Navigator
{
  public:

    Navigator(const ros::NodeHandle& nh,
                   const ros::NodeHandle& nh_private);
    
    virtual ~Navigator();

    void initParams();

    bool publishKeyframesSrvCallback(
      PublishKeyframes::Request& request,
      PublishKeyframes::Response& response);

    bool publishKeyframeSrvCallback(
      PublishKeyframe::Request& request,
      PublishKeyframe::Response& response);

    bool saveKeyframesSrvCallback(
      Save::Request& request,
      Save::Response& response);

    bool savePcdMapSrvCallback(
      Save::Request& request,
      Save::Response& response);
    
    bool saveOctomapSrvCallback(
      Save::Request& request,
      Save::Response& response);
    
    bool loadKeyframesSrvCallback(
      Load::Request& request,
      Load::Response& response);

    bool addManualKeyframeSrvCallback(
      AddManualKeyframe::Request& request,
      AddManualKeyframe::Response& response);
    
     bool generateGraphSrvCallback(
      GenerateGraph::Request& request,
      GenerateGraph::Response& response);

    bool solveGraphSrvCallback(
      SolveGraph::Request& request,
      SolveGraph::Response& response);
    
  protected:
    std::vector<std::vector<double> > aruco_pos;
    std::vector<int> aruco_kfs;
    ros::NodeHandle nh_;          
    ros::NodeHandle nh_private_;  
    
    std::string fixed_frame_;     
    
    int queue_size_;  
    
    double max_range_;  
    double max_stdev_;  

    rgbdtools::KeyframeVector keyframes_;    
    
    virtual void RGBDCallback(const ImageMsg::ConstPtr& rgb_msg,
                              const ImageMsg::ConstPtr& depth_msg,
                              const CameraInfoMsg::ConstPtr& info_msg);

  private:

    ros::Publisher keyframes_pub_;    
    ros::Publisher poses_pub_;        
    ros::Publisher kf_assoc_pub_;     
    ros::Publisher path_pub_;         
    ros::Publisher aruco_pub_;
    
    ros::ServiceServer generate_graph_service_;
    
    ros::ServiceServer solve_graph_service_;   
    
    ros::ServiceServer pub_keyframe_service_;
    
    ros::ServiceServer pub_keyframes_service_;
    
    ros::ServiceServer save_kf_service_;
    
    ros::ServiceServer save_pcd_map_service_;
    
    ros::ServiceServer save_octomap_service_;
    
    ros::ServiceServer load_kf_service_;
    
    ros::ServiceServer add_manual_keyframe_service_;

    tf::TransformListener tf_listener_; 

    boost::shared_ptr<ImageTransport> rgb_it_;
    
    boost::shared_ptr<ImageTransport> depth_it_;
    
    boost::shared_ptr<RGBDSynchronizer3> sync_;
          
    ImageSubFilter      sub_rgb_;
    
    ImageSubFilter      sub_depth_;  
   
    CameraInfoSubFilter sub_info_;
    
    // params
    double pcd_map_res_; 
    double octomap_res_;  
    double kf_dist_eps_;  
    double kf_angle_eps_; 
    bool octomap_with_color_; 
    double max_map_z_;   
    double min_map_z_;   

    // state vars
    bool manual_add_;   

    int rgbd_frame_index_;

    rgbdtools::KeyframeGraphDetector graph_detector_;  
    rgbdtools::KeyframeGraphSolverG2O graph_solver_;    
    rgbdtools::KeyframeGraphSolverISAM solver_;
    rgbdtools::KeyframeAssociationVector associations_; 
    
    PathMsg path_msg_;    
    PathMsg aruco_path;
    bool processFrame(const rgbdtools::RGBDFrame& frame, const AffineTransform& pose);
    
    void addKeyframe(const rgbdtools::RGBDFrame& frame, const AffineTransform& pose);

    void publishKeyframeData(int i);
    
    void publishKeyframePose(int i);
        
    void publishKeyframeAssociations();
    
    void publishKeyframePoses();
    void publishArucoPos();

    void publishPath();
    
    bool savePcdMap(const std::string& path);
           
    void buildPcdMap(PointCloudT& map_cloud);
    void buildFullCloud(PointCloudT& map_cloud);

    bool saveOctomap(const std::string& path);
    
    
    void buildColorOctomap(octomap::ColorOcTree& tree);
        
    static inline octomap::pose6d poseTfToOctomap(
      const tf::Pose& poseTf)
    {
      return octomap::pose6d(
              pointTfToOctomap(poseTf.getOrigin()),
              quaternionTfToOctomap(poseTf.getRotation()));
    }
   
    static inline octomap::point3d pointTfToOctomap(
      const tf::Point& ptTf)
    {
      return octomap::point3d(ptTf.x(), ptTf.y(), ptTf.z());
    }
   
    static inline octomath::Quaternion quaternionTfToOctomap(
      const tf::Quaternion& qTf)
    {
      return octomath::Quaternion(qTf.w(), qTf.x(), qTf.y(), qTf.z());
    }
    
    bool savePath(const std::string& filepath);
    bool savePathTUMFormat(const std::string& filepath);
    void filter_aruco();
    bool loadPath(const std::string& filepath);
    
    void updatePathFromKeyframePoses();
};

} // namespace ccny_rgbd

#endif // CCNY_RGBD_KEYFRAME_MAPPER_H