Rewrite to separate perception display from state visuals

Hopping this will boost performance as now we are building a big
array of simple object instead of an array of complex objects (themselves
composed of simple objects)

src/perceptions_display.cpp

@@ -9,6 +9,7 @@
 #include <rviz/default_plugin/covariance_property.h>
 #include <rviz/properties/int_property.h>
 #include <rviz/ogre_helpers/shape.h>
+#include <rviz/ogre_helpers/arrow.h>
 #include <rviz/ogre_helpers/movable_text.h>
 #include <rviz/ogre_helpers/billboard_line.h>
 #include <rviz/frame_manager.h>
@@ -19,8 +20,6 @@
 namespace rviz_multiception
 {
 
-// The constructor must have no arguments, so we can't give the
-// constructor the parameters it needs to fully initialize.
 PerceptionsDisplay::PerceptionsDisplay()
 {
 	color_property_ = new rviz::ColorProperty( "Color", QColor( 10, 10, 10 ),
@@ -47,55 +46,55 @@ PerceptionsDisplay::PerceptionsDisplay()
 	                                                     this, SLOT( queueRender() ));
 }
 
-// After the top-level rviz::Display::initialize() does its own setup,
-// it calls the subclass's onInitialize() function. This is where we
-// instantiate all the workings of the class. We make sure to also
-// call our immediate super-class's onInitialize() function, since it
-// does important stuff setting up the message filter.
-//
-//  Note that "MFDClass" is a typedef of
-// ``MessageFilterDisplay<message type>``, to save typing that long
-// templated class name every time you need to refer to the
-// superclass.
 void PerceptionsDisplay::onInitialize()
 {
 	MFDClass::onInitialize();
+	frame_node_ = scene_node_->createChildSceneNode();
 }
 
 PerceptionsDisplay::~PerceptionsDisplay()
 {
+	scene_manager_->destroySceneNode(frame_node_);
 }
 
 // Clear the visuals by deleting their objects.
 void PerceptionsDisplay::reset()
 {
 	MFDClass::reset();
-	state_visuals_.clear();
-	shape_visuals_.clear();
-	text_visuals_.clear();
-	trace_buffer_.clear();
-	trace_visuals_.clear();
-	for(Ogre::SceneNode* textNode: text_nodes_) context_->getSceneManager()->destroySceneNode(textNode);
-	text_nodes_.clear();
-	text_visuals_.clear();
+	resetVectors();
+}
+
+void PerceptionsDisplay::resetVectors(size_t N)
+{
+	pose_visuals_.clear();  pose_visuals_.reserve(N);
+	shape_visuals_.clear(); shape_visuals_.reserve(N);
+	cov_visuals_.clear();   cov_visuals_.reserve(N);
+	text_visuals_.clear();  text_visuals_.reserve(N);
+	for(Ogre::SceneNode* textNode: text_nodes_) scene_manager_->destroySceneNode(textNode);
+	text_nodes_.clear();    text_nodes_.reserve(N);
+
+	frame_node_->detachAllObjects();
+	frame_node_->removeAndDestroyAllChildren();
 }
 
 // Set the current color and alpha values for each visual.
 void PerceptionsDisplay::updateColorAndAlpha()
 {
-	float alpha = alpha_property_->getFloat();
-	float shape_alpha = shape_alpha_property_->getFloat();
-	Ogre::ColourValue color = color_property_->getOgreColor();
+	const float alpha = alpha_property_->getFloat();
+	const float shape_alpha = shape_alpha_property_->getFloat();
+	const Ogre::ColourValue color = color_property_->getOgreColor();
+	const Ogre::ColourValue textColor = text_color_property_->getOgreColor();
 
-	for(size_t i=0; i<state_visuals_.size(); i++)
+	for(size_t i=0; i<pose_visuals_.size(); i++)
 	{
-		state_visuals_[i]->setColor(color.r, color.g, color.b, alpha);
-		shape_visuals_[i]->setColor(color.r, color.g, color.b, shape_alpha);
+		pose_visuals_[i]->setColor(color.r, color.g, color.b, alpha);
+		shape_visuals_[i]->setColor(color.r, color.g, color.b, alpha*shape_alpha);
+// 		cov_visuals_[i]->setColor(color.r, color.g, color.b, alpha*shape_alpha);
 	}
 
-	for(size_t i=0; i<state_visuals_.size(); i++)
+	for(size_t i=0; i<text_visuals_.size(); i++)
 	{
-		trace_visuals_[i]->setColor(color.r, color.g, color.b, alpha);
+		text_visuals_[i]->setColor(textColor);
 	}
 }
 
@@ -116,79 +115,57 @@ void PerceptionsDisplay::filterTraceBuffer()
 	});
 }
 
