Merge branch 'master' of github.com:introlab/rtabmap_ros into 0.11.0

Conflicts: src/CoreWrapper.cpp src/CoreWrapper.h
2026-10-04 00:37:46 +08:00 · 2016-03-09 12:22:27 -05:00
parent 6b5ccf99fb 26c656776c
commit d712efe455
12 changed files with 126 additions and 53 deletions
@@ -38,7 +38,7 @@ source ~/catkin_ws/devel/setup.bash
 0. Optional dependencies
  * If you want SURF/SIFT on Indigo/Jade (Hydro has already SIFT/SURF), you have to build [OpenCV]([OpenCV](http://opencv.org/)) from source to have access to *nonfree* module. Install it in `/usr/local` (default) and the rtabmap library should link with it instead of the one installed in ROS. I recommend to use latest 2.4 version ([2.4.11](https://github.com/Itseez/opencv/archive/2.4.11.zip)) and build it from source following these [instructions](http://docs.opencv.org/doc/tutorials/introduction/linux_install/linux_install.html#building-opencv-from-source-using-cmake-using-the-command-line). RTAB-Map can build with OpenCV3+[xfeatures2d](https://github.com/Itseez/opencv_contrib/tree/master/modules/xfeatures2d) module, but rtabmap_ros package will have libraries conflict as cv-bridge is depending on OpenCV2. If you want OpenCV3, you should build ros [vision-opencv](https://github.com/ros-perception/vision_opencv) package yourself (and all ros packages depending on it) so it can link on OpenCV3.
  
-  * ROS (Qt, PCL, dc1394, OpenNI, OpenNI2, Freenect, g2o, Costmap2d, Rviz, Octomap, CvBridge). Note that I've found that [latest g2o version](https://github.com/RainerKuemmerle/g2o) built from source is faster.
+  * ROS (Qt, PCL, dc1394, OpenNI, OpenNI2, Freenect, g2o, Costmap2d, Rviz, Octomap, CvBridge). Note that I've found that [latest g2o version](https://github.com/RainerKuemmerle/g2o) built from source is faster (install `libsuitesparse-dev` before building `g2o`).
    ```bash
 $ sudo apt-get install libqt4-dev libpcl-1.7-all-dev libdc1394-dev ros-indigo-openni-launch ros-indigo-openni2-launch ros-indigo-freenect-launch ros-indigo-costmap-2d ros-indigo-octomap-ros ros-indigo-g2o ros-indigo-rviz ros-indigo-cv-bridge
 ```
@@ -45,6 +45,8 @@
      <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
      <param name="Mem/RehearsedNodesKept" type="string" value="false"/>
      <param name="Mem/ImageDecimation" type="string" value="4"/>
+      <param name="Mem/IncrementalMemory" type="string" value="true"/>
+      <param name="Mem/InitWMWithAllNodes" type="string" value="false"/>
      
      <param name="Rtabmap/StartNewMapOnLoopClosure" type="string" value="true"/>
      <param name="Rtabmap/TimeThr" type="string" value="600"/>
@@ -79,7 +81,7 @@

  <!-- ROS navigation stack move_base -->
  <group ns="planner">
-     <remap from="base_scan" to="/base_scan"/>
+     <remap from="scan" to="/base_scan"/>
     <remap from="obstacles_cloud" to="/obstacles_cloud"/>
     <remap from="ground_cloud" to="/ground_cloud"/>
     <remap from="map" to="/map"/>
@@ -87,9 +89,9 @@
        
     <node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
        <param name="base_global_planner" value="navfn/NavfnROS"/>
-    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
-     	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
-    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params_2d.yaml" command="load" ns="local_costmap" />
+    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="global_costmap" />
+     	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="local_costmap" />
+    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params.yaml" command="load" ns="local_costmap" />
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml" command="load" ns="global_costmap"/>
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/base_local_planner_params.yaml" command="load" />
     </node>
@@ -109,7 +111,7 @@
  
  <!-- Throttling messages -->
  <group ns="camera">
-    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
+    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager">
      <param name="rate" type="double" value="5"/>
      <param name="decimation" type="int" value="2"/>
   
@@ -35,10 +35,13 @@
 	<param name="Kp/WordsPerImage"        type="string" value="-1"/>     <!-- desactivate keypoints extraction -->
 	<param name="Rtabmap/MaxRetrieved"    type="string" value="0"/>      <!-- desactivate global retrieval -->
 	<param name="RGBD/MaxLocalRetrieved"  type="string" value="0"/>      <!-- desactivate local retrieval -->
-	<param name="Rtabmap/MemoryThr"       type="string" value="1"/>      <!-- keep the WM empty --> 
+	<param name="Mem/MapLabelsAdded"      type="string" value="false"/>  <!-- don't create map labels --> 
+	<param name="Rtabmap/MemoryThr"       type="string" value="2"/>      <!-- keep the WM empty --> 
 	<param name="Mem/STMSize"             type="string" value="1"/>      <!-- STM=1 --> 
 	<param name="publish_tf"              type="bool"   value="false"/>  <!-- don't publish TF -->
-	<param name="RGBD/LocalLoopDetectionSpace" type="bool"   value="false"/> 
+	<param name="RGBD/LocalLoopDetectionSpace" type="string"   value="false"/> 
+	<param name="RGBD/LinearUpdate"       type="string" value="0"/> 
+	<param name="RGBD/AngularUpdate"      type="string" value="0"/> 
 	<param unless="$(arg subscribe_odometry)" name="RGBD/Enabled" type="string" value="false"/>
 	
 	<param name="Rtabmap/DetectionRate"   type="string" value="$(arg max_rate)"/>
@@ -48,6 +48,9 @@
  <arg name="approx_sync"             default="false"/>         <!-- if timestamps of the stereo images are not synchronized -->
  
  <arg name="compressed"              default="false"/>         <!-- If you want to subscribe to compressed image topics -->
+                                                                <!-- For depth_topic, "compressedDepth" image_transport is used. --> 
+                                                                <!-- For rgb_topic, see "rgb_image_transport" argument. -->
+  <arg name="rgb_image_transport"     default="compressed"/>    <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
   
  <arg name="subscribe_scan"          default="false"/>
  <arg name="scan_topic"              default="/scan"/>
@@ -55,17 +58,27 @@
  <arg name="visual_odometry"         default="true"/>          <!-- Launch rtabmap visual odometry node -->
  <arg name="odom_topic"              default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
  
+  <!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
+  <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
+  <arg unless="$(arg compressed)" name="rgb_topic_relay"           default="$(arg rgb_topic)"/>
+  <arg if="$(arg compressed)"     name="depth_topic_relay"         default="$(arg depth_topic)_relay"/>
+  <arg unless="$(arg compressed)" name="depth_topic_relay"         default="$(arg depth_topic)"/>
+  <arg if="$(arg compressed)"     name="left_image_topic_relay"    default="$(arg left_image_topic)_relay"/>
+  <arg unless="$(arg compressed)" name="left_image_topic_relay"    default="$(arg left_image_topic)"/>
+  <arg if="$(arg compressed)"     name="right_image_topic_relay"   default="$(arg right_image_topic)_relay"/>
+  <arg unless="$(arg compressed)" name="right_image_topic_relay"   default="$(arg right_image_topic)"/>
+
  <!-- Nodes -->
  <group ns="$(arg namespace)">
  
    <!-- RGB-D Odometry -->
    <group if="$(arg rgbd)">
-      <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="compressed in:=$(arg rgb_topic) raw out:=$(arg rgb_topic)" />
-      <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=$(arg depth_registered_topic) raw out:=$(arg depth_registered_topic)" />
+      <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
+      <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
  
      <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen" launch-prefix="$(arg launch_prefix)">
-        <remap from="rgb/image"       to="$(arg rgb_topic)"/>
-        <remap from="depth/image"     to="$(arg depth_topic)"/>
+        <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
+        <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
        <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
 	  
        <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
@@ -77,12 +90,12 @@
    
    <!-- Stereo Odometry -->
    <group if="$(arg stereo)">
-      <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="compressed in:=$(arg left_image_topic) raw out:=$(arg left_image_topic)" />
-      <node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="compressed in:=$(arg right_image_topic) raw out:=$(arg right_image_topic)" />
+      <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="compressed in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
+      <node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="compressed in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
  
-      <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
-        <remap from="left/image_rect"        to="$(arg left_image_topic)"/>
-        <remap from="right/image_rect"       to="$(arg right_image_topic)"/>
+      <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen" launch-prefix="$(arg launch_prefix)">
+        <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
+        <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
        <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
        <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
 	  
@@ -96,7 +109,7 @@
  
    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
-    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
+    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
      <param name="subscribe_depth"     type="bool"   value="$(arg rgbd)"/>
      <param name="subscribe_stereo"    type="bool"   value="$(arg stereo)"/>
      <param name="subscribe_laserScan" type="bool"   value="$(arg subscribe_scan)"/>
@@ -107,12 +120,12 @@
      <param name="config_path"          type="string" value="$(arg cfg)"/>
      <param name="queue_size"                  type="int" value="$(arg queue_size)"/>
      
-      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
-      <remap from="depth/image"     to="$(arg depth_topic)"/>
+      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
+      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
 	
-      <remap from="left/image_rect"        to="$(arg left_image_topic)"/>
-      <remap from="right/image_rect"       to="$(arg right_image_topic)"/>
+      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
+      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
      
@@ -121,7 +134,7 @@
    </node>
  
    <!-- Visualisation RTAB-Map -->
-    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg cfg)" output="screen">
+    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg cfg)" output="screen" launch-prefix="$(arg launch_prefix)">
      <param name="subscribe_depth"      type="bool"   value="$(arg rgbd)"/>
      <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
      <param name="subscribe_laserScan"  type="bool"   value="$(arg subscribe_scan)"/>
