CameraUtils.hh

Go to the documentation of this file.

/*
 *  Copyright (C) 2018 João Borrego
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *  
 *      http://www.apache.org/licenses/LICENSE-2.0
 *      
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 */

// Gazebo
#include "gazebo/common/Plugin.hh"
#include <gazebo/msgs/msgs.hh>
#include "gazebo/sensors/CameraSensor.hh"
#include "gazebo/rendering/Camera.hh"
#include "gazebo/util/system.hh"
// Custom messages
#include "camera_utils_request.pb.h"
#include "camera_utils_response.pb.h"
// Strings
#include <string>
// To create directories
#include <boost/filesystem.hpp>
// Malloc and memcpy
#include <stdio.h>
#include <string.h>

namespace CameraUtils {

#define REQUEST_TOPIC   "~/gap/camera_utils"
#define RESPONSE_TOPIC  "~/gap/camera_utils/response"

#define MOVE_REQUEST        gap::msgs::CameraUtilsRequest::MOVE
#define MOVE_RESPONSE       gap::msgs::CameraUtilsResponse::MOVE
#define CAPTURE_REQUEST     gap::msgs::CameraUtilsRequest::CAPTURE
#define CAPTURE_RESPONSE    gap::msgs::CameraUtilsResponse::CAPTURE
#define OPTIONS_REQUEST     gap::msgs::CameraUtilsRequest::OPTIONS
#define PROJECTION_REQUEST  gap::msgs::CameraUtilsRequest::PROJECTION
#define PROJECTION_RESPONSE gap::msgs::CameraUtilsResponse::PROJECTION

// Default parameters

#define DEFAULT_OUTPUT_DIR  (const std::string) "/tmp/camera_utils_output/"
#define DEFAULT_EXTENSION   (const std::string) ".png"

}

namespace gazebo{

    typedef const boost::shared_ptr<const gap::msgs::CameraUtilsRequest>
        CameraUtilsRequestPtr;
    typedef const boost::shared_ptr<const gap::msgs::CameraUtilsResponse>
        CameraUtilsReplyPtr;

    // Forward declaration of private data class
    class CameraUtilsPrivate;

    class CameraUtils : public SensorPlugin {

        // Private attributes

        private: std::unique_ptr<CameraUtilsPrivate> dataPtr;
        private: std::string output_dir;
        private: int saved_counter {0};
        private: std::string next_file_name;
        private: bool save_on_update {false};
        private: event::ConnectionPtr newFrameConnection;

        // Protected attributes

        protected: sensors::CameraSensorPtr parentSensor;
        protected: rendering::CameraPtr camera;
        protected: unsigned int width;
        protected: unsigned int height;
        protected: unsigned int depth;
        protected: std::string format;
        protected: std::string extension;

        // Public methods

        public: CameraUtils();

        public: virtual ~CameraUtils();

        public: virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf);

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

        // Private methods

        private: void onRequest(CameraUtilsRequestPtr &_msg);
    };
}