Skip to content
Snippets Groups Projects
Commit de6a7409 authored by Antoine Lima's avatar Antoine Lima
Browse files

Fixed occasional rviz warning

parent 316c0c20
No related branches found
No related tags found
No related merge requests found
......@@ -8,6 +8,7 @@
#include <rviz/properties/float_property.h>
#include <rviz/default_plugin/covariance_property.h>
#include <rviz/properties/int_property.h>
#include <rviz/ogre_helpers/shape.h>
#include <rviz/frame_manager.h>
#include "state_visual.h"
......@@ -25,7 +26,11 @@ PerceptionsDisplay::PerceptionsDisplay()
this, SLOT( updateColorAndAlpha() ));
alpha_property_ = new rviz::FloatProperty( "Alpha", 1.0,
"Object transparency, 0 is fully transparent, 1.0 is fully opaque.",
"State transparency, 0 is fully transparent, 1.0 is fully opaque.",
this, SLOT( updateColorAndAlpha() ));
shape_alpha_property_ = new rviz::FloatProperty( "Shape Alpha", 0.4,
"Shape transparency, 0 is fully transparent, 1.0 is fully opaque.",
this, SLOT( updateColorAndAlpha() ));
covariance_property_ = new rviz::CovarianceProperty( "Covariance", true, "Whether or not the covariances of the messages should be shown.",
......@@ -55,18 +60,21 @@ PerceptionsDisplay::~PerceptionsDisplay()
void PerceptionsDisplay::reset()
{
MFDClass::reset();
visuals_.clear();
state_visuals_.clear();
shape_visuals_.clear();
}
// 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();
for(size_t i=0; i<visuals_.size(); i++)
for(size_t i=0; i<state_visuals_.size(); i++)
{
visuals_[i]->setColor(color.r, color.g, color.b, alpha);
state_visuals_[i]->setColor(color.r, color.g, color.b, alpha);
shape_visuals_[i]->setColor(color.r, color.g, color.b, shape_alpha);
}
}
......@@ -83,8 +91,8 @@ void PerceptionsDisplay::processMessage(const multiception::Perceptions::ConstPt
}
// We are keeping a vector of state visual pointers
visuals_.clear();
visuals_.reserve(N);
state_visuals_.clear(); shape_visuals_.clear();
state_visuals_.reserve(N); shape_visuals_.reserve(N);
for(const multiception::Perception& perception : msg->perceptions)
{
......@@ -98,20 +106,32 @@ void PerceptionsDisplay::processMessage(const multiception::Perceptions::ConstPt
}
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_));
// Now set or update the contents of the chosen visual.
// 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
Ogre::Vector3 shapePose(perception.state.x, perception.state.y, 0.1);
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);
float alpha = alpha_property_->getFloat();
float shape_alpha = shape_alpha_property_->getFloat();
Ogre::ColourValue color = color_property_->getOgreColor();
state->setColor(color.r, color.g, color.b, alpha);
shape->setColor(color.r, color.g, color.b, shape_alpha);
// And add it to the vector
visuals_.push_back(state);
state_visuals_.push_back(state);
shape_visuals_.push_back(shape);
}
context_->queueRender();
}
......
......@@ -19,6 +19,7 @@ class ColorProperty;
class FloatProperty;
class IntProperty;
class CovarianceProperty;
class Shape;
}
// All the source in this plugin is in its own namespace. This is not
......@@ -66,11 +67,13 @@ private:
void processMessage(const multiception::Perceptions::ConstPtr& msg);
// Storage for the list of visuals
std::vector<boost::shared_ptr<StateVisual> > visuals_;
std::vector<boost::shared_ptr<StateVisual> > state_visuals_;
std::vector<boost::shared_ptr<rviz::Shape> > shape_visuals_;
// User-editable property variables.
rviz::ColorProperty* color_property_;
rviz::FloatProperty* alpha_property_;
rviz::FloatProperty* shape_alpha_property_;
rviz::IntProperty* history_length_property_;
rviz::CovarianceProperty* covariance_property_;
};
......
......@@ -61,9 +61,12 @@ void StateDisplay::reset()
// Set the current color and alpha values for each visual.
void StateDisplay::updateColorAndAlpha()
{
float alpha = alpha_property_->getFloat();
Ogre::ColourValue color = color_property_->getOgreColor();
visual_->setColor(color.r, color.g, color.b, alpha);
if(visual_)
{
float alpha = alpha_property_->getFloat();
Ogre::ColourValue color = color_property_->getOgreColor();
visual_->setColor(color.r, color.g, color.b, alpha);
}
}
// This is our callback to handle an incoming message.
......
......@@ -2,6 +2,7 @@
#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>
......@@ -43,11 +44,11 @@ StateVisual::~StateVisual()
void StateVisual::setContent(const multiception::State& state)
{
// Convert the geometry_msgs::Vector3 to an Ogre::Vector3.
Ogre::Vector3 pose(state.x, state.y, 0);
Ogre::Vector3 pose(state.x, state.y, 1.5);
pose_arrow_->setPosition(pose);
// Scale the arrow by the bounded velocity.
float length = std::min(4.0, std::max(1.0, state.v));
// Scale the arrow by the bounded velocity
float length = std::min(4.0, std::max(1.0, state.v/5.0));
Ogre::Vector3 scale(length, 1, 1);
pose_arrow_->setScale(scale);
......@@ -62,17 +63,25 @@ void StateVisual::setContent(const multiception::State& state)
cov.pose.orientation = tf2::toMsg(q);
cov.covariance[0*6+0] = state.cov[0*6+0]; // xx
cov.covariance[0*6+1] = state.cov[0*6+1]; // xy
cov.covariance[0*6+5] = state.cov[0*6+2]; // xyaw
cov.covariance[1*6+0] = state.cov[1*6+0]; // yx
cov.covariance[1*6+1] = state.cov[1*6+1]; // yy
cov.covariance[1*6+5] = state.cov[1*6+2]; // yyaw
cov.covariance[5*6+0] = state.cov[2*6+0]; // yawx
cov.covariance[5*6+1] = state.cov[2*6+1]; // yawy
cov.covariance[5*6+5] = state.cov[2*6+2]; // yawyaw
// 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(state.cov[2*6+2]<M_PI)
{
cov.covariance[0*6+5] = state.cov[0*6+2]; // xyaw
cov.covariance[1*6+5] = state.cov[1*6+2]; // yyaw
cov.covariance[5*6+0] = state.cov[2*6+0]; // yawx
cov.covariance[5*6+1] = state.cov[2*6+1]; // yawy
cov.covariance[5*6+5] = std::max(.0, state.cov[2*6+2]); // yawyaw
}
covariance_->setCovariance(cov);
covariance_->setPosition(pose);
covariance_->setOrientation(Ogre::Quaternion(Ogre::Radian(state.yaw), Ogre::Vector3::UNIT_Z));
Ogre::Quaternion covq(Ogre::Radian(state.yaw), Ogre::Vector3::UNIT_Z);
covariance_->setOrientation(covq);
}
// Position and orientation are passed through to the SceneNode.
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment