Unable to subscribe to pcl::PointCloud<Eigen::MatrixXf>
I am trying to subscribe to point cloud data consists of 3D position and color values (XYZRGB). However, I need to get the point cloud data as Eigen matrix, so I am trying to use the following formulation of point cloud pcl::PointCloud<eigen::matrixxf>
Below is the sample code-
#include <ros/ros.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
typedef pcl::PointCloud<Eigen::MatrixXf> PointCloud;
void callback(const PointCloud::ConstPtr& msg)
{
printf ("Cloud: width = %d, height = %d\n", msg->width, msg->height);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "sub_pcl");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe<PointCloud>("points2", 1, callback);
ros::spin();
}
The above code throws many errors at compile time. Please see below-
In file included from /usr/include/pcl-1.7/pcl/register_point_struct.h:55:0,
from /usr/include/pcl-1.7/pcl/point_types.h:44,
from /home/ravi/ros_ws/src/my_project/src/test.cpp:2:
/usr/include/pcl-1.7/pcl/point_traits.h: In instantiation of ‘struct pcl::traits::fieldList<Eigen::Matrix<float, -1, -1> >’:
/usr/include/pcl-1.7/pcl/conversions.h:127:70: required from ‘void pcl::createMapping(const std::vector<pcl::PCLPointField>&, pcl::MsgFieldMap&) [with PointT = Eigen::Matrix<float, -1, -1>; pcl::MsgFieldMap = std::vector<pcl::detail::FieldMapping>]’
/opt/ros/indigo/include/pcl_conversions/pcl_conversions.h:569:59: required from ‘void pcl::createMapping(const std::vector<sensor_msgs::PointField_<std::allocator<void> > >&, pcl::MsgFieldMap&) [with PointT = Eigen::Matrix<float, -1, -1>; pcl::MsgFieldMap = std::vector<pcl::detail::FieldMapping>]’
/opt/ros/indigo/include/pcl_ros/point_cloud.h:234:49: required from ‘static void ros::serialization::Serializer<pcl::PointCloud<PointT> >::read(Stream&, pcl::PointCloud<PointT>&) [with Stream = ros::serialization::IStream; T = Eigen::Matrix<float, -1, -1>]’
/opt/ros/indigo/include/ros/serialization.h:163:32: required from ‘void ros::serialization::deserialize(Stream&, T&) [with T = pcl::PointCloud<Eigen::Matrix<float, -1, -1> >; Stream = ros::serialization::IStream]’
/opt/ros/indigo/include/ros/subscription_callback_helper.h:136:34: required from ‘ros::VoidConstPtr ros::SubscriptionCallbackHelperT<P, Enabled>::deserialize(const ros::SubscriptionCallbackHelperDeserializeParams&) [with P = const boost::shared_ptr<const pcl::PointCloud<Eigen::Matrix<float, -1, -1> > >&; Enabled = void; ros::VoidConstPtr = boost::shared_ptr<const void>]’
/home/ravi/ros_ws/src/my_project/src/test.cpp:18:1: required from here
/usr/include/pcl-1.7/pcl/point_traits.h:152:12: error: invalid use of incomplete type ‘struct pcl::traits::fieldList<Eigen::Matrix<float, -1, -1> >’
struct fieldList : fieldList<typename POD<PointT>::type>
^
/usr/include/pcl-1.7/pcl/point_traits.h:152:12: error: declaration of ‘struct pcl::traits::fieldList<Eigen::Matrix<float, -1, -1> >’
In file included from /usr/include/boost/mpl/aux_/na_assert.hpp:23:0,
from /usr/include/boost/mpl/arg.hpp:25,
from /usr/include/boost/mpl/placeholders.hpp:24,
from /usr/include/boost/mpl/apply.hpp:24,
from /usr/include/boost/mpl/aux_/iter_apply.hpp:17,
from /usr/include/boost/mpl/aux_/find_if_pred.hpp:14,
from /usr/include/boost/mpl/find_if.hpp:17,
from /usr/include/boost/mpl/find.hpp:17,
from /usr/include/boost/mpl/aux_/contains_impl.hpp:20,
from /usr ...