diff --git a/src/perceptions_display.cpp b/src/perceptions_display.cpp
index 8c68a5f48b2fb5c69cab1fffdfde0dcba28d47c5..618afdf925f7541966dcbd561fca9e1771dc089e 100644
--- a/src/perceptions_display.cpp
+++ b/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();
}
diff --git a/src/perceptions_display.h b/src/perceptions_display.h
index 2b46291b3308ee5fef9f2ccf07f2ef0b838be172..3c9cd118c9d16897bb26b94b0360fa8227f7e46f 100644
--- a/src/perceptions_display.h
+++ b/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_;
};
diff --git a/src/state_display.cpp b/src/state_display.cpp
index 1e3cd6754c70bede4a66cebac16b8065852c995a..72dcf1f38722e060ca4b7b51eb91627beb9bf457 100644
--- a/src/state_display.cpp
+++ b/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.
diff --git a/src/state_visual.cpp b/src/state_visual.cpp
index 7ae597616f4f7df6b2e89369c18ca9db258f414b..7e2405d5283a27a94a26a2a7032d4ba32e6b1111 100644
--- a/src/state_visual.cpp
+++ b/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.