odom_estimation_node.hpp

Go to the documentation of this file.

 
 
// Original Author of FLOAM: Wang Han 
// Email wh200720041@gmail.com
// Homepage https://wanghan.pro
 
#ifndef FLOAM__ODOM_ESTIMATION_NODE_HPP_
#define FLOAM__ODOM_ESTIMATION_NODE_HPP_
 
#include "ros/ros.h"
#include "nodelet/nodelet.h"
#include "sensor_msgs/PointCloud2.h"
#include "message_filters/synchronizer.h"
#include "message_filters/sync_policies/exact_time.h"
#include "message_filters/sync_policies/approximate_time.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
 
 
#include "floam/lidar_utils.hpp"
#include "floam/odom_estimation.hpp"
 
namespace floam
{
namespace odom
{
 
typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ExactSyncPolicy;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ApproximateSyncPolicy;
 
class OdomEstimationNode : public nodelet::Nodelet
{
public:
  OdomEstimationNode();
 
  ~OdomEstimationNode();
 
  void onInit();
 
private:
  ros::NodeHandle m_nodeHandle;
 
  ros::Publisher m_pubLidarOdometry;
 
  geometry_msgs::TransformStampedPtr m_tfGlobal;
 
  typedef message_filters::Synchronizer<ApproximateSyncPolicy> ApproximateSynchronizer;
  typedef message_filters::Synchronizer<ExactSyncPolicy> ExactSynchronizer;
 
  message_filters::Subscriber<sensor_msgs::PointCloud2> m_subEdges;
  message_filters::Subscriber<sensor_msgs::PointCloud2> m_subSurfaces;
 
  std::shared_ptr<ApproximateSynchronizer> m_approximateSync;
  std::shared_ptr<ExactSynchronizer> m_exactSync;
 
  void handleClouds(
    const sensor_msgs::PointCloud2ConstPtr & edges,
    const sensor_msgs::PointCloud2ConstPtr & surfaces);
 
  bool m_isInitialized = false;
  bool m_useExactSync = false;
 
  int m_queueSize = 5;
 
  double m_mapResolution = 0.4;
 
  std::string m_frameId, m_parentFrameId;
 
  floam::lidar::Total m_totals;
 
private:
  floam::odom::OdomEstimation m_odomEstimation;
};
 
}  // namespace odom
}  // namespace floam
 
#endif  // FLOAM__ODOM_ESTIMATION_NODE_HPP_