hand_control/src/pcl_displayer.cpp
Louis-Guillaume DUBOIS ee98eb2e17 better file organization
2015-04-29 12:27:24 +02:00

48 lines
1.2 KiB
C++

#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;
}