ros: added MapGraph rviz plugin to show trajectory, added ros service PublishMap

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1682 f169173b-cf89-36c8-b27e-44dbe73f0c83

CMakeLists.txt

@@ -49,6 +49,7 @@ add_message_files(
  add_service_files(
    FILES
    GetMap.srv
+   PublishMap.srv
  )
 
 ## Generate added messages and services with any dependencies listed here
@@ -103,13 +104,14 @@ SET(Libraries
 ## RVIZ plugin
 qt4_wrap_cpp(MOC_FILES
   src/rviz/MapCloudDisplay.h
+  src/rviz/MapGraphDisplay.h
   src/rviz/InfoDisplay.h
   src/rviz/OrbitOrientedViewController.h
 )
 
 # tf:message_filters, mixing boost and Qt signals
 set_property(
-   SOURCE src/rviz/MapCloudDisplay.cpp src/rviz/InfoDisplay.cpp src/rviz/OrbitOrientedViewController.cpp
+   SOURCE src/rviz/MapCloudDisplay.cpp src/rviz/MapGraphDisplay.cpp src/rviz/InfoDisplay.cpp src/rviz/OrbitOrientedViewController.cpp
    PROPERTY COMPILE_DEFINITIONS QT_NO_KEYWORDS
    )
    
@@ -121,6 +123,7 @@ add_library(rtabmap_ros
    src/nodelets/disparity_to_depth.cpp 
    src/MsgConversion.cpp
    src/rviz/MapCloudDisplay.cpp
+   src/rviz/MapGraphDisplay.cpp
    src/rviz/InfoDisplay.cpp
    src/rviz/OrbitOrientedViewController.cpp
    ${MOC_FILES}

msg/MapData.msg

@@ -1,7 +1,7 @@
 
 Header header
 
-# Map ids
+# Map ids std::map<nodeId, mapId>
 int32[] mapIDs
 int32[] maps
 

plugin_description.xml

@@ -6,6 +6,13 @@
       Displays graph point clouds from rtabmap/MapData messages.
     </description>
   </class>
+  <class name="rtabmap/MapGraph"
+         type="rtabmap::MapGraphDisplay"
+         base_class_type="rviz::Display">
+    <description>
+      Displays graphs from rtabmap/MapData messages.
+    </description>
+  </class>
   <class name="rtabmap/Info"
          type="rtabmap::InfoDisplay"
          base_class_type="rviz::Display">

src/CoreWrapper.cpp

@@ -54,6 +54,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/InfoEx.h"
 #include "rtabmap/MapData.h"
 #include "rtabmap/GetMap.h"
+#include "rtabmap/PublishMap.h"
 
 #include "rtabmap/MsgConversion.h"
 
@@ -708,6 +709,7 @@ bool CoreWrapper::getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap:
 		rtabmap_.getGraph(
 				poses,
 				constraints,
+				mapIds,
 				req.optimized,
 				req.global);
 	}
@@ -850,11 +852,12 @@ bool CoreWrapper::getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap:
 	return true;
 }
 
-bool CoreWrapper::publishMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+bool CoreWrapper::publishMapCallback(rtabmap::PublishMap::Request& req, rtabmap::PublishMap::Response& res)
 {
-	ROS_INFO("rtabmap: Publishing map...");
 	if(mapData_.getNumSubscribers())
 	{
+		ROS_INFO("rtabmap: Publishing map...");
+
 		std::map<int, std::vector<unsigned char> > images;
 		std::map<int, std::vector<unsigned char> > depths;
 		std::map<int, std::vector<unsigned char> > depths2d;
@@ -867,20 +870,32 @@ bool CoreWrapper::publishMapCallback(std_srvs::Empty::Request&, std_srvs::Empty:
 		std::multimap<int, Link> constraints;
 		std::map<int, int> mapIds;
 
-		rtabmap_.get3DMap(
-				images,
-				depths,
-				depths2d,
-				depthFxs,
-				depthFys,
-				depthCxs,
-				depthCys,
-				localTransforms,
-				poses,
-				constraints,
-				mapIds,
-				true,
-				true);
+		if(req.graphOnly)
+		{
+			rtabmap_.getGraph(
+					poses,
+					constraints,
+					mapIds,
+					req.optimized,
+					req.global);
+		}
+		else
+		{
+			rtabmap_.get3DMap(
+					images,
+					depths,
+					depths2d,
+					depthFxs,
+					depthFys,
+					depthCxs,
+					depthCys,
+					localTransforms,
+					poses,
+					constraints,
+					mapIds,
+					req.optimized,
+					req.global);
+		}
 
 		//RGB-D SLAM data
 		rtabmap::MapDataPtr msg(new rtabmap::MapData);

src/CoreWrapper.h

@@ -52,6 +52,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Rtabmap.h>
 
 #include "rtabmap/GetMap.h"
+#include "rtabmap/PublishMap.h"
 
 #include <message_filters/subscriber.h>
 #include <message_filters/synchronizer.h>
@@ -100,7 +101,7 @@ private:
 	bool setModeLocalizationCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap::Response& rep);
-	bool publishMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool publishMapCallback(rtabmap::PublishMap::Request&, rtabmap::PublishMap::Response&);
 
 	rtabmap::ParametersMap loadParameters(const std::string & configFile);
 	void saveParameters(const std::string & configFile);

src/rviz/MapGraphDisplay.cpp

@@ -0,0 +1,148 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <boost/bind.hpp>
+
+#include <OgreSceneNode.h>
+#include <OgreSceneManager.h>
+#include <OgreManualObject.h>
+#include <OgreBillboardSet.h>
+#include <OgreMatrix4.h>
+
+#include <tf/transform_listener.h>
+
+#include <rviz/display_context.h>
+#include <rviz/frame_manager.h>
+#include "rviz/properties/color_property.h"
+#include "rviz/properties/float_property.h"
+#include "rviz/properties/int_property.h"
+
+#include "MapGraphDisplay.h"
+
+namespace rtabmap
+{
+
+MapGraphDisplay::MapGraphDisplay()
+{
+  color_property_ = new rviz::ColorProperty( "Color", QColor( 25, 255, 0 ),
+                                       "Color to draw the path.", this );
+
+  alpha_property_ = new rviz::FloatProperty( "Alpha", 1.0,
+                                       "Amount of transparency to apply to the path.", this );
+}
+
+MapGraphDisplay::~MapGraphDisplay()
+{
+	destroyObjects();
+}
+
+void MapGraphDisplay::onInitialize()
+{
+  MFDClass::onInitialize();
+  destroyObjects();
+}
+
+void MapGraphDisplay::reset()
+{
+  MFDClass::reset();
+  destroyObjects();
+}
+
+void MapGraphDisplay::destroyObjects()
+{
+	for(unsigned int i=0; i<manual_objects_.size(); ++i)
+	{
+		manual_objects_[i]->clear();
+		scene_manager_->destroyManualObject( manual_objects_[i] );
+	}
+	manual_objects_.clear();
+}
+
+void MapGraphDisplay::processMessage( const rtabmap::MapData::ConstPtr& msg )
+{
+	if(!(msg->maps.size() == msg->poseIDs.size() && msg->poses.size() == msg->poseIDs.size()))
+	{
+		ROS_ERROR("rtambap::MapGraph: Error map ids, pose ids and poses must have all the same size.");
+		return;
+	}
+
+	// Find all graphs
+	std::map<int, std::map<int, geometry_msgs::Point> > graphs;
+	for(unsigned int i=0; i<msg->poses.size(); ++i)
+	{
+		std::map<int, std::map<int, geometry_msgs::Point> >::iterator iter = graphs.find(msg->maps[i]);
+		if(iter == graphs.end())
+		{
+			iter = graphs.insert(std::make_pair(msg->maps[i], std::map<int, geometry_msgs::Point>())).first;
+		}
+		iter->second.insert(std::make_pair(msg->poseIDs[i], msg->poses[i].position));
+	}
+
+	destroyObjects();
+
+	Ogre::Vector3 position;
+	Ogre::Quaternion orientation;
+	if( !context_->getFrameManager()->getTransform( msg->header, position, orientation ))
+	{
+		ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
+	}
+
+	Ogre::Matrix4 transform( orientation );
+	transform.setTrans( position );
+
+	Ogre::ColourValue color = color_property_->getOgreColor();
+	color.a = alpha_property_->getFloat();
+
+	for(std::map<int, std::map<int, geometry_msgs::Point> >::iterator iter=graphs.begin(); iter!=graphs.end(); ++iter)
+	{
+		uint32_t num_points = iter->second.size();
+		if(num_points > 0)
+		{
+			Ogre::ManualObject* manual_object = scene_manager_->createManualObject();
+			manual_object->setDynamic( true );
+			scene_node_->attachObject( manual_object );
+			manual_objects_.push_back(manual_object);
+
+			manual_object->estimateVertexCount( num_points );
+			manual_object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP );
+			for( std::map<int, geometry_msgs::Point>::iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
+			{
+				const geometry_msgs::Point& pos = jter->second;
+				Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z );
+				manual_object->position( xpos.x, xpos.y, xpos.z );
+				manual_object->colour( color );
+			}
+
+			manual_object->end();
+		}
+	}
+}
+
+} // namespace rviz
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS( rtabmap::MapGraphDisplay, rviz::Display )

src/rviz/MapGraphDisplay.h

@@ -0,0 +1,85 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+
+#ifndef RVIZ_PATH_DISPLAY_H
+#define RVIZ_PATH_DISPLAY_H
+
+#include <rtabmap/MapData.h>
+
+#include <rviz/message_filter_display.h>
+
+namespace Ogre
+{
+class ManualObject;
+}
+
+namespace rviz
+{
+class ColorProperty;
+class FloatProperty;
+}
+
+using namespace rviz;
+
+namespace rtabmap
+{
+
+/**
+ * \class MapGraphDisplay
+ * \brief Displays the graph in rtabmap::MapData message
+ */
+class MapGraphDisplay: public MessageFilterDisplay<rtabmap::MapData>
+{
+Q_OBJECT
+public:
+  MapGraphDisplay();
+  virtual ~MapGraphDisplay();
+
+  /** @brief Overridden from Display. */
+  virtual void reset();
+
+protected:
+  /** @brief Overridden from Display. */
+  virtual void onInitialize();
+
+  /** @brief Overridden from MessageFilterDisplay. */
+  void processMessage( const rtabmap::MapData::ConstPtr& msg );
+
+private:
+  void destroyObjects();
+
+  std::vector<Ogre::ManualObject*> manual_objects_;
+
+  ColorProperty* color_property_;
+  FloatProperty* alpha_property_;
+};
+
+} // namespace rviz
+
+#endif /* RVIZ_PATH_DISPLAY_H */
+

srv/PublishMap.srv

@@ -0,0 +1,6 @@
+#request
+bool global
+bool optimized
+bool graphOnly
+---
+#response