diff --git a/.travis.yml b/.travis.yml index 8be1a88a..80e5b32c 100644 --- a/.travis.yml +++ b/.travis.yml @@ -41,6 +41,7 @@ script: - cd ../../catkin_ws - source devel/setup.bash - catkin_make + - catkin_make install notifications: email: diff --git a/CMakeLists.txt b/CMakeLists.txt index dbf8a459..8c354ef7 100644 --- a/CMakeLists.txt +++ b/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 @@ -127,22 +120,19 @@ generate_dynamic_reconfigure_options(cfg/Camera.cfg) ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need -SET(optional_dependencies "") -IF(costmap_2d_FOUND) +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) -ENDIF(octomap_msgs_FOUND) -IF(rviz_FOUND) - SET(optional_dependencies ${optional_dependencies} rviz) +ENDIF(costmap_2d_FOUND) +IF(octomap_msgs_FOUND) + SET(optional_dependencies ${optional_dependencies} octomap_msgs) +ENDIF(octomap_msgs_FOUND) +IF(rviz_FOUND) + SET(optional_dependencies ${optional_dependencies} rviz) ENDIF(rviz_FOUND) -IF(find_object_2d_FOUND) - SET(optional_dependencies ${optional_dependencies} find_object_2d) -ENDIF(find_object_2d_FOUND) +IF(find_object_2d_FOUND) + SET(optional_dependencies ${optional_dependencies} find_object_2d) +ENDIF(find_object_2d_FOUND) catkin_package( INCLUDE_DIRS include @@ -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) ############# diff --git a/costmap_plugins.xml b/costmap_plugins.xml index 8988bb55..6ca09934 100644 --- a/costmap_plugins.xml +++ b/costmap_plugins.xml @@ -1,5 +1,5 @@ - + Listens to OccupancyGrid messages and copies them in, like from map_server. diff --git a/docker/README.md b/docker/README.md index 758bb164..8de92b0f 100644 --- a/docker/README.md +++ b/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" ``` diff --git a/docker/kinetic/latest/Dockerfile b/docker/kinetic/latest/Dockerfile index d3857077..44ca0102 100644 --- a/docker/kinetic/latest/Dockerfile +++ b/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 diff --git a/docker/melodic/latest/Dockerfile b/docker/melodic/latest/Dockerfile index 8589ca1e..02d8d6c8 100644 --- a/docker/melodic/latest/Dockerfile +++ b/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 diff --git a/include/rtabmap_ros/CoreWrapper.h b/include/rtabmap_ros/CoreWrapper.h index b8a18a1a..5bd6681c 100644 --- a/include/rtabmap_ros/CoreWrapper.h +++ b/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 tags_; ros::Subscriber imuSub_; std::map imus_; + ros::Subscriber interOdomSub_; + std::list interOdoms_; bool stereoToDepth_; bool odomSensorSync_; diff --git a/package.xml b/package.xml index 854b4798..df2648b6 100644 --- a/package.xml +++ b/package.xml @@ -82,7 +82,7 @@ - + diff --git a/plugin_description.xml b/rviz_plugins.xml similarity index 96% rename from plugin_description.xml rename to rviz_plugins.xml index e308b929..a04c2bd6 100644 --- a/plugin_description.xml +++ b/rviz_plugins.xml @@ -1,4 +1,4 @@ - + diff --git a/scripts/assemble_local_grids.py b/scripts/assemble_local_grids.py new file mode 100755 index 00000000..eb166c0d --- /dev/null +++ b/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 diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index e812e1e9..61f7b607 100644 --- a/src/CoreWrapper.cpp +++ b/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::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(0,0)) || covariance.at(0,0)<=0.0f) + { + covariance = cv::Mat::eye(6,6,CV_64FC1); + if(odomDefaultLinVariance_ > 0.0f) + { + covariance.at(0,0) = odomDefaultLinVariance_; + covariance.at(1,1) = odomDefaultLinVariance_; + covariance.at(2,2) = odomDefaultLinVariance_; + } + if(odomDefaultAngVariance_ > 0.0f) + { + covariance.at(3,3) = odomDefaultAngVariance_; + covariance.at(4,4) = odomDefaultAngVariance_; + covariance.at(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; }