Initial files

src/image_display.cc

+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/OgreSceneManager.h>
+
+#include <tf/transform_listener.h>
+
+#include <rviz/visualization_manager.h>
+#include <rviz/properties/color_property.h>
+#include <rviz/properties/float_property.h>
+#include <rviz/properties/int_property.h>
+#include <rviz/properties/string_property.h>
+#include <rviz/properties/property.h>
+#include <rviz/frame_manager.h>
+
+#include "image_visual.h"
+
+#include "image_display.h"
+
+namespace lrs_rviz {
+
+  ImageDisplay::ImageDisplay() : x_offset(0.0), y_offset(0.0), z_offset(0.0), Display() {
+
+    visual_enabled_property_ = 
+      new rviz::Property( "Visual Enabled", true,
+			  "Whether to display the visual representation of the image.",
+			  this, SLOT( updateVisualVisible() ));
+
+    image_description_property_ = 
+      new rviz::StringProperty( "Image Description", "image_description",
+				"Name of the parameter to search for to load the image filename.",
+				this, SLOT( updateImageDescription() ));
+
+    z_offset_property_ = 
+      new rviz::FloatProperty( "Z offset", 0.0, "z_offset of image in meter.", this, SLOT( update_z_offset() ));
+
+
+#if 0
+    dx_property_ = 
+      new rviz::IntProperty( "DX", 0, "x width of image in meter.", this, SLOT( updateDx() ));
+
+    dy_property_ = 
+      new rviz::IntProperty( "DY", 0, "y width of image in meter.", this, SLOT( updateDy() ));
+#endif
+  }
+
+  ImageDisplay::~ImageDisplay() {
+    if ( initialized() ) {
+      delete image_visual;
+    }
+  }
+
+
+  void ImageDisplay::onInitialize() {
+    load();
+    if ((image_filename == "") || (dx <= 0) || (dy <= 0)) {
+      return;
+    }
+    image_visual = new ImageVisual(scene_node_, context_, 
+				   image_filename, dx, dy,
+				   x_offset, y_offset, z_offset,
+				   this);
+    updateVisualVisible();
+  }
+
+  void ImageDisplay::updateVisualVisible() {
+    image_visual->setVisualVisible( visual_enabled_property_->getValue().toBool() );
+    context_->queueRender();
+  }
+  
+  void ImageDisplay::updateImageDescription() {
+    //    load();
+  }
+
+  void ImageDisplay::updateDx() {
+
+  }
+  
+  void ImageDisplay::updateDy() {
+
+  }
+
+  void ImageDisplay::update_z_offset() {
+    float z_offset = z_offset_property_->getFloat();
+    image_visual->set_z_offset(z_offset);
+  }
+
+
+  void ImageDisplay::reset() {
+    Display::reset();
+  }
+
+  void ImageDisplay::load() {
+    std::string filename;
+    if( !update_nh_.getParam(image_description_property_->getStdString() + "/filename", filename)) {
+      setStatus( rviz::StatusProperty::Error, "FILENAME",
+                 "Parameter [" + image_description_property_->getString() + "/filename"
+                 + "] does not exist" );
+      return;
+    } else {
+      setStatus( rviz::StatusProperty::Ok, "FILENAME", filename.c_str());
+    }
+
+    dx = 0;
+    if( !update_nh_.getParam(image_description_property_->getStdString() + "/dx", dx)) {
+      setStatus( rviz::StatusProperty::Error, "DX",
+                 "Parameter [" + image_description_property_->getString() + "/dx"
+                 + "] does not exist" );
+      return;
+    } else {
+      setStatus( rviz::StatusProperty::Ok, "DX", QString::number(dx));
+    }
+
+    dy = 0;
+    if( !update_nh_.getParam(image_description_property_->getStdString() + "/dy", dy)) {
+      setStatus( rviz::StatusProperty::Error, "DY",
+                 "Parameter [" + image_description_property_->getString() + "/dy"
+                 + "] does not exist" );
+      return;
+    } else {
+      setStatus( rviz::StatusProperty::Ok, "DY", QString::number(dy));
+    }
+
+    update_nh_.getParam(image_description_property_->getStdString() + "/x_offset", x_offset);
+    update_nh_.getParam(image_description_property_->getStdString() + "/y_offset", y_offset);
+
+    if( filename.empty() ) {
+      setStatus( rviz::StatusProperty::Error, "FILENAME", "Filename is empty" );
+      return;
+    }
+
+    if( dx <= 0 ) {
+      setStatus( rviz::StatusProperty::Error, "DX", "Zero is not allowed as value" );
+      return;
+    }
+
+    if( dy <= 0 ) {
+      setStatus( rviz::StatusProperty::Error, "DY", "Zero is not allowed as value" );
+      return;
+    }
+
+    if( filename == image_filename ) {
+      return;
+    }
+
+    image_filename = filename;
+
+    //
+    // Create visual thingy
+    //
+  
+  }
+
+void ImageDisplay::onEnable() {
+  load();
+  image_visual->setVisualVisible( true );
+}
+
+void ImageDisplay::onDisable() {
+  image_visual->setVisualVisible( false );
+}
+
+
+}
+
+// Tell pluginlib about this class.  It is important to do this in
+// global scope, outside our package's namespace.
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(lrs_rviz::ImageDisplay,rviz::Display )

