ar_kinect.h

Go to the documentation of this file.

#ifndef AR_POSE_AR_MULTI_H
#define AR_POSE_AR_MULTI_H

#include <string.h>
#include <stdarg.h>

#include <AR/ar.h>
#include <AR/arMulti.h>

#include <geometry_msgs/TransformStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/Marker.h>
#include <pcl/ros/conversions.h>
#include <pcl/point_types.h>
#include <message_filters/subscriber.h>

#include <opencv/cv.h>
#include <cv_bridge/CvBridge.h>

#include <ar_pose/ARMarkers.h>
#include <ar_kinect/object.h>

#include <boost/multi_array.hpp>

#define AR_BOUNDING_BOX
#define DEBUG_AR_KINECT

#define ORIGIN "/camera_rgb_optical_frame"

#define TARGET "/PATTERN_BASE"

#define DBG(str) //str

const std::string cameraImageTopic_ = "/camera/depth_registered/image";
const std::string cameraInfoTopic_  = "/camera/depth_registered/camera_info";
const std::string cloudTopic_ = "/camera/depth_registered/points";


class CoordinateFrame;

const double AR_TO_ROS = 0.001;
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;

class ARPublisher
{
    public:
        ARPublisher (ros::NodeHandle & n);
        ~ARPublisher (void);

    private:
        void arInit ();
        void camInfoCallback (const sensor_msgs::CameraInfoConstPtr &);
        void cloudCallback (const sensor_msgs::PointCloud2ConstPtr& msg);
        void extractObject(const sensor_msgs::PointCloud2ConstPtr& msg);

        ros::NodeHandle n_;
        tf::TransformBroadcaster broadcaster_;
        ros::Subscriber sub_;
        image_transport::Subscriber cam_sub_;
        ros::Subscriber cloud_sub_;    
        ros::Publisher arMarkerPub_;
#ifdef DEBUG_AR_KINECT
        ros::Publisher kinect_pclPub_;
#endif

        image_transport::ImageTransport it_;
        sensor_msgs::CvBridge bridge_;
        sensor_msgs::CameraInfo cam_info_;
        PointCloud cloud_;

        // **** for visualisation in rviz
        ros::Publisher rvizMarkerPub_;
        visualization_msgs::Marker rvizMarker_;

        // **** parameters
        ARParam cam_param_;
        ARMultiMarkerInfoT *config;
        ar_object::ObjectData_T * object;
        int objectnum;
        char pattern_filename_[FILENAME_MAX];
        char data_directory_[FILENAME_MAX];

        ar_pose::ARMarkers arPoseMarkers_;
        int threshold_;
        bool getCamInfo_;
        bool publishTf_;
        bool publishVisualMarkers_;
        CvSize sz_;
        IplImage *capture_;
        bool gotcloud_;
        int cloud_width_;
        cv::Mat1f distances;
        double boundingbox_size;
        double marker_dist_threshold;

        message_filters::Subscriber<sensor_msgs::PointCloud2> sub;
        tf::TransformListener tf_;

        ros::Publisher result_pcl;
        ros::Publisher bb_pcl;

        typedef boost::multi_array<CoordinateFrame*, 3> transform_type;
        transform_type transformations;

};                            // end class ARPublisher

#endif