MapGraphDisplay.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include <boost/bind.hpp>
00029 
00030 #include <OgreSceneNode.h>
00031 #include <OgreSceneManager.h>
00032 #include <OgreManualObject.h>
00033 #include <OgreBillboardSet.h>
00034 #include <OgreMatrix4.h>
00035 
00036 #include <tf/transform_listener.h>
00037 
00038 #include <rviz/display_context.h>
00039 #include <rviz/frame_manager.h>
00040 #include "rviz/properties/color_property.h"
00041 #include "rviz/properties/float_property.h"
00042 #include "rviz/properties/int_property.h"
00043 
00044 #include "MapGraphDisplay.h"
00045 
00046 #include <rtabmap/core/Link.h>
00047 #include <rtabmap_ros/MsgConversion.h>
00048 
00049 namespace rtabmap_ros
00050 {
00051 
00052 MapGraphDisplay::MapGraphDisplay()
00053 {
00054         color_neighbor_property_ = new rviz::ColorProperty( "Neighbor", Qt::blue,
00055                                                "Color to draw neighbor links.", this );
00056         color_neighbor_merged_property_ = new rviz::ColorProperty( "Merged neighbor", QColor(255,170,0),
00057                                                "Color to draw merged neighbor links.", this );
00058         color_global_property_ = new rviz::ColorProperty( "Global loop closure", Qt::red,
00059                                                "Color to draw global loop closure links.", this );
00060         color_local_property_ = new rviz::ColorProperty( "Local loop closure", Qt::yellow,
00061                                                "Color to draw local loop closure links.", this );
00062         color_user_property_ = new rviz::ColorProperty( "User", Qt::red,
00063                                                "Color to draw user links.", this );
00064         color_virtual_property_ = new rviz::ColorProperty( "Virtual", Qt::magenta,
00065                                                "Color to draw virtual links.", this );
00066 
00067         alpha_property_ = new rviz::FloatProperty( "Alpha", 1.0,
00068                                        "Amount of transparency to apply to the path.", this );
00069 }
00070 
00071 MapGraphDisplay::~MapGraphDisplay()
00072 {
00073         destroyObjects();
00074 }
00075 
00076 void MapGraphDisplay::onInitialize()
00077 {
00078   MFDClass::onInitialize();
00079   destroyObjects();
00080 }
00081 
00082 void MapGraphDisplay::reset()
00083 {
00084   MFDClass::reset();
00085   destroyObjects();
00086 }
00087 
00088 void MapGraphDisplay::destroyObjects()
00089 {
00090         for(unsigned int i=0; i<manual_objects_.size(); ++i)
00091         {
00092                 manual_objects_[i]->clear();
00093                 scene_manager_->destroyManualObject( manual_objects_[i] );
00094         }
00095         manual_objects_.clear();
00096 }
00097 
00098 void MapGraphDisplay::processMessage( const rtabmap_ros::MapGraph::ConstPtr& msg )
00099 {
00100         if(!(msg->poses.size() == msg->posesId.size()))
00101         {
00102                 ROS_ERROR("rtabmap_ros::MapGraph: Error pose ids and poses must have all the same size.");
00103                 return;
00104         }
00105 
00106         // Get links
00107         std::map<int, rtabmap::Transform> poses;
00108         std::multimap<int, rtabmap::Link> links;
00109         rtabmap::Transform mapToOdom;
00110         rtabmap_ros::mapGraphFromROS(*msg, poses, links, mapToOdom);
00111 
00112         destroyObjects();
00113 
00114         Ogre::Vector3 position;
00115         Ogre::Quaternion orientation;
00116         if( !context_->getFrameManager()->getTransform( msg->header, position, orientation ))
00117         {
00118                 ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
00119         }
00120 
00121         Ogre::Matrix4 transform( orientation );
00122         transform.setTrans( position );
00123 
00124         if(links.size())
00125         {
00126                 Ogre::ColourValue color;
00127                 Ogre::ManualObject* manual_object = scene_manager_->createManualObject();
00128                 manual_object->setDynamic( true );
00129                 scene_node_->attachObject( manual_object );
00130                 manual_objects_.push_back(manual_object);
00131 
00132                 manual_object->estimateVertexCount(links.size() * 2);
00133                 manual_object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST );
00134                 for(std::map<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
00135                 {
00136                         std::map<int, rtabmap::Transform>::iterator poseIterFrom = poses.find(iter->second.from());
00137                         std::map<int, rtabmap::Transform>::iterator poseIterTo = poses.find(iter->second.to());
00138                         if(poseIterFrom != poses.end() && poseIterTo != poses.end())
00139                         {
00140                                 if(iter->second.type() == rtabmap::Link::kNeighbor)
00141                                 {
00142                                         color = color_neighbor_property_->getOgreColor();
00143                                 }
00144                                 else if(iter->second.type() == rtabmap::Link::kNeighborMerged)
00145                                 {
00146                                         color = color_neighbor_merged_property_->getOgreColor();
00147                                 }
00148                                 else if(iter->second.type() == rtabmap::Link::kVirtualClosure)
00149                                 {
00150                                         color = color_virtual_property_->getOgreColor();
00151                                 }
00152                                 else if(iter->second.type() == rtabmap::Link::kUserClosure)
00153                                 {
00154                                         color = color_user_property_->getOgreColor();
00155                                 }
00156                                 else if(iter->second.type() == rtabmap::Link::kLocalSpaceClosure || iter->second.type() == rtabmap::Link::kLocalTimeClosure)
00157                                 {
00158                                         color = color_local_property_->getOgreColor();
00159                                 }
00160                                 else
00161                                 {
00162                                         color = color_global_property_->getOgreColor();
00163                                 }
00164                                 color.a = alpha_property_->getFloat();
00165                                 Ogre::Vector3 pos;
00166                                 pos = transform * Ogre::Vector3( poseIterFrom->second.x(), poseIterFrom->second.y(), poseIterFrom->second.z() );
00167                                 manual_object->position( pos.x, pos.y, pos.z );
00168                                 manual_object->colour( color );
00169                                 pos = transform * Ogre::Vector3( poseIterTo->second.x(), poseIterTo->second.y(), poseIterTo->second.z() );
00170                                 manual_object->position( pos.x, pos.y, pos.z );
00171                                 manual_object->colour( color );
00172                         }
00173                 }
00174 
00175                 manual_object->end();
00176         }
00177 }
00178 
00179 } // namespace rtabmap_ros
00180 
00181 #include <pluginlib/class_list_macros.h>
00182 PLUGINLIB_EXPORT_CLASS( rtabmap_ros::MapGraphDisplay, rviz::Display )


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Sun Jul 24 2016 03:49:08