rviz: added downloadGraph property

added relays for images on az3_mapping_client_rviz launch file

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

launch/azimut3/az3_mapping_client_rviz.launch

@@ -5,10 +5,11 @@
   <include file="$(find az3_bringup)/joystick.launch"/> 
 
   <!-- We have two nodes (grid_map_assembler and rviz) subscribing to /rtabmap/mapData, so use -->
-  <!-- a relay on this machine -->
+  <!-- a relay on this machine, same for images -->
-  <node name="mapData_relay" type="relay" pkg="topic_tools" args="/rtabmap/mapData /rtabmap/mapData_relay" >
+  <node name="mapData_relay" type="relay" pkg="topic_tools" args="/rtabmap/mapData /rtabmap/mapData_relay"/>
-    <param name="lazy" type="bool" value="false"/>
+  <node name="camera_info_relay" type="relay" pkg="topic_tools" args="/camera/data_throttled_camera_info /camera/data_throttled_camera_info_relay"/>
-  </node>
+  <node name="republish_rgb" type="republish" pkg="image_transport" args="theora in:=/camera/data_throttled_image raw out:=/camera/data_throttled_image_relay" />
+  <node name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=/camera/data_throttled_image_depth raw out:=/camera/data_throttled_image_depth_relay" />
 
   <!-- Grid map assembler for rviz -->
   <node pkg="rtabmap" type="grid_map_assembler" name="grid_map_assembler" output="screen">
@@ -18,19 +19,14 @@
   <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap)/launch/config/azimut3.rviz"/>
   
   <!-- Below, construct point cloud of the latest throttled data, disabled for bandwidth efficiency -->
-<!--
   <node pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>
   <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap/point_cloud_xyzrgb standalone_nodelet">
-    <remap from="rgb/image"       to="/camera/data_throttled_image"/>
+    <remap from="rgb/image"       to="/camera/data_throttled_image_relay"/>
-    <remap from="depth/image"     to="/camera/data_throttled_image_depth"/>
+    <remap from="depth/image"     to="/camera/data_throttled_image_depth_relay"/>
-    <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
+    <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info_relay"/>
     <remap from="cloud"           to="voxel_cloud" />
     
-    <param name="rgb/image_transport" type="string" value="theora"/>
-    <param name="depth/image_transport" type="string" value="compressedDepth"/>
-    
     <param name="queue_size" type="int" value="10"/>
     <param name="voxel_size" type="double" value="0.01"/>
   </node>
---> 
 </launch>

launch/config/azimut3.rviz

@@ -7,7 +7,6 @@ Panels:
         - /Global Options1
         - /TF1/Frames1
         - /Info1
-        - /Info1/Status1
       Splitter Ratio: 0.5
     Tree Height: 527
   - Class: rviz/Selection
@@ -62,7 +61,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: RGB8
       Decay Time: 0
-      Enabled: false
+      Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 2.34177e-38
@@ -78,7 +77,7 @@ Visualization Manager:
       Topic: /voxel_cloud
       Use Fixed Frame: true
       Use rainbow: true
-      Value: false
+      Value: true
     - Class: rviz/TF
       Enabled: true
       Frame Timeout: 15
@@ -156,8 +155,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 0.167568
+        Max Value: 0.526358
-        Min Value: -4.37384
+        Min Value: -2.54801
         Value: true
       Axis: X
       Channel Name: intensity
@@ -200,6 +199,61 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_laser_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheelLB_linkWheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheelLB_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheelLF_linkWheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheelLF_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheelRB_linkWheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheelRB_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheelRF_linkWheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheelRF_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
       Name: RobotModel
       Robot Description: robot_description
       TF Prefix: ""
@@ -208,14 +262,14 @@ Visualization Manager:
       Visual Enabled: true
     - Class: rviz/Image
       Enabled: true
-      Image Topic: /camera/data_throttled_image
+      Image Topic: /camera/data_throttled_image_relay
       Max Value: 1
       Median window: 5
       Min Value: 0
       Name: Image
       Normalize Range: true
       Queue Size: 2
-      Transport Hint: theora
+      Transport Hint: raw
       Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
@@ -231,6 +285,7 @@ Visualization Manager:
       Cloud voxel size (m): 0.02
       Color: 255; 255; 255
       Color Transformer: RGB8
