diff --git a/rtabmap_demos/launch/config/catvehicle.rviz b/rtabmap_demos/launch/config/catvehicle.rviz
index c0fea844..f9433776 100644
--- a/rtabmap_demos/launch/config/catvehicle.rviz
+++ b/rtabmap_demos/launch/config/catvehicle.rviz
@@ -254,7 +254,7 @@ Visualization Manager:
         Value: true
       Axis: Z
       Channel Name: z
-      Class: rtabmap_ros/MapCloud
+      Class: rtabmap_rviz_plugins/MapCloud
       Cloud decimation: 1
       Cloud from scan: true
       Cloud max depth (m): 0
@@ -284,7 +284,7 @@ Visualization Manager:
       Use rainbow: true
       Value: true
     - Alpha: 1
-      Class: rtabmap_ros/MapGraph
+      Class: rtabmap_rviz_plugins/MapGraph
       Enabled: true
       Global loop closure: 255; 0; 0
       Landmark: 0; 128; 0
@@ -324,7 +324,7 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Class: rtabmap_ros/OrbitOriented
+      Class: rtabmap_rviz_plugins/OrbitOriented
       Distance: 32.8247184753418
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
@@ -342,7 +342,7 @@ Visualization Manager:
       Near Clip Distance: 0.009999999776482582
       Pitch: 0.5253984332084656
       Target Frame: catvehicle/base_link
-      Value: OrbitOriented (rtabmap_ros)
+      Value: OrbitOriented (rtabmap_rviz_plugins)
       Yaw: 2.720399856567383
     Saved: ~
 Window Geometry:
diff --git a/rtabmap_demos/launch/config/demo_find_object.rviz b/rtabmap_demos/launch/config/demo_find_object.rviz
index 3f410433..4ad949c2 100644
--- a/rtabmap_demos/launch/config/demo_find_object.rviz
+++ b/rtabmap_demos/launch/config/demo_find_object.rviz
@@ -249,7 +249,7 @@ Visualization Manager:
         Value: true
       Axis: Z
       Channel Name: intensity
-      Class: rtabmap_ros/MapCloud
+      Class: rtabmap_rviz_plugins/MapCloud
       Cloud decimation: 4
       Cloud from scan: false
       Cloud max depth (m): 3
@@ -279,7 +279,7 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
-    - Class: rtabmap_ros/Info
+    - Class: rtabmap_rviz_plugins/Info
       Enabled: true
       Name: Info
       Topic: /rtabmap/info
@@ -339,7 +339,7 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Class: rtabmap_ros/OrbitOriented
+      Class: rtabmap_rviz_plugins/OrbitOriented
       Distance: 7.50918865
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
diff --git a/rtabmap_demos/launch/config/demo_robot_mapping.rviz b/rtabmap_demos/launch/config/demo_robot_mapping.rviz
index 97648aab..d6d6722d 100644
--- a/rtabmap_demos/launch/config/demo_robot_mapping.rviz
+++ b/rtabmap_demos/launch/config/demo_robot_mapping.rviz
@@ -141,7 +141,7 @@ Visualization Manager:
         Value: true
       Axis: Z
       Channel Name: intensity
-      Class: rtabmap_ros/MapCloud
+      Class: rtabmap_rviz_plugins/MapCloud
       Cloud decimation: 8
       Cloud max depth (m): 4
       Cloud min depth (m): 0
@@ -171,7 +171,7 @@ Visualization Manager:
       Use rainbow: true
       Value: true
     - Alpha: 1
-      Class: rtabmap_ros/MapGraph
+      Class: rtabmap_rviz_plugins/MapGraph
       Enabled: true
       Global loop closure: 255; 0; 0
       Local loop closure: 255; 255; 0
@@ -192,7 +192,7 @@ Visualization Manager:
       Topic: /rtabmap/grid_map
       Unreliable: false
       Value: true
-    - Class: rtabmap_ros/Info
+    - Class: rtabmap_rviz_plugins/Info
       Enabled: true
       Name: Info
       Topic: /rtabmap/info
diff --git a/rtabmap_demos/launch/config/demo_stereo_outdoor.rviz b/rtabmap_demos/launch/config/demo_stereo_outdoor.rviz
index a827b441..ddd96f46 100644
--- a/rtabmap_demos/launch/config/demo_stereo_outdoor.rviz
+++ b/rtabmap_demos/launch/config/demo_stereo_outdoor.rviz
@@ -124,7 +124,7 @@ Visualization Manager:
         Value: true
       Axis: Z
       Channel Name: intensity
-      Class: rtabmap_ros/MapCloud
+      Class: rtabmap_rviz_plugins/MapCloud
       Cloud decimation: 4
       Cloud max depth (m): 4
       Cloud min depth (m): 0
@@ -154,7 +154,7 @@ Visualization Manager:
       Use rainbow: true
       Value: true
     - Alpha: 1
-      Class: rtabmap_ros/MapGraph
+      Class: rtabmap_rviz_plugins/MapGraph
       Enabled: true
       Global loop closure: 255; 0; 0
       Local loop closure: 255; 255; 0
@@ -187,7 +187,7 @@ Visualization Manager:
       Topic: /rtabmap/grid_map
       Unreliable: false
       Value: true
-    - Class: rtabmap_ros/Info
+    - Class: rtabmap_rviz_plugins/Info
       Enabled: true
       Name: Info
       Topic: /rtabmap/info
@@ -276,7 +276,7 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Class: rtabmap_ros/OrbitOriented
+      Class: rtabmap_rviz_plugins/OrbitOriented
       Distance: 8.28384018
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
diff --git a/rtabmap_demos/launch/config/find_object.ini b/rtabmap_demos/launch/config/find_object.ini
index 3f219c14..d9140fb9 100644
--- a/rtabmap_demos/launch/config/find_object.ini
+++ b/rtabmap_demos/launch/config/find_object.ini
@@ -1,6 +1,6 @@
 [General]
-windowGeometry=@ByteArray(\x1\xd9\xd0\xcb\0\x2\0\0\0\0\x1\x2\0\0\x2\xeb\0\0\x2\x96\0\0\x4*\0\0\x1\x2\0\0\x3\a\0\0\x2\x96\0\0\x4*\0\0\0\0\0\0\0\0\a\x80)
-windowState="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\0\xc8\0\0\x3\a\xfc\x2\0\0\0\x1\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0o\0\x62\0j\0\x65\0\x63\0t\0s\0\0\0\0\x19\0\0\x3\a\0\0\0\xc4\0\xff\xff\xff\0\0\0\x1\0\0\x1h\0\0\x3\a\xfc\x2\0\0\0\x2\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0\x61\0r\0\x61\0m\0\x65\0t\0\x65\0r\0s\0\0\0\0\x19\0\0\x3\a\0\0\0\xa8\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0i\0s\0t\0i\0\x63\0s\0\0\0\0\0\xff\xff\xff\xff\0\0\x1;\0\xff\xff\xff\0\0\0\x3\0\0\0\0\0\0\0\0\xfc\x1\0\0\0\x1\xfb\0\0\0\x1e\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0l\0o\0t\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\0\0\x1\x95\0\0\x1\xe\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\0)"
+windowGeometry=@ByteArray(\x1\xd9\xd0\xcb\0\x3\0\0\0\0\x1\x2\0\0\x2\xe2\0\0\x2\x96\0\0\x4*\0\0\x1\x2\0\0\x3\a\0\0\x2\x96\0\0\x4*\0\0\0\0\0\0\0\0\a\x80\0\0\x1\x2\0\0\x3\a\0\0\x2\x96\0\0\x4*)
+windowState=@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\0\xc8\0\0\x3\a\xfc\x2\0\0\0\x1\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0o\0\x62\0j\0\x65\0\x63\0t\0s\0\0\0\0\x19\0\0\x3\a\0\0\0\xc4\0\xff\xff\xff\0\0\0\x1\0\0\x1h\0\0\x3\a\xfc\x2\0\0\0\x2\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0\x61\0r\0\x61\0m\0\x65\0t\0\x65\0r\0s\0\0\0\0\x19\0\0\x3\a\0\0\0\xa8\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0i\0s\0t\0i\0\x63\0s\0\0\0\0\0\xff\xff\xff\xff\0\0\x1\x35\0\xff\xff\xff\0\0\0\x3\0\0\0\0\0\0\0\0\xfc\x1\0\0\0\x1\xfb\0\0\0\x1e\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0l\0o\0t\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\0\0\x1\x95\0\0\0\xf8\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\0)
 
 [Camera]
 1deviceId=0
