Added demo_catvehicle_mapping.launch (#592). MapCloud display: fixed clouds requested when changing sensor modality.

launch/config/catvehicle_triclops.gazebo

@@ -0,0 +1,110 @@
+<?xml version="1.0"?>
+<!--
+
+Author: Jonathan Sprinkle, Sam Taylor, Alex Warren, Rahul Bhadani
+Hannah Grace Mason, Joe Macinnes, Landon Chase Bentley
+
+Copyright (c) 2015-2018 Arizona Board of Regents
+All rights reserved.
+
+Permission is hereby granted, without written agreement and without 
+license or royalty fees, to use, copy, modify, and distribute this
+software and its documentation for any purpose, provided that the 
+above copyright notice and the following two paragraphs appear in 
+all copies of this software.
+
+IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY 
+FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES 
+ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN 
+IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF 
+SUCH DAMAGE.
+
+THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, 
+INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY 
+AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
+IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
+TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
+
+Summary: 
+This file includes gazebo reference for front stereo camera simulation mounted
+on the top of the car. For more information and for the tutorials used to create
+this file, see
+http://gazebosim.org/tutorials/?tut=ros_control#Prerequisites
+
+
+
+-->
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+<xacro:property name="M_PI" value="3.1415926535897931" />
+
+<!--triclops camera-->
+<gazebo reference="triclops_link">
+   <mu1>0.6</mu1>
+   <mu2>0.5</mu2>
+</gazebo>
+
+<joint name="triclops_optical" type="fixed">
+   <origin xyz="0 0 0" rpy="-${M_PI/2} 0 -${M_PI/2}"/>
+   <parent link="triclops_link"/>
+   <child link="triclops_left_optical_link"/>
+</joint>
+<link name="triclops_left_optical_link"/>
+
+<gazebo reference="triclops_link">
+   <sensor type="multicamera" name="triclops">
+      <update_rate>16.0</update_rate>
+      <camera name="left">
+        <horizontal_fov>1.3962634</horizontal_fov>
+        <image>
+          <width>1280</width>
+          <height>960</height>
+          <format>R8G8B8</format>
+        </image>
+        <clip>
+          <near>0.02</near>
+          <far>300</far>
+        </clip>
+        <noise>
+          <type>gaussian</type>
+           <mean>0.0</mean>
+          <stddev>0.007</stddev>
+        </noise>
+      </camera>
+      <camera name="right">
+        <pose>0 -0.07 0 0 0 0</pose>
+        <horizontal_fov>1.3962634</horizontal_fov>
+        <image>
+          <width>1280</width>
+          <height>960</height>
+          <format>R8G8B8</format>
+        </image>
+        <clip>
+          <near>0.02</near>
+          <far>300</far>
+        </clip>
+        <noise>
+          <type>gaussian</type>
+           <mean>0.0</mean>
+          <stddev>0.007</stddev>
+        </noise>
+      </camera>
+      <plugin name="camera_triclops_controller" filename="libgazebo_ros_multicamera.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>0.0</updateRate>
+    	<robotNamespace>/$(arg roboname)</robotNamespace>
+        <cameraName>triclops</cameraName>
+        <imageTopicName>image_rect_color</imageTopicName>
+        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
+        <frameName>triclops_left_optical_link</frameName>
+        <hackBaseline>0.07</hackBaseline>
+        <distortionK1>0.0</distortionK1>
+        <distortionK2>0.0</distortionK2>
+        <distortionK3>0.0</distortionK3>
+        <distortionT1>0.0</distortionT1>
+        <distortionT2>0.0</distortionT2>
+      </plugin>
+    </sensor>
+</gazebo>
+
+</robot>
+

launch/config/catvehicle_velodyne_points.gazebo

@@ -0,0 +1,43 @@
+<?xml version="1.0"?>
+<!--
+
+Author: Jonathan Sprinkle, Sam Taylor, Alex Warren
+Copyright (c) 2015 Arizona Board of Regents
+All rights reserved.
+
+Permission is hereby granted, without written agreement and without 
+license or royalty fees, to use, copy, modify, and distribute this
+software and its documentation for any purpose, provided that the 
+above copyright notice and the following two paragraphs appear in 
+all copies of this software.
+
+IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY 
+FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES 
+ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN 
+IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF 
+SUCH DAMAGE.
+
+THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, 
+INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY 
+AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
+IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
+TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
+
+Summary: 
+This file includes the control interfaces for ROS-based control
+through Gazebo. For more information and for the tutorials used to create
+this file, see
+http://gazebosim.org/tutorials/?tut=ros_control#Prerequisites
+
+Sensors are included separately, based on the arguments passed to the xacro include
+
+-->
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+<xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro"/>
+<xacro:VLP-16 parent="velodyne_link" name="velodyne" topic="/$(arg roboname)/velodyne_points" organize_cloud="false" hz="10" samples="440" gpu="true">
+    <origin xyz="0 0 0.0" rpy="0 0 0" />
+</xacro:VLP-16>
+
+</robot>
+

launch/demo/demo_catvehicle_mapping.launch

@@ -0,0 +1,85 @@
+<?xml version="1.0"?>
+<launch>
+    <!-- 1) Make sure rtabmap is built with libpointmatcher for lidar:=true option (lidar SLAM)
+
+         2) Install https://github.com/jmscslgroup/catvehicle
+            We need a stereo camera and velodyne point cloud looking like the real 
+            sensor (ring-like pattern), so we have to modifiy the default catvehicle 
+            urdf. First we have to install velodyne_simulator package to have the 
+            right gazebo plugin and copy this modified velodyne config: 
+               * sudo apt install ros-$ROS_DISTRO-velodyne-simulator
+               * cp ~/catkin_ws/src/rtabmap_ros/launch/config/catvehicle_velodyne_points.gazebo ~/catkin_ws/src/catvehicle/urdf/.
+            Secondly, we have to modify the triclops config to make it stereo:
+               * cp ~/catkin_ws/src/rtabmap_ros/launch/config/catvehicle_triclops.gazebo ~/catkin_ws/src/catvehicle/urdf/.
+            Set world->map frame instead of world->odom frame here:
+               * https://github.com/jmscslgroup/catvehicle/blob/f58cc64103538bc93cd42dd30f59c4b937151f88/launch/catvehicle.launch#L104
+
+         3) roslaunch catvehicle catvehicle_city.launch gui:=true
+         4) roslaunch catvehicle catvehicle_spawn.launch velodyne_points:=true triclops:=true Z:=5
+         5) roslaunch catvehicle joystick.launch
+         6) SLAM, 3 choices:
+            A) LiDAR-SLAM + Visual loop closure detection: 
+              roslaunch rtabmap_ros demo_catvehicle_mapping.launch
+            B) LiDAR-SLAM without camera: 
+              roslaunch rtabmap_ros demo_catvehicle_mapping.launch camera:=false
+            C) Visual-SLAM without lidar: 
+              roslaunch rtabmap_ros demo_catvehicle_mapping.launch lidar:=false
+
+         Note: Gazebo real-time factor should be equal or below 1. If it is over 1, 
+               in gazebo client, select Physics, then change real time update rate to 500.
+    -->
+    <arg name="camera"     default="true" />
+    <arg name="lidar"      default="true" />
+    <arg name="cell_size"  default="0.2" />
+    <arg name="rtabmapviz" default="true" />
+    <arg name="rviz"       default="true" />
+    <arg name="light"      default="false" /> <!-- Don't record all scans if false -->
+    <arg name="altitude"   default="0" /> <!-- assemble occupancy grids by altitude (radius in meters, 0=disabled) --> 
+
+    <!-- RTAB-Map -->
+    <remap from="/catvehicle/grid_map" to="/catvehicle/map"/>
+    <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
+        <arg name="namespace" value="catvehicle"/>
+
+        <!-- LiDAR SLAM parameters -->
+        <arg     if="$(arg lidar)" name="args" value="-d --Rtabmap/DetectionRate 2 --Reg/Strategy 1 --OdomF2M/ScanMaxSize 30000 --Odom/ScanKeyFrameThr 0.6 --Icp/VoxelSize $(arg cell_size) --Icp/Iterations 10 --Icp/MaxTranslation 1 --Icp/RangeMin 1 --Icp/PointToPlaneGroundNormalsUp 0.9 --Icp/PointToPlaneRadius 0 --Icp/MaxCorrespondenceDistance 1 --Grid/ClusterRadius 1 --Grid/RangeMax 20 --Grid/RangeMin 1 --Grid/RayTracing true --Grid/CellSize $(arg cell_size) --Mem/LaserScanNormalK 10 --Grid/PreVoxelFiltering false --Grid/3D false --Kp/RoiRatios '0 0 0 0.3' --Vis/RoiRatios '0 0 0 0.3' --RGBD/LinearUpdate 0.2 --Grid/MaxObstacleHeight 4 --GridGlobal/AltitudeDelta $(arg altitude)"/>
+        
+        <!-- Visual SLAM parameters -->
+        <arg unless="$(arg lidar)" name="args" value="-d --Rtabmap/DetectionRate 2 --Reg/Strategy 0 --Icp/VoxelSize $(arg cell_size) --Grid/ClusterRadius 1 --Grid/RangeMax 20 --Grid/RangeMin 2 --Grid/RayTracing true --Grid/CellSize $(arg cell_size) --Grid/3D false --Kp/RoiRatios '0 0 0 0.3' --Vis/RoiRatios '0 0 0 0.3' --Grid/DepthRoiRatios '0 0 0 0.3' --Grid/NormalsSegmentation false --Grid/MaxObstacleHeight 4 --Grid/MaxGroundHeight 0.7 --Grid/NoiseFilteringRadius 0.5 --GridGlobal/AltitudeDelta $(arg altitude)"/>
+
+        <arg name="subscribe_scan_cloud" value="$(arg lidar)"/>
+        <arg name="stereo"            value="$(arg camera)"/>
+        <arg unless="$(arg camera)" name="depth"          value="false"/>
+        <arg unless="$(arg camera)" name="subscribe_rgb"  value="false"/>
+        <arg     if="$(arg lidar)" name="icp_odometry"    value="true"/>
+        <arg unless="$(arg lidar)" name="visual_odometry" value="true"/>
+
+        <arg     if="$(arg lidar)" name="odom_topic" value="lidar_odom"/>
+        <arg unless="$(arg lidar)" name="odom_topic" value="visual_odom"/>
+        
+        <arg name="odom_guess_frame_id" value="catvehicle/odom"/>
+        <arg name="frame_id"            value="catvehicle/base_link"/>
+        <arg name="map_frame_id"        value="catvehicle/map"/>
+        <arg     if="$(arg lidar)" name="vo_frame_id" value="catvehicle/lidar_odom"/>
+        <arg unless="$(arg lidar)" name="vo_frame_id" value="catvehicle/visual_odom"/>
+        
+        <arg name="stereo_namespace"  value="/catvehicle/triclops"/>
+        <arg name="right_image_topic" value="/catvehicle/triclops/right/image_rect_color"/>
+        <arg name="scan_cloud_topic"  value="/catvehicle/velodyne_points"/>
+        <arg name="rgbd_sync"         value="$(arg camera)"/>
+        <arg name="approx_rgbd_sync"  value="false"/>
+        <arg name="approx_sync"       value="$(eval camera and lidar)"/>
+        <arg name="use_sim_time"      value="true"/>
+        <arg name="wait_for_transform" value="0.3"/>
+
+        <arg name="rtabmapviz"      value="$(arg rtabmapviz)"/>
+
+        <arg name="scan_cloud_assembling"             value="$(eval lidar and not light)"/>
+        <arg name="scan_cloud_assembling_fixed_frame" value="catvehicle/lidar_odom"/>
+        <arg name="scan_cloud_assembling_range_max"   value="60"/>
+        <arg name="scan_cloud_assembling_voxel_size"  value="$(arg cell_size)"/>
+        <arg name="scan_cloud_assembling_range_min"   value="1"/>
+    </include>
+
+    <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find catvehicle)/config/catvehicle.rviz"/>
+</launch> 