-// This is our callback to handle an incoming message.
 void PerceptionsDisplay::processMessage(const multiception::Perceptions::ConstPtr& msg)
 {
-	// Here we call the rviz::FrameManager to get the transform from the
-	// fixed frame to the frame in the header of this message. If
-	// it fails, we can't do anything else so we return.
-	const size_t N = msg->perceptions.size();
-	if(N==0)
+	if(msg->perceptions.size()>0)
+	{
+		resetVectors(msg->perceptions.size());
+	}
+	else
 	{
 		return;
 	}
 
-	// We are keeping a vector of state visual pointers
-	state_visuals_.clear(); state_visuals_.reserve(N);
-	shape_visuals_.clear(); shape_visuals_.reserve(N);
-	for(Ogre::SceneNode* textNode: text_nodes_) context_->getSceneManager()->destroySceneNode(textNode);
-	text_nodes_.clear(); text_nodes_.reserve(N);
-	text_visuals_.clear(); text_visuals_.reserve(N);
-	
-	const float alpha = alpha_property_->getFloat();
-	const float shape_alpha = shape_alpha_property_->getFloat();
-	const Ogre::ColourValue color = color_property_->getOgreColor();
+	Ogre::Vector3 position;
+	Ogre::Quaternion orientation;
+	if(context_->getFrameManager()->getTransform(msg->header.frame_id, msg->header.stamp, position, orientation))
+	{
+		frame_node_->setPosition(position);
+		frame_node_->setOrientation(orientation);
+	}
+	else
+	{
+		ROS_DEBUG("Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable(fixed_frame_));
+		return;
+	}
 
 	// Process the perceptions in the message
 	for(const multiception::Perception& perception : msg->perceptions)
 	{
-		Ogre::Quaternion orientation;
-		Ogre::Vector3 position;
+		boost::shared_ptr<rviz::Arrow> arrowVis { new rviz::Arrow(scene_manager_, frame_node_) };
+		boost::shared_ptr<rviz::Shape> shapeVis { new rviz::Shape(rviz::Shape::Cube, scene_manager_, frame_node_) };
+		boost::shared_ptr<rviz::CovarianceVisual> covVis = covariance_property_->createAndPushBackVisual(scene_manager_, frame_node_);
 
-		if(!context_->getFrameManager()->getTransform(perception.header.frame_id, perception.header.stamp, position, orientation))
-		{
-			ROS_DEBUG("Error transforming from frame '%s' to frame '%s'", perception.header.frame_id.c_str(), qPrintable(fixed_frame_));
-			continue;
-		}
+		computeStateObjects(perception.state, *arrowVis, *covVis);
 
-		boost::shared_ptr<StateVisual> state(new StateVisual(context_->getSceneManager(), scene_node_, covariance_property_));
-		boost::shared_ptr<rviz::Shape> shape(new rviz::Shape(rviz::Shape::Cube, context_->getSceneManager(), scene_node_));
-
-		// Set the state visual's pose and content
-		state->setContent(perception.state);
-		state->setFramePosition(position);
-		state->setFrameOrientation(orientation);
-
-		// Set the shape visual's pose and shape
+		// Process the shape
 		const float z = (perception.height>0.1) ? perception.height/2+.1 : 1;
-		Ogre::Vector3 shapePose(perception.state.x, perception.state.y, z);
-		Ogre::Quaternion shapeOrientation(Ogre::Radian(perception.state.yaw), Ogre::Vector3::UNIT_Z);
-		Ogre::Vector3 shapeScale (perception.length, perception.width, std::max(0.1f, perception.height));
-		shape->setPosition(shapePose);
-		shape->setOrientation(shapeOrientation);
-		shape->setScale(shapeScale);
-
-		state->setColor(color.r, color.g, color.b, alpha);
-		shape->setColor(color.r, color.g, color.b, shape_alpha);
+		shapeVis->setScale( Ogre::Vector3(perception.length, perception.width, std::max(0.1f, perception.height)) );
+		shapeVis->setPosition( Ogre::Vector3(perception.state.x, perception.state.y, z) );
+		shapeVis->setOrientation( Ogre::Quaternion(Ogre::Radian(perception.state.yaw), Ogre::Vector3::UNIT_Z) );
 
-		// Create a hovering debug text if one is provided
+		// Process the debug text
 		if(!perception.debug.empty())
 		{
-			Ogre::SceneNode* childNode = scene_node_->createChildSceneNode();
-			boost::shared_ptr<rviz::MovableText> debugText { new rviz::MovableText(perception.debug, "Liberation Sans", .7, text_color_property_->getOgreColor()) };
+			Ogre::SceneNode* childNode = frame_node_->createChildSceneNode();
+			boost::shared_ptr<rviz::MovableText> debugText { new rviz::MovableText(perception.debug, "Liberation Sans", .7) };
 			debugText->setTextAlignment(rviz::MovableText::H_CENTER, rviz::MovableText::V_CENTER);
-			shapePose.z = z+perception.height/2+.1+1;
-			childNode->setPosition(shapePose);
+			childNode->setPosition(Ogre::Vector3(perception.state.x, perception.state.y, z+perception.height/2+.1+1) );
 			childNode->attachObject(debugText.get());
 			text_visuals_.push_back(debugText);
 			text_nodes_.push_back(childNode);
 		}
 
-		// And add it to the vector
-		state_visuals_.push_back(state);
-		shape_visuals_.push_back(shape);
-	}
-
 	// Process the buffered trace
 	trace_buffer_.push_back(msg); filterTraceBuffer(); trace_visuals_.clear();
 
@@ -208,14 +185,17 @@ void PerceptionsDisplay::processMessage(const multiception::Perceptions::ConstPt
 				trace_visuals_[p.object_id]->addPoint(Ogre::Vector3(p.state.x, p.state.y, 0), color);
 			}
 		}
+
+		pose_visuals_.push_back(arrowVis);
+		shape_visuals_.push_back(shapeVis);
+		cov_visuals_.push_back(covVis);
 	}
 