@@ -14,8 +14,8 @@ windowState="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\0\xc8\0\0\x3
 9queueSize=1
 
 [Feature2D]
-1Detector="5:Dense;Fast;GFTT;MSER;ORB;SIFT;Star;SURF;BRISK;AGAST;KAZE;AKAZE"
-2Descriptor="2:Brief;ORB;SIFT;SURF;BRISK;FREAK;KAZE;AKAZE;LUCID;LATCH;DAISY"
+1Detector="10:Dense;Fast;GFTT;MSER;ORB;SIFT;Star;SURF;BRISK;AGAST;KAZE;AKAZE;SuperPointTorch"
+2Descriptor="6:Brief;ORB;SIFT;SURF;BRISK;FREAK;KAZE;AKAZE;LUCID;LATCH;DAISY;SuperPointTorch"
 3MaxFeatures=0
 4Affine=false
 5AffineCount=6
@@ -109,6 +109,11 @@ Star_lineThresholdProjected=10
 Star_maxSize=45
 Star_responseThreshold=30
 Star_suppressNonmaxSize=5
+SuperPointTorch_NMS=true
+SuperPointTorch_NMS_radius=4
+SuperPointTorch_cuda=false
+SuperPointTorch_modelPath=
+SuperPointTorch_threshold=0.2
 
 [%General]
 autoPauseOnDetection=false
diff --git a/rtabmap_demos/launch/config/turtlebot_navigation.rviz b/rtabmap_demos/launch/config/turtlebot_navigation.rviz
index 4dff18b8..b332c934 100644
--- a/rtabmap_demos/launch/config/turtlebot_navigation.rviz
+++ b/rtabmap_demos/launch/config/turtlebot_navigation.rviz
@@ -363,7 +363,7 @@ Visualization Manager:
         Value: true
       Axis: Z
       Channel Name: intensity
-      Class: rtabmap_ros/MapCloud
+      Class: rtabmap_rviz_plugins/MapCloud
       Cloud decimation: 4
       Cloud max depth (m): 4
       Cloud voxel size (m): 0.05
diff --git a/rtabmap_demos/launch/demo_appearance_mapping.launch b/rtabmap_demos/launch/demo_appearance_mapping.launch
index 435c6c5c..acf82d29 100644
--- a/rtabmap_demos/launch/demo_appearance_mapping.launch
+++ b/rtabmap_demos/launch/demo_appearance_mapping.launch
@@ -12,7 +12,7 @@
 
   <group ns="rtabmap">
 	<!-- 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_slam" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
 
 	    <param name="subscribe_rgb"       type="bool" value="false"/>    <!-- must be false for appearance-based mode -->
 	    <param name="subscribe_depth"     type="bool" value="false"/>    <!-- must be false for appearance-based mode -->
@@ -40,11 +40,11 @@
 	</node>
 
 	<!-- visualization of the "infoEx" topic sent by rtabmap node -->
-	<node name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen" args="-d $(find rtabmap_ros)/launch/config/appearance_gui.ini">
+	<node name="rtabmap_viz" pkg="rtabmap_viz" type="rtabmap_viz" output="screen" args="-d $(find rtabmap_demos)/launch/config/appearance_gui.ini">
 	    <param name="subscribe_odom"      type="bool" value="false"/>    <!-- must be false for appearance-based mode -->
 
-	    <!-- This enables the GUI to pause a rtabmap_ros/camera when action "pause" is checked. -->
-	    <!-- NOTE: It is specific to rtabmap_ros/camera. Action "pause" in the GUI will still pause the rtabmap node. -->
+	    <!-- This enables the GUI to pause a rtabmap_legacy/camera when action "pause" is checked. -->
+	    <!-- NOTE: It is specific to rtabmap_legacy/camera. Action "pause" in the GUI will still pause the rtabmap node. -->
 		<param name="camera_node_name" type="string" value="/camera"/>
 
 	</node>
@@ -55,7 +55,7 @@
 		<remap from="image" to="image"/>
 
 		<param name="device_id"            value="0" type="int"/>
-		<param name="video_or_images_path" value="$(find rtabmap_ros)/launch/data/demo_appearance" type="string"/>
+		<param name="video_or_images_path" value="$(find rtabmap_demos)/launch/data/demo_appearance" type="string"/>
 		<param name="frame_rate"           value="2.0" type="double"/>
 		<param name="width"                value="0" type="int"/>
 		<param name="height"               value="0" type="int"/>
diff --git a/rtabmap_demos/launch/demo_catvehicle_mapping.launch b/rtabmap_demos/launch/demo_catvehicle_mapping.launch
index 60f61c8b..5254ef36 100644
--- a/rtabmap_demos/launch/demo_catvehicle_mapping.launch
+++ b/rtabmap_demos/launch/demo_catvehicle_mapping.launch
@@ -1,5 +1,4 @@
 <?xml version="1.0"?>
-<?xml version="1.0"?>
 <launch>
     <!-- 1) Make sure rtabmap is built with libpointmatcher for lidar:=true option (lidar SLAM)
 
@@ -9,9 +8,9 @@
             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/.
+               * cp ~/catkin_ws/src/rtabmap_ros/rtabmap_demos/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/.
+               * cp ~/catkin_ws/src/rtabmap_ros/rtabmap_demos/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
 
@@ -20,11 +19,11 @@
          5) roslaunch catvehicle joystick.launch
          6) SLAM, 3 choices:
             A) LiDAR-SLAM + Visual loop closure detection:
-              roslaunch rtabmap_ros demo_catvehicle_mapping.launch
+              roslaunch rtabmap_demos demo_catvehicle_mapping.launch
             B) LiDAR-SLAM without camera:
-              roslaunch rtabmap_ros demo_catvehicle_mapping.launch camera:=false
+              roslaunch rtabmap_demos demo_catvehicle_mapping.launch camera:=false
             C) Visual-SLAM without lidar:
-              roslaunch rtabmap_ros demo_catvehicle_mapping.launch lidar:=false
+              roslaunch rtabmap_demos 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.
@@ -32,7 +31,7 @@
     <arg name="camera"     default="true" />
     <arg name="lidar"      default="true" />
     <arg name="cell_size"  default="0.2" />
-    <arg name="rtabmapviz" default="true" />
+    <arg name="rtabmap_viz" 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) -->
@@ -72,7 +71,7 @@
 
     <!-- RTAB-Map -->
     <remap from="/catvehicle/grid_map" to="/catvehicle/map"/>
-    <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
+    <include file="$(find rtabmap_launch)/launch/rtabmap.launch">
         <arg name="namespace" value="catvehicle"/>
 
         <!-- LiDAR SLAM parameters -->
@@ -112,7 +111,7 @@
         <arg name="use_sim_time"      value="true"/>
         <arg name="wait_for_transform" value="0.3"/>
 
-        <arg name="rtabmapviz"      value="$(arg rtabmapviz)"/>
+        <arg name="rtabmap_viz"      value="$(arg rtabmap_viz)"/>
 
         <arg name="scan_cloud_assembling"             value="$(eval lidar and not light)"/>
         <arg name="scan_cloud_assembling_fixed_frame" value="catvehicle/lidar_odom"/>
@@ -121,5 +120,5 @@
         <arg name="scan_cloud_assembling_range_min"   value="2"/>
     </include>
 
-    <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/catvehicle.rviz"/>
+    <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_demos)/launch/config/catvehicle.rviz"/>
 </launch>
diff --git a/rtabmap_demos/launch/demo_data_recorder.launch b/rtabmap_demos/launch/demo_data_recorder.launch
index 7f3c17d6..8a8b00be 100644
--- a/rtabmap_demos/launch/demo_data_recorder.launch
+++ b/rtabmap_demos/launch/demo_data_recorder.launch
@@ -7,7 +7,7 @@
 
   <param name="use_sim_time" type="bool" value="True"/>
 
-  <include file="$(find rtabmap_ros)/launch/data_recorder.launch">
+  <include file="$(find rtabmap_launch)/launch/data_recorder.launch">
     <arg name="subscribe_odometry"   value="true"/>
     <arg name="subscribe_depth"      value="true"/>
     <arg name="subscribe_stereo"     value="false"/>
