pcl.h

Go to the documentation of this file.

#pragma once

#include "ensenso_camera/ros2/core.h"

#include <pcl_conversions/pcl_conversions.h>

#ifdef ROS2 /**********************************************************************************************************/
#include "ensenso_camera/ros2/namespace.h"

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <sensor_msgs/msg/point_cloud2.hpp>

USING_MSG(sensor_msgs, PointCloud2)

template <typename T>
using PointCloudPublisher = ensenso::ros::Publisher<sensor_msgs::msg::PointCloud2>;

template <typename T>
using PointCloudSubscription = ensenso::ros::Subscription<sensor_msgs::msg::PointCloud2>;

#define POINT_CLOUD_SUBSCRIPTION_CALLBACK(callback, T, arg_name)                                                       \
  callback(const std::shared_ptr<sensor_msgs::msg::PointCloud2> arg_name)

namespace ensenso
{
namespace pcl
{
template <typename T>
inline PointCloudPublisher<T> create_publisher(ensenso::ros::NodeHandle& nh, ::std::string const& topic_name,
                                               int queue_size)
{
  return nh->node()->create_publisher<sensor_msgs::msg::PointCloud2>(topic_name, queue_size);
}

template <typename T, typename C, typename M>
inline PointCloudSubscription<T> create_subscription(ensenso::ros::NodeHandle& nh, ::std::string const& topic_name,
                                                     int queue_size, void (C::*callback)(M), C* object)
{
  return ensenso::ros::create_subscription<sensor_msgs::msg::PointCloud2>(nh, topic_name, queue_size, callback, object);
}
}  // namespace pcl
}  // namespace ensenso

#define STORE_POINT_CLOUD(point_cloud_msg, point_cloud_pcl)                                                            \
  /* In ROS2 we have to convert from sensor_msgs PointCloud2 to PCL PointCloud2. */                                    \
  pcl::fromROSMsg(*point_cloud_msg, *point_cloud_pcl);

template <typename T>
inline void publishPointCloud(ensenso::ros::Publisher<sensor_msgs::msg::PointCloud2> const& publisher,
                              std::unique_ptr<T> pointCloud)
{
  auto cloudMsg = ensenso::std::make_unique<sensor_msgs::msg::PointCloud2>();
  pcl::toROSMsg(*pointCloud, *cloudMsg);
  publisher->publish(std::move(cloudMsg));
}

#else /***ROS1*********************************************************************************************************/
#include <pcl_ros/point_cloud.h>

template <typename T>
using PointCloudPublisher = ensenso::ros::Publisher<T>;

template <typename T>
using PointCloudSubscription = ensenso::ros::Subscription<T>;

#define POINT_CLOUD_SUBSCRIPTION_CALLBACK(callback, T, arg_name) callback(T::Ptr const& arg_name)

namespace ensenso
{
namespace pcl
{
template <typename T>
inline PointCloudPublisher<T> create_publisher(ensenso::ros::NodeHandle& nh, ::std::string const& topic_name,
                                               int queue_size)
{
  return ensenso::ros::create_publisher<T>(nh, topic_name, queue_size);
}

template <typename T, typename C, typename M>
inline PointCloudSubscription<T> create_subscription(ensenso::ros::NodeHandle& nh, ::std::string const& topic_name,
                                                     int queue_size, void (C::*callback)(M), C* object)
{
  return ensenso::ros::create_subscription<T>(nh, topic_name, queue_size, callback, object);
}
}  // namespace pcl
}  // namespace ensenso

#define STORE_POINT_CLOUD(point_cloud_boost_ptr, point_cloud_std_ptr)                                                  \
  /* In ROS1 we only have to convert the point cloud ptr from boost to std. */                                         \
  point_cloud_std_ptr = ensenso::std::to_std(point_cloud_boost_ptr);

template <typename T>
inline void publishPointCloud(ensenso::ros::Publisher<T> const& publisher, std::unique_ptr<T> pointCloud)
{
  auto cloud_ = boost::shared_ptr<T>(std::move(pointCloud));
  publisher->publish(cloud_);
}
#endif