merged master to devel

CMakeLists.txt

@@ -19,6 +19,8 @@ find_package(octomap_ros)
 # find_package(Boost REQUIRED COMPONENTS system)
 find_package(RTABMap 0.10.0 REQUIRED)
 
+find_package(OpenCV REQUIRED)
+
 #Qt stuff
 FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED)
 INCLUDE(${QT_USE_FILE})
@@ -101,11 +103,13 @@ catkin_package(
 include_directories(
   ${CMAKE_CURRENT_SOURCE_DIR}/include
   ${RTABMap_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
   ${catkin_INCLUDE_DIRS}
 )
 
 # libraries
 SET(Libraries
+   ${OpenCV_LIBRARIES}
    ${catkin_LIBRARIES}
    ${RTABMap_LIBRARIES}
 )

launch/demo/demo_turtlebot_mapping.launch

@@ -25,6 +25,7 @@
   <arg name="localization"      default="false"/>
   <arg name="rgbd_odometry"     default="false"/>
   <arg name="args"              default=""/>
+  <arg name="version083"        default="false"/>
   
   <!-- Navigation stuff (move_base) -->
   <include file="$(find turtlebot_bringup)/launch/3dsensor.launch"/>
@@ -48,8 +49,8 @@
   	  <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
   	  
   	  <!-- output -->
-  	  <remap from="grid_map" to="/map"/>
-  	  <!-- <remap from="proj_map" to="/map"/> -->
+  	  <remap unless="$(arg version083)" from="grid_map" to="/map"/>
+  	  <!-- <remap unless="$(arg version083)" from="proj_map" to="/map"/> -->
 	
 	  <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
 	  <param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>   <!-- Local loop closure detection (using estimated position) with locations in WM -->
@@ -69,7 +70,7 @@
 	  <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
 	  <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/> 
     </node>
-    
+   
     <!-- Odometry : ONLY for testing without the actual robot! /odom TF should not be already published. -->
     <node if="$(arg rgbd_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
       <param name="frame_id"           type="string" value="base_footprint"/>
@@ -79,6 +80,11 @@
       <remap from="depth/image"     to="/camera/depth_registered/image_raw"/>
       <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
     </node>
+
+    <!-- backward compatibility with hydro 0.8.3 only -->
+    <node if="$(arg version083)" pkg="rtabmap_ros" type="grid_map_assembler" name="grid_map_assembler">
+      <remap from="grid_map" to="/map"/>
+    </node>
     
   </group>
 </launch>

launch/rgbd_mapping_kinect2.launch

@@ -5,19 +5,21 @@
         Install Kinect2 : Follow ALL directives at https://github.com/code-iai/iai_kinect2
                           Make sure it is calibrated!
         Run:
+           $ roslaunch kinect2_bridge kinect2_bridge.launch publish_tf:=true
            $ roslaunch rtabmap_ros rgbd_mapping_kinect2.launch
-           $ rosrun kinect2_bridge kinect2_bridge  _publish_tf:=true
-
-        Prefixes:
-           sd: rgb_lowres / depth_lowres
-           hd: rgb_rect / depth_highres
-           ir: ir_rect / depth_rect
    -->
-   <arg name="rgb_prefix" default="rgb_lowres" />
-   <arg name="depth_prefix" default="depth_lowres" />
+
+   <!-- Which image resolution to process in rtabmap: sd, qhd, hd -->
+   <arg name="resolution" default="qhd" />
    
    <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
-   <arg name="frame_id" default="kinect2_link"/>
+   <arg name="frame_id" default="kinect2_base_link"/>
+   
+   <!-- Rotate the camera -->
+   <arg name="pi/2" value="1.5707963267948966"/>
+   <arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
+   <node pkg="tf" type="static_transform_publisher" name="kinect2_base_link"
+        args="$(arg optical_rotate) kinect2_base_link kinect2_link 100" /> 
   
    <!-- Choose visualization -->
    <arg name="rviz" default="false" />
@@ -38,7 +40,7 @@
    <arg name="strategy" default="0" />
    <arg name="feature" default="6" />
    <arg name="nn" default="3" />
-   <arg name="max_depth" default="8.0" />
+   <arg name="max_depth" default="10.0" />
    <arg name="min_inliers" default="20" />
    <arg name="inlier_distance" default="0.02" />
    <arg name="local_map" default="1000" />
@@ -49,9 +51,9 @@
   
     <!-- Odometry -->
     <node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen">
-      <remap from="rgb/image" to="/kinect2/$(arg rgb_prefix)/image"/>
-      <remap from="depth/image" to="/kinect2/$(arg depth_prefix)/image"/>
-      <remap from="rgb/camera_info" to="/kinect2/$(arg rgb_prefix)/camera_info"/>
+      <remap from="rgb/image" to="/kinect2/$(arg resolution)/image_mono_rect"/>
+      <remap from="depth/image" to="/kinect2/$(arg resolution)/image_depth_rect"/>
+      <remap from="rgb/camera_info" to="/kinect2/$(arg resolution)/camera_info"/>
 
       <param name="frame_id" type="string" value="$(arg frame_id)"/>
       <param name="approx_sync" type="bool" value="true"/>
@@ -74,9 +76,9 @@
       <param name="subscribe_depth" type="bool" value="true"/>
 	  <param name="frame_id" type="string" value="$(arg frame_id)"/>
 	
-      <remap from="rgb/image" to="/kinect2/$(arg rgb_prefix)/image"/>
-      <remap from="depth/image" to="/kinect2/$(arg depth_prefix)/image"/>
-      <remap from="rgb/camera_info" to="/kinect2/$(arg rgb_prefix)/camera_info"/>
+      <remap from="rgb/image" to="/kinect2/$(arg resolution)/image_color_rect"/>
+      <remap from="depth/image" to="/kinect2/$(arg resolution)/image_depth_rect"/>
+      <remap from="rgb/camera_info" to="/kinect2/$(arg resolution)/camera_info"/>
 
       <param name="approx_sync" type="bool" value="true"/>
 
@@ -91,9 +93,9 @@
       <param name="approx_sync" type="bool" value="true"/>
       <param name="frame_id" type="string" value="$(arg frame_id)"/>
     
-      <remap from="rgb/image" to="/kinect2/$(arg rgb_prefix)/image"/>
-      <remap from="depth/image" to="/kinect2/$(arg depth_prefix)/image"/>
-      <remap from="rgb/camera_info" to="/kinect2/$(arg rgb_prefix)/camera_info"/>
+      <remap from="rgb/image" to="/kinect2/$(arg resolution)/image_color_rect"/>
+      <remap from="depth/image" to="/kinect2/$(arg resolution)/image_depth_rect"/>
+      <remap from="rgb/camera_info" to="/kinect2/$(arg resolution)/camera_info"/>
     </node>
   
   </group>
@@ -103,9 +105,9 @@
   <!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
   <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>
   <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
-    <remap from="rgb/image_in"       to="/kinect2/$(arg rgb_prefix)/image"/>
-    <remap from="depth/image_in"     to="/kinect2/$(arg depth_prefix)/image"/>
-    <remap from="rgb/camera_info_in" to="/kinect2/$(arg rgb_prefix)/camera_info"/>
+    <remap from="rgb/image_in"       to="/kinect2/$(arg resolution)/image_color_rect"/>
+    <remap from="depth/image_in"     to="/kinect2/$(arg resolution)/image_depth_rect"/>
+    <remap from="rgb/camera_info_in" to="/kinect2/$(arg resolution)/camera_info"/>
 
     <remap from="odom_in"             to="rtabmap/odom"/>
 

src/CoreWrapper.cpp

@@ -752,7 +752,8 @@ void CoreWrapper::commonDepthCallback(
 	std::vector<CameraModel> cameraModels;
 	for(unsigned int i=0; i<imageMsgs.size(); ++i)
 	{
-		if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+		if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
+			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
 			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
 			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
 			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
@@ -786,7 +787,11 @@ void CoreWrapper::commonDepthCallback(
 		}
 
 		cv_bridge::CvImageConstPtr ptrImage;
-		if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+		if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
+		{
+			ptrImage = cv_bridge::toCvShare(imageMsgs[i]);
+		}
+		else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
 		   imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
 		{
 			ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");

src/GuiWrapper.cpp

@@ -500,7 +500,8 @@ void GuiWrapper::commonDepthCallback(
 		std::vector<CameraModel> cameraModels;
 		for(unsigned int i=0; i<imageMsgs.size(); ++i)
 		{
-			if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+			if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+				 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
 				 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
 				 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
 				 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
@@ -534,7 +535,11 @@ void GuiWrapper::commonDepthCallback(
 			}
 
 			cv_bridge::CvImageConstPtr ptrImage;
-			if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+			if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
+			{
+				ptrImage = cv_bridge::toCvShare(imageMsgs[i]);
+			}
+			else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
 			   imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
 			{
 				ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");

src/RGBDOdometryNode.cpp

@@ -163,7 +163,8 @@ public:
 	{
 		if(!this->isPaused())
 		{
-			if(!(image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+			if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+				 image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
 				 image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
 				 image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
 				 image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
@@ -171,7 +172,9 @@ public:
 				 depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
 				 depth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))
 			{
-				ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended) and image_depth=16UC1,32FC1,mono16");
+				ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 "
+						  "recommended) and image_depth=16UC1,32FC1,mono16. Types detected: %s %s",
+						image->encoding.c_str(), depth->encoding.c_str());
 				return;
 			}
 			else if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0)
@@ -214,7 +217,7 @@ public:
 						model.cx(),
 						model.cy(),
 						rtabmap_ros::transformFromTF(localTransform));
-				cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image, "mono8");
+				cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
 				cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depth);
 
 				rtabmap::SensorData data(
@@ -258,7 +261,8 @@ public:
 			std::vector<CameraModel> cameraModels;
 			for(unsigned int i=0; i<imageMsgs.size(); ++i)
 			{
-				if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+				if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+					 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
 					 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
 					 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
 					 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
@@ -292,7 +296,11 @@ public:
 				}
 
 				cv_bridge::CvImageConstPtr ptrImage;
-				if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+				if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
+				{
+					ptrImage = cv_bridge::toCvShare(imageMsgs[i]);
+				}
+				else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
 				   imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
 				{
 					ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");

src/nodelets/point_cloud_xyzrgb.cpp

@@ -156,7 +156,8 @@ private:
 			  const sensor_msgs::ImageConstPtr& imageDepth,
 			  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
 	{
-		if(!(image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+		if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+			image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
 			image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
 			image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
 			image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) &&
@@ -170,7 +171,7 @@ private:
 
 		if(cloudPub_.getNumSubscribers())
 		{
-			cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image, "bgr8");
+			cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
 			cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
 
 			image_geometry::PinholeCameraModel model;