+      Download graph: false
       Download map: false
       Enabled: true
       Invert Rainbow: false

src/rviz/MapCloudDisplay.cpp

@@ -131,6 +131,10 @@ MapCloudDisplay::MapCloudDisplay()
 										 "Download the optimized global map using rtabmap/GetMap service. This will force to re-create all clouds.",
 										 this, SLOT( downloadMap() ), this );
 
+	download_graph_ = new rviz::BoolProperty( "Download graph", false,
+											 "Download the optimized global graph (without cloud data) using rtabmap/GetMap service.",
+											 this, SLOT( downloadGraph() ), this );
+
 	// PointCloudCommon sets up a callback queue with a thread for each
 	// instance.  Use that for processing incoming messages.
 	update_nh_.setCallbackQueue( &cbqueue_ );
@@ -491,6 +495,61 @@ void MapCloudDisplay::downloadMap()
 	}
 }
 
+void MapCloudDisplay::downloadGraph()
+{
+	if(download_graph_->getBool())
+	{
+		rtabmap::GetMap getMapSrv;
+		getMapSrv.request.global = true;
+		getMapSrv.request.optimized = true;
+		getMapSrv.request.graphOnly = true;
+		ros::NodeHandle nh;
+		QMessageBox * messageBox = new QMessageBox(
+				QMessageBox::NoIcon,
+				tr("Calling \"%1\" service...").arg(nh.resolveName("rtabmap/get_map").c_str()),
+				tr("Downloading the graph... please wait (rviz could become gray!)"),
+				QMessageBox::NoButton);
+		messageBox->setAttribute(Qt::WA_DeleteOnClose, true);
+		messageBox->show();
+		QApplication::processEvents();
+		uSleep(100); // hack make sure the text in the QMessageBox is shown...
+		QApplication::processEvents();
+		if(!ros::service::call("rtabmap/get_map", getMapSrv))
+		{
+			ROS_ERROR("MapCloudDisplay: Can't call \"%s\" service. "
+					  "Tip: if rtabmap node is not in rtabmap namespace, you can remap the service "
+					  "to \"get_map\" in the launch "
+					  "file like: <remap from=\"rtabmap/get_map\" to=\"get_map\"/>.",
+					  nh.resolveName("rtabmap/get_map").c_str());
+			messageBox->setText(tr("MapCloudDisplay: Can't call \"%1\" service. "
+					  "Tip: if rtabmap node is not in rtabmap namespace, you can remap the service "
+					  "to \"get_map\" in the launch "
+					  "file like: <remap from=\"rtabmap/get_map\" to=\"get_map\"/>.").
+					  arg(nh.resolveName("rtabmap/get_map").c_str()));
+		}
+		else
+		{
+			messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(getMapSrv.response.data.poses.size()));
+			QApplication::processEvents();
+			processMapData(getMapSrv.response.data);
+			messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.poses.size()));
+
+			QTimer::singleShot(1000, messageBox, SLOT(close()));
+		}
+		download_graph_->blockSignals(true);
+		download_graph_->setBool(false);
+		download_graph_->blockSignals(false);
+	}
+	else
+	{
+		// just stay true if double-clicked on DownloadGraph property, let the
+		// first process above finishes
+		download_graph_->blockSignals(true);
+		download_graph_->setBool(true);
+		download_graph_->blockSignals(false);
+	}
+}
+
 void MapCloudDisplay::causeRetransform()
 {
   needs_retransform_ = true;

src/rviz/MapCloudDisplay.h

@@ -84,6 +84,7 @@ public:
 	rviz::FloatProperty* node_filtering_radius_;
 	rviz::FloatProperty* node_filtering_angle_;
 	rviz::BoolProperty* download_map_;
+	rviz::BoolProperty* download_graph_;
 
 public Q_SLOTS:
 	void causeRetransform();
@@ -98,6 +99,7 @@ private Q_SLOTS:
 	void setColorTransformerOptions( EnumProperty* prop );
 	void updateCloudParameters();
 	void downloadMap();
+	void downloadGraph();
 
 protected:
 	/** @brief Do initialization. Overridden from MessageFilterDisplay. */