30 #include <boost/bind.hpp> 32 #include <OgreManualObject.h> 33 #include <OgreMaterialManager.h> 34 #include <OgreSceneManager.h> 35 #include <OgreSceneNode.h> 36 #include <OgreTextureManager.h> 37 #include <OgreTechnique.h> 38 #include <OgreSharedPtr.h> 71 void visit( Ogre::Renderable *rend, ushort lodIndex,
bool isDebug, Ogre::Any *pAny=0)
81 : parent_(parent), manual_object_(
NULL ), x_(x), y_(y), width_(width), height_(height)
84 static int material_count = 0;
86 ss <<
"MapMaterial" << material_count++;
87 material_ = Ogre::MaterialManager::getSingleton().getByName(
"rviz/Indexed8BitImage");
91 material_->getTechnique(0)->setLightingEnabled(
false);
93 material_->setCullingMode( Ogre::CULL_NONE );
96 static int map_count = 0;
97 std::stringstream ss2;
98 ss2 <<
"MapObject" << map_count++;
101 static int node_count = 0;
102 std::stringstream ss3;
103 ss3 <<
"NodeObject" << node_count++;
148 scene_node_->setPosition( x*resolution, y*resolution, 0 );
149 scene_node_->setScale( width*resolution, height*resolution, 1.0 );
169 Ogre::Pass* pass =
material_->getTechnique( 0 )->getPass( 0 );
170 Ogre::TextureUnitState* tex_unit =
NULL;
172 material_->setSceneBlending( sceneBlending );
173 material_->setDepthWriteEnabled( depthWrite );
183 unsigned char* pixels =
new unsigned char[pixels_size];
184 memset(pixels, 255, pixels_size);
185 unsigned char* ptr = pixels;
190 int index = yy * fw +
x_;
191 int pixels_to_copy = std::min((
int)
width_, N-index);
194 if(index+pixels_to_copy>=N)
break;
197 Ogre::DataStreamPtr pixel_stream;
198 pixel_stream.bind(
new Ogre::MemoryDataStream( pixels, pixels_size ));
202 Ogre::TextureManager::getSingleton().remove(
texture_->getName() );
206 static int tex_count = 0;
207 std::stringstream ss;
208 ss <<
"MapTexture" << tex_count++;
209 texture_ = Ogre::TextureManager::getSingleton().loadRawData( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
210 pixel_stream,
width_,
height_, Ogre::PF_L8, Ogre::TEX_TYPE_2D,
220 , resolution_( 0.0
f )
226 QString::fromStdString( ros::message_traits::datatype<nav_msgs::OccupancyGrid>() ),
227 "nav_msgs::OccupancyGrid topic to subscribe to.",
231 "Amount of transparency to apply to the map.",
244 "Rendering option, controls whether or not the map is always" 245 " drawn behind everything else.",
249 "Resolution of the map. (not editable)",
this );
253 "Width of the map, in meters. (not editable)",
this );
257 "Height of the map, in meters. (not editable)",
this );
261 "Position of the bottom left corner of the map, in meters. (not editable)",
266 "Orientation of the map. (not editable)",
271 "Prefer UDP topic transport",
276 "Use map header timestamp when transforming",
285 for (
unsigned i=0; i <
swatches.size(); i++){
293 unsigned char* palette =
new unsigned char[256*4];
294 unsigned char* palette_ptr = palette;
296 for(
int i = 0; i <= 100; i++ )
298 unsigned char v = 255 - (255 * i) / 100;
302 *palette_ptr++ = 255;
305 for(
int i = 101; i <= 127; i++ )
308 *palette_ptr++ = 255;
310 *palette_ptr++ = 255;
313 for(
int i = 128; i <= 254; i++ )
315 *palette_ptr++ = 255;
316 *palette_ptr++ = (255*(i-128))/(254-128);
318 *palette_ptr++ = 255;
321 *palette_ptr++ = 0x70;
322 *palette_ptr++ = 0x89;
323 *palette_ptr++ = 0x86;
324 *palette_ptr++ = 255;
331 unsigned char* palette =
new unsigned char[256*4];
332 unsigned char* palette_ptr = palette;
341 for(
int i = 1; i <= 98; i++ )
343 unsigned char v = (255 * i) / 100;
346 *palette_ptr++ = 255 - v;
347 *palette_ptr++ = 255;
351 *palette_ptr++ = 255;
352 *palette_ptr++ = 255;
353 *palette_ptr++ = 255;
355 *palette_ptr++ = 255;
357 *palette_ptr++ = 255;
358 *palette_ptr++ = 255;
360 for(
int i = 101; i <= 127; i++ )
363 *palette_ptr++ = 255;
365 *palette_ptr++ = 255;
368 for(
int i = 128; i <= 254; i++ )
370 *palette_ptr++ = 255;
371 *palette_ptr++ = (255*(i-128))/(254-128);
373 *palette_ptr++ = 255;
376 *palette_ptr++ = 0x70;
377 *palette_ptr++ = 0x89;
378 *palette_ptr++ = 0x86;
379 *palette_ptr++ = 255;
386 unsigned char* palette =
new unsigned char[256*4];
387 unsigned char* palette_ptr = palette;
389 for(
int i = 0; i < 256; i++ )
394 *palette_ptr++ = 255;
402 Ogre::DataStreamPtr palette_stream;
403 palette_stream.bind(
new Ogre::MemoryDataStream( palette_bytes, 256*4 ));
405 static int palette_tex_count = 0;
406 std::stringstream ss;
407 ss <<
"MapPaletteTexture" << palette_tex_count++;
408 return Ogre::TextureManager::getSingleton().loadRawData( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
409 palette_stream, 256, 1, Ogre::PF_BYTE_RGBA, Ogre::TEX_TYPE_1D, 0 );
479 Ogre::SceneBlendType sceneBlending;
484 sceneBlending = Ogre::SBT_TRANSPARENT_ALPHA;
489 sceneBlending = Ogre::SBT_REPLACE;
495 for (
unsigned i=0; i <
swatches.size(); i++) {
496 swatches[i]->updateAlpha(sceneBlending, depthWrite, &alpha_setter);
506 for (
unsigned i=0; i <
swatches.size(); i++)
507 swatches[i]->material_->setDepthWriteEnabled( !draw_under );
510 int group = draw_under ? Ogre::RENDER_QUEUE_4 : Ogre::RENDER_QUEUE_MAIN;
511 for (
unsigned i=0; i <
swatches.size(); i++){
513 swatches[i]->manual_object_->setRenderQueueGroup( group );
533 for (
unsigned i=0; i <
swatches.size(); i++){
535 swatches[i]->manual_object_->setVisible(
false );
537 if( !
swatches[i]->texture_.isNull() )
539 Ogre::TextureManager::getSingleton().remove(
swatches[i]->texture_->getName() );
583 for(
size_t y = 0;
y < update->height;
y++ )
586 &update->data[
y * update->width ],
603 for(
int i=0;i<4;i++){
604 ROS_INFO(
"Creating %d swatches", n_swatches);
605 for (
unsigned i=0; i <
swatches.size(); i++){
613 for(
int i=0;i<n_swatches;i++){
615 if(width - x - sw >= sw)
620 if(height - y - sh >= sh)
636 }
catch(Ogre::RenderingAPIException&)
638 ROS_WARN(
"Failed to create %d swatches", n_swatches);
661 ROS_WARN_ONCE_NAMED(
"quaternions",
"Map received on topic '%s' contains unnormalized quaternions. " 662 "This warning will only be output once but may be true for others; " 663 "enable DEBUG messages for ros.rviz.quaternions to see more details.",
665 ROS_DEBUG_NAMED(
"quaternions",
"Map received on topic '%s' contains unnormalized quaternions.",
671 std::stringstream ss;
679 ROS_DEBUG(
"Received a %d X %d map @ %.3f m/pix\n",
696 Ogre::Vector3 position(
current_map_.info.origin.position.x,
699 Ogre::Quaternion orientation;
708 bool map_status_set =
false;
711 std::stringstream ss;
712 ss <<
"Data size doesn't match width*height: width = " << width
713 <<
", height = " << height <<
", data size = " <<
current_map_.data.size();
715 map_status_set =
true;
721 Ogre::Pass* pass =
swatches[i]->material_->getTechnique(0)->getPass(0);
722 Ogre::TextureUnitState* tex_unit =
NULL;
723 if (pass->getNumTextureUnitStates() > 0)
725 tex_unit = pass->getTextureUnitState(0);
729 tex_unit = pass->createTextureUnitState();
732 tex_unit->setTextureName(
swatches[i]->texture_->getName());
733 tex_unit->setTextureFiltering( Ogre::TFO_NONE );
734 swatches[i]->manual_object_->setVisible(
true );
738 if( !map_status_set )
759 for (
unsigned i=0; i <
swatches.size(); i++){
760 Ogre::Pass* pass =
swatches[i]->material_->getTechnique(0)->getPass(0);
761 Ogre::TextureUnitState* palette_tex_unit =
NULL;
762 if( pass->getNumTextureUnitStates() > 1 )
764 palette_tex_unit = pass->getTextureUnitState( 1 );
768 palette_tex_unit = pass->createTextureUnitState();
771 palette_tex_unit->setTextureFiltering( Ogre::TFO_NONE );
791 Ogre::Vector3 position;
792 Ogre::Quaternion orientation;
796 ROS_DEBUG(
"Error transforming map '%s' from frame '%s' to frame '%s'",
800 "No transform from [" + QString::fromStdString(
frame_ ) +
"] to [" +
fixed_frame_ +
"]" );
virtual bool setValue(const QVariant &new_value)
Set the new value for this property. Returns true if the new value is different from the old value...
unsigned char * makeCostmapPalette()
Ogre::TexturePtr makePaletteTexture(unsigned char *palette_bytes)
virtual void setString(const QString &str)
unsigned char * makeMapPalette()
Ogre::ManualObject * manual_object_
TransportHints & reliable()
virtual bool setVector(const Ogre::Vector3 &vector)
virtual void reset()
Called to tell the display to clear its state.
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
virtual void setTopic(const QString &topic, const QString &datatype)
Set the ROS topic to listen to for this display.
std::vector< Ogre::TexturePtr > palette_textures_
QuaternionProperty * orientation_property_
std::vector< bool > color_scheme_transparency_
ros::NodeHandle update_nh_
A NodeHandle whose CallbackQueue is run from the main GUI thread (the "update" thread).
Ogre::TexturePtr texture_
virtual void onInitialize()
Override this function to do subclass-specific initialization.
FloatProperty * alpha_property_
virtual float getFloat() const
void incomingUpdate(const map_msgs::OccupancyGridUpdate::ConstPtr &update)
Copy update's data into current_map_ and call showMap().
unsigned char * makeRawPalette()
virtual void setReadOnly(bool read_only)
Overridden from Property to propagate read-only-ness to children.
Ogre::MaterialPtr material_
Property specialized to enforce floating point max/min.
TFSIMD_FORCE_INLINE const tfScalar & y() const
Property specialized to provide max/min enforcement for integers.
ros::Subscriber update_sub_
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
VectorProperty * position_property_
FloatProperty * resolution_property_
virtual bool setValue(const QVariant &new_value)
Set the new value for this property. Returns true if the new value is different from the old value...
bool isEnabled() const
Return true if this Display is enabled, false if not.
virtual bool getBool() const
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
virtual void fixedFrameChanged()
Called by setFixedFrame(). Override to respond to changes to fixed_frame_.
#define ROS_DEBUG_NAMED(name,...)
TransportHints & unreliable()
virtual void unsubscribe()
bool validateFloats(const sensor_msgs::CameraInfo &msg)
virtual void addOption(const QString &option, int value=0)
virtual void reset()
Called to tell the display to clear its state.
virtual void onEnable()
Derived classes override this to do the actual work of enabling themselves.
std::vector< Swatch * > swatches
virtual void onDisable()
Derived classes override this to do the actual work of disabling themselves.
void showMap()
Show current_map_ in the scene.
virtual void update(float wall_dt, float ros_dt)
Called periodically by the visualization manager.
virtual void setReadOnly(bool read_only)
Overridden from Property to propagate read-only-ness to children.
virtual bool setQuaternion(const Ogre::Quaternion &quaternion)
void updateAlpha(const Ogre::SceneBlendType sceneBlending, bool depthWrite, AlphaSetter *alpha_setter)
void incomingMap(const nav_msgs::OccupancyGrid::ConstPtr &msg)
Copy msg into current_map_ and call showMap().
bool transform(const Header &header, const geometry_msgs::Pose &pose, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
Transform a pose from a frame into the fixed frame.
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
EnumProperty * color_scheme_property_
TFSIMD_FORCE_INLINE const tfScalar & x() const
Displays a map along the XY plane.
RosTopicProperty * topic_property_
IntProperty * height_property_
float normalizeQuaternion(float &w, float &x, float &y, float &z)
#define ROS_WARN_ONCE_NAMED(name,...)
Ogre::SceneManager * scene_manager_
A convenience variable equal to context_->getSceneManager().
Property * draw_under_property_
BoolProperty * unreliable_property_
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
BoolProperty * transform_timestamp_property_
Property(const QString &name=QString(), const QVariant default_value=QVariant(), const QString &description=QString(), Property *parent=0, const char *changed_slot=0, QObject *receiver=0)
Constructor.
bool validateQuaternions(const visualization_msgs::InteractiveMarker &marker)
virtual QVariant getValue() const
Return the value of this Property as a QVariant. If the value has never been set, an invalid QVariant...
Ogre::SceneNode * scene_node_
IntProperty * width_property_
std::string getTopicStd() const
virtual void setReadOnly(bool read_only)
Prevent or allow users to edit this property from a PropertyTreeWidget.
virtual int getOptionInt()
Return the int value of the currently-chosen option, or 0 if the current option string does not have ...
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void visit(Ogre::Renderable *rend, ushort lodIndex, bool isDebug, Ogre::Any *pAny=0)
BoolProperty(const QString &name=QString(), bool default_value=false, const QString &description=QString(), Property *parent=0, const char *changed_slot=0, QObject *receiver=0)
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
Show status level and text. This is thread-safe.
virtual QString getName() const
Return the name of this Property as a QString.
nav_msgs::OccupancyGrid current_map_
Swatch(MapDisplay *parent, unsigned int x, unsigned int y, unsigned int width, unsigned int height, float resolution)
void mapUpdated()
Emitted when a new map is received.