launch/demo/demo_isaac_carter_navigation.launch

@@ -26,6 +26,8 @@
     <arg name="lidar3d_ray_tracing" default="true" />
     <arg name="lidar3d_grid3d"      default="true" />
 
+    <arg name="camera"              default="true" />
+
     <!-- Load Robot Description -->
     <arg name="model" default="$(find carter_description)/urdf/carter.urdf"/>
     <param name="robot_description" textfile="$(arg model)" />
@@ -33,17 +35,19 @@
     <!-- Lidar 3D based on demo_husky.launch -->
     <arg     if="$(arg lidar3d)" name="cell_size" default="0.2" />
     <arg unless="$(arg lidar3d)" name="cell_size" default="0.05" />
-    <arg     if="$(arg lidar3d)" name="args3d" value="--Icp/Iterations 10 --Icp/PointToPlaneGroundNormalsUp 0.9 --Icp/PointToPlaneRadius 0 --Icp/MaxCorrespondenceDistance 1 --Grid/ClusterRadius 1 --Grid/RangeMax 10 --Grid/RayTracing $(arg lidar3d_ray_tracing) --Grid/CellSize $(arg cell_size) --Mem/LaserScanNormalK --Grid/3D $(arg lidar3d_grid3d)" />
+    <arg     if="$(arg lidar3d)" name="args3d" value="--Icp/Iterations 10 --Icp/PointToPlaneGroundNormalsUp 0.9 --Icp/PointToPlaneRadius 0 --Icp/MaxCorrespondenceDistance 1 --Grid/ClusterRadius 1 --Grid/RangeMax 10 --Grid/RayTracing $(arg lidar3d_ray_tracing) --Grid/CellSize $(arg cell_size) --Mem/LaserScanNormalK 10 --Grid/3D $(arg lidar3d_grid3d)" />
     <arg unless="$(arg lidar3d)" name="args3d" value="--Grid/RangeMax 10" /> <!-- 2d scan, use default params -->
 
     <!-- RTAB-Map -->
     <remap from="/rtabmap/grid_map" to="/map"/>
     <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
