50 lines
1.2 KiB
C++
50 lines
1.2 KiB
C++
// a not very useful test tool
|
|
#include <ros/ros.h>
|
|
#include <pcl_ros/point_cloud.h>
|
|
#include <pcl/point_types.h>
|
|
|
|
#include <iostream>
|
|
#include <string>
|
|
#include <sstream>
|
|
|
|
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
|
|
|
|
class Callback {
|
|
public:
|
|
void
|
|
operator()(const PointCloud::ConstPtr& msg)
|
|
{
|
|
std::ostringstream stream;
|
|
stream << "PointCloud published :" << std::endl;
|
|
for(int i = 0; i < msg->points.size(); ++i)
|
|
{
|
|
pcl::PointXYZRGB p = msg->points[i];
|
|
stream << std::endl
|
|
<< "point # " << i << std::endl
|
|
<< "x : " << p.x << std::endl
|
|
<< "y : " << p.y << std::endl
|
|
<< "z : " << p.z << std::endl
|
|
<< "r : " << (int) p.r << std::endl
|
|
<< "g : " << (int) p.g << std::endl
|
|
<< "b : " << (int) p.b << std::endl;
|
|
}
|
|
ROS_INFO("%s", stream.str().c_str());
|
|
}
|
|
};
|
|
|
|
int
|
|
main(int argc, char** argv)
|
|
{
|
|
ros::init(argc, argv, "pcl_displayer");
|
|
ros::NodeHandle node;
|
|
|
|
// initialisation
|
|
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, Callback());
|
|
|
|
// démarrage
|
|
ROS_INFO("node started");
|
|
ros::spin();
|
|
ROS_INFO("exit");
|
|
return 0;
|
|
}
|