@@ -130,12 +143,12 @@
      <param name="wait_for_transform_duration"   type="double"   value="$(arg wait_for_transform)"/>
      <param name="queue_size"                  type="int" value="$(arg queue_size)"/>
    
-      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
-      <remap from="depth/image"     to="$(arg depth_topic)"/>
+      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
+      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      
-      <remap from="left/image_rect"        to="$(arg left_image_topic)"/>
-      <remap from="right/image_rect"       to="$(arg right_image_topic)"/>
+      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
+      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
      
@@ -148,12 +161,12 @@
  <!-- Visualization RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="$(arg rviz_cfg)"/>
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
-    <remap from="left/image"        to="$(arg left_image_topic)"/>
-    <remap from="right/image"       to="$(arg right_image_topic)"/>
+    <remap from="left/image"        to="$(arg left_image_topic_relay)"/>
+    <remap from="right/image"       to="$(arg right_image_topic_relay)"/>
    <remap from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
    <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
-    <remap from="rgb/image"         to="$(arg rgb_topic)"/>
-    <remap from="depth/image"       to="$(arg depth_topic)"/>
+    <remap from="rgb/image"         to="$(arg rgb_topic_relay)"/>
+    <remap from="depth/image"       to="$(arg depth_topic_relay)"/>
    <remap from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
    <remap from="cloud"             to="voxel_cloud" />

