44 #include <sensor_msgs/Image.h> 45 #include <sensor_msgs/PointCloud2.h> 47 #include <pcl/io/pcd_io.h> 57 cloud_cb (
const sensor_msgs::PointCloud2ConstPtr& cloud)
59 if (cloud->height <= 1)
61 ROS_ERROR(
"Input point cloud is not organized, ignoring!");
67 image_.header = cloud->header;
70 catch (std::runtime_error &e)
98 main (
int argc,
char **argv)
100 ros::init (argc, argv,
"convert_pointcloud_to_image");
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::string resolveName(const std::string &name, bool remap=true) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL void spin(Spinner &spinner)
ros::Publisher image_pub_
int main(int argc, char **argv)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &cloud)
#define ROS_INFO_STREAM(args)
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
#define ROS_ERROR_STREAM(args)
sensor_msgs::Image image_