diff --git a/rtabmap_demos/launch/demo_find_object.launch b/rtabmap_demos/launch/demo_find_object.launch
index 5fe89927..8be8525b 100644
--- a/rtabmap_demos/launch/demo_find_object.launch
+++ b/rtabmap_demos/launch/demo_find_object.launch
@@ -4,10 +4,10 @@
 
   <!-- Choose visualization -->
   <arg name="rviz" default="true" />
-  <arg name="rtabmapviz" default="false" />
+  <arg name="rtabmap_viz" default="false" />
   <arg name="save_objects" default="false"/>
   <arg name="localization" default="false"/>
-  <arg name="save_objects_as_landmarks" default="false"/> <!-- apriltag_ros package should be installed and rtabmap_ros built with it -->
+  <arg name="save_objects_as_landmarks" default="false"/> <!-- apriltag_ros package should be installed and rtabmap_slam built with it -->
   <arg     if="$(arg localization)" name="rtabmap_args" default=""/>
   <arg unless="$(arg localization)" name="rtabmap_args" default="--delete_db_on_start"/>
 
@@ -16,7 +16,7 @@
   <!-- SLAM (robot side) -->
   <!-- args: "delete_db_on_start" and "udebug" -->
   <group ns="rtabmap">
-    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
+    <node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
 	  <param name="frame_id" type="string" value="base_footprint"/>
 
 	  <param name="subscribe_depth" type="bool" value="true"/>
@@ -59,7 +59,7 @@
     </node>
 
     <!-- Visualisation RTAB-Map -->
-    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
+    <node if="$(arg rtabmap_viz)" pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(find rtabmap_demos)/launch/config/rgbd_gui.ini" output="screen">
   	  <param name="subscribe_depth" type="bool" value="true"/>
       <param name="subscribe_scan"  type="bool" value="true"/>
       <param name="frame_id"        type="string" value="base_footprint"/>
@@ -82,9 +82,9 @@
   -->
 
   <!-- Visualisation -->
-  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_find_object.rviz" output="screen"/>
+  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_demos)/launch/config/demo_find_object.rviz" output="screen"/>
 
-  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
+  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_util/point_cloud_xyzrgb">
     <remap from="rgb/image"       to="/camera/data_throttled_image"/>
     <remap from="depth/image"     to="/camera/data_throttled_image_depth"/>
     <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
@@ -100,9 +100,9 @@
   <!-- Find-Object -->
   <node name="find_object_3d" pkg="find_object_2d" type="find_object_2d" output="screen">
     <param name="gui"             value="true" type="bool"/>
-    <param name="settings_path"   value="$(find rtabmap_ros)/launch/config/find_object.ini" type="str"/>
+    <param name="settings_path"   value="$(find rtabmap_demos)/launch/config/find_object.ini" type="str"/>
     <param name="subscribe_depth" value="true" type="bool"/>
-    <param name="objects_path"    value="$(find rtabmap_ros)/launch/data/books" type="str"/>
+    <param name="objects_path"    value="$(find rtabmap_demos)/launch/data/books" type="str"/>
 
     <remap from="rgb/image_rect_color"         to="/camera/data_throttled_image"/>
     <remap from="depth_registered/image_raw"   to="/camera/data_throttled_image_depth"/>
@@ -113,13 +113,13 @@
   </node>
 
   <!-- Save objects to database example -->
-  <node if="$(arg save_objects)" name="save_objects_example" pkg="rtabmap_ros" type="save_objects_example" output="screen">
+  <node if="$(arg save_objects)" name="save_objects_example" pkg="rtabmap_demos" type="save_objects_example" output="screen">
     <remap from="mapData" to="/rtabmap/mapData"/>
     <param name="frame_id" value="base_footprint"/>
   </node>
 
   <!-- Convert objects to tags -->
-  <node if="$(arg save_objects_as_landmarks)" name="objects_to_tags" pkg="rtabmap_ros" type="objects_to_tags.py" output="screen">
+  <node if="$(arg save_objects_as_landmarks)" name="objects_to_tags" pkg="rtabmap_util" type="objects_to_tags.py" output="screen">
     <remap from="tag_detections" to="/rtabmap/tag_detections"/>
     <param name="distance_max" value="2.0"/>
   </node>
diff --git a/rtabmap_demos/launch/demo_hector_mapping.launch b/rtabmap_demos/launch/demo_hector_mapping.launch
index d566b2bc..36fa3f84 100644
--- a/rtabmap_demos/launch/demo_hector_mapping.launch
+++ b/rtabmap_demos/launch/demo_hector_mapping.launch
@@ -10,7 +10,7 @@
 
   <!-- Choose visualization -->
   <arg name="rviz" default="true" />
-  <arg name="rtabmapviz" default="false" />
+  <arg name="rtabmap_viz" default="false" />
 
   <!-- Choose hector_slam or icp_odometry for odometry -->
   <arg name="hector" default="true" />
@@ -64,7 +64,7 @@
   </node>
 
   <!-- If argument "hector" is false, we use rtabmap's icp odometry to generate odometry for us -->
-  <node unless="$(arg hector)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen" >
+  <node unless="$(arg hector)" pkg="rtabmap_odom" type="icp_odometry" name="icp_odometry" output="screen" >
      <remap from="scan"      to="/jn0/base_scan"/>
      <remap from="odom"      to="/scanmatch_odom"/>
      <remap from="odom_info"      to="/rtabmap/odom_info"/>
@@ -95,7 +95,7 @@
   </node>
 
   <group ns="rtabmap">
-    <node if="$(arg camera)" pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
+    <node if="$(arg camera)" pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_sync/rgbd_sync" output="screen">
       <remap from="rgb/image"       to="/data_throttled_image"/>
       <remap from="depth/image"     to="/data_throttled_image_depth"/>
       <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
@@ -105,7 +105,7 @@
 
     <!-- SLAM -->
     <!-- args: "delete_db_on_start" and "udebug" -->
-    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
+    <node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="--delete_db_on_start">
       <param name="frame_id" type="string" value="base_footprint"/>
 
       <param name="subscribe_rgb"   type="bool" value="false"/>
@@ -134,7 +134,7 @@
     </node>
 
     <!-- Visualisation RTAB-Map -->
-    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
+    <node if="$(arg rtabmap_viz)" pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(find rtabmap_demos)/launch/config/rgbd_gui.ini" output="screen">
       <param name="subscribe_rgbd"      type="bool" value="$(arg camera)"/>
       <param name="subscribe_laserScan" type="bool" value="true"/>
       <param name="frame_id"            type="string" value="base_footprint"/>
@@ -151,8 +151,8 @@
   </group>
 
   <!-- Visualisation RVIZ -->
-  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_robot_mapping.rviz" output="screen"/>
-   <node if="$(arg camera)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
+  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_demos)/launch/config/demo_robot_mapping.rviz" output="screen"/>
+   <node if="$(arg camera)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_util/point_cloud_xyzrgb">
     <remap from="rgbd_image"      to="/rtabmap/rgbd_image"/>
     <remap from="cloud"           to="voxel_cloud" />
 
diff --git a/rtabmap_demos/launch/demo_husky.launch b/rtabmap_demos/launch/demo_husky.launch
index a9969f75..8b44b526 100644
--- a/rtabmap_demos/launch/demo_husky.launch
+++ b/rtabmap_demos/launch/demo_husky.launch
@@ -3,7 +3,7 @@
 <launch>
 
   <!-- Bringup the Husky with SICK (2D LiDAR), realsense camera (RGB-D camera) and velodyne (3D LiDAR):
-       $ export HUSKY_URDF_EXTRAS=$(rospack find rtabmap_ros)/launch/config/husky_velodyne_extra.urdf.xacro
+       $ export HUSKY_URDF_EXTRAS=$(rospack find rtabmap_demos)/launch/config/husky_velodyne_extra.urdf.xacro
        $ roslaunch husky_gazebo husky_playpen.launch realsense_enabled:=true
        $ roslaunch husky_viz view_robot.launch
 
@@ -11,37 +11,37 @@
 
        Examples:
          1) 6DoF mapping with 3D LiDAR