@@ -611,8 +611,8 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
 		lastPoseIntermediate_ = false;
 		lastPose_ = odom;
 		lastPoseStamp_ = odomMsg->header.stamp;
-		double transVariance = uMax3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
-		double rotVariance = uMax3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
+		float transVariance = uMax3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
+		float rotVariance = uMax3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
 		if(uIsFinite(rotVariance) && rotVariance > rotVariance_)
 		{
 			rotVariance_ = rotVariance;
@@ -799,6 +799,7 @@ void CoreWrapper::commonDepthCallback(
 		Transform localTransform = getTransform(frameId_, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp);
 		if(localTransform.isNull())
 		{
+			ROS_ERROR("TF of received depth image %d at time %fs is not set, aborting rtabmap update.", i, depthMsgs[i]->header.stamp.toSec());
 			return;
 		}
 		// sync with odometry stamp
@@ -809,9 +810,13 @@ void CoreWrapper::commonDepthCallback(
 				Transform sensorT = getTransform(odomFrameId, frameId_, depthMsgs[i]->header.stamp);
 				if(sensorT.isNull())
 				{
-					return;
+					ROS_WARN("Could not get odometry value for depth image %d stamp (%fs). Latest odometry "
+							 "stamp is %fs. The depth image pose will not be synchronized with odometry.", i, depthMsgs[i]->header.stamp.toSec(), lastPoseStamp_.toSec());
+				}
+				else
+				{
+					localTransform = odomT.inverse() * sensorT * localTransform;
 				}
-				localTransform = odomT.inverse() * sensorT * localTransform;
 			}
 		}

@@ -896,8 +901,11 @@ void CoreWrapper::commonDepthCallback(
 	if(scan2dMsg.get() != 0)
 	{
 		// make sure the frame of the laser is updated too
-		if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull())
+		if(getTransform(frameId_,
+				scan2dMsg->header.frame_id,
+				scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)).isNull())
 		{
+			ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting rtabmap update.", scan2dMsg->header.stamp.toSec());
 			return;
 		}

@@ -916,10 +924,14 @@ void CoreWrapper::commonDepthCallback(
 				Transform sensorT = getTransform(odomFrameId, frameId_, scan2dMsg->header.stamp);
 				if(sensorT.isNull())
 				{
-					return;
+					ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry "
+							"stamp is %fs. The laser scan pose will not be synchronized with odometry.", scanMsg->header.stamp.toSec(), lastPoseStamp_.toSec());
+				}
+				else
+				{
+					Transform t = odomT.inverse() * sensorT;
+					pclScan = util3d::transformPointCloud(pclScan, t);
 				}
-				Transform t = odomT.inverse() * sensorT;
-				pclScan = util3d::transformPointCloud(pclScan, t);

 			}
 		}
