openni_node.cpp

Go to the documentation of this file.

#include "openni_cam.h"

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>

#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

#include <stdio.h>
//#include "error.h"
#include <cmath>

image_transport::Publisher publisher1;
image_transport::Publisher publisher2;

int main(int argc, char **argv) {
    ros::init(argc, argv, "openni_node");
    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);
    publisher1 = it.advertise("openni_camera/rgb", 1);
    publisher2 = it.advertise("openni_camera/depth", 1);
    ros::Publisher publisher_rgb_info = nh.advertise<sensor_msgs::CameraInfo>("openni_camera/camera_info", 1);
    //
    openni_cam::openni_cam cam;
    cam.init();
    ros::Rate r(5);
    int count = 0;
    printf("openni_node %4.4i  ",(count%10000)); fflush(stdout);
    fflush(stdout);
    while (ros::ok()) {
        ros::spinOnce();
        cam.get_image();
        ++count;
        printf("\b\b\b\b\b\b%4.4i  ",(count%10000));
        fflush(stdout);
        ros::Time t1 = ros::Time::now();
        //
        {
            sensor_msgs::ImagePtr msg(new sensor_msgs::Image());
            msg->header.stamp = t1;
            int num_bytes = cam.rgb.byte_size();
            msg->data.resize(num_bytes);
            memcpy(&msg->data[0],cam.rgb.begin(),num_bytes);
            msg->encoding = sensor_msgs::image_encodings::RGB8;
            msg->step = cam.rgb.byte_stride(0,0,1);
            msg->width = cam.rgb.size()[1];
            msg->height = cam.rgb.size()[2];
            msg->header.frame_id = "openni_camera";
            msg->header.seq = count;
            publisher1.publish(msg);
//
            sensor_msgs::CameraInfoPtr ci(new sensor_msgs::CameraInfo());
            ci->header = msg->header;
            ci->height = msg->height;
            ci->width = msg->width;
            for(int i=0; i<3; ++i) {
                ci->P[i*4 + 3] = 0;
                for(int j=0; j<3; ++j) {
                    ci->R[i*3+j] = (i==j);
                    ci->P[i*4+j] = cam.K(i,j);
                    ci->K[i*3+j] = cam.K(i,j);
                };
            }
            ci->distortion_model = "";//"plumb_bob";
            //
            ci->roi.height = msg->height;
            ci->roi.width = msg->width;
            ci->binning_x = 1;
            ci->binning_y = 1;
            publisher_rgb_info.publish(ci);
        };
        {
            sensor_msgs::ImagePtr msg(new sensor_msgs::Image());
            msg->header.stamp = t1;
            int num_bytes = cam.depth.byte_size();
            msg->data.resize(num_bytes);
            memcpy(&msg->data[0],cam.depth.begin(),num_bytes);
            msg->encoding = sensor_msgs::image_encodings::MONO16;
            msg->step = cam.depth.byte_stride(0,1);
            msg->width = cam.depth.size()[0];
            msg->height = cam.depth.size()[1];
            msg->header.frame_id = "openni_camera";
            msg->header.seq = count;
            publisher2.publish(msg);
        };
        static tf::TransformBroadcaster br;
        tf::Transform transform;
        transform.setOrigin( tf::Vector3(0, 0, 0) );
        transform.setRotation( tf::Quaternion(0, 0, 0) );
        br.sendTransform(tf::StampedTransform(transform, t1, "/ptu_mounting", "/openni_camera"));
        r.sleep();
        //usleep(1000);
    };
};