From 3b2c57462408f6d214dc4f72f5787aa113a2dff3 Mon Sep 17 00:00:00 2001
From: Antoine Lima <antoine.lima@hds.utc.fr>
Date: Tue, 16 Feb 2021 13:16:13 +0100
Subject: [PATCH] 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 | 142 ++++++++++++++++--------------------
src/perceptions_display.h | 27 ++-----
src/state_display.cpp | 60 ++++++---------
src/state_display.h | 20 ++---
src/state_visual.cpp | 80 ++++----------------
src/state_visual.h | 52 +------------
6 files changed, 115 insertions(+), 266 deletions(-)
diff --git a/src/perceptions_display.cpp b/src/perceptions_display.cpp
index 1caa158..4e5b689 100644
--- a/src/perceptions_display.cpp
+++ b/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)
diff --git a/src/perceptions_display.h b/src/perceptions_display.h
index 0bc339b..c564d44 100644
--- a/src/perceptions_display.h
+++ b/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_;
diff --git a/src/state_display.cpp b/src/state_display.cpp
index 72dcf1f..c83380b 100644
--- a/src/state_display.cpp
+++ b/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)
diff --git a/src/state_display.h b/src/state_display.h
index caa812c..d0dfa26 100644
--- a/src/state_display.h
+++ b/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_;
diff --git a/src/state_visual.cpp b/src/state_visual.cpp
index 2d80ff6..f4d5639 100644
--- a/src/state_visual.cpp
+++ b/src/state_visual.cpp
@@ -1,11 +1,9 @@
#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
diff --git a/src/state_visual.h b/src/state_visual.h
index 42e0fd6..3c92782 100644
--- a/src/state_visual.h
+++ b/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
--
GitLab