@@ -1020,8 +1032,11 @@ void CoreWrapper::commonStereoCallback(
 	if(scan2dMsg.get() != 0)
 	{
 		// make sure the frame of the laser is updated too
-		if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull())
+		if(getTransform(frameId_,
+				scan2dMsg->header.frame_id,
+				scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)).isNull())
 		{
+			ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting rtabmap update.", scan2dMsg->header.stamp.toSec());
 			return;
 		}

@@ -1325,8 +1340,8 @@ void CoreWrapper::process(
 		const SensorData & data,
 		const Transform & odom,
 		const std::string & odomFrameId,
-		double odomRotationalVariance,
-		double odomTransitionalVariance)
+		float odomRotationalVariance,
+		float odomTransitionalVariance)
 {
 	UTimer timer;
 	if(rtabmap_.isIDsGenerated() || data.id() > 0)
@@ -1795,7 +1810,7 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros:
 	rtabmap_ros::mapDataToROS(poses,
 		constraints,
 		signatures,
-		Transform::getIdentity(),
+		mapToOdom_,
 		res.data);

 	res.data.header.stamp = ros::Time::now();
@@ -1938,7 +1953,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 			rtabmap_ros::mapDataToROS(poses,
 				constraints,
 				signatures,
-				Transform::getIdentity(),
+				mapToOdom_,
 				*msg);

 			mapDataPub_.publish(msg);
@@ -1952,7 +1967,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab

 			rtabmap_ros::mapGraphToROS(poses,
 				constraints,
-				Transform::getIdentity(),
+				mapToOdom_,
 				*msg);

 			mapGraphPub_.publish(msg);
@@ -211,8 +211,8 @@ private:
 			const rtabmap::SensorData & data,
 			const rtabmap::Transform & odom = rtabmap::Transform(),
 			const std::string & odomFrameId = "",
-			double odomRotationalVariance = 1.0,
-			double odomTransitionalVariance = 1.0);
+			float odomRotationalVariance = 1.0,
+			float odomTransitionalVariance = 1.0);

 	bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