-            $ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=false
+            $ roslaunch rtabmap_demos demo_husky.launch lidar3d:=true slam2d:=false
 
          2) 6DoF mapping with 3D LiDAR and RGB-D camera
-            $ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=false camera:=true
+            $ roslaunch rtabmap_demos demo_husky.launch lidar3d:=true slam2d:=false camera:=true
 
          3) 6DoF mapping with 3D LiDAR and RGB-D camera and ICP odometry (with wheel odometry as guess)
-            $ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=false camera:=true icp_odometry:=true
+            $ roslaunch rtabmap_demos demo_husky.launch lidar3d:=true slam2d:=false camera:=true icp_odometry:=true
 
          4) 3DoF mapping with 3D LiDAR
-            $ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=true
+            $ roslaunch rtabmap_demos demo_husky.launch lidar3d:=true slam2d:=true
 
          5) 3DoF mapping with 3D LiDAR and RGB-D camera
-            $ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=true camera:=true
+            $ roslaunch rtabmap_demos demo_husky.launch lidar3d:=true slam2d:=true camera:=true
 
          6) 3DoF mapping with 3D LiDAR and RGB-D camera and ICP odometry (with wheel odometry as guess)
-            $ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=true camera:=true icp_odometry:=true
+            $ roslaunch rtabmap_demos demo_husky.launch lidar3d:=true slam2d:=true camera:=true icp_odometry:=true
 
          7) 3DoF mapping with 2D LiDAR
-            $ roslaunch rtabmap_ros demo_husky.launch lidar2d:=true slam2d:=true
+            $ roslaunch rtabmap_demos demo_husky.launch lidar2d:=true slam2d:=true
 
          8) 3DoF mapping with 2D LiDAR and RGB-D camera
-            $ roslaunch rtabmap_ros demo_husky.launch lidar2d:=true slam2d:=true camera:=true
+            $ roslaunch rtabmap_demos demo_husky.launch lidar2d:=true slam2d:=true camera:=true
 
          9) 3DoF mapping with 2D LiDAR and RGB-D camera and ICP odometry (with wheel odometry as guess)
-            $ roslaunch rtabmap_ros demo_husky.launch lidar2d:=true slam2d:=true camera:=true icp_odometry:=true
+            $ roslaunch rtabmap_demos demo_husky.launch lidar2d:=true slam2d:=true camera:=true icp_odometry:=true
 
          10) 6DoF mapping with 3D LiDAR and RGB camera (depth generated by lidar projection) and ICP odometry (with wheel odometry as guess)
-            $ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=false camera:=true icp_odometry:=true depth_from_lidar:=true rtabmapviz:=true
+            $ roslaunch rtabmap_demos demo_husky.launch lidar3d:=true slam2d:=false camera:=true icp_odometry:=true depth_from_lidar:=true rtabmap_viz:=true
 
          11) 3DoF mapping with 3D LiDAR and RGB camera (depth generated by lidar projection) and ICP odometry (with wheel odometry as guess)
-            $ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=true camera:=true icp_odometry:=true depth_from_lidar:=true rtabmapviz:=true
+            $ roslaunch rtabmap_demos demo_husky.launch lidar3d:=true slam2d:=true camera:=true icp_odometry:=true depth_from_lidar:=true rtabmap_viz:=true
 
        Issues:
          When setting icp_odometry:=true with navigation, sending a goal to move_base could cause errors like:
@@ -54,7 +54,7 @@
   <arg name="navigation"   default="true"/>
   <arg name="localization" default="false"/>
   <arg name="icp_odometry" default="false"/>
-  <arg name="rtabmapviz"   default="false"/>
+  <arg name="rtabmap_viz"  default="false"/>
   <arg name="camera"       default="false"/>
   <arg name="lidar2d"      default="false"/>
   <arg name="lidar3d"      default="false"/>
@@ -66,6 +66,8 @@
   <arg     if="$(arg lidar3d)" name="cell_size" default="0.3"/>
   <arg unless="$(arg lidar3d)" name="cell_size" default="0.05"/>
 
+  <arg if="$(eval not lidar2d and not lidar3d)" name="lidar_args" default=""/>
+
   <arg if="$(arg lidar2d)" name="lidar_args" default="
       --Reg/Strategy 1
       --RGBD/NeighborLinkRefining true
@@ -95,7 +97,7 @@
 
   <!--- Run rtabmap -->
   <remap from="/rtabmap/grid_map" to="/map"/>
-  <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
+  <include file="$(find rtabmap_launch)/launch/rtabmap.launch">
     <arg     if="$(arg localization)" name="args" value="--Reg/Force3DoF $(arg slam2d) $(arg lidar_args)" />
     <arg unless="$(arg localization)" name="args" value="--Reg/Force3DoF $(arg slam2d) $(arg lidar_args) -d" /> <!-- create new map -->
     <arg name="localization"    value="$(arg localization)" />
@@ -104,7 +106,7 @@
     <arg name="imu_topic"       value="/imu/data" />
     <arg unless="$(arg icp_odometry)" name="odom_topic" value="/odometry/filtered" />
     <arg name="frame_id"        value="base_link" />
-    <arg name="rtabmapviz"      value="$(arg rtabmapviz)" />
+    <arg name="rtabmap_viz"      value="$(arg rtabmap_viz)" />
     <arg name="gps_topic"       value="/navsat/fix"/>
 
     <!-- 2D LiDAR -->
diff --git a/rtabmap_demos/launch/demo_isaac_carter_navigation.launch b/rtabmap_demos/launch/demo_isaac_carter_navigation.launch
index f3987d42..0708e743 100644
--- a/rtabmap_demos/launch/demo_isaac_carter_navigation.launch
+++ b/rtabmap_demos/launch/demo_isaac_carter_navigation.launch
@@ -6,8 +6,8 @@
          1) In Isaac sim, click on menu Isaac Examples - ROS - Navigation
          2) Terminal:  $ roscore
          3) To teleop: $ rosrun teleop_twist_keyboard teleop_twist_keyboard.py _speed:=0.15 _turn:=0.20
-         4) Navigation: roslaunch rtabmap_ros demo_isaac_carter_navigation.launch
-         5) Set max depth range to 10m in rtabmapviz for correct visualization (Preferences->3D Rendering under map and odom columns)
+         4) Navigation: roslaunch rtabmap_demos demo_isaac_carter_navigation.launch
+         5) Set max depth range to 10m in rtabmap_viz for correct visualization (Preferences->3D Rendering under map and odom columns)
          6) Press Play in Isaac sim
 
          Note: carter_2dnav package can be copied to your catkin_ws from:
@@ -49,7 +49,7 @@
 
     <!-- RTAB-Map -->
     <remap from="/rtabmap/grid_map" to="/map"/>
-    <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
+    <include file="$(find rtabmap_launch)/launch/rtabmap.launch">
         <arg     if="$(arg localization)" name="args" value="--Reg/Strategy 1 --Reg/Force3DoF true --RGBD/NeighborLinkRefining true --Icp/MaxTranslation 0.5 $(arg args3d)"/>
         <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)"/>
diff --git a/rtabmap_demos/launch/demo_multi-session_mapping.launch b/rtabmap_demos/launch/demo_multi-session_mapping.launch
index ea60ff7f..d9d3790d 100644
--- a/rtabmap_demos/launch/demo_multi-session_mapping.launch
+++ b/rtabmap_demos/launch/demo_multi-session_mapping.launch
@@ -6,7 +6,7 @@
 
    <!-- Choose visualization -->
   <arg name="rviz" default="false" />
-  <arg name="rtabmapviz" default="true" />
+  <arg name="rtabmap_viz" default="true" />
   <arg name="rtabmap_args" default="" />
 
   <param name="use_sim_time" type="bool" value="True"/>
@@ -15,7 +15,7 @@
 
   <!-- 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_slam" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
 	<param name="frame_id" type="string" value="base_footprint"/>
 
 	<param name="subscribe_depth" type="bool" value="true"/>
@@ -60,7 +60,7 @@
   </node>
 
   <!-- Visualisation RTAB-Map -->
-  <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
+  <node if="$(arg rtabmap_viz)" pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(find rtabmap_demos)/launch/config/rgbd_gui.ini" output="screen">
   	  <param name="subscribe_depth" type="bool" value="true"/>
       <param name="subscribe_scan"  type="bool" value="true"/>
       <param name="queue_size"      type="int" value="10"/>
