mrpt_ekf_slam_3d_wrapper.h

Go to the documentation of this file.

/*
 * File: mrpt_ekf_slam_3d_wrapper.h
 * Author: Vladislav Tananaev
 *
 */

#pragma once
#include <iostream>  // std::cout
#include <fstream>   // std::ifstream
#include <string>
#include "mrpt_ekf_slam_3d/mrpt_ekf_slam_3d.h"
// add ros libraries
#include <ros/ros.h>
#include <ros/package.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
//#include <tf_conversions/tf_eigen.h>
#include "Eigen/Core"
#include "Eigen/Geometry"
// add ros msgs
#include <std_msgs/String.h>
#include <std_msgs/Int32.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>
// mrpt bridge libs
#include <mrpt_bridge/pose.h>
#include <mrpt_bridge/landmark.h>
#include <mrpt_bridge/mrpt_log_macros.h>
#include <mrpt_bridge/time.h>
#include <mrpt/utils/CFileGZInputStream.h>
#include <mrpt/utils/CFileGZOutputStream.h>
#include <mrpt/utils/CConfigFile.h>
#include <mrpt/random.h>

#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/poses/CPose3DPDF.h>
#include <mrpt/opengl/CSetOfLines.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CEllipsoid.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt_msgs/ObservationRangeBearing.h>

#if MRPT_VERSION >= 0x130
#include <mrpt/obs/CActionRobotMovement2D.h>
#include <mrpt/obs/CActionRobotMovement3D.h>
#include <mrpt/obs/CObservationOdometry.h>
#include <mrpt/obs/CRawlog.h>
using namespace mrpt::maps;
using namespace mrpt::obs;
#else
#include <mrpt/slam/CActionRobotMovement2D.h>
#include <mrpt/slam/CActionRobotMovement3D.h>
#include <mrpt/slam/CObservationOdometry.h>
#include <mrpt/slam/CRawlog.h>
#endif

class EKFslamWrapper : EKFslam
{
public:
  EKFslamWrapper();
  ~EKFslamWrapper();
  void get_param();
  void makeRightHanded(Eigen::Matrix3d& eigenvectors, Eigen::Vector3d& eigenvalues);

  void computeEllipseOrientationScale(tf::Quaternion& orientation, Eigen::Vector3d& scale,
                                      const mrpt::math::CMatrixDouble covariance);
  void init();
  bool rawlogPlay();
  bool is_file_exists(const std::string& name);
  void viz_state();
  void viz_dataAssociation();
  void odometryForCallback(CObservationOdometry::Ptr& _odometry, const std_msgs::Header& _msg_header);
  void landmarkCallback(const mrpt_msgs::ObservationRangeBearing& _msg);
  void updateSensorPose(std::string _frame_id);
  bool waitForTransform(mrpt::poses::CPose3D& des, const std::string& target_frame, const std::string& source_frame,
                        const ros::Time& time, const ros::Duration& timeout,
                        const ros::Duration& polling_sleep_duration = ros::Duration(0.01));
  void publishTF();

private:
  ros::NodeHandle n_;        
  double rawlog_play_delay;  
  double ellipse_scale_;
  bool rawlog_play_;  
  // Subscribers
  std::vector<ros::Subscriber> sensorSub_;  
  std::string rawlog_filename;              
  std::string ini_filename;                 
  std::string global_frame_id;              
  std::string odom_frame_id;                
  std::string base_frame_id;                

  // Sensor source
  std::string sensor_source;  

  std::map<std::string, mrpt::poses::CPose3D> landmark_poses_;  

  CTicTac tictac;  
  float t_exec;    

  ros::Publisher data_association_viz_pub_, state_viz_pub_;
  tf::TransformListener listenerTF_;         
  tf::TransformBroadcaster tf_broadcaster_;  
};