diff --git a/plugin_description.xml b/plugin_description.xml
new file mode 100644
index 0000000000000000000000000000000000000000..85d68053e51b01b1283cdbe715b0ef795000dfc1
--- /dev/null
+++ b/plugin_description.xml
@@ -0,0 +1,7 @@
+<library path="lib/liblrs_rviz_plugin">
+ <class name="lrs_rviz/Image" type="lrs_rviz::ImageDisplay" base_class_type="rviz::Display">
+ <description>
+ Displays an image on the ground plane.
+ </description>
+ </class>
+</library>
diff --git a/src/image_display.cc b/src/image_display.cc
new file mode 100644
index 0000000000000000000000000000000000000000..cd15c6050af2064d0ed3cdcff6bd78a1dd0ee00f
--- /dev/null
+++ b/src/image_display.cc
@@ -0,0 +1,167 @@
+#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 )
diff --git a/src/image_display.h b/src/image_display.h
new file mode 100644
index 0000000000000000000000000000000000000000..ed615420f7b89a62c8735a7b5407594ae7453c9f
--- /dev/null
+++ b/src/image_display.h
@@ -0,0 +1,77 @@
+#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
diff --git a/src/image_visual.cc b/src/image_visual.cc
new file mode 100644
index 0000000000000000000000000000000000000000..9e10c97581eeb9d10b20b8796e04c00516fb3842
--- /dev/null
+++ b/src/image_visual.cc
@@ -0,0 +1,114 @@
+#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);
+ }
+
+}
diff --git a/src/image_visual.h b/src/image_visual.h
new file mode 100644
index 0000000000000000000000000000000000000000..79822e569f553dd8f4512af99b4b6407709a4e7f
--- /dev/null
+++ b/src/image_visual.h
@@ -0,0 +1,51 @@
+#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