-        <arg     if="$(arg localization)" name="args" value="--Reg/Strategy 1 --Reg/Force3DoF true --RGBD/NeighborLinkRefining true $(arg args3d)"/>
-        <arg unless="$(arg localization)" name="args" value="-d --Reg/Strategy 1 --Reg/Force3DoF true --RGBD/NeighborLinkRefining true $(arg args3d)"/>
+        <arg     if="$(arg localization)" name="args" value="--Reg/Strategy 1 --Reg/Force3DoF true --RGBD/NeighborLinkRefining true --Icp/MaxTranslation 0.5 $(arg args3d)  --Kp/MaxFeatures -1 --RGBD/ProximityBySpace false --Rtabmap/DetectionRate 3"/>
+        <arg unless="$(arg localization)" name="args" value="-d --Reg/Strategy 1 --Reg/Force3DoF true --RGBD/NeighborLinkRefining true --Icp/MaxTranslation 0.5 $(arg args3d)"/>
         <arg name="localization"      value="$(arg localization)"/>
         <arg     if="$(arg lidar3d)" name="subscribe_scan_cloud" value="true"/>
         <arg unless="$(arg lidar3d)" name="subscribe_scan" value="true"/>
+        <arg name="subscribe_depth"   value="$(arg camera)"/>
+        <arg name="subscribe_rgb"     value="$(arg camera)"/>
         <arg name="visual_odometry"   value="false"/>
         <arg name="odom_topic"        value="/odom"/>
         <arg name="frame_id"          value="base_link"/>
