Merge pull request #1 from introlab/master

Fork Sync

.travis.yml

@@ -41,6 +41,7 @@ script:
     - cd ../../catkin_ws
     - source devel/setup.bash
     - catkin_make
+    - catkin_make install
 
 notifications:
   email:

CMakeLists.txt

@@ -4,12 +4,12 @@ project(rtabmap_ros)
 # Policy CMP0043 introduced in cmake version 3.0 IGNORES the use of COMPILE_DEFINITIONS suffixed variables, e.g. COMPILE_DEFINITIONS_DEBUG
 # Set to OLD behavior until minimum cmake version >= 2.8.10 (version that COMPILE_DEFINITIONS can be set by generator expressions instead)
 if (POLICY CMP0043)
-	cmake_policy(SET CMP0043 OLD)
+    cmake_policy(SET CMP0043 OLD)
 endif (POLICY CMP0043)
 # Policy CMP0042 introduced in cmake version 3.0 enables the use of @rpath in an install name via MACOSX_RPATH by default
 # Set to OLD behavior so that all versions use the same behavior, or until minimum cmake version >= 2.8.12 (version where @rpath is available)
 if (POLICY CMP0042)
-	cmake_policy(SET CMP0042 OLD)
+    cmake_policy(SET CMP0042 OLD)
 endif (POLICY CMP0042)
 
 ## Find catkin macros and libraries
@@ -58,13 +58,6 @@ IF(RTABMAP_GUI OR rviz_FOUND)
       ENDIF()
    ENDIF()   
 ENDIF(RTABMAP_GUI OR rviz_FOUND)
-IF(rviz_FOUND)
-   ## We also use Ogre for rviz plugins
-   include($ENV{ROS_ROOT}/core/rosbuild/FindPkgConfig.cmake)
-   pkg_check_modules(OGRE OGRE)
-   include_directories( ${OGRE_INCLUDE_DIRS} )
-   link_directories( ${OGRE_LIBRARY_DIRS} )
-ENDIF(rviz_FOUND)
 
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
@@ -130,9 +123,6 @@ generate_dynamic_reconfigure_options(cfg/Camera.cfg)
 SET(optional_dependencies "")
 IF(costmap_2d_FOUND)
    SET(optional_dependencies ${optional_dependencies} costmap_2d)
-   IF(${costmap_2d_VERSION_MAJOR} GREATER 1 OR ${costmap_2d_VERSION_MINOR} GREATER 15)
-       ADD_DEFINITIONS("-DCOSTMAP_2D_POINTCLOUD2")
-   ENDIF(${costmap_2d_VERSION_MAJOR} GREATER 1 OR ${costmap_2d_VERSION_MINOR} GREATER 15)
 ENDIF(costmap_2d_FOUND)
 IF(octomap_msgs_FOUND)
    SET(optional_dependencies ${optional_dependencies} octomap_msgs)
@@ -223,21 +213,6 @@ ELSE()
    ADD_DEFINITIONS("-DCV_BRIDGE_HYDRO")
 ENDIF()   
 
-# If costmap_2d is found, add the plugin
-IF(costmap_2d_FOUND)
-MESSAGE(STATUS "WITH costmap_2d")
-include_directories(${costmap_2d_INCLUDE_DIRS})
-SET(Libraries
-  ${costmap_2d_LIBRARIES}
-  ${Libraries}
-  )
-SET(rtabmap_plugins_lib_src
-  src/costmap_2d/static_layer.cpp
-  src/costmap_2d/voxel_layer.cpp
-  ${rtabmap_plugins_lib_src}
-  )
-ENDIF(costmap_2d_FOUND)
-
 # If octomap is found, add definition
 IF(octomap_msgs_FOUND)
 MESSAGE(STATUS "WITH octomap_msgs")
