odom_estimation_node.cpp

Go to the documentation of this file.

 
 
 
// Original Author of FLOAM: Wang Han 
// Email wh200720041@gmail.com
// Homepage https://wanghan.pro
 
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <message_filters/subscriber.h>
#include <nav_msgs/Odometry.h>
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2_ros/transform_broadcaster.h"
 
//pcl lib
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
 
//local lib
#include "floam/odom_estimation_node.hpp"
#include "floam/odom_estimation.hpp"
 
namespace floam
{
namespace odom
{
 
OdomEstimationNode::OdomEstimationNode()
{
  // constructor
}
 
OdomEstimationNode::~OdomEstimationNode()
{
  // destructor
}
 
void OdomEstimationNode::onInit()
{
  m_nodeHandle = getPrivateNodeHandle();
 
  m_nodeHandle.getParam("use_exact_sync", m_useExactSync);
  m_nodeHandle.getParam("queue_size", m_queueSize);
  m_nodeHandle.getParam("map_resolution", m_mapResolution);
  m_nodeHandle.getParam("frame_id", m_frameId);
  m_nodeHandle.getParam("parent_frame_id", m_parentFrameId);
 
  m_odomEstimation.init(m_mapResolution);
 
 
  ROS_INFO_STREAM(m_nodeHandle.getNamespace() << "/frame_id: " << m_frameId);
  ROS_INFO_STREAM(m_nodeHandle.getNamespace() << "/parent_frame_id: " << m_parentFrameId);
  ROS_INFO_STREAM(m_nodeHandle.getNamespace() << "/use_exact_sync: " << m_useExactSync);
  ROS_INFO_STREAM(m_nodeHandle.getNamespace() << "/queue_size: " << m_queueSize);
  ROS_INFO_STREAM(m_nodeHandle.getNamespace() << "/map_resolution: " << m_mapResolution);
 
  m_tfGlobal.reset(new geometry_msgs::TransformStamped());
 
  m_tfGlobal->child_frame_id = m_frameId;
 
  // should these topic names be parameters instead of remapped?
  m_subEdges.subscribe(m_nodeHandle, "points_edge", 100);
  m_subSurfaces.subscribe(m_nodeHandle, "points_surface", 100);
 
  m_pubLidarOdometry = m_nodeHandle.advertise<nav_msgs::Odometry>("odom", 100);
  
  // initialize callbacks using sync policy
  if (m_useExactSync) {
    ROS_INFO("Exact Synchronization Policy chosen");
    m_exactSync.reset(new ExactSynchronizer(ExactSyncPolicy(m_queueSize), m_subEdges, m_subSurfaces));
    m_exactSync->registerCallback(
      std::bind(&OdomEstimationNode::handleClouds, this, std::placeholders::_1, std::placeholders::_2));
  } else {
    ROS_INFO("Approximate Synchronization Policy chosen");
    m_approximateSync.reset(new ApproximateSynchronizer(ApproximateSyncPolicy(m_queueSize), m_subEdges, m_subSurfaces));
    m_approximateSync->registerCallback(
      std::bind(&OdomEstimationNode::handleClouds, this, std::placeholders::_1, std::placeholders::_2));
  }
}
 
void OdomEstimationNode::handleClouds(
  const sensor_msgs::PointCloud2ConstPtr & edges_msg,
  const sensor_msgs::PointCloud2ConstPtr & surfaces_msg)
{
  // convert to PCL msgs
  pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud_surf_in(new pcl::PointCloud<pcl::PointXYZ>());
  pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud_edge_in(new pcl::PointCloud<pcl::PointXYZ>());
  pcl::fromROSMsg(*edges_msg, *pointcloud_edge_in);
  pcl::fromROSMsg(*surfaces_msg, *pointcloud_surf_in);
 
  // get timestamp from edges pointcloud msg, should (always) be the same as
  ros::Time pointcloud_time = edges_msg->header.stamp;
 
  // check if odometry is initialized
  if (m_isInitialized == false) {
    m_odomEstimation.initMapWithPoints(pointcloud_edge_in, pointcloud_surf_in);
    m_isInitialized = true;
    ROS_INFO("odometry initialized");  // should only be called once
  } else {
    std::chrono::time_point<std::chrono::system_clock> start, end;
    start = std::chrono::system_clock::now();
    m_odomEstimation.updatePointsToMap(pointcloud_edge_in, pointcloud_surf_in);
    end = std::chrono::system_clock::now();
    // calculate elapsted time
    std::chrono::duration<float> elapsed_seconds = end - start;
    m_totals.frames++;
    float time_temp = elapsed_seconds.count() * 1000;
    m_totals.time += time_temp;
    ROS_INFO("average odom estimation time %f ms", m_totals.time / m_totals.frames);
  }
 
  m_tfGlobal->transform.translation.x = m_odomEstimation.m_currentTranslation.x();
  m_tfGlobal->transform.translation.y = m_odomEstimation.m_currentTranslation.y();
  m_tfGlobal->transform.translation.z = m_odomEstimation.m_currentTranslation.z();
 
  m_tfGlobal->transform.rotation.x = m_odomEstimation.m_currentRotation.x();
  m_tfGlobal->transform.rotation.y = m_odomEstimation.m_currentRotation.y();
  m_tfGlobal->transform.rotation.z = m_odomEstimation.m_currentRotation.z();
  m_tfGlobal->transform.rotation.w = m_odomEstimation.m_currentRotation.w();
 
  static tf2_ros::TransformBroadcaster br;
  m_tfGlobal->header.stamp = edges_msg->header.stamp;
  m_tfGlobal->header.frame_id = m_parentFrameId;
  br.sendTransform(*m_tfGlobal);
 
  // publish odometry
  nav_msgs::Odometry lidarOdometry;
  lidarOdometry.header.frame_id = m_parentFrameId;
  lidarOdometry.child_frame_id = m_frameId;
  lidarOdometry.header.stamp = pointcloud_time;
  lidarOdometry.pose.pose.orientation.x = m_tfGlobal->transform.rotation.x;
  lidarOdometry.pose.pose.orientation.y = m_tfGlobal->transform.rotation.y;
  lidarOdometry.pose.pose.orientation.z = m_tfGlobal->transform.rotation.z;
  lidarOdometry.pose.pose.orientation.w = m_tfGlobal->transform.rotation.w;
  lidarOdometry.pose.pose.position.x = m_tfGlobal->transform.translation.x;
  lidarOdometry.pose.pose.position.y = m_tfGlobal->transform.translation.y;
  lidarOdometry.pose.pose.position.z = m_tfGlobal->transform.translation.z;
  m_pubLidarOdometry.publish(lidarOdometry);
}
 
}  // namespace odom
}  // namespace floam
 
#include <pluginlib/class_list_macros.h>  // NO LINT
PLUGINLIB_EXPORT_CLASS(floam::odom::OdomEstimationNode, nodelet::Nodelet)