c_cpp 这允许在ROS Rviz中使用OpenGL代码。在水电测试，可能在groovy工作。不会在fuerte下工作（我也有fuerte代码可用

Posted 2021-05-14

#ifndef __NRS_OPENGL_BASE_H__
#define __NRS_OPENGL_BASE_H__

#include <OGRE/OgrePlatform.h>
#include <OGRE/OgreString.h>

namespace Ogre 
{
class Node;
class SceneManager;
class RenderQueueListener;
}

// this is an abstract base class.
// Derive it and implement the nativeRender function, place you OpenGL code there.
// the OpenGL coordinates match the scene_node coordinates: a vertice at (0,0,0) 
// will be placed at the origin of the scence_node
class NRSOpenGlBase : public Ogre::RenderQueueListener
{
public:
  NRSOpenGlBase(Ogre::Node* scene_node, Ogre::SceneManager* scene_mgr);

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

  void enable() { enabled_ = true; }
  void disable() { enabled_ = false; }
  bool isEnabled() const { return enabled_; }

protected:
  // derived base classes must implement this function. 
  // This is where you place your opengl drawing code.
	virtual void nativeRender() = 0;

  Ogre::Node*         scene_node_;
  Ogre::SceneManager* scene_manager_;

  // when this is false, no drawing occurs
	bool enabled_;
};

#endif // __NRS_OPENGL_BASE_H__

#include <GL/gl.h>

#include <OGRE/OgreNode.h>
#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <OGRE/OgreMaterialManager.h>
#include <OGRE/OgreRenderQueueListener.h>

#include <ros/ros.h> //ROS_ASSERT

#include "nrs_opengl_base.h"



NRSOpenGlBase::NRSOpenGlBase(Ogre::Node* node, Ogre::SceneManager* sceneMgr)
: scene_node_(node)
, scene_manager_(sceneMgr)
, enabled_(false)
{
  // check that Native rendering system is supported
  ROS_ASSERT( scene_manager_->getDestinationRenderSystem()->getName() == "OpenGL Rendering Subsystem" );
}

void NRSOpenGlBase::renderQueueEnded(Ogre::uint8 queueGroupId,
    const Ogre::String& invocation, bool& repeatThisInvocation)
{
  if (!enabled_)
    return;

  // 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.

  scene_manager_->getDestinationRenderSystem()->_setWorldMatrix( scene_node_->_getFullTransform() );

  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();
}

#ifndef __DIAMOND_VISUAL_H__
#define __DIAMOND_VISUAL_H__

#include "nrs_opengl_base.h"

// This is an example of how to use NRSOpenGlBase
class DiamondVisual : public NRSOpenGlBase
{
public:
  DiamondVisual(Ogre::Node* node, Ogre::SceneManager* sceneMgr);

protected:
  void nativeRender();
};

#endif //__DIAMOND_VISUAL_H__

#include "diamond_visual.h"

#include <GL/gl.h>

#include <OGRE/OgreNode.h>
#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <OGRE/OgreMaterialManager.h>
#include <OGRE/OgreRenderQueueListener.h>

DiamondVisual::DiamondVisual(Ogre::Node* node, Ogre::SceneManager* sceneMgr)
  : NRSOpenGlBase(node, sceneMgr)
{
}

void DiamondVisual::nativeRender()
{
  GLboolean cullFaceEnabled = glIsEnabled(GL_CULL_FACE);
  if (cullFaceEnabled) glDisable(GL_CULL_FACE);

  // do some OpenGL drawing here
  static const double x = 0, y = 0, r = 10;
  glBegin(GL_POLYGON);
  glVertex2f(x + r, y);
  glVertex2f(x, y + r);
  glVertex2f(x - r, y);
  glVertex2f(x, y - r);
  glEnd();

  if (cullFaceEnabled) glEnable(GL_CULL_FACE);
}

#ifndef __DIAMOND_DISPLAY_H__
#define __DIAMOND_DISPLAY_H__

#include <string>

#include <rviz/display.h>

#include "diamond_visual.h"

namespace Ogre
{
class SceneNode;
}

// this is an example display that will show our diamond
class DiamondDisplay : public rviz::Display
{
Q_OBJECT
public:
  DiamondDisplay();
  virtual ~DiamondDisplay();

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

private:
  Ogre::SceneNode* diamond_scene_node_;
  DiamondVisual* nrs_queue_listener_;
};


#endif //__DIAMOND_DISPLAY_H__

#include <OGRE/OgreNode.h>
#include <OGRE/OgreSceneManager.h>
#include <OGRE/OgreCamera.h>
#include <OGRE/OgreRenderQueueListener.h>

#include <rviz/frame_manager.h>
#include <rviz/helpers/color.h>
#include <rviz/visualization_manager.h>
#include <rviz/properties/color_property.h>
#include <rviz/properties/ros_topic_property.h>
#include <rviz/properties/float_property.h>
#include <rviz/display.h>

#include "diamond_display.h"



DiamondDisplay::DiamondDisplay()
: passat_scene_node_(0)
, nrs_queue_listener_(NULL)
{
  // usual stuffs in the constructor of your display:
  // create properties, etc.
}

DiamondDisplay::~DiamondDisplay()
{
  if (nrs_queue_listener_) {
    context_->getSceneManager()->removeRenderQueueListener(nrs_queue_listener_);
    delete nrs_queue_listener_;
  }
  if( diamond_scene_node_ )
    context_->getSceneManager()->destroySceneNode(diamond_scene_node_->getName());
}

void DiamondDisplay::onInitialize()
{
  diamond_scene_node_ = scene_node_->createChildSceneNode();
  nrs_queue_listener_ = new DiamondVisual(diamond_scene_node_, context_->getSceneManager());
  context_->getSceneManager()->addRenderQueueListener(nrs_queue_listener_);
}

void DiamondDisplay::onEnable()
{
  nrs_queue_listener_->enable();
}

void PassatDisplay::onDisable()
{
  nrs_queue_listener_->disable();
}

void PassatDisplay::update(float wall_dt, float ros_dt)
{
  // move the diamond_scene_node_
  // our opengl rendering is going to happen in that scene node, so here we set the origin.
  // in this example, the OpenGL code draws a diamond at the origin (of the diamond_scene_node_)
  
  Ogre::Vector3 position;
  Ogre::Quaternion orientation;

  // Here we want to draw the diamond at the origin of the base_link frame so we get
  // the position of the base_link frame in the fixed frame
  if( context_->getFrameManager()->getTransform( "base_link",
                                                 ros::Time(0),
                                                 position, orientation ))
  {
    setStatus(rviz::StatusProperty::Ok, "Transform", "Transform OK");
  }
  else
  {
    setStatus(rviz::StatusProperty::Error, "Transform", "Unable to lookup transform to base_link");
    ROS_DEBUG( "Error transforming from frame 'base_link' to frame '%s'", qPrintable( fixed_frame_ ));
    return;
  }

  diamond_scene_node_->setPosition( position );
  diamond_scene_node_->setOrientation( orientation );
}

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(PassatDisplay, rviz::Display)