but_projection.h

Go to the documentation of this file.

/******************************************************************************
 * \file
 *
 * $Id:$
 *
 * Copyright (C) Brno University of Technology
 *
 * This file is part of software developed by dcgm-robotics@FIT group.
 *
 * Author: Vit Stancl (stancl@fit.vutbr.cz)
 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
 * Date: dd/mm/2011
 *
 * This file is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This file is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this file.  If not, see <http://www.gnu.org/licenses/>.
 */
#pragma once
#ifndef BUT_PROJECTION_H
#define BUT_PROJECTION_H

// ROS includes
#include <message_filters/subscriber.h>

// Rviz includes
#include <rviz/display.h>
#include <rviz/view_controller.h>
#include "rviz/properties/forwards.h"
#include "rviz/properties/property.h"
#include "rviz/properties/edit_enum_property.h"
#include "rviz/properties/property_manager.h"
//#include "rviz/image/ros_image_texture.h"
#include "rviz/properties/forwards.h"
#include <image_geometry/pinhole_camera_model.h>

// OGRE includes
#include <OGRE/OgreSceneNode.h>
#include <OgreManualObject.h>
#include <OgreCamera.h>
#include "OGRE/OgreMaterialManager.h"
#include <OGRE/OgreTexture.h>

// Std includes
#include <string>

// Wx includes
#include <wx/wx.h>

// Local includes
#include "ros_rtt_texture.h"
#include "but_rostexture.h"
#include "but_depthtexture.h"

namespace rviz
{
    class WindowManagerInterface;
}

namespace srs_ui_but
{

    /*********************************************************************************************************************/

    class CMaterialListener : public Ogre::MaterialManager::Listener
    {
    protected:
        Ogre::Material * m_materialPtr;

        std::string m_schemeName;

        bool m_bNotPaused;

        float m_testingDistance;

        bool m_bDistanceChanged;

        bool m_bMatricesChanged;

        Ogre::Vector4 m_camPosition;

        Ogre::Matrix4 m_projMatrix;

        Ogre::Vector4 m_camPlane;

                boost::mutex m_lock;

    public:
        //CMaterialListener( const std::string & materialName, const std::string & groupName, const std::string & schemeName );

        CMaterialListener( Ogre::Material * material, const std::string & schemeName );

        ~CMaterialListener();

        void setPaused( bool bPaused ) { m_bNotPaused = !bPaused; }

        void setTestedDistance( float distance );

        void setCameraPosition( const Ogre::Vector4 & position, const Ogre::Vector4 & camPlane, const Ogre::Matrix4 & projMatrix );

        float getTestedDistance() { return m_testingDistance; }

        Ogre::Technique *handleSchemeNotFound(unsigned short, const Ogre::String& schemeName, Ogre::Material*mat, unsigned short, const Ogre::Renderable*);
    }; // class CMaterialListener

    /*********************************************************************************************************************/

    class CProjectionData
    {
        //typedef rviz::CRosTexture tRosTexture;
        typedef CRosTopicTexture tRosTextureRGB;

        typedef CRosDepthTexture tRosTextureDepth;

    protected:
        enum ETextureMode
        {
            TM_INTERNAL, // Create and use internal ogre texture
            TM_ROS               // Use ros texture (typically connected to some topic)
        };

    public:
        CProjectionData( Ogre::SceneManager * manager, const ros::NodeHandle &nh, const std::string & materialName, const std::string & groupName = Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME );

        virtual ~CProjectionData();

        Ogre::Material * getMaterialPtr() { return m_materialPtr.get(); }

        void setListenerPtr(CMaterialListener * ptr ) { m_materialListener = ptr; if( ptr != 0 ) updateMatrices(); }

        void setFrustrumSize( Ogre::Vector2 size );

        void setTextureSize( int width, int height );

        void setRGBTextureTopic( const std::string & topic );

        void setDepthTextureTopic( const std::string & topic );

        int getTextureWidth() { return m_texW; }

                int getTextureHeight() { return m_texH; }

        void clear();

        void update() { if( m_textureRosRGB != 0 ) m_textureRosRGB->update(); }

        void setProjectorPosition( const Ogre::Vector3 & v ){ m_projectorNode->setPosition(v); }

        void setProjectorOrientation( const Ogre::Quaternion & q ) { m_projectorNode->setOrientation( q ); }

        void setProjectorEquation( const Ogre::Vector4 & e ) { m_cameraEquation = e; }

        void updateMatrices();

