Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <tf/transform_listener.h>
#include <rviz/visualization_manager.h>
#include <rviz/properties/color_property.h>
#include <rviz/properties/float_property.h>
#include <rviz/properties/int_property.h>
#include <rviz/frame_manager.h>
#include "perception_visual.h"
#include "perceptions_display.h"
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( 204, 51, 204 ),
"Color to draw the arrows.",
this, SLOT( updateColorAndAlpha() ));
alpha_property_ = new rviz::FloatProperty( "Alpha", 1.0,
"Object transparency, 0 is fully transparent, 1.0 is fully opaque.",
this, SLOT( updateColorAndAlpha() ));
}
// 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();
}
PerceptionsDisplay::~PerceptionsDisplay()
{
}
// Clear the visuals by deleting their objects.
void PerceptionsDisplay::reset()
{
MFDClass::reset();
visuals_.clear();
}
// Set the current color and alpha values for each visual.
void PerceptionsDisplay::updateColorAndAlpha()
{
float alpha = alpha_property_->getFloat();
Ogre::ColourValue color = color_property_->getOgreColor();
for(size_t i=0; i<visuals_.size(); i++)
{
visuals_[i]->setColor(color.r, color.g, color.b, alpha);
}
}
// 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)
{
return;
}
// We are keeping a vector of visual pointers as well as the originating message to avoid it
// being cleared while visual still references its content.
msg_ = boost::static_pointer_cast<const multiception::Perceptions>(msg);
visuals_.clear();
visuals_.reserve(N);
for(const multiception::Perception& perception : msg->perceptions)
{
Ogre::Quaternion orientation;
Ogre::Vector3 position;
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_));
return;
}
boost::shared_ptr<PerceptionVisual> visual(new PerceptionVisual(context_->getSceneManager(), scene_node_));
// Now set or update the contents of the chosen visual.
visual->setMessage(&perception);
visual->setFramePosition(position);
visual->setFrameOrientation(orientation);
float alpha = alpha_property_->getFloat();
Ogre::ColourValue color = color_property_->getOgreColor();
visual->setColor(color.r, color.g, color.b, alpha);
// And add it to the vector
visuals_.push_back(visual);
}
//~ 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)