+	updateColorAndAlpha();
 	context_->queueRender();
 }
 
 } // end namespace rviz_multiception
 
-// 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(rviz_multiception::PerceptionsDisplay,rviz::Display)

src/perceptions_display.h

@@ -18,28 +18,17 @@ namespace rviz
 class ColorProperty;
 class FloatProperty;
 class IntProperty;
-class CovarianceProperty;
 class Shape;
 class BillboardLine;
 class MovableText;
+class Arrow;
+class CovarianceVisual;
+class CovarianceProperty;
 }
 
-// All the source in this plugin is in its own namespace. This is not
-// required but is good practice.
 namespace rviz_multiception
 {
 
-class StateVisual;
-
-// Here we declare our new subclass of rviz::Display. Every display
-// which can be listed in the "Displays" panel is a subclass of
-// rviz::Display.
-//
-// The PerceptionsDisplay class just implements the vector,
-// editable parameters, and Display subclass machinery. The visuals
-// themselves are represented by separate classes. The
-// idiom for the visuals is that when the objects exist, they appear
-// in the scene, and when they are deleted, they disappear.
 class PerceptionsDisplay: public rviz::MessageFilterDisplay<multiception::Perceptions>
 {
 Q_OBJECT
@@ -49,16 +38,12 @@ public:
 	PerceptionsDisplay();
 	virtual ~PerceptionsDisplay();
 
-	// 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();
+	void resetVectors(size_t N=0);
 
 	// Qt slots connected to signals indicating changes in the user-editable properties.
 private Q_SLOTS:
@@ -70,14 +55,16 @@ private:
 	void processMessage(const multiception::Perceptions::ConstPtr& msg);
 
 	// Storage for the list of visuals
-	std::vector<boost::shared_ptr<StateVisual>> state_visuals_;
+	std::vector<boost::shared_ptr<rviz::Arrow>> pose_visuals_;
 	std::vector<boost::shared_ptr<rviz::Shape>> shape_visuals_;
+	std::vector<boost::shared_ptr<rviz::CovarianceVisual>> cov_visuals_;
 	std::vector<boost::shared_ptr<rviz::MovableText>> text_visuals_;
 	std::map<uint8_t, boost::shared_ptr<rviz::BillboardLine>> trace_visuals_;
 	std::vector<Ogre::SceneNode*> text_nodes_;
 
 	std::vector<multiception::Perceptions::ConstPtr> trace_buffer_;
 	float trace_duration_;
+	Ogre::SceneNode* frame_node_;
 
 	// User-editable property variables.
 	rviz::ColorProperty* color_property_;

src/state_display.cpp

@@ -4,6 +4,8 @@
 #include <tf/transform_listener.h>
 
 #include <rviz/visualization_manager.h>
+#include <rviz/ogre_helpers/arrow.h>
+#include <rviz/default_plugin/covariance_visual.h>
 #include <rviz/properties/color_property.h>
 #include <rviz/properties/float_property.h>
 #include <rviz/default_plugin/covariance_property.h>
@@ -16,8 +18,6 @@
 namespace rviz_multiception
 {
 
-// The constructor must have no arguments, so we can't give the
-// constructor the parameters it needs to fully initialize.
 StateDisplay::StateDisplay()
 {
 	color_property_ = new rviz::ColorProperty( "Color", QColor( 204, 51, 204 ),
@@ -32,72 +32,60 @@ StateDisplay::StateDisplay()
 	                                                     this, SLOT( queueRender() ));
 }
 
-// After the top-level rviz::Display::initialize() does its own setup,
-// it calls the subclass's onInitialize() function. This is where we
-// instantiate all the workings of the class. We make sure to also
-// call our immediate super-class's onInitialize() function, since it
-// does important stuff setting up the message filter.
-//
-//  Note that "MFDClass" is a typedef of
-// ``MessageFilterDisplay<message type>``, to save typing that long
-// templated class name every time you need to refer to the
-// superclass.
 void StateDisplay::onInitialize()
 {
 	MFDClass::onInitialize();
+	frame_node_ = scene_node_->createChildSceneNode();
 }
 
 StateDisplay::~StateDisplay()
 {
+	frame_node_->detachAllObjects();
+	frame_node_->removeAndDestroyAllChildren();
+	scene_manager_->destroySceneNode(frame_node_);
 }
 
-// Clear the visuals by deleting their objects.
 void StateDisplay::reset()
 {
 	MFDClass::reset();
-	visual_.reset();
+	pose_visual_.reset();
+	cov_visual_.reset();
 }
 
-// Set the current color and alpha values for each visual.
 void StateDisplay::updateColorAndAlpha()
 {
-	if(visual_)
+	if(pose_visual_)
 	{
-		float alpha = alpha_property_->getFloat();
-		Ogre::ColourValue color = color_property_->getOgreColor();
-		visual_->setColor(color.r, color.g, color.b, alpha);
+		const float alpha = alpha_property_->getFloat();
+		const Ogre::ColourValue color = color_property_->getOgreColor();
+
+		pose_visual_->setColor(color.r, color.g, color.b, alpha);
+// 		cov_visual_->setColor(color.r, color.g, color.b, alpha);
 	}
 }
 
-// This is our callback to handle an incoming message.
 void StateDisplay::processMessage(const multiception::StateStamped::ConstPtr& msg)
 {
-	// Here we call the rviz::FrameManager to get the transform from the
-	// fixed frame to the frame in the header of this message. If
-	// it fails, we can't do anything else so we return.
-	Ogre::Quaternion orientation;
 	Ogre::Vector3 position;
-
-	if(!context_->getFrameManager()->getTransform(msg->header.frame_id, msg->header.stamp, position, orientation))
+	Ogre::Quaternion orientation;
+	if(context_->getFrameManager()->getTransform(msg->header.frame_id, msg->header.stamp, position, orientation))
+	{
+		frame_node_->setPosition(position);
+		frame_node_->setOrientation(orientation);
+	}
+	else
 	{
 		ROS_DEBUG("Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable(fixed_frame_));
 		return;
 	}
 
-	visual_.reset(new StateVisual(context_->getSceneManager(), scene_node_, covariance_property_));
-
-	// Now set or update the contents of the visual.
-	visual_->setContent(msg->state);
-	visual_->setFramePosition(position);
-	visual_->setFrameOrientation(orientation);
+	pose_visual_.reset( new rviz::Arrow(scene_manager_, frame_node_) );
+	cov_visual_ = covariance_property_->createAndPushBackVisual(scene_manager_, frame_node_);
 
-	updateColorAndAlpha();
-	context_->queueRender();
+	computeStateObjects(msg->state, *pose_visual_, *cov_visual_);
 }
 
 } // end namespace rviz_multiception
 
-// 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(rviz_multiception::StateDisplay,rviz::Display)

src/state_display.h

@@ -15,28 +15,17 @@ class SceneNode;
 
 namespace rviz
 {
+class Arrow;
+class CovarianceVisual;
 class ColorProperty;
 class FloatProperty;
 class IntProperty;
 class CovarianceProperty;
 }
 
-// All the source in this plugin is in its own namespace. This is not
-// required but is good practice.
 namespace rviz_multiception
 {
 
-class StateVisual;
-
-// Here we declare our new subclass of rviz::Display. Every display
-// which can be listed in the "Displays" panel is a subclass of
-// rviz::Display.
-//
-// The StateDisplay class just implements the vector,
-// editable parameters, and Display subclass machinery. The visuals
-// themselves are represented by a separate class, StateVisual. The
-// idiom for the visuals is that when the objects exist, they appear
-// in the scene, and when they are deleted, they disappear.
 class StateDisplay: public rviz::MessageFilterDisplay<multiception::StateStamped>
 {
 Q_OBJECT
@@ -66,7 +55,10 @@ private:
 	void processMessage(const multiception::StateStamped::ConstPtr& msg);
 
 	// Storage of the visual
-	boost::shared_ptr<StateVisual> visual_;
+	boost::shared_ptr<rviz::Arrow> pose_visual_;
+	boost::shared_ptr<rviz::CovarianceVisual> cov_visual_;
+
+	Ogre::SceneNode* frame_node_;
 
 	// User-editable property variables.
 	rviz::ColorProperty* color_property_;

src/state_visual.cpp

 #include <OGRE/OgreVector3.h>
 #include <OGRE/OgreSceneNode.h>
-#include <OGRE/OgreSceneManager.h>
 
 #include <ros/console.h>
 #include <rviz/ogre_helpers/arrow.h>
 #include <rviz/default_plugin/covariance_visual.h>
-#include <rviz/default_plugin/covariance_property.h>
 #include <geometry_msgs/Vector3.h>
 
 #include <tf2/LinearMath/Quaternion.h>
@@ -16,92 +14,46 @@
 namespace rviz_multiception
 {
 
-StateVisual::StateVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, rviz::CovarianceProperty* covariance_property)
-{
-	scene_manager_ = scene_manager;
-
-	// Ogre::SceneNode s form a tree, with each node storing the
-	// transform (position and orientation) of itself relative to its
-	// parent. Ogre does the math of combining those transforms when it
-	// is time to render.
-	//
-	// Here we create a node to store the pose of the header frame
-	// relative to the RViz fixed frame.
-	frame_node_ = parent_node->createChildSceneNode();
-
-	// We create the arrow object within the frame node so that we can
-	// set its position and direction relative to its header frame.
-	pose_arrow_.reset(new rviz::Arrow(scene_manager_, frame_node_));
-	covariance_ = covariance_property->createAndPushBackVisual(scene_manager, frame_node_);
-}
-
-StateVisual::~StateVisual()
-{
-	// Destroy the frame node since we don't need it anymore.
-	scene_manager_->destroySceneNode(frame_node_);
-}
-
-void StateVisual::setContent(const multiception::State& state)
-{
-	// Convert the geometry_msgs::Vector3 to an Ogre::Vector3.
+void computeStateObjects(const multiception::State& state, rviz::Arrow& arrowVis, rviz::CovarianceVisual& covVis) {
+	// Process the pose arrow, scaled by velocity and oriented by heading
 	Ogre::Vector3 pose(state.x, state.y, 1.5);
-	pose_arrow_->setPosition(pose);
+	arrowVis.setPosition(pose);
 
-	// Scale the arrow by the bounded velocity
+	// Special case, if yaw is 0, we consider the state to be static
+	// without orientation (e.g: pole) and displaying an orientation makes no sense
 	float length = std::min(4.0, std::max(1.0, state.v/5.0));
-	Ogre::Vector3 scale(length, 1, 1);
-	pose_arrow_->setScale(scale);
+	if(state.v<=0) arrowVis.setScale(Ogre::Vector3(0.1, 0.1, 0.1));
+	else arrowVis.setScale(Ogre::Vector3(length, 1, 1));
 
-	// Set the orientation of the arrow according to the heading.
 	Ogre::Vector3 direction(std::cos(state.yaw), std::sin(state.yaw), 0);
-	pose_arrow_->setDirection(direction);
+	arrowVis.setDirection(direction);
 
-	// Set the covariance ellipsis
+	// Process the covariance ellipsis
 	std::array<double, 36> covTot;
 	for(size_t i=0;i<covTot.size(); i++) covTot[i] = state.cov[i] + state.cov_ind[i];
 
 	geometry_msgs::PoseWithCovariance cov;
-	//cov.pose.position.x = state.x; cov.pose.position.y = state.y;
-	tf2::Quaternion q; 	q.setRPY(0.0, 0.0, state.yaw);
+	cov.pose.position.x = state.x; cov.pose.position.y = state.y;
+	tf2::Quaternion q; q.setRPY(0.0, 0.0, state.yaw);
 	cov.pose.orientation = tf2::toMsg(q);
 	cov.covariance[0*6+0] = covTot[0*6+0]; // xx
 	cov.covariance[0*6+1] = covTot[0*6+1]; // xy
 	cov.covariance[1*6+0] = covTot[1*6+0]; // yx
 	cov.covariance[1*6+1] = covTot[1*6+1]; // yy
 
-	// Special case, if yaw is 0, we consider the state to be static
-	// without orientation (e.g: pole) and displaying an orientation makes no sense
-	if(state.v==0) pose_arrow_->setScale(Ogre::Vector3(0.1, 0.1, 0.1));
-	else if(covTot[2*6+2]<M_PI)
+	if(state.v>0)
 	{
 		cov.covariance[0*6+5] = covTot[0*6+2]; // xyaw
 		cov.covariance[1*6+5] = covTot[1*6+2]; // yyaw
 		cov.covariance[5*6+0] = covTot[2*6+0]; // yawx
 		cov.covariance[5*6+1] = covTot[2*6+1]; // yawy
-		cov.covariance[5*6+5] = std::max(.0, covTot[2*6+2]); // yawyaw
+		cov.covariance[5*6+5] = std::max(0., std::min(2., covTot[2*6+2])); // yawyaw
 	}
 
-	covariance_->setCovariance(cov);
-	covariance_->setPosition(pose);
 	Ogre::Quaternion covq(Ogre::Radian(state.yaw), Ogre::Vector3::UNIT_Z);
-	covariance_->setOrientation(covq);
-}
-
-// Position and orientation are passed through to the SceneNode.
-void StateVisual::setFramePosition(const Ogre::Vector3& position)
-{
-	frame_node_->setPosition(position);
-}
-
-void StateVisual::setFrameOrientation(const Ogre::Quaternion& orientation)
-{
-	frame_node_->setOrientation(orientation);
-}
-
-// Color is passed through to the Arrow object.
-void StateVisual::setColor(float r, float g, float b, float a)
-{
-	pose_arrow_->setColor(r, g, b, a);
+	covVis.setCovariance(cov);
+	covVis.setPosition(pose);
+	covVis.setOrientation(covq);
 }
 
 } // namespace rviz_multiception

src/state_visual.h

@@ -3,66 +3,16 @@
 
 #include <multiception/Perception.h>
 
-namespace Ogre
-{
-class Vector3;
-class Quaternion;
-}
-
 namespace rviz
 {
 class Arrow;
 class CovarianceVisual;
-class CovarianceProperty;
 }
 
 namespace rviz_multiception
 {
 
-// Declare the visual class for this display.
-//
-// Each instance of StateVisual represents the visualization of a single
-// multiception::Perception message. Currently it just shows an arrow with
-// the direction and magnitude of the acceleration vector, but could
-// easily be expanded to include more of the message data.
-class StateVisual
-{
-public:
-	// Constructor. Creates the visual stuff and puts it into the
-	// scene, but in an unconfigured state.
-	StateVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, rviz::CovarianceProperty* covariance_property);
-
-	// Destructor. Removes the visual stuff from the scene.
-	virtual ~StateVisual();
-
-	// Configure the visual to show the data in the message.
-	void setContent(const multiception::State& state);
-
-	// Set the pose of the coordinate frame the message refers to.
-	// These could be done inside setMessage(), but that would require
-	// calls to FrameManager and error handling inside setMessage(),
-	// which doesn't seem as clean. This way PerceptionVisual is only
-	// responsible for visualization.
-	void setFramePosition(const Ogre::Vector3& position);
-	void setFrameOrientation(const Ogre::Quaternion& orientation);
-
-	// Set the color and alpha of the visual, which are user-editable
-	// parameters and therefore don't come from the message.
-	void setColor(float r, float g, float b, float a);
-
-private:
-	// The object implementing the actual arrow shape
-	boost::shared_ptr<rviz::Arrow> pose_arrow_;
-	boost::shared_ptr<rviz::CovarianceVisual> covariance_;
-
-	// A SceneNode whose pose is set to match the coordinate frame of
-	// the Imu message header.
-	Ogre::SceneNode* frame_node_;
-
-	// The SceneManager, kept here only so the destructor can ask it to
-	// destroy the ``frame_node_``.
-	Ogre::SceneManager* scene_manager_;
-};
+void computeStateObjects(const multiception::State& state, rviz::Arrow& arrowVis, rviz::CovarianceVisual& covVis);
 
 } // namespace rviz_multiception