mrpt_icp_slam_2d_wrapper.h

Go to the documentation of this file.

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

#ifndef MRPT_ICP_SLAM_2D_WRAPPER_H
#define MRPT_ICP_SLAM_2D_WRAPPER_H

// MRPT libraries
#include <mrpt/slam/CMetricMapBuilderICP.h>
#include <mrpt/opengl/COpenGLScene.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/utils/CConfigFile.h>
#include <mrpt/utils/CFileGZInputStream.h>
#include <mrpt/utils/CFileGZOutputStream.h>
#include <mrpt/system/os.h>
#include <mrpt/system/threads.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/opengl/CPlanarLaserScan.h>  // This class lives in the lib [mrpt-maps] and must be included by hand
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/poses/CPose3DPDF.h>

#include <mrpt/gui/CDisplayWindow3D.h>

#include <stdint.h>
#include <iostream>  // std::cout
#include <fstream>   // std::ifstream
#include <string>

// add ros libraries
#include <ros/ros.h>
#include <ros/package.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
// add ros msgs
#include <nav_msgs/OccupancyGrid.h>
#include "nav_msgs/MapMetaData.h"
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>
#include <std_msgs/Header.h>
#include <std_msgs/Int32.h>
#include <nav_msgs/GetMap.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>

// mrpt bridge libs
#include <mrpt_bridge/pose.h>
#include <mrpt_bridge/map.h>
#include <mrpt_bridge/mrpt_log_macros.h>
#include <mrpt_bridge/laser_scan.h>
#include <mrpt_bridge/time.h>
#include <mrpt_bridge/point_cloud.h>
#include <mrpt/version.h>
#if MRPT_VERSION >= 0x130
#include <mrpt/obs/CActionRobotMovement2D.h>
#include <mrpt/obs/CActionRobotMovement3D.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/obs/CObservationOdometry.h>
#include <mrpt/obs/CSensoryFrame.h>
#include <mrpt/maps/CMultiMetricMap.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/CActionCollection.h>
#include <mrpt/slam/CObservationOdometry.h>
#include <mrpt/slam/CSensoryFrame.h>
#include <mrpt/slam/CMultiMetricMap.h>
#include <mrpt/slam/CRawlog.h>
#endif
using namespace mrpt;
using namespace mrpt::slam;
using namespace mrpt::opengl;
using namespace mrpt::gui;
using namespace mrpt::system;
using namespace mrpt::math;
using namespace mrpt::utils;
using namespace mrpt::poses;
using namespace std;

class ICPslamWrapper
{
public:
  ICPslamWrapper();

  ~ICPslamWrapper();

  void read_iniFile(std::string ini_filename);
  void init3Dwindow();
  void run3Dwindow();

  void get_param();
  void init();
  bool rawlogPlay();
  bool is_file_exists(const std::string &name);
  void laserCallback(const sensor_msgs::LaserScan &_msg);
  void publishTF();
  void publishMapPose();
  void updateSensorPose(std::string _frame_id);
  void updateTrajectoryTimerCallback(const ros::TimerEvent& event);
  void publishTrajectoryTimerCallback(const ros::TimerEvent& event);

protected:
  CMetricMapBuilderICP mapBuilder;  
  ros::NodeHandle n_;               
  double rawlog_play_delay;         
  bool rawlog_play_;                

  std::string rawlog_filename;  
  std::string ini_filename;     
  std::string global_frame_id;  
  std::string odom_frame_id;    
  std::string base_frame_id;    
  geometry_msgs::PoseStamped pose; 

  ros::Publisher trajectory_pub_;  
  nav_msgs::Path path;             

  ros::Timer update_trajector_timer;   
  ros::Timer publish_trajectory_timer; 

  double trajectory_update_rate;    
  double trajectory_publish_rate;   

  // Sensor source
  std::string sensor_source;                                 
  std::map<std::string, mrpt::poses::CPose3D> laser_poses_;  

  // Subscribers
  std::vector<ros::Subscriber> sensorSub_;  

  const CMultiMetricMap *metric_map_;  
  // CPose3DPDFPtr curPDF;          ///<current robot pose
  ros::Publisher pub_map_, pub_metadata_, pub_pose_, pub_point_cloud_;  

  tf::TransformListener listenerTF_;         
  tf::TransformBroadcaster tf_broadcaster_;  

  CTicTac tictac;  
  float t_exec;    
  CSensoryFramePtr observations;
  CObservationPtr observation;
  mrpt::system::TTimeStamp timeLastUpdate_;  

  ros::Time stamp;  

  mrpt::gui::CDisplayWindow3DPtr win3D_;  

  std::vector<CObservation2DRangeScanPtr> lst_current_laser_scans;
  bool isObsBasedRawlog;
  bool SHOW_PROGRESS_3D_REAL_TIME;
  int SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS;
  bool SHOW_LASER_SCANS_3D;
  bool CAMERA_3DSCENE_FOLLOWS_ROBOT;
};

#endif /* MRPT_ICP_SLAM_2D_WRAPPER_H */