mrpt_rbpf_slam_wrapper.h

Go to the documentation of this file.

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

#pragma once

#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>

#include <mrpt/obs/CObservationBeaconRanges.h>

namespace mrpt_rbpf_slam
{
class PFslamWrapper : public PFslam
{
public:
  PFslamWrapper();
  ~PFslamWrapper() = default;

  bool getParams(const ros::NodeHandle& nh_p);

  bool init(ros::NodeHandle& nh);

  bool rawlogPlay();

  void publishMapPose();

  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(mrpt::obs::CObservationOdometry::Ptr& odometry, const std_msgs::Header& msg_header);

  void updateSensorPose(const std::string& frame_id);

  void publishTF();

  void vizBeacons();

private:
  double rawlog_play_delay_;   
  bool rawlog_play_{ false };  

  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<mrpt::obs::CActionCollection, mrpt::obs::CSensoryFrame>> data_;  



  std::vector<mrpt::opengl::CEllipsoid::Ptr> viz_beacons_;

  ros::Publisher pub_map_, pub_metadata_, pub_particles_, pub_particles_beacons_,
      beacon_viz_pub_;  

  tf::TransformListener listenerTF_;         
  tf::TransformBroadcaster tf_broadcaster_;  
#if MRPT_VERSION >= 0x199
  mrpt::system::CTicTac tictac_;  
#else
  mrpt::utils::CTicTac tictac_;
#endif
  float t_exec_;  
};
}  // namespace mrpt_rbpf_slam