RGBDCameraPlugin.hh

Go to the documentation of this file.

#ifndef _RGBD_CAMERA_PLUGIN_HH_
#define _RGBD_CAMERA_PLUGIN_HH_

// Gazebo
#include "gazebo/common/Plugin.hh"
#include <gazebo/msgs/msgs.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/rendering/DepthCamera.hh>
#include <gazebo/sensors/sensors.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/common/Events.hh>
// OGRE
#include "gazebo/rendering/ogre_gazebo.h"
// Custom messages
#include "camera_request.pb.h"
#include "camera_response.pb.h"
// Queue for threaded access
#include "ConcurrentQueue.hh"


// TODO - Profiling
#include <chrono>

namespace RGBDCameraPlugin {

    // SDF parameters

    #define PARAM_CAMERA        "cameraName"
    #define PARAM_QUEUE_SIZE    "renderQueueSize"
    #define PARAM_OUTPUT_DIR    "outputDir"
    #define PARAM_EXTENSION     "imageFormat"
    #define PARAM_REQ_TOPIC     "requestTopic"
    #define PARAM_RES_TOPIC     "responseTopic"

    // Default values for SDF parameters

    #define DEFAULT_OUTPUT_DIR  "/tmp/RGBDCameraPlugin"
    #define DEFAULT_EXTENSION   "png"
    #define DEFAULT_REQ_TOPIC   "~/grasp/rgbd"
    #define DEFAULT_RES_TOPIC   "~/grasp/rgbd/response"

    // Message enums

    #define CAPTURE_REQUEST     grasp::msgs::CameraRequest::CAPTURE
    #define MOVE_REQUEST        grasp::msgs::CameraRequest::MOVE
    #define PREFIX_REQUEST      grasp::msgs::CameraRequest::PREFIX
    #define CAPTURE_RESPONSE    grasp::msgs::CameraResponse::CAPTURE
    #define MOVE_RESPONSE       grasp::msgs::CameraResponse::MOVE
    #define PREFIX_RESPONSE     grasp::msgs::CameraResponse::PREFIX
}

namespace gazebo {

    typedef const boost::shared_ptr<const grasp::msgs::CameraRequest>
        CameraRequestPtr;
    typedef const boost::shared_ptr<const grasp::msgs::CameraResponse>
        CameraResponsePtr;

    // Forward declaration of private data class
    class RGBDCameraPluginPrivate;

    // TODO
    class RGBDCameraPlugin : public ModelPlugin
    {
        // Private attributes

        private: std::unique_ptr<RGBDCameraPluginPrivate> data_ptr;

        private: physics::ModelPtr model;
        private: physics::WorldPtr world;

        private: rendering::DepthCameraPtr camera;
        private: event::ConnectionPtr updateConn;
        private: event::ConnectionPtr newRGBFrameConn;
        private: event::ConnectionPtr newDepthFrameConn;

        private: std::thread thread_rgb;
        private: std::thread thread_depth;
        private: bool stop_thread {false};
        private: ConcurrentQueue<unsigned char*> *rgb_queue;
        private: ConcurrentQueue<float*> *depth_queue;

        private: std::string output_dir;
        private: std::string output_prefix {""};
        private: std::string output_ext;

        private: bool capture {false};
        private: bool update_pose {false};
        private: ignition::math::Pose3d new_pose;

        private: bool rgb_captured {false};
        private: bool depth_captured {false};

        // Public methods

        public: RGBDCameraPlugin();

        public: virtual ~RGBDCameraPlugin();

        public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);

        public: void onNewRGBFrame(
            const unsigned char *_image,
            unsigned int _width,
            unsigned int _height,
            unsigned int _depth,
            const std::string &_format);

        public: void onNewDepthFrame(
            const float *_image,
            unsigned int _width,
            unsigned int _height,
            unsigned int _depth,
            const std::string &_format);

        // Private methods

        private: void onUpdate();

        private: void onRequest(CameraRequestPtr &_msg);

        private: void saveRenderRGB();

        private: void saveRenderDepth();

        private: void saveDepthFrame(
            const float *_image,
            unsigned int _width,
            unsigned int _height,
            unsigned int _depth,
            const std::string &_format,
            const std::string &_filename);

        private: void clearQueues();

        private: static int OgrePixelFormat(const std::string &_format);
    };
}

#endif