mrpt_rbpf_slam_wrapper.h

Go to the documentation of this file.

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

#ifndef MRPT_RBPF_SLAM_WRAPPER_H
#define MRPT_RBPF_SLAM_WRAPPER_H

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

// 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 <std_msgs/String.h>
#include <std_msgs/Int32.h>
#include <nav_msgs/GetMap.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <sensor_msgs/LaserScan.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>
// mrpt msgs
#include "mrpt_msgs/ObservationRangeBeacon.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/beacon.h>
#include <mrpt_bridge/time.h>

#if MRPT_VERSION >= 0x130
#include <mrpt/obs/CObservationBeaconRanges.h>
using namespace mrpt::obs;
#else
#include <mrpt/slam/CObservationBeaconRanges.h>
using namespace mrpt::slam;
#endif

class PFslamWrapper : PFslam
{
public:
  PFslamWrapper();
  ~PFslamWrapper();
  void get_param();
  void init();
  bool rawlogPlay();
  void publishMapPose();

  bool is_file_exists(const std::string& name);

  void callbackBeacon(const mrpt_msgs::ObservationRangeBeacon& _msg);

  void laserCallback(const sensor_msgs::LaserScan& _msg);

  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 odometryForCallback(CObservationOdometryPtr& _odometry, const std_msgs::Header& _msg_header);

  void updateSensorPose(std::string frame_id);

  void publishTF();
  void vizBeacons();

private:
  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;    

  // Sensor source
  std::string sensor_source;  

  std::map<std::string, mrpt::poses::CPose3D> laser_poses_;   
  std::map<std::string, mrpt::poses::CPose3D> beacon_poses_;  

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

  // read rawlog file
  std::vector<std::pair<CActionCollection, CSensoryFrame>> data;  


  std::vector<mrpt::opengl::CEllipsoidPtr> viz_beacons;

  ros::Publisher pub_map_, pub_metadata_, pub_Particles_, pub_Particles_Beacons_,
      beacon_viz_pub_;  

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

  CTicTac tictac;  
  float t_exec;    
};

#endif /*MRPT_RBPF_SLAM_WRAPPER_H*/