Fixed occasional rviz warning

src/perceptions_display.cpp

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

src/perceptions_display.h

@@ -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_;
 };

src/state_display.cpp

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

src/state_visual.cpp

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