src/image_display.h

+#ifndef IMAGE_DISPLAY_H
+#define IMAGE_DISPLAY_H
+
+#include "rviz/display.h"
+#include <OgreVector3.h>
+
+#include <map>
+
+namespace Ogre
+{
+class Entity;
+class SceneNode;
+}
+
+namespace rviz
+{
+class Property;
+class StringProperty;
+class IntProperty;
+class FloatProperty;
+}
+
+namespace lrs_rviz
+{
+  class ImageVisual;
+
+  class ImageDisplay: public rviz::Display {
+Q_OBJECT
+  public:
+    ImageDisplay();
+    virtual ~ImageDisplay();
+  
+  // Overrides of protected virtual functions from Display.  As much
+  // as possible, when Displays are not enabled, they should not be
+  // subscribed to incoming data and should not show anything in the
+  // 3D view.  These functions are where these connections are made
+  // and broken.
+  protected:
+    virtual void onInitialize();
+
+  // A helper to clear this display back to the initial state.
+    virtual void reset();
+
+    virtual void load();
+    virtual void onEnable();
+    virtual void onDisable();
+
+  // These Qt slots get connected to signals indicating changes in the user-editable properties.
+  private Q_SLOTS:
+    void updateImageDescription();
+    void updateVisualVisible();
+    void updateDx();
+    void updateDy();
+    void update_x_offset();
+    void update_y_offset();
+    void update_z_offset();
+
+
+  private:
+    std::string image_filename;
+    int dx;
+    int dy;
+    float x_offset;
+    float y_offset;
+    float z_offset;
+    ImageVisual * image_visual;
+
+    rviz::Property* visual_enabled_property_;
+    rviz::StringProperty* image_description_property_;
+    rviz::FloatProperty* z_offset_property_;
+//    rviz::IntProperty* dx_property_;
+//    rviz::IntProperty* dy_property_;
+  };
+};
+
+
+#endif

src/image_visual.cc

+#include "image_visual.h"
+//#include "properties/property.h"
+//#include "properties/enum_property.h"
+//#include "properties/bool_property.h"
+#include "display_context.h"
+
+#include "ogre_helpers/object.h"
+#include "ogre_helpers/shape.h"
+#include "ogre_helpers/axes.h"
+
+#include <OgreSceneNode.h>
+#include <OgreSceneManager.h>
+#include <OgreEntity.h>
+#include <OgreMaterialManager.h>
+#include <OgreMaterial.h>
+#include <OgreResourceGroupManager.h>
+#include <OgreManualObject.h>
+
+
+bool LoadImage(const Ogre::String& texture_name, const Ogre::String& texture_path)
+{
+	bool image_loaded = false;
+	std::ifstream ifs(texture_path.c_str(), std::ios::binary|std::ios::in);
+	if (ifs.is_open())
+	{
+		Ogre::String tex_ext;
+		Ogre::String::size_type index_of_extension = texture_path.find_last_of('.');
+		if (index_of_extension != Ogre::String::npos)
+		{
+			tex_ext = texture_path.substr(index_of_extension+1);
+			Ogre::DataStreamPtr data_stream(new Ogre::FileStreamDataStream(texture_path, &ifs, false));
+			Ogre::Image img;
+			img.load(data_stream, tex_ext);
+			Ogre::TextureManager::getSingleton().loadImage(texture_name,
+				Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, img, Ogre::TEX_TYPE_2D, 0, 1.0f);
+			image_loaded = true;
+		}
+		ifs.close();
+	}
+	return image_loaded;
+}
+
+namespace lrs_rviz {
+
+  ImageVisual::ImageVisual(Ogre::SceneNode* root_node, rviz::DisplayContext* context, 
+			   const std::string& filename, int dx, int dy,
+			   float x_off, float y_off, float z_off,
+			   rviz::Property* parent_property )
+    : scene_manager_(context->getSceneManager())
+    , visual_visible_(true), x_offset(x_off), y_offset(y_off), z_offset(z_off) {
+
+    root_visual_node_ = root_node->createChildSceneNode();
+
+    if (LoadImage("image_ground", filename)) {
+
+      Ogre::MaterialPtr material = 
+	Ogre::MaterialManager::getSingleton().create("image_ground_material",
+						     Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
+ 
+      material->getTechnique(0)->getPass(0)->createTextureUnitState("image_ground");
+      material->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
+    
+      manual = scene_manager_->createManualObject("manual");
+      //    manual->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP);
+      //    manual->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_POINT_LIST);
+      manual->begin("image_ground_material", Ogre::RenderOperation::OT_TRIANGLE_LIST);
+
+      int x0 = -dx/2.0;
+      int x1 = dx/2.0;
+      int y0 = -dy/2.0;
+      int y1 = dy/2.0;
+
+      float z = 0.0;
+ 
+      manual->position(x0, y0, z);
+      manual->textureCoord(0,1);
+      manual->position(x1, y0, z);
+      manual->textureCoord(1,1);
+      manual->position(x1, y1, z);
+      manual->textureCoord(1,0);
+
+
+      manual->position(x1, y1, z);
+      manual->textureCoord(1,0);
+      manual->position(x0, y1, z);
+      manual->textureCoord(0,0);
+      manual->position(x0, y0, z);
+      manual->textureCoord(0,1);
+    
+      manual->end();
+      root_visual_node_->attachObject(manual);
+    }
+
+    root_visual_node_->setPosition(x_offset, y_offset, z_offset);
+
+    setVisualVisible( visual_visible_ );
+
+
+  }
+
+  ImageVisual::~ImageVisual() {
+    scene_manager_->destroySceneNode( root_visual_node_->getName() );
+  }
+
+  void ImageVisual::setVisualVisible(bool visible) {
+    visual_visible_ = visible;
+  }
+
+  void ImageVisual::set_z_offset(float offset) {
+    z_offset = offset;
+    root_visual_node_->setPosition(x_offset, y_offset, z_offset);
+  }
+
+}

src/image_visual.h

+#ifndef _IMAGEVISUAL_H
+#define _IMAGEVISUAL_H
+
+#include <QObject>
+
+namespace Ogre {
+  class Vector3;
+  class Quaternion;
+  class SceneManager;
+  class SceneNode;
+  class ManualObject;
+}
+
+namespace rviz {
+  class DisplayContext;
+  class Property;
+}
+
+namespace lrs_rviz {
+
+ class ImageVisual : public QObject {
+   Q_OBJECT
+  private:
+  Ogre::SceneManager* scene_manager_;
+  Ogre::SceneNode* root_visual_node_;           ///< Node all our visual nodes are children of
+  bool visual_visible_;                         ///< Should we show the visual representation?
+  rviz::DisplayContext* context_;
+  Ogre::ManualObject* manual;
+  float x_offset;
+  float y_offset;
+  float z_offset;
+
+public:
+  //  ImageVisual( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node );
+  //  virtual ~ImageVisual();
+
+  ImageVisual(Ogre::SceneNode* root_node, rviz::DisplayContext* context, 
+	      const std::string& filename, int dx, int dy,
+	      float x_offset, float y_offset, float z_offset,
+	      rviz::Property* parent_property);
+  virtual ~ImageVisual();
+
+  //virtual void setVisible( bool visible );
+
+  void setVisualVisible( bool visible );
+  void set_z_offset(float offset);
+};
+
+};
+
+#endif