render with opengl in rviz

edit

I have lots of OpenGL code to draw various things, using display lists and textures. For instance I have an OpenGL model of our robot as a display list, and I have a visualization of our localization map rendered as a set of PNG images.

Under Fuerte, I used a Ogre::RenderQueueListener to display those things (see code below for a rough sketch).

Now I am switching to hydro, and it does not work any more: if both the fixed and target frames are set to base_link, then I can see the diamond, but it's displayed in the center of the visualization window, and not rotated, even when I move the camera around. I.e it is drawn in window frame, rather than in the fixed frame. If I set the fixed frame to /odom, then I can't see my diamond at all. I suspect that it's because the transform is several meters, and so in the window frame it's off the screen.

I'd appreciate some help fixing this code. What are the related changes between the fuerte version and the hydro version?

Also, is there a recommended way to render OpenGL code in rviz?

class NRSOpenGlBase : public Ogre::RenderQueueListener
{
public:
  NRSOpenGlBase(Ogre::Node* node, const Ogre::Camera* camera, Ogre::SceneManager* sceneMgr)
  : scene_node_(node), camera_(camera), scene_manager_(sceneMgr)
  { }

  virtual void renderQueueEnded(Ogre::uint8 queueGroupId, const Ogre::String& invocation, bool& repeatThisInvocation);

protected:
  virtual void nativeRender() = 0;

  Ogre::Node*         scene_node_;
  const Ogre::Camera* camera_;
  Ogre::SceneManager* scene_manager_;
};

void NRSOpenGlBase::renderQueueEnded(Ogre::uint8 queueGroupId,
    const Ogre::String& invocation, bool& repeatThisInvocation)
{
  // Set wanted render queue here - make sure there are - make sure that something is on
  // this queue - else you will never pass this if.
  if (queueGroupId != Ogre::RENDER_QUEUE_MAIN)
    return;

  // save matrices
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  glMatrixMode(GL_PROJECTION);
  glPushMatrix();
  glMatrixMode(GL_TEXTURE);
  glPushMatrix();
  glLoadIdentity(); //Texture addressing should start out as direct.

  Ogre::RenderSystem* renderSystem = scene_manager_->getDestinationRenderSystem();

  // Ogre coordinates are -Y forward, apply offset here
  Ogre::Matrix4 xfm = scene_node_->_getFullTransform();
  Ogre::Quaternion robotToOgreRot;
  Ogre::Matrix4 robotToOgreMat(robotToOgreRot);
  renderSystem->_setWorldMatrix(xfm * robotToOgreMat);

  renderSystem->_setViewMatrix(camera_->getViewMatrix());
  renderSystem->_setProjectionMatrix(camera_->getProjectionMatrixRS());

  static Ogre::Pass* clearPass = NULL;
  if (!clearPass) {
    Ogre::MaterialPtr clearMat =
        Ogre::MaterialManager::getSingleton().getByName("BaseWhite");
    clearPass = clearMat->getTechnique(0)->getPass(0);
  }
  //Set a clear pass to give the renderer a clear renderstate
  scene_manager_->_setPass(clearPass, true, false);

  // save attribs
  glPushAttrib(GL_ALL_ATTRIB_BITS);

  // call native rendering function
  //////////////////
  nativeRender();
  //////////////////

  // restore original state
  glPopAttrib();

  // restore matrices
  glMatrixMode(GL_TEXTURE);
  glPopMatrix();
  glMatrixMode(GL_PROJECTION);
  glPopMatrix();
  glMatrixMode(GL_MODELVIEW);
  glPopMatrix();
}


class NRSDiamond : public NRSOpenGlBase
{
public:
  NRSDiamond(Ogre::Node* node, const Ogre::Camera* camera, Ogre::SceneManager* sceneMgr) 
  : NRSOpenGlBase(node, camera, sceneMgr) 
  { }

protected:
  virtual void nativeRender();
};

void NRSDiamond::nativeRender()
{
  static const double x = 0, y = 0, r = 10;
  glColor3f(0.5f, 0.0f, 1.0f);
  glBegin(GL_POLYGON);
  glVertex2f(x + r, y);
  glVertex2f(x, y + r);
  glVertex2f(x - r, y);
  glVertex2f(x, y - r);
  glEnd();
}


class DiamondDisplay : public rviz::Display
{
Q_OBJECT
public:
  DiamondDisplay()
    : diamond_scene_node_(0)
    , nrs_queue_listener_(NULL)
    { }

  virtual ~DiamondDisplay();

protected:
  // Overrides from Display
  virtual void update(float wall_dt, float ros_dt);
  virtual void onInitialize();

private:
  SceneNode ...