@@ -79,8 +79,8 @@
   </group>
 
   <!-- Visualisation RVIZ -->
-  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_robot_mapping.rviz"/>
-  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
+  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_demos)/launch/config/demo_robot_mapping.rviz"/>
+  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_util/point_cloud_xyzrgb">
     <remap from="rgb/image"       to="/data_throttled_image"/>
     <remap from="depth/image"     to="/data_throttled_image_depth"/>
     <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
diff --git a/rtabmap_demos/launch/demo_robot_mapping.launch b/rtabmap_demos/launch/demo_robot_mapping.launch
index 7a43dc90..91893c66 100644
--- a/rtabmap_demos/launch/demo_robot_mapping.launch
+++ b/rtabmap_demos/launch/demo_robot_mapping.launch
@@ -8,7 +8,7 @@
 
   <!-- Choose visualization -->
   <arg name="rviz" default="false" />
-  <arg name="rtabmapviz" default="true" />
+  <arg name="rtabmap_viz" default="true" />
 
   <param name="use_sim_time" type="bool" value="True"/>
 
@@ -20,7 +20,7 @@
   <group ns="rtabmap">
     <!-- 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_slam" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
 	  <param name="frame_id"           type="string" value="base_footprint"/>
 	  <param name="wait_for_transform" type="bool" value="true"/>
 
@@ -67,7 +67,7 @@
    </node>
 
     <!-- Visualisation RTAB-Map -->
-    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
+    <node if="$(arg rtabmap_viz)" pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(find rtabmap_demos)/launch/config/rgbd_gui.ini" output="screen">
   	  <param name="subscribe_depth"    type="bool" value="true"/>
       <param name="subscribe_scan"     type="bool" value="true"/>
       <param name="frame_id"           type="string" value="base_footprint"/>
@@ -85,8 +85,8 @@
   </group>
 
   <!-- Visualisation RVIZ -->
-  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_robot_mapping.rviz" output="screen"/>
-   <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
+  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_demos)/launch/config/demo_robot_mapping.rviz" output="screen"/>
+   <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_util/point_cloud_xyzrgb">
     <remap from="rgb/image"       to="/data_throttled_image"/>
     <remap from="depth/image"     to="/data_throttled_image_depth"/>
     <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
diff --git a/rtabmap_demos/launch/demo_stereo_outdoor.launch b/rtabmap_demos/launch/demo_stereo_outdoor.launch
index 599919af..a06f41d7 100644
--- a/rtabmap_demos/launch/demo_stereo_outdoor.launch
+++ b/rtabmap_demos/launch/demo_stereo_outdoor.launch
@@ -11,13 +11,13 @@
             /stereo_camera/right/camera_info_throttle
             /tf
 
-      $ roslaunch rtabmap demo_stereo_outdoor.launch
+      $ roslaunch rtabmap_demos demo_stereo_outdoor.launch
       $ rosbag play -.-clock stereo_oudoorA.bag
    -->
 
    <!-- Choose visualization -->
    <arg name="rviz" default="true" />
-   <arg name="rtabmapviz" default="false" />
+   <arg name="rtabmap_viz" default="false" />
    <arg name="local_bundle" default="true" />
    <arg name="stereo_sync" default="false" />
 
@@ -37,7 +37,7 @@
          <param name="disparity_range" value="128"/>
       </node>
 
-      <node if="$(arg stereo_sync)" pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_ros/stereo_sync">
+      <node if="$(arg stereo_sync)" pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_sync/stereo_sync">
       <remap from="left/image_rect"   to="left/image_rect_color"/>
       <remap from="right/image_rect"   to="right/image_rect"/>
       <remap from="left/camera_info"   to="left/camera_info_throttle"/>
@@ -48,7 +48,7 @@
    <group ns="rtabmap">
 
       <!-- Stereo Odometry -->
-      <node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
+      <node pkg="rtabmap_odom" type="stereo_odometry" name="stereo_odometry" output="screen">
          <remap from="left/image_rect"       to="/stereo_camera/left/image_rect"/>
          <remap from="right/image_rect"      to="/stereo_camera/right/image_rect"/>
          <remap from="left/camera_info"      to="/stereo_camera/left/camera_info_throttle"/>
@@ -73,7 +73,7 @@
       </node>
 
       <!-- Visual SLAM: args: "delete_db_on_start" and "udebug" -->
-      <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
+      <node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="--delete_db_on_start">
          <param name="frame_id"         type="string" value="base_footprint"/>
          <param unless="$(arg stereo_sync)" name="subscribe_stereo" type="bool" value="true"/>
          <param name="subscribe_depth"  type="bool" value="false"/>
@@ -102,7 +102,7 @@
       </node>
 
       <!-- Visualisation RTAB-Map -->
-      <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
+      <node if="$(arg rtabmap_viz)" pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(find rtabmap_demos)/launch/config/rgbd_gui.ini" output="screen">
          <param unless="$(arg stereo_sync)" name="subscribe_stereo"    type="bool" value="true"/>
          <param name="subscribe_odom_info" type="bool" value="true"/>
          <param name="subscribe_rgbd"      type="bool" value="$(arg stereo_sync)"/>
@@ -122,6 +122,6 @@
    </group>
 
    <!-- Visualisation RVIZ -->
-   <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_stereo_outdoor.rviz"/>
+   <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_demos)/launch/config/demo_stereo_outdoor.rviz"/>
 
 </launch>
diff --git a/rtabmap_demos/launch/demo_stereo_pr2.launch b/rtabmap_demos/launch/demo_stereo_pr2.launch
index 80d8ae98..975d8f92 100644
--- a/rtabmap_demos/launch/demo_stereo_pr2.launch
+++ b/rtabmap_demos/launch/demo_stereo_pr2.launch
@@ -4,7 +4,7 @@
     You will need to remove the transform /combined_odometry from the /tf messages:
     $ rosbag filter 2011-01-20-07-18-45.bag out.bag 'topic != "/tf" or topic == "/tf" and m.transforms[0].header.frame_id != "/odom_combined"'
     Run the example:
-    $ roslaunch rtabmap demo_stereo.launch
+    $ roslaunch rtabmap_demos demo_stereo.launch
     $ rosbag play -.-clock out.bag        (replace -.- by double-dashes)
     -->
 
@@ -14,7 +14,7 @@
 	<group ns="/wide_stereo">
 	   <node pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>
        <node pkg="nodelet" type="nodelet" name="disparity" args="load stereo_image_proc/disparity standalone_nodelet"/>
-	   <node pkg="nodelet" type="nodelet" name="disparity2depth" args="load rtabmap_ros/disparity_to_depth standalone_nodelet"/>
+	   <node pkg="nodelet" type="nodelet" name="disparity2depth" args="load rtabmap_util/disparity_to_depth standalone_nodelet"/>
 	</group>
 
 	<!-- Odometry: Run the viso2_ros package -->
@@ -30,7 +30,7 @@
 
     <!-- Visual SLAM (robot side) -->
     <!-- args: "delete_db_on_start" and "udebug" -->
-    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
+    <node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="--delete_db_on_start">
       <param name="subscribe_depth" type="bool" value="true"/>
       <param name="subscribe_laserScan" type="bool" value="true"/>
 
@@ -68,7 +68,7 @@
     </node>
 
     <!-- Visualisation (client side) -->
-    <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
+    <node pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(find rtabmap_demos)/launch/config/rgbd_gui.ini" output="screen">
       <param name="subscribe_depth" type="bool" value="true"/>
       <param name="subscribe_laserScan" type="bool" value="true"/>
       <param name="queue_size" type="int" value="30"/>
@@ -82,4 +82,4 @@
     </node>
   </group>
 
-</launch>
\ No newline at end of file
+</launch>
diff --git a/rtabmap_demos/launch/demo_turtlebot3_navigation.launch b/rtabmap_demos/launch/demo_turtlebot3_navigation.launch
index c9760fe1..b5c9e1a1 100644
--- a/rtabmap_demos/launch/demo_turtlebot3_navigation.launch
+++ b/rtabmap_demos/launch/demo_turtlebot3_navigation.launch
@@ -11,13 +11,13 @@
     $ roslaunch turtlebot3_gazebo turtlebot3_world.launch
 
     $ export TURTLEBOT3_MODEL=waffle