@@ -258,8 +258,8 @@ private:
 	rtabmap::Transform lastPose_;
 	ros::Time lastPoseStamp_;
 	bool lastPoseIntermediate_;
-	double rotVariance_;
-	double transVariance_;
+	float rotVariance_;
+	float transVariance_;
 	rtabmap::Transform currentMetricGoal_;
 	bool latestNodeWasReached_;
 	rtabmap::ParametersMap parameters_;
@@ -34,6 +34,7 @@ MapsManager::MapsManager(bool usePublicNamespace) :
 		cloudMaxDepth_(4.0), // meters
 		cloudVoxelSize_(0.05), // meters
 		cloudFloorCullingHeight_(0.0),
+		cloudCeilingCullingHeight_(0.0),
 		cloudOutputVoxelized_(false),
 		cloudFrustumCulling_(false),
 		cloudNoiseFilteringRadius_(0.0),
@@ -60,6 +61,14 @@ MapsManager::MapsManager(bool usePublicNamespace) :
 	pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_);
 	pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_);
 	pnh.param("cloud_floor_culling_height", cloudFloorCullingHeight_, cloudFloorCullingHeight_);
+	pnh.param("cloud_ceiling_culling_height", cloudCeilingCullingHeight_, cloudCeilingCullingHeight_);
+	if(cloudFloorCullingHeight_ > 0 &&
+	   cloudCeilingCullingHeight_ > 0 &&
+	   cloudCeilingCullingHeight_ < cloudFloorCullingHeight_)
+	{
+		ROS_WARN("\"cloud_floor_culling_height\" should be lower than \"cloud_ceiling_culling_height\", setting \"cloud_ceiling_culling_height\" to 0 (disabled).");
+		cloudCeilingCullingHeight_ = 0;
+	}
 	pnh.param("cloud_output_voxelized", cloudOutputVoxelized_, cloudOutputVoxelized_);
 	pnh.param("cloud_frustum_culling", cloudFrustumCulling_, cloudFrustumCulling_);
 	pnh.param("cloud_noise_filtering_radius", cloudNoiseFilteringRadius_, cloudNoiseFilteringRadius_);
@@ -512,9 +521,11 @@ void MapsManager::publishMaps(
 				}
 			}

-			if(assembledCloud->size() && cloudFloorCullingHeight_ > 0.0)
+			if(assembledCloud->size() && (cloudFloorCullingHeight_ > 0.0 || cloudCeilingCullingHeight_ > 0.0))
 			{
-				assembledCloud = util3d::passThrough(assembledCloud, "z", cloudFloorCullingHeight_, 99999.0f);
+				assembledCloud = util3d::passThrough(assembledCloud, "z",
+						cloudFloorCullingHeight_>0.0?cloudFloorCullingHeight_:-999.0,
+						cloudCeilingCullingHeight_>0.0 && (cloudFloorCullingHeight_<=0.0 || cloudCeilingCullingHeight_>cloudFloorCullingHeight_)?cloudCeilingCullingHeight_:999.0);
 			}

 			if(assembledCloud->size() && cloudVoxelSize_ > 0 && cloudOutputVoxelized_)
@@ -70,6 +70,7 @@ private:
 	double cloudMaxDepth_;
 	double cloudVoxelSize_;
 	double cloudFloorCullingHeight_;