        void setFOVy( Ogre::Radian fovy ) { m_frustum->setFOVy( fovy );  }

        void setAspectRatio( Ogre::Real ar ) { m_frustum->setAspectRatio( ar ); }

        void setCameraModel(const sensor_msgs::CameraInfo& msg) { m_textureRosDepth->setCameraModel(msg); }

    protected:
        CMaterialListener * m_materialListener;

        Ogre::MaterialPtr m_materialPtr;

        Ogre::Vector2 m_frustrumSize;

        size_t m_texW, m_texH;

        ETextureMode m_mode;

        tRosTextureRGB * m_textureRosRGB;
        tRosTextureDepth* m_textureRosDepth;

        Ogre::SceneNode * m_projectorNode;

        Ogre::Frustum * m_frustum;

        tf::TransformListener m_tfListener;

        Ogre::TextureUnitState * m_texState;

        boost::mutex m_lock;

        Ogre::SceneManager * m_manager;

        Ogre::Vector3 m_cameraPosition;

        Ogre::Vector4 m_cameraEquation;

    }; // class CProjectionData

    /*********************************************************************************************************************/


    class CButProjection : public rviz::Display
    {
    public:

        class CControllPane : public wxPanel
        {
        public:
            typedef boost::signal< void (bool) > tSigCheckboxState;

            typedef boost::signal< void ( std::string ) > tSigSave;

        public:
            CControllPane(wxWindow *parent, const wxString& title, rviz::WindowManagerInterface * wmi );

            void OnChckToggle(wxCommandEvent& event);

            virtual void OnSave(wxCommandEvent& event);

            tSigCheckboxState & getSigChckBox(){ return m_sigCheckBox; }

            tSigSave & getSigSave(){ return m_sigSave; }

        protected:
            rviz::WindowManagerInterface * m_wmi;

            wxCheckBox * m_chkb;

            wxButton * m_button;

            tSigCheckboxState m_sigCheckBox;

            tSigSave m_sigSave;
        private:
            DECLARE_EVENT_TABLE()
        }; // class CControllPane

    public:
        CButProjection(const std::string & name,rviz::VisualizationManager * manager);

        ~CButProjection();

        //OverridesfromDisplay
        virtual void targetFrameChanged(){}
        virtual void fixedFrameChanged(){}
        virtual void createProperties();

        const std::string& getRgbTopic() { return m_imageRGBInputTopicName; }

        void setRgbTopic(const std::string& topic);

                const std::string& getDepthTopic() { return m_imageDepthInputTopicName; }

                void setDepthTopic(const std::string& topic);

        virtual void update (float wall_dt, float ros_dt);

    protected:
        //overridesfromDisplay
        virtual void onEnable();
        virtual void onDisable();

        void subscribe();

        void unsubscribe();

        bool createGeometry(const ros::NodeHandle & nh);

        void destroyGeometry();

        //void setTimerPeriod(float period);

        void onTimerPublish(const ros::TimerEvent&);

        void onPublishStateChanged(bool state);

        void onSave( std::string filename );

        void createMaterials(Ogre::Camera * camera);

        void removeMaterials();

        void clear();

        void cameraInfoCB(const sensor_msgs::CameraInfo::ConstPtr &cam_info);

        void connectML( bool bConnect );

        void setTestedDistance( float distance );

        float getTestedDistance();

    protected:
        Ogre::SceneNode * m_sceneNode;

        std::string m_imageRGBInputTopicName, m_imageDepthInputTopicName;

        ros::Timer m_timer;

        rviz::ROSTopicStringPropertyWPtr m_rgb_topic_property;

        rviz::ROSTopicStringPropertyWPtr m_depth_topic_property;

        rviz::FloatPropertyWPtr m_distance_property;

        double m_timerPeriod;

        CControllPane * m_pane;

        CProjectionData * m_projectionData;

        std::string m_camera_info_topic;

        // TF transformation listener for camera info message
        tf::TransformListener *m_tf_cam_info_Listener;

        message_filters::Subscriber< sensor_msgs::CameraInfo > *  m_ciSubscriber;

        tf::MessageFilter<sensor_msgs::CameraInfo> * m_camInfoTransformFilter;

        image_geometry::PinholeCameraModel m_camera_model;

        cv::Size m_camera_size;

        boost::mutex m_cameraInfoLock;

        CMaterialListener * m_ml;

        bool m_bMLConnected;

        bool m_bCameraInitialized;



    };//class CButCamCast

} // namespace srs_ui_but

#endif // BUT_PROJECTION_H