-    $ roslaunch rtabmap_ros demo_turtlebot3_navigation.launch
+    $ roslaunch rtabmap_demos demo_turtlebot3_navigation.launch
   -->
 
   <!-- Arguments -->
   <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
   <arg name="open_rviz" default="true"/>
-  <arg name="rtabmapviz" default="true"/>
+  <arg name="rtabmap_viz" default="true"/>
   <arg name="move_forward_only" default="false"/>
 
   <arg name="with_camera" default="true"/>
@@ -32,13 +32,13 @@
   </include>
 
   <group ns="rtabmap">
-    <node if="$(eval model=='waffle')" pkg="rtabmap_ros" type="rgbd_sync" name="rgbd_sync" output="screen">
+    <node if="$(eval model=='waffle')" pkg="rtabmap_sync" type="rgbd_sync" name="rgbd_sync" output="screen">
       <remap from="rgb/image"         to="/camera/rgb/image_raw"/>
       <remap from="depth/image"       to="/camera/depth/image_raw"/>
       <remap from="rgb/camera_info"   to="/camera/rgb/camera_info"/>
     </node>
 
-    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
+    <node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
       <param name="database_path"       type="string" value="$(arg database_path)"/>
       <param name="frame_id"            type="string" value="base_footprint"/>
       <param name="subscribe_rgb"   type="bool" value="false"/>
@@ -70,8 +70,8 @@
       <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
     </node>
 
-    <!-- visualization with rtabmapviz -->
-    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
+    <!-- visualization with rtabmap_viz -->
+    <node if="$(arg rtabmap_viz)" pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(find rtabmap_demos)/launch/config/rgbd_gui.ini" output="screen">
       <param name="subscribe_scan"   type="bool"   value="true"/>
       <param name="subscribe_odom"   type="bool"   value="true"/>
       <param name="frame_id"         type="string" value="base_footprint"/>
diff --git a/rtabmap_demos/launch/demo_turtlebot_mapping.launch b/rtabmap_demos/launch/demo_turtlebot_mapping.launch
index dbdb9101..aa0f0617 100644
--- a/rtabmap_demos/launch/demo_turtlebot_mapping.launch
+++ b/rtabmap_demos/launch/demo_turtlebot_mapping.launch
@@ -6,10 +6,10 @@
        $ roslaunch turtlebot_bringup minimal.launch
 
        Mapping:
-       $ roslaunch rtabmap_ros demo_turtlebot_mapping.launch
+       $ roslaunch rtabmap_demos demo_turtlebot_mapping.launch
 
        Visualization:
-       $ roslaunch rtabmap_ros demo_turtlebot_rviz.launch
+       $ roslaunch rtabmap_demos demo_turtlebot_rviz.launch
 
        This launch file is a one to one replacement of the gmapping_demo.launch in the
        "SLAM Map Building with TurtleBot" tutorial:
@@ -23,13 +23,13 @@
 
        For turtlebot in simulation (Gazebo):
          $ roslaunch turtlebot_gazebo turtlebot_world.launch
-         $ roslaunch rtabmap_ros demo_turtlebot_mapping.launch simulation:=true
-         $ roslaunch rtabmap_ros demo_turtlebot_rviz.launch
+         $ roslaunch rtabmap_demos demo_turtlebot_mapping.launch simulation:=true
+         $ roslaunch rtabmap_demos demo_turtlebot_rviz.launch
   -->
 
   <arg name="database_path"     default="rtabmap.db"/>
   <arg name="rgbd_odometry"     default="false"/>
-  <arg name="rtabmapviz"        default="false"/>
+  <arg name="rtabmap_viz"        default="false"/>
   <arg name="localization"      default="false"/>
   <arg name="simulation"        default="false"/>
   <arg name="sw_registered"     default="false"/>
@@ -58,7 +58,7 @@
   <!-- Mapping -->
   <group ns="rtabmap">
 
-    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg args)">
+    <node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="$(arg args)">
 	  <param name="database_path"       type="string" value="$(arg database_path)"/>
 	  <param name="frame_id"            type="string" value="base_footprint"/>
 	  <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
@@ -108,7 +108,7 @@
     </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">
+    <node if="$(arg rgbd_odometry)" pkg="rtabmap_odom" type="rgbd_odometry" name="rgbd_odometry" output="screen">
       <param name="frame_id"                    type="string" value="base_footprint"/>
       <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
       <param name="Reg/Force3DoF"               type="string" value="true"/>
@@ -119,8 +119,8 @@
       <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
     </node>
 
-    <!-- visualization with rtabmapviz -->
-    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
+    <!-- visualization with rtabmap_viz -->
+    <node if="$(arg rtabmap_viz)" pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(find rtabmap_demos)/launch/config/rgbd_gui.ini" output="screen">
   	  <param name="subscribe_depth"             type="bool" value="true"/>
       <param name="subscribe_scan"              type="bool" value="true"/>
       <param name="frame_id"                    type="string" value="base_footprint"/>
diff --git a/rtabmap_demos/launch/demo_turtlebot_rviz.launch b/rtabmap_demos/launch/demo_turtlebot_rviz.launch
index 45977f6f..4872cf5b 100644
--- a/rtabmap_demos/launch/demo_turtlebot_rviz.launch
+++ b/rtabmap_demos/launch/demo_turtlebot_rviz.launch
@@ -3,5 +3,5 @@
   Used for visualising the turtlebot while building a map or navigating with the ros navistack.
  -->
 <launch>
-  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find rtabmap_ros)/launch/config/turtlebot_navigation.rviz"/>
-</launch>
\ No newline at end of file
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find rtabmap_demos)/launch/config/turtlebot_navigation.rviz"/>
+</launch>
diff --git a/rtabmap_demos/launch/demo_turtlebot_tango.launch b/rtabmap_demos/launch/demo_turtlebot_tango.launch
index d9d92cae..671759f6 100644
--- a/rtabmap_demos/launch/demo_turtlebot_tango.launch
+++ b/rtabmap_demos/launch/demo_turtlebot_tango.launch
@@ -6,10 +6,10 @@
        $ roslaunch turtlebot_bringup minimal.launch
 
        Mapping:
-       $ roslaunch rtabmap_ros demo_turtlebot_tango.launch
+       $ roslaunch rtabmap_demos demo_turtlebot_tango.launch
 
        Visualization:
-       $ roslaunch rtabmap_ros demo_turtlebot_rviz.launch
+       $ roslaunch rtabmap_demos demo_turtlebot_rviz.launch
 
        This launch file is a one to one replacement of the gmapping_demo.launch in the
        "SLAM Map Building with TurtleBot" tutorial:
@@ -25,7 +25,7 @@
   <arg name="database_path"     default="rtabmap.db"/>
   <arg name="tango_odometry"    default="true"/>
   <arg name="localization"      default="false"/>
-  <arg name="rtabmapviz"        default="false"/>
+  <arg name="rtabmap_viz"        default="false"/>
   <arg     if="$(arg localization)" name="args"  default=""/>
   <arg unless="$(arg localization)" name="args"  default="--delete_db_on_start"/>
 
@@ -41,7 +41,7 @@
   <node unless="$(arg tango_odometry)" pkg="tf" type="static_transform_publisher" name="base_device_link" args="0 0 0.3 0 0 0 device base_link 100" />
 
   <!-- Generate registered depth image -->
-  <node name="pointcloud_to_depthimage" pkg="rtabmap_ros" type="pointcloud_to_depthimage">
+  <node name="pointcloud_to_depthimage" pkg="rtabmap_util" type="pointcloud_to_depthimage">
     <remap from="cloud"            to="/tango/point_cloud"/>
     <remap from="image"            to="/tango/registered_depth"/>
     <remap from="camera_info"      to="/tango/camera/color_1/camera_info"/>
@@ -61,7 +61,7 @@
   <!-- Mapping -->
   <group ns="rtabmap">
 
-    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg args)">
+    <node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="$(arg args)">
 	  <param name="database_path"       type="string" value="$(arg database_path)"/>
 	  <param name="frame_id"            type="string" value="base_footprint"/>
 	  <param name="odom_frame_id"       type="string" value="odom"/>
@@ -104,8 +104,8 @@
 	  <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
     </node>
 