+	double cloudCeilingCullingHeight_;
 	bool cloudOutputVoxelized_;
 	bool cloudFrustumCulling_;
 	double cloudNoiseFilteringRadius_;
@@ -366,6 +366,21 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 			odom.pose.covariance.at(28) = info.variance; // pp
 			odom.pose.covariance.at(35) = info.variance; // yawyaw

+			//set velocity
+			if(previousStamp_.isValid())
+			{
+				float dt = 1.0f/(stamp - previousStamp_).toSec();
+				float x,y,z,roll,pitch,yaw;
+				odometry_->previousTransform().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+				odom.twist.twist.linear.x = x*dt;
+				odom.twist.twist.linear.y = y*dt;
+				odom.twist.twist.linear.z = z*dt;
+				odom.twist.twist.angular.x = roll*dt;
+				odom.twist.twist.angular.y = pitch*dt;
+				odom.twist.twist.angular.z = yaw*dt;
+			}
+			previousStamp_ = stamp;
+
 			//publish the message
 			odomPub_.publish(odom);
 		}
@@ -465,6 +480,7 @@ bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
 {
 	ROS_INFO("visual_odometry: reset odom!");
 	odometry_->reset();
+	previousStamp_ = ros::Time();
 	return true;
 }

@@ -473,6 +489,7 @@ bool OdometryROS::resetToPose(rtabmap_ros::ResetPose::Request& req, rtabmap_ros:
 	Transform pose(req.x, req.y, req.z, req.roll, req.pitch, req.yaw);
 	ROS_INFO("visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str());
 	odometry_->reset(pose);
+	previousStamp_ = ros::Time();
 	return true;
 }

@@ -101,6 +101,7 @@ private:
 	tf::TransformListener tfListener_;

 	bool paused_;
+	ros::Time previousStamp_;
 };

 }
@@ -156,6 +156,13 @@ MapCloudDisplay::MapCloudDisplay()
 	cloud_filter_floor_height_->setMin( 0.0f );
 	cloud_filter_floor_height_->setMax( 999.0f );

+	cloud_filter_ceiling_height_ = new rviz::FloatProperty( "Filter ceiling (m)", 0.0f,
+										 "Filter the ceiling at the specified height set here "
+										 "(only appropriate for 2D mapping).",
+										 this, SLOT( updateCloudParameters() ), this );
+	cloud_filter_ceiling_height_->setMin( 0.0f );
+	cloud_filter_ceiling_height_->setMax( 999.0f );
+
 	node_filtering_radius_ = new rviz::FloatProperty( "Node filtering radius (m)", 0.2f,
 										 "(Disabled=0) Only keep one node in the specified radius.",
 										 this, SLOT( updateCloudParameters() ), this );
@@ -276,9 +283,11 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)

 					if(cloud->size())
 					{
-						if(cloud_filter_floor_height_->getFloat() > 0.0f)
+						if(cloud_filter_floor_height_->getFloat() > 0.0f || cloud_filter_ceiling_height_->getFloat() > 0.0f)
 						{
-							cloud = rtabmap::util3d::passThrough(cloud, "z", cloud_filter_floor_height_->getFloat(), 999.0f);
+							cloud = rtabmap::util3d::passThrough(cloud, "z",
+									cloud_filter_floor_height_->getFloat()>0.0f?cloud_filter_floor_height_->getFloat():-999.0f,
+									cloud_filter_ceiling_height_->getFloat()>0.0f && (cloud_filter_floor_height_->getFloat()<=0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f);
 						}

 						sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
@@ -108,6 +108,7 @@ public:
 	rviz::FloatProperty* cloud_max_depth_;
 	rviz::FloatProperty* cloud_voxel_size_;
 	rviz::FloatProperty* cloud_filter_floor_height_;
+	rviz::FloatProperty* cloud_filter_ceiling_height_;
 	rviz::FloatProperty* node_filtering_radius_;
 	rviz::FloatProperty* node_filtering_angle_;
 	rviz::BoolProperty* download_map_;