ts_pointcloud_ros.cpp

Go to the documentation of this file.

#include "toposens_pointcloud/ts_pointcloud_ros.h"

namespace toposens_pointcloud
{
TSPointCloudROS::TSPointCloudROS(ros::NodeHandle nh, ros::NodeHandle private_nh)
{
  const auto buffer_size = int{100};

  // Read parameters
  int pcd_save_interval{0};
  private_nh.param<std::string>("scans_topic", scans_topic_, "ts_scans");
  private_nh.param<std::string>("target_frame", target_frame_, "toposens");
  private_nh.param<float>("lifetime_normals_vis", lifetime_normals_vis_, 0.0);
  private_nh.param<int>("pcd_save_interval", pcd_save_interval, 0);
  ROS_INFO("Reading scans from \"%s\"", scans_topic_.c_str());  // NOLINT
  ROS_INFO("target_frame: \"%s\"", target_frame_.c_str());      // NOLINT
  ROS_INFO("lifetime_normals_vis: %f", lifetime_normals_vis_);  // NOLINT
  ROS_INFO("pcd_save_interval: %f", pcd_save_interval);         // NOLINT

  // Subscribers
  scans_sub_ = nh.subscribe(scans_topic_, buffer_size, &TSPointCloudROS::cloudCallback, this);
  tf2_listener_ = new tf2_ros::TransformListener(tf2_buffer_);

  // Publishers
  cloud_pub_ = nh.advertise<XYZINCloud>(kPointCloudTopic, buffer_size);
  if (lifetime_normals_vis_ != 0.0)
  {
    marker_pub_ =
        nh.advertise<visualization_msgs::Marker>(kPointCloudTopic + "_normals", buffer_size);
  }

  // Processing entities
  pointTransform_ = std::make_unique<Mapping>();
  if (pcd_save_interval > 0)
  {
    log_thread_ = new boost::thread(&Logging::logPointcloud);
  }
}

TSPointCloudROS::~TSPointCloudROS()
{
  delete tf2_listener_;

  if (log_thread_ != nullptr)
  {
    log_thread_->interrupt();
    log_thread_->join();
    delete log_thread_;
  }
}

void TSPointCloudROS::cloudCallback(const toposens_msgs::TsScan::ConstPtr &msg)
{
  XYZINCloud::Ptr transformed_cloud(new XYZINCloud);
  pcl_conversions::toPCL(msg->header.stamp, transformed_cloud->header.stamp);
  transformed_cloud->header.frame_id = target_frame_;
  transformed_cloud->height = 1;

  geometry_msgs::TransformStamped transform; 
  try
  {
    transform = tf2_buffer_.lookupTransform(target_frame_, msg->header.frame_id, msg->header.stamp,
                                            ros::Duration(0.5));

    for (auto it = msg->points.begin(); it != msg->points.end(); ++it)
    {
      if (it->intensity <= 0)
      {
        continue;
      }

      toposens_msgs::TsPoint msg_point = *it;
      pcl::PointXYZINormal pcl_point = pointTransform_->convertToXYZINormal(msg_point);
      TSPointCloudROS::doTransform(transform, pcl_point, msg->header);
      transformed_cloud->points.push_back(pcl_point);
    }
  }
  catch (tf2::TransformException ex)
  {
    ROS_INFO_STREAM(ex.what());  // NOLINT
  }

  transformed_cloud->width = transformed_cloud->points.size();
  cloud_pub_.publish(transformed_cloud);

  if (lifetime_normals_vis_ != 0.0)
  {
    TSPointCloudROS::publishNormals(transformed_cloud);
  }
}

void TSPointCloudROS::doTransform(geometry_msgs::TransformStamped transform,
                                  pcl::PointXYZINormal &pcl_point, std_msgs::Header header)
{
  // Skip if current and target frames are the same
  if (header.frame_id == target_frame_)
  {
    return;
  }

  // Intermediate translation variable
  Vector3f translation;
  translation[0] = transform.transform.translation.x;
  translation[1] = transform.transform.translation.y;
  translation[2] = transform.transform.translation.z;
  // Intermediate quaternion variable
  Vector4f quaternion;
  quaternion[0] = transform.transform.rotation.x;
  quaternion[1] = transform.transform.rotation.y;
  quaternion[2] = transform.transform.rotation.z;
  quaternion[3] = transform.transform.rotation.w;

  // Perform the transformations to target frames
  try
  {
    pointTransform_->doTransform(pcl_point, translation, quaternion);
  }
  catch (tf2::TransformException ex)
  {
    ROS_ERROR("Point or normal transformation failed: %s", ex.what());
  }
}

void TSPointCloudROS::publishNormals(XYZINCloud::ConstPtr transformed_cloud)
{
  // Initialize the marker
  marker.type = visualization_msgs::Marker::ARROW;
  marker.header = pcl_conversions::fromPCL(transformed_cloud->header);
  marker.ns = "normals";  // Namespace the normal marker will be posted to
  marker.action = visualization_msgs::Marker::ADD;
  marker.lifetime = ros::Duration(lifetime_normals_vis_);
  marker.pose.orientation.x = 0.0;
  marker.pose.orientation.y = 0.0;
  marker.pose.orientation.z = 0.0;
  marker.pose.orientation.w = 1.0;
  marker.scale.x = 0.005f;
  marker.scale.y = 0.01f;
  marker.color.r = 1.f;
  marker.color.g = 0.f;
  marker.color.b = 0.f;
  marker.color.a = 1.f;

  // Publish normal markers individually
  for (const auto &cloud_point : transformed_cloud->points)
  {
    marker.id = marker_id_++;

    marker.points.clear();
    geometry_msgs::Point point_msg;

    point_msg.x = cloud_point.x;
    point_msg.y = cloud_point.y;
    point_msg.z = cloud_point.z;

    marker.points.push_back(point_msg);

    point_msg.x += cloud_point.normal_x * .1f;
    point_msg.y += cloud_point.normal_y * .1f;
    point_msg.z += cloud_point.normal_z * .1f;

    marker.points.push_back(point_msg);
    marker_pub_.publish(marker);
  }
}

}  // namespace toposens_pointcloud