-    <!-- visualization with rtabmapviz -->
-    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
+    <!-- visualization with rtabmap_viz -->
+    <node if="$(arg rtabmap_viz)" pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(find rtabmap_demos)/launch/config/rgbd_gui.ini" output="screen">
       <param name="subscribe_depth"             type="bool" value="true"/>
       <param name="subscribe_scan"              type="bool" value="true"/>
       <param name="frame_id"                    type="string" value="base_footprint"/>
diff --git a/rtabmap_demos/launch/demo_two_kinects.launch b/rtabmap_demos/launch/demo_two_kinects.launch
index 315ebcce..a8130121 100644
--- a/rtabmap_demos/launch/demo_two_kinects.launch
+++ b/rtabmap_demos/launch/demo_two_kinects.launch
@@ -24,7 +24,7 @@
 
    <!-- Choose visualization -->
    <arg name="rviz"       default="false" />
-   <arg name="rtabmapviz" default="true" />
+   <arg name="rtabmap_viz" default="true" />
 
    <!-- ODOMETRY MAIN ARGUMENTS:
         -"strategy"        : Strategy: 0=Frame-to-Map 1=Frame-to-Frame
@@ -50,14 +50,14 @@
 
    <!-- sync rgb/depth images per camera -->
    <group ns="camera1">
-    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera1_nodelet_manager">
+    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_sync/rgbd_sync camera1_nodelet_manager">
       <remap from="rgb/image"         to="rgb/image_rect_color"/>
       <remap from="depth/image"       to="depth_registered/image_raw"/>
       <remap from="rgb/camera_info"   to="rgb/camera_info"/>
     </node>
    </group>
    <group ns="camera2">
-    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera2_nodelet_manager">
+    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_sync/rgbd_sync camera2_nodelet_manager">
       <remap from="rgb/image"         to="rgb/image_rect_color"/>
       <remap from="depth/image"       to="depth_registered/image_raw"/>
       <remap from="rgb/camera_info"   to="rgb/camera_info"/>
@@ -67,7 +67,7 @@
   <group ns="rtabmap">
 
     <!-- Odometry -->
-    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
+    <node pkg="rtabmap_odom" type="rgbd_odometry" name="rgbd_odometry" output="screen">
       <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
       <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>
 
@@ -90,7 +90,7 @@
 
     <!-- Visual SLAM (robot side) -->
     <!-- args: "delete_db_on_start" and "udebug" -->
-    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
+    <node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="--delete_db_on_start">
       <param name="subscribe_depth"  type="bool"   value="false"/>
       <param name="subscribe_rgbd"   type="bool"   value="true"/>
       <param name="rgbd_cameras"    type="int"    value="2"/>
@@ -110,7 +110,7 @@
     </node>
 
     <!-- Visualisation RTAB-Map -->
-    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
+    <node if="$(arg rtabmap_viz)" pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(find rtabmap_demos)/launch/config/rgbd_gui.ini" output="screen">
       <param name="subscribe_depth"  type="bool"   value="false"/>
       <param name="subscribe_rgbd"   type="bool"   value="true"/>
       <param name="subscribe_odom_info" type="bool"   value="$(arg odom_info_data)"/>
@@ -125,6 +125,6 @@
   </group>
 
   <!-- Visualization RVIZ -->
-  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
+  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_demos)/launch/config/rgbd.rviz"/>
 
 </launch>
diff --git a/rtabmap_demos/package.xml b/rtabmap_demos/package.xml
index c5434594..2f812711 100644
--- a/rtabmap_demos/package.xml
+++ b/rtabmap_demos/package.xml
@@ -20,5 +20,12 @@
   <exec_depend>rtabmap_ros</exec_depend>
   <exec_depend>rtabmap_rviz_plugins</exec_depend>
   <exec_depend>rtabmap_viz</exec_depend>
+  <exec_depend>rtabmap_util</exec_depend>
+  
+  <exec_depend>hector_mapping</exec_depend>
+  <exec_depend>husky_navigation</exec_depend>
+  <exec_depend>turtlebot3_gazebo</exec_depend>
+  <exec_depend>turtlebot3_bringup</exec_depend>
+  <exec_depend>turtlebot3_navigation</exec_depend>
 
 </package>
diff --git a/rtabmap_launch/launch/rtabmap.launch b/rtabmap_launch/launch/rtabmap.launch
index 2570b782..dc6c0c9d 100644
--- a/rtabmap_launch/launch/rtabmap.launch
+++ b/rtabmap_launch/launch/rtabmap.launch
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <!-- -->
 <launch>
-  <!-- Convenience launch file to launch odometry, rtabmap and rtabmapviz nodes at once -->
+  <!-- Convenience launch file to launch odometry, rtabmap and rtabmap_viz nodes at once -->
 
   <!-- For stereo:=false
         Your RGB-D sensor should be already started with "depth_registration:=true".
@@ -22,7 +22,7 @@
   <arg name="subscribe_rgb"  default="$(arg depth)"/>
 
   <!-- Choose visualization -->
-  <arg name="rtabmapviz"              default="true" />
+  <arg name="rtabmap_viz"             default="true" />
   <arg name="rviz"                    default="false" />
 
   <!-- Localization-only mode -->
@@ -417,7 +417,7 @@
     </node>
 
     <!-- Visualisation RTAB-Map -->
-    <node if="$(arg rtabmapviz)" pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(arg gui_cfg)" clear_params="$(arg clear_params)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
+    <node if="$(arg rtabmap_viz)" pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(arg gui_cfg)" clear_params="$(arg clear_params)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
       <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
       <param unless="$(arg stereo)" name="subscribe_depth"  type="bool" value="$(arg depth)"/>
       <param name="subscribe_rgb"        type="bool"   value="$(arg subscribe_rgb)"/>
diff --git a/rtabmap_ros/launch/rtabmap.launch b/rtabmap_ros/launch/rtabmap.launch
index 1274d817..c29895c6 100644
--- a/rtabmap_ros/launch/rtabmap.launch
+++ b/rtabmap_ros/launch/rtabmap.launch
@@ -10,7 +10,8 @@
   <arg name="subscribe_rgb"  default="$(arg depth)"/>
 
   <!-- Choose visualization -->
-  <arg name="rtabmapviz"              default="true" />
+  <arg name="rtabmap_viz"             default="true" />
+  <arg name="rtabmapviz"              default="$(arg rtabmap_viz)" /> <!-- deprecated, use rtabmap_viz -->
   <arg name="rviz"                    default="false" />
 
   <!-- Localization-only mode -->
@@ -154,7 +155,7 @@
     <arg name="depth"          value="$(arg depth)"/>
     <arg name="subscribe_rgb"  value="$(arg subscribe_rgb)"/>
 
-    <arg name="rtabmapviz"     value="$(arg rtabmapviz)" />
+    <arg name="rtabmap_viz"    value="$(arg rtabmapviz)" />
     <arg name="rviz"           value="$(arg rviz)" />
 
     <arg name="localization"            value="$(arg localization)"/>
diff --git a/rtabmap_viz/src/GuiNode.cpp b/rtabmap_viz/src/GuiNode.cpp
index 044b506d..aeecae91 100644
--- a/rtabmap_viz/src/GuiNode.cpp
+++ b/rtabmap_viz/src/GuiNode.cpp
@@ -49,7 +49,7 @@ int main(int argc, char** argv)
 	ULogger::setType(ULogger::kTypeConsole);
 	ULogger::setLevel(ULogger::kWarning);
 
-	ros::init(argc, argv, "rtabmapviz");
+	ros::init(argc, argv, "rtabmap_viz");
 
 	app = new QApplication(argc, argv);
 	app->connect( app, SIGNAL( lastWindowClosed() ), app, SLOT( quit() ) );
@@ -68,16 +68,16 @@ int main(int argc, char** argv)
 	spinner = new ros::AsyncSpinner(1); // Use 1 thread
 	spinner->start();
 
-	ROS_INFO("rtabmapviz started.");
+	ROS_INFO("rtabmap_viz started.");
 	// Now wait for application to finish
 	int r = app->exec();// MUST be called by the Main Thread
 
-	ROS_INFO("rtabmapviz stopping spinner...");
+	ROS_INFO("rtabmap_viz stopping spinner...");
 	delete spinner;
 