@@ -55,6 +59,9 @@
         <arg name="rgbd_sync"         value="true"/>
         <arg name="approx_rgbd_sync"  value="false"/>
         <arg name="use_sim_time"      value="true"/>
+        <arg name="tag_linear_variance" value="0.01"/>
+        <arg name="tag_angular_variance" value="1"/>
+        
 
         <arg name="scan_cloud_assembling"             value="$(arg lidar3d)"/>
         <arg name="scan_cloud_assembling_fixed_frame" value="odom"/>

launch/rtabmap.launch

@@ -321,6 +321,7 @@
       <param name="range_max"       type="double" value="$(arg scan_cloud_assembling_range_max)"/>
       <param name="noise_radius"        type="double" value="$(arg scan_cloud_assembling_noise_radius)"/>
       <param name="noise_min_neighbors" type="int"    value="$(arg scan_cloud_assembling_noise_min_neighbors)"/>
+      <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
     </node>
   
     <!-- Visual SLAM (robot side) -->

src/MapAssemblerNode.cpp

@@ -74,6 +74,8 @@ public:
 		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
 		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("GridGlobal"));
 		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
+		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoSGBM"));
+		uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(rtabmap::Parameters::defaultIcpPointToPlaneGroundNormalsUp())));
 		if(!configPath.empty())
 		{
 			if(UFile::exists(configPath.c_str()))
@@ -374,6 +376,8 @@ int main(int argc, char** argv)
 			uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
 			uInsert(parameters, rtabmap::Parameters::getDefaultParameters("GridGlobal"));
 			uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
+			uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoSGBM"));
+			uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(rtabmap::Parameters::defaultIcpPointToPlaneGroundNormalsUp())));
 			for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
 			{
 				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";

src/rviz/MapCloudDisplay.cpp

@@ -137,6 +137,7 @@ MapCloudDisplay::MapCloudDisplay()
 	cloud_from_scan_ = new rviz::BoolProperty( "Cloud from scan", false,
 										 "Create the cloud from laser scans instead of the RGB-D/Stereo images.",
 										 this, SLOT( updateCloudParameters() ), this );
+	fromScan_ = cloud_from_scan_->getBool();
 
 	cloud_decimation_ = new rviz::IntProperty( "Cloud decimation", 4,
 										 "Decimation of the input RGB and depth images before creating the cloud.",
@@ -281,6 +282,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 
 	// Add new clouds...
 	bool fromDepth = !cloud_from_scan_->getBool();
+	std::set<int> nodeDataReceived;
 	for(unsigned int i=0; i<map.nodes.size() && i<map.nodes.size(); ++i)
 	{
 		int id = map.nodes[i].id;
@@ -378,6 +380,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 				}
 			}
 		}