@@ -264,51 +239,6 @@ SET(Libraries
 ADD_DEFINITIONS("-DWITH_APRILTAG_ROS")
 ENDIF(apriltag_ros_FOUND)
 
-# If rviz is found, add plugins
-IF(rviz_FOUND)
-MESSAGE(STATUS "WITH rviz")
-include_directories(
-  ${rviz_INCLUDE_DIRS}
-)
-SET(Libraries
-  ${Libraries}
-  ${rviz_LIBRARIES}
-  ${rviz_DEFAULT_PLUGIN_LIBRARIES}
-)
-
-## RVIZ plugin
-IF(QT4_FOUND)
-qt4_wrap_cpp(MOC_FILES
-  src/rviz/MapCloudDisplay.h
-  src/rviz/MapGraphDisplay.h
-  src/rviz/InfoDisplay.h
-  src/rviz/OrbitOrientedViewController.h
-)
-ELSE()
-qt5_wrap_cpp(MOC_FILES
-  src/rviz/MapCloudDisplay.h
-  src/rviz/MapGraphDisplay.h
-  src/rviz/InfoDisplay.h
-  src/rviz/OrbitOrientedViewController.h
-)
-ENDIF()
-
-# tf:message_filters, mixing boost and Qt signals
-set_property(
-   SOURCE src/rviz/MapCloudDisplay.cpp src/rviz/MapGraphDisplay.cpp src/rviz/InfoDisplay.cpp src/rviz/OrbitOrientedViewController.cpp
-   PROPERTY COMPILE_DEFINITIONS QT_NO_KEYWORDS
-)
-
-SET(rtabmap_plugins_lib_src
-   ${rtabmap_plugins_lib_src}
-   src/rviz/MapCloudDisplay.cpp
-   src/rviz/MapGraphDisplay.cpp
-   src/rviz/InfoDisplay.cpp
-   src/rviz/OrbitOrientedViewController.cpp
-   ${MOC_FILES}
-)
-ENDIF(rviz_FOUND)
-
 ############################
 ## Declare a cpp library
 ############################
@@ -331,9 +261,6 @@ target_link_libraries(rtabmap_ros
 target_link_libraries(rtabmap_plugins
   rtabmap_ros
 )
-IF(Qt5_FOUND)
-    QT5_USE_MODULES(rtabmap_plugins Widgets Core Gui)
-ENDIF(Qt5_FOUND)
 add_dependencies(rtabmap_ros ${${PROJECT_NAME}_EXPORTED_TARGETS})
 add_dependencies(rtabmap_plugins ${${PROJECT_NAME}_EXPORTED_TARGETS})
 
@@ -385,11 +312,11 @@ set_target_properties(rtabmap_wifi_signal_sub PROPERTIES OUTPUT_NAME "wifi_signa
 
 # If find_object_2d is found, add save objects example
 IF(find_object_2d_FOUND)
-MESSAGE(STATUS "WITH find_object_2d")
-include_directories(${find_object_2d_INCLUDE_DIRS})
-add_executable(rtabmap_save_objects_example src/SaveObjectsExample.cpp)
-target_link_libraries(rtabmap_save_objects_example ${Libraries} rtabmap_ros ${find_object_2d_LIBRARIES})
-set_target_properties(rtabmap_save_objects_example PROPERTIES OUTPUT_NAME "save_objects_example")
+    MESSAGE(STATUS "WITH find_object_2d")
+    include_directories(${find_object_2d_INCLUDE_DIRS})
+    add_executable(rtabmap_save_objects_example src/SaveObjectsExample.cpp)
+    target_link_libraries(rtabmap_save_objects_example ${Libraries} rtabmap_ros ${find_object_2d_LIBRARIES})
+    set_target_properties(rtabmap_save_objects_example PROPERTIES OUTPUT_NAME "save_objects_example")
 ENDIF(find_object_2d_FOUND)
 
 add_executable(rtabmap_camera src/CameraNode.cpp)
@@ -423,6 +350,80 @@ add_executable(rtabmap_pointcloud_to_depthimage src/PointCloudToDepthImageNode.c
 target_link_libraries(rtabmap_pointcloud_to_depthimage ${Libraries})
 set_target_properties(rtabmap_pointcloud_to_depthimage PROPERTIES OUTPUT_NAME "pointcloud_to_depthimage")
 
+# If rviz is found, add plugins
+IF(rviz_FOUND)
+
+    ## We also use Ogre for rviz plugins
+    include($ENV{ROS_ROOT}/core/rosbuild/FindPkgConfig.cmake)
+    pkg_check_modules(OGRE OGRE)
+    include_directories( ${OGRE_INCLUDE_DIRS} )
+    link_directories( ${OGRE_LIBRARY_DIRS} )
+
+    MESSAGE(STATUS "WITH rviz")
+    include_directories(
+      ${rviz_INCLUDE_DIRS}
+    )
+    SET(Libraries
+      ${Libraries}
+      ${rviz_LIBRARIES}
+      ${rviz_DEFAULT_PLUGIN_LIBRARIES}
+    )
+    
+    ## RVIZ plugin
+    IF(QT4_FOUND)
+    qt4_wrap_cpp(MOC_FILES
+      src/rviz/MapCloudDisplay.h
+      src/rviz/MapGraphDisplay.h
+      src/rviz/InfoDisplay.h
+      src/rviz/OrbitOrientedViewController.h
+    )
+    ELSE()
+    qt5_wrap_cpp(MOC_FILES
+      src/rviz/MapCloudDisplay.h
+      src/rviz/MapGraphDisplay.h
+      src/rviz/InfoDisplay.h
+      src/rviz/OrbitOrientedViewController.h
+    )
+    ENDIF()
+    
+    # tf:message_filters, mixing boost and Qt signals
+    set_property(
+       SOURCE src/rviz/MapCloudDisplay.cpp src/rviz/MapGraphDisplay.cpp src/rviz/InfoDisplay.cpp src/rviz/OrbitOrientedViewController.cpp
+       PROPERTY COMPILE_DEFINITIONS QT_NO_KEYWORDS
+    )
+    add_library(rtabmap_rviz_plugins
+       src/rviz/MapCloudDisplay.cpp
+       src/rviz/MapGraphDisplay.cpp
+       src/rviz/InfoDisplay.cpp
+       src/rviz/OrbitOrientedViewController.cpp
+       ${MOC_FILES}
+    )
+    target_link_libraries(rtabmap_rviz_plugins
+      rtabmap_ros
+    )
+    IF(Qt5_FOUND)
+        QT5_USE_MODULES(rtabmap_rviz_plugins Widgets Core Gui)
+    ENDIF(Qt5_FOUND)
+    add_dependencies(rtabmap_rviz_plugins ${${PROJECT_NAME}_EXPORTED_TARGETS})
+
+ENDIF(rviz_FOUND)
+
+# If costmap_2d is found, add the plugins
+IF(costmap_2d_FOUND)
+    MESSAGE(STATUS "WITH costmap_2d")
+    IF(${costmap_2d_VERSION_MAJOR} GREATER 1 OR ${costmap_2d_VERSION_MINOR} GREATER 15)
+      ADD_DEFINITIONS("-DCOSTMAP_2D_POINTCLOUD2")
+    ENDIF(${costmap_2d_VERSION_MAJOR} GREATER 1 OR ${costmap_2d_VERSION_MINOR} GREATER 15)
+    include_directories(${costmap_2d_INCLUDE_DIRS})
+    add_library(rtabmap_costmap_plugins
+       src/costmap_2d/static_layer.cpp
+       src/costmap_2d/voxel_layer.cpp
+    )
+    target_link_libraries(rtabmap_costmap_plugins
+      ${costmap_2d_LIBRARIES}
+    )
+ENDIF(costmap_2d_FOUND)
+
 #############
 ## Install ##
 #############
@@ -442,13 +443,11 @@ install(PROGRAMS
 )
 
 ## Mark executables and/or libraries for installation
-IF(RTABMAP_GUI)
 install(TARGETS 
    rtabmap_sync
    rtabmap_ros
    rtabmap_plugins 
    rtabmap 
-   rtabmapviz 
    rtabmap_rgbd_odometry 
    rtabmap_icp_odometry
    rtabmap_rgbdicp_odometry 
@@ -464,30 +463,15 @@ install(TARGETS
    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
- )
- ELSE() # without rtabmapviz
- install(TARGETS 
-   rtabmap_sync
-   rtabmap_ros
-   rtabmap_plugins 
-   rtabmap 
-   rtabmap_rgbd_odometry 
-   rtabmap_icp_odometry
-   rtabmap_rgbdicp_odometry 
-   rtabmap_stereo_odometry
-   rtabmap_map_assembler
-   rtabmap_map_optimizer
-   rtabmap_data_player
-   rtabmap_odom_msg_to_tf
-   rtabmap_pointcloud_to_depthimage
-   rtabmap_camera
-   rtabmap_rgbd_sync
-   rtabmap_rgbd_relay
-   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
- )
- ENDIF()
+)
+IF(RTABMAP_GUI)
+    install(TARGETS 
+       rtabmapviz
+       ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+       LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+       RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+     )
+ENDIF(RTABMAP_GUI)
 
 ## Mark cpp header files for installation
 install(DIRECTORY include/${PROJECT_NAME}/
@@ -515,15 +499,32 @@ install(DIRECTORY
 ## install plugins/nodelets xml
 install(FILES
    nodelet_plugins.xml
-   plugin_description.xml
-   costmap_plugins.xml
    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
+IF(rviz_FOUND)
+    install(TARGETS 
+       rtabmap_rviz_plugins
+       ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+       LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+       RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+     )
+    install(FILES
+       rviz_plugins.xml
+       DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+    )
+ENDIF(rviz_FOUND)
+
 IF(costmap_2d_FOUND)
-install(FILES
-   costmap_plugins.xml
-   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
+    install(TARGETS 
+       rtabmap_costmap_plugins
+       ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+       LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+       RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+     )
+    install(FILES
+       costmap_plugins.xml
+       DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+    )
 ENDIF(costmap_2d_FOUND)
 
 #############

costmap_plugins.xml

@@ -1,5 +1,5 @@
 <class_libraries>
-  <library path="lib/librtabmap_plugins">
+  <library path="lib/librtabmap_costmap_plugins">
     <class type="rtabmap_ros::StaticLayer"     base_class_type="costmap_2d::Layer">
       <description>Listens to OccupancyGrid messages and copies them in, like from map_server.</description>
     </class>

docker/README.md

@@ -1,20 +1,13 @@
 ### Docker
 
-* Create rtabmap image (example with melodic) from this Dockerfile:
-    ```dockerfile
-    FROM ros:melodic-perception
-    # install rtabmap packages
-    RUN apt-get update && apt-get install -y \
-        ros-melodic-rtabmap \
-        ros-melodic-rtabmap-ros \
-        && rm -rf /var/lib/apt/lists/
+* Available images on [introlab3it/rtabmap_ros](https://hub.docker.com/r/introlab3it/rtabmap_ros/):
     ```
-    * To build an image with latest rtabmap version from source, use Dockerfile inside one of the `latest` subdirectories.
+    indigo, indigo-latest
+    kinetic, kinetic-latest
+    melodic, melodic-latest
+    ```
+    * The `-latest` images are automatically built from latest version of `rtabmap` and `rtabmap_ros` from source. The other images have the same version than the binaries released on ROS. 
 
-* Build it:
-    ```bash
-    $ docker build --tag ros:rtabmap .
-    ```
 
 * The following example show how to launch a camera on host computer and run our pre-built rtabmap container. All examples from [RGB-D tutorial](http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping) and [stereo tutorial](http://wiki.ros.org/rtabmap_ros/Tutorials/StereoHandHeldMapping) should work using rtabmap from the container instead. Launch camera on host computer (set ROS_IP as the IP used for docker):
     ```bash
@@ -24,11 +17,11 @@
     $ export ROS_NAMESPACE=rtabmap && rosrun rtabmap_ros rtabmapviz _frame_id:=camera_link
     ```
 
-* Launch rtabmap from inside the container, saving the database on host `~/.ros/rtabmap.db`:
+* Launch `rtabmap` from inside the container (no gui), saving the database on host `~/.ros/rtabmap.db`:
     ```bash
     $ docker run -it --rm \
      --env ROS_MASTER_URI=http://172.17.0.1:11311 --env ROS_IP=172.17.0.2 \
      -v ~/.ros:/root  \
-     ros:rtabmap \
+     introlab3it/rtabmap_ros:kinetic-latest \
      roslaunch rtabmap_ros rtabmap.launch rtabmapviz:=false database_path:=/root/rtabmap.db rtabmap_args:="--delete_db_on_start"
    ```

docker/kinetic/latest/Dockerfile

@@ -1,35 +1,13 @@
-FROM ros:kinetic-perception
-# install rtabmap packages
-RUN apt-get update && apt-get install -y \
-    ros-kinetic-rtabmap-ros \
-    && apt-get remove -y \
-    ros-kinetic-rtabmap \
-    && rm -rf /var/lib/apt/lists/
-
-RUN rm /bin/sh && ln -s /bin/bash /bin/sh
-
-WORKDIR /root/
+FROM introlab3it/rtabmap:16.04
 
 ARG CACHE_DATE=2016-01-01
 
-RUN git clone https://github.com/introlab/rtabmap.git
-
-# Build RTAB-Map project
-RUN source /ros_entrypoint.sh && \
-    cd rtabmap/build && \
-    cmake .. && \
-    make -j$(nproc) && \
-    make install && \
-    cd ../.. && \
-    rm -rf rtabmap && \
-    ldconfig
-
 RUN source /ros_entrypoint.sh && \
     mkdir -p catkin_ws/src && \
     cd catkin_ws/src && \
     catkin_init_workspace && \
     git clone https://github.com/introlab/rtabmap_ros.git && \
     cd .. && \
-    catkin_make -DCMAKE_INSTALL_PREFIX=/opt/ros/kinetic install && \
+    catkin_make -j1 -DCMAKE_INSTALL_PREFIX=/opt/ros/kinetic install && \
     cd && \
     rm -rf catkin_ws

docker/melodic/latest/Dockerfile

@@ -1,35 +1,13 @@
-FROM ros:melodic-perception
-# install rtabmap packages
-RUN apt-get update && apt-get install -y \
-    ros-melodic-rtabmap-ros \
-    && apt-get remove -y \
-    ros-melodic-rtabmap \
-    && rm -rf /var/lib/apt/lists/
-
-RUN rm /bin/sh && ln -s /bin/bash /bin/sh
-
-WORKDIR /root/
+FROM introlab3it/rtabmap:18.04
 
 ARG CACHE_DATE=2016-01-01
 
-RUN git clone https://github.com/introlab/rtabmap.git
-
-# Build RTAB-Map project
-RUN source /ros_entrypoint.sh && \
-    cd rtabmap/build && \
-    cmake .. && \
-    make -j$(nproc) && \
-    make install && \
-    cd ../.. && \
-    rm -rf rtabmap && \
-    ldconfig
-
 RUN source /ros_entrypoint.sh && \
     mkdir -p catkin_ws/src && \
     cd catkin_ws/src && \
     catkin_init_workspace && \
     git clone https://github.com/introlab/rtabmap_ros.git && \
     cd .. && \
-    catkin_make -DCMAKE_INSTALL_PREFIX=/opt/ros/melodic install && \
+    catkin_make -j1 -DCMAKE_INSTALL_PREFIX=/opt/ros/melodic install && \
     cd && \
     rm -rf catkin_ws

include/rtabmap_ros/CoreWrapper.h

@@ -143,6 +143,7 @@ private:
 	void tagDetectionsAsyncCallback(const apriltag_ros::AprilTagDetectionArray & tagDetections);
 #endif
 	void imuAsyncCallback(const sensor_msgs::ImuConstPtr & tagDetections);
+	void interOdomCallback(const nav_msgs::OdometryConstPtr & msg);
 
 	void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg);
 
@@ -319,6 +320,8 @@ private:
 	std::map<int, geometry_msgs::PoseWithCovarianceStamped> tags_;
 	ros::Subscriber imuSub_;
 	std::map<double, rtabmap::Transform> imus_;
+	ros::Subscriber interOdomSub_;
+	std::list<nav_msgs::Odometry> interOdoms_;
 
 	bool stereoToDepth_;
 	bool odomSensorSync_;

package.xml

@@ -82,7 +82,7 @@
 
   <export>
 	<nodelet plugin="${prefix}/nodelet_plugins.xml" />
-	<rviz plugin="${prefix}/plugin_description.xml"/>
+	<rviz plugin="${prefix}/rviz_plugins.xml"/>
     <costmap_2d plugin="${prefix}/costmap_plugins.xml"/>
   </export>
 </package>

rviz_plugins.xml

@@ -1,4 +1,4 @@
-<library path="lib/librtabmap_plugins">
+<library path="lib/librtabmap_rviz_plugins">
   <class name="rtabmap_ros/MapCloud"
          type="rtabmap_ros::MapCloudDisplay"
          base_class_type="rviz::Display">

scripts/assemble_local_grids.py

@@ -0,0 +1,119 @@
+#!/usr/bin/env python
+
+# Similar to map_assembler node, this minimal python example shows how
+# to reconstruct the obstacle map by subscribing only to
+# graph and latest data added to map (for constant network bandwidth usage).
+
+import rospy
+from sets import Set
+
+import message_filters
+from rtabmap_ros.msg import MapGraph
+from sensor_msgs.msg import PointCloud2
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import TransformStamped
+from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud
+
+
+posesDict = {}
+cloudsDict = {}
+assembledCloud = PointCloud2()
+pub = rospy.Publisher('assembled_local_grids', PointCloud2, queue_size=10)
+
+def callback(graph, cloud):
+    global assembledCloud
+    global posesDict
+    global cloudsDict
+    global pub
+
+    begin = rospy.get_time()
+
+    nodeId = graph.posesId[-1]
+    pose = graph.poses[-1]
+    size = cloud.width
+
+    posesDict[nodeId] = pose
+    cloudsDict[nodeId] = cloud
+
+    # Update pose of our buffered clouds.
+    # Check also if the clouds have moved because of a loop closure. If so, we have to update the rendering.
+    maxDiff = 0
+    for i in range(0,len(graph.posesId)):
+        if graph.posesId[i] in posesDict:
+            currentPose = posesDict[graph.posesId[i]].position
+            newPose = graph.poses[i].position
+            diff = max([abs(currentPose.x-newPose.x), abs(currentPose.y-newPose.y), abs(currentPose.z-newPose.z)])
+            if maxDiff < diff:
+                maxDiff = diff
+        else:
+            rospy.loginfo("Old node %d not found in cache, creating an empty cloud.", graph.posesId[i])
+            posesDict[graph.posesId[i]] = graph.poses[i]
+            cloudsDict[graph.posesId[i]] = PointCloud2()
+
+    # If we don't move, some nodes would be removed from the graph, so remove them from our buffered clouds.
+    newGraph = Set(graph.posesId)
+    totalPoints = 0
+    for p in posesDict.keys():
+        if p not in newGraph:
+            posesDict.pop(p)
+            cloudsDict.pop(p)
+        else:
+            totalPoints = totalPoints + cloudsDict[p].width
+
+    if maxDiff > 0.1:
+        # if any node moved more than 10 cm, request an update of the assembled map so far
+        newAssembledCloud = PointCloud2()
+        rospy.loginfo("Map has been optimized! maxDiff=%.3fm, re-updating the whole map...", maxDiff)
+        for i in range(0,len(graph.posesId)):
+            posesDict[graph.posesId[i]] = graph.poses[i]
+            t = TransformStamped()
+            p = posesDict[graph.posesId[i]]
+            t.transform.translation = p.position
+            t.transform.rotation = p.orientation
+            transformedCloud = do_transform_cloud(cloudsDict[graph.posesId[i]], t)
+            if i==0:
+                newAssembledCloud = transformedCloud
+            else:
+                newAssembledCloud.data = newAssembledCloud.data + transformedCloud.data
+                newAssembledCloud.width = newAssembledCloud.width + transformedCloud.width
+                newAssembledCloud.row_step = newAssembledCloud.row_step + transformedCloud.row_step
+        assembledCloud = newAssembledCloud
+    else:
+        t = TransformStamped()
+        t.transform.translation = pose.position
+        t.transform.rotation = pose.orientation
+        transformedCloud = do_transform_cloud(cloud, t)
+        # just concatenate new cloud to current assembled map
+        if assembledCloud.width == 0:
+            assembledCloud = transformedCloud
+        else:
+            # Adding only the difference would be more efficient
+            assembledCloud.data = assembledCloud.data + transformedCloud.data
+            assembledCloud.width = assembledCloud.width + transformedCloud.width
+            assembledCloud.row_step = assembledCloud.row_step + transformedCloud.row_step
+
+    updateTime = rospy.get_time() - begin
+
+    rospy.loginfo("Received node %d (%d pts) at xyz=%.2f %.2f %.2f, q_xyzw=%.2f %.2f %.2f %.2f (Map: Nodes=%d Points=%d Assembled=%d Update=%.0fms)", 
+        nodeId, size,
+        pose.position.x, pose.position.y, pose.position.z,
+        pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w,
+        len(cloudsDict), totalPoints, assembledCloud.width, updateTime*1000)
+
+    assembledCloud.header = graph.header
+    pub.publish(assembledCloud)
+
+def main():
+    rospy.init_node('assemble_local_grids', anonymous=True)
+    graph_sub = message_filters.Subscriber('rtabmap/mapGraph', MapGraph)
+    cloud_sub = message_filters.Subscriber('rtabmap/local_grid_obstacle', PointCloud2)
+
+    ts = message_filters.TimeSynchronizer([graph_sub, cloud_sub], 2)
+    ts.registerCallback(callback)
+    rospy.spin()
+
+if __name__ == '__main__':
+    try:
+        main()
+    except rospy.ROSInterruptException:
+        pass

src/CoreWrapper.cpp

@@ -516,6 +516,11 @@ void CoreWrapper::onInit()
 		if(createIntermediateNodes_)
 		{
 			NODELET_INFO("Create intermediate nodes");
+			if(rate_ == 0.0f)
+			{
+				NODELET_INFO("Subscribe to inter odom messges");
+				interOdomSub_ = nh.subscribe("inter_odom", 1, &CoreWrapper::interOdomCallback, this);
+			}
 		}
 	}
 	if(parameters_.find(Parameters::kGridGlobalMaxNodes()) != parameters_.end())
@@ -1632,6 +1637,75 @@ void CoreWrapper::process(
 	UTimer timer;
 	if(rtabmap_.isIDsGenerated() || data.id() > 0)
 	{
+		// Add intermediate nodes?
+		for(std::list<nav_msgs::Odometry>::iterator iter=interOdoms_.begin(); iter!=interOdoms_.end();)
+		{
+			if(iter->header.stamp < lastPoseStamp_)
+			{
+				Transform interOdom = rtabmap_ros::transformFromPoseMsg(iter->pose.pose);
+				if(!interOdom.isNull())
+				{
+					cv::Mat covariance;
+					double variance = iter->twist.covariance[0];
+					if(variance == BAD_COVARIANCE || variance <= 0.0f)
+					{
+						//use the one of the pose
+						covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->pose.covariance.data()).clone();
+						covariance /= 2.0;
+					}
+					else
+					{
+						covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->twist.covariance.data()).clone();
+					}
+					if(!uIsFinite(covariance.at<double>(0,0)) || covariance.at<double>(0,0)<=0.0f)
+					{
+						covariance = cv::Mat::eye(6,6,CV_64FC1);
+						if(odomDefaultLinVariance_ > 0.0f)
+						{
+							covariance.at<double>(0,0) = odomDefaultLinVariance_;
+							covariance.at<double>(1,1) = odomDefaultLinVariance_;
+							covariance.at<double>(2,2) = odomDefaultLinVariance_;
+						}
+						if(odomDefaultAngVariance_ > 0.0f)
+						{
+							covariance.at<double>(3,3) = odomDefaultAngVariance_;
+							covariance.at<double>(4,4) = odomDefaultAngVariance_;
+							covariance.at<double>(5,5) = odomDefaultAngVariance_;
+						}
+					}
+
+					cv::Mat rgb = cv::Mat::zeros(2,1,CV_8UC1);
+					cv::Mat depth = cv::Mat::zeros(2,1,CV_16UC1);
+					CameraModel model(
+							1,
+							1,
+							0.5,
+							1,
+							Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
+							0,
+							cv::Size(1,2));
+					SensorData interData(rgb, depth, model, -1, rtabmap_ros::timestampFromROS(iter->header.stamp));
+					Transform gt;
+					if(!groundTruthFrameId_.empty())
+					{
+						gt = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, iter->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
+					}
+					interData.setGroundTruth(gt);
+					rtabmap_.process(interData, interOdom, covariance);
+				}
+				interOdoms_.erase(iter++);
+			}
+			else if(iter->header.stamp == lastPoseStamp_)
+			{
+				interOdoms_.erase(iter++);
+				break;
+			}
+			else
+			{
+				break;
+			}
+		}
+
 		//Add async stuff
 		Transform groundTruthPose;
 		if(!groundTruthFrameId_.empty())
@@ -2103,6 +2177,14 @@ void CoreWrapper::imuAsyncCallback(const sensor_msgs::ImuConstPtr & msg)
 	}
 }
 
+void CoreWrapper::interOdomCallback(const nav_msgs::OdometryConstPtr & msg)
+{
+	if(!paused_)
+	{
+		interOdoms_.push_back(*msg);
+	}
+}
+
 
 void CoreWrapper::initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg)
 {
@@ -2377,6 +2459,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
 	userData_ = cv::Mat();
 	userDataMutex_.unlock();
 	imus_.clear();
+	interOdoms_.clear();
 	return true;
 }