-	ROS_INFO("rtabmapviz deleting qt stuff...");
+	ROS_INFO("rtabmap_viz deleting qt stuff...");
 	delete gui;
 	delete app;
-	ROS_INFO("rtabmapviz: All done! Closing...");
+	ROS_INFO("rtabmap_viz: All done! Closing...");
 	return r;
 }
diff --git a/rtabmap_viz/src/GuiWrapper.cpp b/rtabmap_viz/src/GuiWrapper.cpp
index 1366a6d6..bff9ba8d 100644
--- a/rtabmap_viz/src/GuiWrapper.cpp
+++ b/rtabmap_viz/src/GuiWrapper.cpp
@@ -98,7 +98,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 
 	pnh.param("rtabmap", rtabmapNodeName_, rtabmapNodeName_);
 
-	ROS_INFO("rtabmapviz: Using configuration from \"%s\"", configFile.toStdString().c_str());
+	ROS_INFO("rtabmap_viz: Using configuration from \"%s\"", configFile.toStdString().c_str());
 	uSleep(500);
 	prefDialog_ = new PreferencesDialogROS(configFile, rtabmapNodeName_);
 	mainWindow_ = new MainWindow(prefDialog_);
@@ -127,7 +127,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 		{
 			initCachePath = UDirectory::currentDir(true) + initCachePath;
 		}
-		ROS_INFO("rtabmapviz: Initializing cache with local database \"%s\"", initCachePath.c_str());
+		ROS_INFO("rtabmap_viz: Initializing cache with local database \"%s\"", initCachePath.c_str());
 		uSleep(2000); // make sure rtabmap node is created if launched at the same time
 		rtabmap_msgs::GetMap getMapSrv;
 		getMapSrv.request.global = false;
@@ -136,7 +136,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 		if(!ros::service::call("get_map", getMapSrv))
 		{
 			ROS_WARN("Can't call \"get_map\" service. The cache will still be loaded "
-					"but the clouds won't be created until next time rtabmapviz "
+					"but the clouds won't be created until next time rtabmap_viz "
 					"receives the optimized graph.");
 		}
 		else
@@ -189,7 +189,7 @@ void GuiWrapper::infoMapCallback(
 		const rtabmap_msgs::InfoConstPtr & infoMsg,
 		const rtabmap_msgs::MapDataConstPtr & mapMsg)
 {
-	//ROS_INFO("rtabmapviz: RTAB-Map info ex received!");
+	//ROS_INFO("rtabmap_viz: RTAB-Map info ex received!");
 
 	// Map from ROS struct to rtabmap struct
 	rtabmap::Statistics stat;
@@ -567,7 +567,7 @@ void GuiWrapper::commonMultiCameraCallback(
 					waitForTransform_?waitForTransformDuration_:0.0,
 					imagesAlreadyRectified))
 			{
-				ROS_ERROR("Could not convert rgb/depth msgs! Aborting rtabmapviz update...");
+				ROS_ERROR("Could not convert rgb/depth msgs! Aborting rtabmap_viz update...");
 				return;
 			}
 		}
@@ -583,7 +583,7 @@ void GuiWrapper::commonMultiCameraCallback(
 					tfListener_,
 					waitForTransform_?waitForTransformDuration_:0))
 			{
-				ROS_ERROR("Could not convert laser scan msg! Aborting rtabmapviz update...");
+				ROS_ERROR("Could not convert laser scan msg! Aborting rtabmap_viz update...");
 				return;
 			}
 		}
@@ -598,7 +598,7 @@ void GuiWrapper::commonMultiCameraCallback(
 					tfListener_,
 					waitForTransform_?waitForTransformDuration_:0))
 			{
-				ROS_ERROR("Could not convert 3d laser scan msg! Aborting rtabmapviz update...");
+				ROS_ERROR("Could not convert 3d laser scan msg! Aborting rtabmap_viz update...");
 				return;
 			}
 		}
@@ -756,7 +756,7 @@ void GuiWrapper::commonStereoCallback(
 				waitForTransform_?waitForTransformDuration_:0.0,
 				imagesAlreadyRectified))
 		{
-			ROS_ERROR("Could not convert stereo msgs! Aborting rtabmapviz update...");
+			ROS_ERROR("Could not convert stereo msgs! Aborting rtabmap_viz update...");
 			return;
 		}
 
@@ -771,7 +771,7 @@ void GuiWrapper::commonStereoCallback(
 					tfListener_,
 					waitForTransform_?waitForTransformDuration_:0))
 			{
-				ROS_ERROR("Could not convert laser scan msg! Aborting rtabmapviz update...");
+				ROS_ERROR("Could not convert laser scan msg! Aborting rtabmap_viz update...");
 				return;
 			}
 		}
@@ -786,7 +786,7 @@ void GuiWrapper::commonStereoCallback(
 					tfListener_,
 					waitForTransform_?waitForTransformDuration_:0))
 			{
-				ROS_ERROR("Could not convert 3d laser scan msg! Aborting rtabmapviz update...");
+				ROS_ERROR("Could not convert 3d laser scan msg! Aborting rtabmap_viz update...");
 				return;
 			}
 		}
@@ -918,7 +918,7 @@ void GuiWrapper::commonLaserScanCallback(
 					tfListener_,
 					waitForTransform_?waitForTransformDuration_:0))
 			{
-				ROS_ERROR("Could not convert laser scan msg! Aborting rtabmapviz update...");
+				ROS_ERROR("Could not convert laser scan msg! Aborting rtabmap_viz update...");
 				return;
 			}
 		}
@@ -933,7 +933,7 @@ void GuiWrapper::commonLaserScanCallback(
 					tfListener_,
 					waitForTransform_?waitForTransformDuration_:0))
 			{
-				ROS_ERROR("Could not convert 3d laser scan msg! Aborting rtabmapviz update...");
+				ROS_ERROR("Could not convert 3d laser scan msg! Aborting rtabmap_viz update...");
 				return;
 			}
 		}
diff --git a/rtabmap_viz/src/PreferencesDialogROS.cpp b/rtabmap_viz/src/PreferencesDialogROS.cpp
index f89d2fdc..83a5931f 100644
--- a/rtabmap_viz/src/PreferencesDialogROS.cpp
+++ b/rtabmap_viz/src/PreferencesDialogROS.cpp
@@ -118,7 +118,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
 	}
 
 	ros::NodeHandle rnh(rtabmapNodeName_);
-	ROS_INFO("rtabmapviz: %s", this->getParamMessage().toStdString().c_str());
+	ROS_INFO("rtabmap_viz: %s", this->getParamMessage().toStdString().c_str());
 	bool validParameters = true;
 	int readCount = 0;
 	rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
@@ -145,7 +145,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
 		{
 			if(!warned)
 			{
-				ROS_INFO("rtabmapviz: Cannot get rtabmap's parameters, waiting max 5 seconds in case the node has just been launched.");
+				ROS_INFO("rtabmap_viz: Cannot get rtabmap's parameters, waiting max 5 seconds in case the node has just been launched.");
 				warned = true;
 			}
 			uSleep(100);
@@ -154,11 +154,11 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
 		{
 			if(UTimer::now()-stamp < 5.0)
 			{
-				ROS_INFO("rtabmapviz: rtabmap's parameters seem now there! continuing...");
+				ROS_INFO("rtabmap_viz: rtabmap's parameters seem now there! continuing...");
 			}
 			else
 			{
-				ROS_WARN("rtabmapviz: rtabmap's parameters seem not all there yet! continuing with those there if some...");
+				ROS_WARN("rtabmap_viz: rtabmap's parameters seem not all there yet! continuing with those there if some...");
 			}
 		}
 	}
@@ -207,17 +207,17 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
 			}
 			else
 			{
-				ROS_WARN("rtabmapviz: Parameter %s not found", i->first.c_str());
+				ROS_WARN("rtabmap_viz: Parameter %s not found", i->first.c_str());
 				validParameters = false;
 			}
 		}
 	}
 
-	ROS_INFO("rtabmapviz: Parameters read = %d", readCount);
+	ROS_INFO("rtabmap_viz: Parameters read = %d", readCount);
 
 	if(validParameters)
 	{
-		ROS_INFO("rtabmapviz: Parameters successfully read.");
+		ROS_INFO("rtabmap_viz: Parameters successfully read.");
 	}
 	else
 	{