+		nodeDataReceived.insert(id);
 	}
 
 	// Update graph
@@ -392,6 +395,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 		boost::mutex::scoped_lock lock(current_map_mutex_);
 		current_map_ = poses;
 		current_map_updated_ = true;
+		nodeDataReceived_.insert(nodeDataReceived.begin(), nodeDataReceived.end());
 	}
 }
 
@@ -531,7 +535,14 @@ void MapCloudDisplay::updateBillboardSize()
 
 void MapCloudDisplay::updateCloudParameters()
 {
-	// do nothing... only take effect on next generated clouds
+	// do nothing for most parameters... only take effect on next generated clouds
+
+	// if we change the kind of map, clear
+	if(fromScan_ != cloud_from_scan_->getBool())
+	{
+		reset();
+	}
+	fromScan_ = cloud_from_scan_->getBool();
 }
 
 void MapCloudDisplay::downloadMap(bool graphOnly)
@@ -758,7 +769,7 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
 								cloudInfoIt->second->message_->header.frame_id.c_str());
 					}
 				}
-				else if(it->first>0 && current_map_updated_)
+				else if(it->first>0 && current_map_updated_&& nodeDataReceived_.find(it->first) == nodeDataReceived_.end())
 				{
 					missingNodes.push_back(it->first);
 				}
@@ -816,6 +827,7 @@ void MapCloudDisplay::reset()
 		boost::mutex::scoped_lock lock(current_map_mutex_);
 		current_map_.clear();
 		current_map_updated_ = false;
+		nodeDataReceived_.clear();
 	}
 	MFDClass::reset();
 }

src/rviz/MapCloudDisplay.h

@@ -175,6 +175,9 @@ private:
 	std::map<int, CloudInfoPtr> new_cloud_infos_;
 	boost::mutex new_clouds_mutex_;
 
+	std::set<int> nodeDataReceived_;
+	bool fromScan_;
+
 	std::map<int, rtabmap::Transform> current_map_;
 	boost::mutex current_map_mutex_;
 	bool current_map_updated_;