diff --git a/CMakeLists.txt b/CMakeLists.txt index 25ab0fc3..24eb11f6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,7 +18,7 @@ find_package(rviz) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) -find_package(RTABMap 0.11.7 REQUIRED) +find_package(RTABMap 0.11.8 REQUIRED) find_package(OpenCV REQUIRED) @@ -149,6 +149,8 @@ SET(Libraries ) SET(rtabmap_ros_lib_src + src/nodelets/rgbd_odometry.cpp + src/nodelets/stereo_odometry.cpp src/nodelets/data_throttle.cpp src/nodelets/stereo_throttle.cpp src/nodelets/data_odom_sync.cpp @@ -157,8 +159,8 @@ SET(rtabmap_ros_lib_src src/nodelets/disparity_to_depth.cpp src/nodelets/obstacles_detection.cpp src/nodelets/point_cloud_aggregator.cpp + src/nodelets/OdometryROS.cpp src/MsgConversion.cpp - src/OdometryROS.cpp src/MapsManager.cpp ) @@ -176,6 +178,19 @@ SET(rtabmap_ros_lib_src ) ENDIF(costmap_2d_FOUND) +# If octomap is found, add definition +IF(octomap_ros_FOUND) +MESSAGE(STATUS "WITH octomap") +include_directories( + ${octomap_ros_INCLUDE_DIRS} +) +SET(Libraries + ${octomap_ros_LIBRARIES} + ${Libraries} +) +ADD_DEFINITIONS("-DWITH_OCTOMAP_ROS") +ENDIF(octomap_ros_FOUND) + IF(QT4_FOUND OR Qt5_FOUND) SET(Libraries ${Libraries} @@ -244,27 +259,14 @@ IF(Qt5_FOUND) ENDIF(Qt5_FOUND) add_dependencies(rtabmap_ros ${${PROJECT_NAME}_EXPORTED_TARGETS}) -# If octomap is found, add definition -IF(octomap_ros_FOUND) -MESSAGE(STATUS "WITH octomap") -include_directories( - ${octomap_ros_INCLUDE_DIRS} -) -SET(Libraries - ${octomap_ros_LIBRARIES} - ${Libraries} -) -add_definitions(-DWITH_OCTOMAP) -ENDIF(octomap_ros_FOUND) - add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp) target_link_libraries(rtabmap rtabmap_ros ${Libraries}) add_executable(rgbd_odometry src/RGBDOdometryNode.cpp) -target_link_libraries(rgbd_odometry rtabmap_ros ${Libraries}) +target_link_libraries(rgbd_odometry ${Libraries}) add_executable(stereo_odometry src/StereoOdometryNode.cpp) -target_link_libraries(stereo_odometry rtabmap_ros ${Libraries}) +target_link_libraries(stereo_odometry ${Libraries}) add_executable(map_optimizer src/MapOptimizerNode.cpp) target_link_libraries(map_optimizer rtabmap_ros ${Libraries}) @@ -276,6 +278,9 @@ add_executable(camera src/CameraNode.cpp) add_dependencies(camera ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_link_libraries(camera ${Libraries}) +add_executable(stereo_camera src/StereoCameraNode.cpp) +target_link_libraries(stereo_camera rtabmap_ros ${Libraries}) + IF(RTABMAP_GUI) add_executable(rtabmapviz src/GuiNode.cpp src/GuiWrapper.cpp src/PreferencesDialogROS.cpp) target_link_libraries(rtabmapviz rtabmap_ros ${QT_LIBRARIES} ${Libraries}) diff --git a/LICENSE b/LICENSE index 5ee6eb9e..c115abd1 100644 --- a/LICENSE +++ b/LICENSE @@ -1,4 +1,4 @@ -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/include/rtabmap_ros/MsgConversion.h b/include/rtabmap_ros/MsgConversion.h index fd4bb016..9af81a23 100644 --- a/include/rtabmap_ros/MsgConversion.h +++ b/include/rtabmap_ros/MsgConversion.h @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/launch/config/rgbd_gui.ini b/launch/config/rgbd_gui.ini index fdd9e9c6..9c552b4b 100644 --- a/launch/config/rgbd_gui.ini +++ b/launch/config/rgbd_gui.ini @@ -6,17 +6,12 @@ General\loggerPauseLevel=3 General\loggerType=1 General\loggerPrintTime=true General\verticalLayoutUsed=true -General\imageFlipped=false General\imageRejectedShown=true General\imageHighestHypShown=true General\beep=false -General\keypointsOpacity=16 -General\voxelSize=0 -General\decimation=16 -MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\0\x80\0\0\x2\x92\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_\0s\0t\0\x61\0t\0s\0V\0\x32\0\0\0\0(\0\0\x2\x92\0\0\0\xe0\0\xff\xff\xff\0\0\0\x1\0\0\x5\0\0\0\x2\x92\xfc\x2\0\0\0\x2\xfc\0\0\0(\0\0\x2\x92\0\0\x1\x46\0\xff\xff\xff\xfc\x1\0\0\0\x2\xfc\0\0\0\0\0\0\x1\\\0\0\0q\0\xff\xff\xff\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_\0o\0\x64\0o\0m\0\x65\0t\0r\0y\x1\0\0\0(\0\0\x1\x1b\0\0\0g\0\xff\xff\xff\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0i\0m\0\x61\0g\0\x65\0V\0i\0\x65\0w\x1\0\0\x1I\0\0\x1q\0\0\0\xd9\0\xff\xff\xff\xfc\0\0\x1\x62\0\0\x3\x9e\0\0\0\xc8\0\xff\xff\xff\xfa\0\0\0\x1\x2\0\0\0\x2\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0g\0r\0\x61\0p\0h\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0g\0\xff\xff\xff\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0l\0o\0u\0\x64\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\0(\0\0\x2\x92\0\0\0\xe1\0\xff\xff\xff\xfb\0\0\0\x38\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0o\0o\0p\0\x43\0l\0o\0s\0u\0r\0\x65\0V\0i\0\x65\0w\0\x65\0r\0\0\0\x1I\0\0\0\xf7\0\0\0\xf7\0\xff\xff\xff\0\0\0\x3\0\0\x5\0\0\0\0\x9a\xfc\x1\0\0\0\x5\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0o\0s\0t\0\x65\0r\0i\0o\0r\0\0\0\0\0\0\0\x5\0\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0o\0n\0s\0o\0l\0\x65\0\0\0\0\0\0\0\x5\0\0\0\0g\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0r\0\x61\0w\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0m\0\x61\0p\0V\0i\0s\0i\0\x62\0i\0l\0i\0t\0y\0\0\0\0\0\xff\xff\xff\xff\0\0\0`\0\xff\xff\xff\0\0\0\0\0\0\x2\x92\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\x1\0\0\0\x2\0\0\0\x1\0\0\0\xe\0t\0o\0o\0l\0\x42\0\x61\0r\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0)" -MainWindow\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\x1V\0\0\0\xa1\0\0\x6\x65\0\0\x3\x94\0\0\x1^\0\0\0\xbd\0\0\x6]\0\0\x3\x8c\0\0\0\0\0\0) +MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\0\x80\0\0\x2\x92\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_\0s\0t\0\x61\0t\0s\0V\0\x32\0\0\0\0(\0\0\x2\x92\0\0\0\xe0\0\xff\xff\xff\0\0\0\x1\0\0\x5\0\0\0\x2\xa8\xfc\x2\0\0\0\x2\xfc\0\0\0(\0\0\x2\xa8\0\0\0\xe1\0\xff\xff\xff\xfc\x1\0\0\0\x2\xfc\0\0\0\0\0\0\x1\\\0\0\0O\0\xff\xff\xff\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_\0o\0\x64\0o\0m\0\x65\0t\0r\0y\x1\0\0\0(\0\0\x1%\0\0\0\x19\0\xff\xff\xff\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0i\0m\0\x61\0g\0\x65\0V\0i\0\x65\0w\x1\0\0\x1S\0\0\x1}\0\0\0=\0\xff\xff\xff\xfc\0\0\x1\x62\0\0\x3\x9e\0\0\0\xc8\0\xff\xff\xff\xfa\0\0\0\x1\x2\0\0\0\x2\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0g\0r\0\x61\0p\0h\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0g\0\xff\xff\xff\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0l\0o\0u\0\x64\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\0(\0\0\x2\x92\0\0\0\xe1\0\xff\xff\xff\xfb\0\0\0\x38\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0o\0o\0p\0\x43\0l\0o\0s\0u\0r\0\x65\0V\0i\0\x65\0w\0\x65\0r\0\0\0\x1I\0\0\0\xf7\0\0\0\xf7\0\xff\xff\xff\0\0\0\x3\0\0\x5\0\0\0\0\x9a\xfc\x1\0\0\0\x5\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0o\0s\0t\0\x65\0r\0i\0o\0r\0\0\0\0\0\0\0\x5\0\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0o\0n\0s\0o\0l\0\x65\0\0\0\0\0\0\0\x5\0\0\0\x1\x33\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0r\0\x61\0w\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0m\0\x61\0p\0V\0i\0s\0i\0\x62\0i\0l\0i\0t\0y\0\0\0\0\0\xff\xff\xff\xff\0\0\0m\0\xff\xff\xff\0\0\0\0\0\0\x2\xa8\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\x1\0\0\0\x2\0\0\0\x2\0\0\0\xe\0t\0o\0o\0l\0\x42\0\x61\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0\0\0\0\x12\0t\0o\0o\0l\0\x42\0\x61\0r\0_\0\x32\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0)" +MainWindow\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0\xc4\0\0\0\xc4\0\0\x5\xd7\0\0\x3\xc3\0\0\0\xce\0\0\0\xea\0\0\x5\xcd\0\0\x3\xb9\0\0\0\0\0\0) General\showClouds0=true -General\voxelSize0=0 General\decimation0=4 General\maxDepth0=4 General\showScans0=true @@ -25,7 +20,6 @@ General\ptSize0=1 General\opacityScan0=1 General\ptSizeScan0=1 General\showClouds1=true -General\voxelSize1=0 General\decimation1=4 General\maxDepth1=0 General\showScans1=true @@ -33,45 +27,25 @@ General\opacity1=1 General\ptSize1=2 General\opacityScan1=1 General\ptSizeScan1=1 -General\showClouds2=true -General\voxelSize2=0.01 -General\decimation2=1 -General\maxDepth2=4 -General\showScans2=true -General\meshing0=false -General\cloudFiltering=true +General\cloudFiltering=false General\cloudFilteringRadius=0.2 General\cloudFilteringAngle=20 -General\meshNormalKSearch0=20 -General\meshGP3Radius0=0.04 -General\meshSmoothing0=false -General\meshSmoothingRadius0=0.04 -General\meshNormalKSearch1=20 -General\meshGP3Radius1=0.04 -General\meshSmoothing1=true -General\meshSmoothingRadius1=0.04 General\odomQualityThr=50 General\meshing=false -General\meshGP3Radius=0.04 -General\meshNormalKSearch=20 -General\meshSmoothing=false -General\meshSmoothingRadius=0.04 General\gridMapShown=false General\gridMapResolution=0.05 -General\gridMapFillEmptySpace=true General\gridMapOccupancyFrom3DCloud=false -General\gridMapFillEmptyRadius=0 General\gridMapOpacity=0.75 General\posteriorGraphView=true General\showGraphs=true -PreferencesDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\x1\xde\0\0\0n\0\0\x5\xe8\0\0\x3\x82\0\0\x1\xde\0\0\0n\0\0\x5\xe8\0\0\x3\x82\0\0\0\0\0\0) +PreferencesDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\x1\xde\0\0\0R\0\0\x5\xe8\0\0\x3\x66\0\0\x1\xde\0\0\0R\0\0\x5\xe8\0\0\x3\x66\0\0\0\0\0\0) AboutDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0\0\0\0\0\x18\0\0\x2_\0\0\x1\xf2\0\0\0\0\0\0\0\x18\0\0\x2_\0\0\x1\xf2\0\0\0\0\0\0) widget_cloudViewer\camera_pose=@Variant(\0\0\0T\xbf\xf0\0\x13\0\0\0\0\xbe\xb6\0\0\0\0\0\0>\xa4\0\0\0\0\0\0) -widget_cloudViewer\camera_focal=@Variant(\0\0\0T>\xa4\0\0\0\0\0\0\xbe\xa1\0\0\0\0\0\0>t\0\0\0\0\0\0) +widget_cloudViewer\camera_focal=@Variant(\0\0\0T>\xa4\0\0\0\0\0\0\xbe\xa1\0\0\0\0\0\0>\xa4\0\0\0\0\0\0) widget_cloudViewer\camera_up=@Variant(\0\0\0T\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0?\xf0\0\0\0\0\0\0) widget_cloudViewer\grid=false widget_cloudViewer\grid_cell_count=50 -widget_cloudViewer\grid_cell_size=@Variant(\0\0\0\x87?\x80\0\0) +widget_cloudViewer\grid_cell_size=1 widget_cloudViewer\trajectory_shown=true widget_cloudViewer\trajectory_size=100 widget_cloudViewer\camera_target_locked=false @@ -110,3 +84,117 @@ PostProcessingDialog\iterations=1 PostProcessingDialog\reextract_features=false PostProcessingDialog\refine_neigbors=false PostProcessingDialog\refine_lc=false +MainWindow\maximized=false +MainWindow\status_bar=false +widget_cloudViewer\frustum_shown=false +widget_cloudViewer\frustum_scale=@Variant(\0\0\0\x87?\0\0\0) +widget_cloudViewer\frustum_color=@Variant(\0\0\0\x43\x1\xff\xff\xa0\xa0\xa0\xa0\xa4\xa4\0\0) +widget_cloudViewer\rendering_rate=5 +imageView_source\alpha=50 +imageView_source\graphics_view=false +imageView_source\graphics_view_scale=true +imageView_loopClosure\alpha=50 +imageView_loopClosure\graphics_view=false +imageView_loopClosure\graphics_view_scale=true +imageView_odometry\alpha=200 +imageView_odometry\graphics_view=false +imageView_odometry\graphics_view_scale=true +ExportCloudsDialog\pipeline=0 +ExportCloudsDialog\normals_k=10 +ExportCloudsDialog\regenerate_min_depth=0 +ExportCloudsDialog\filtering=false +ExportCloudsDialog\filtering_radius=0.02 +ExportCloudsDialog\filtering_min_neighbors=2 +ExportCloudsDialog\subtract=false +ExportCloudsDialog\subtract_point_radius=0.02 +ExportCloudsDialog\subtract_point_angle=0 +ExportCloudsDialog\subtract_min_neighbors=5 +ExportCloudsDialog\mls_polygonial_order=2 +ExportCloudsDialog\mls_upsampling_method=0 +ExportCloudsDialog\mls_upsampling_radius=0.01 +ExportCloudsDialog\mls_upsampling_step=0 +ExportCloudsDialog\mls_point_density=0 +ExportCloudsDialog\mls_dilation_voxel_size=0.01 +ExportCloudsDialog\mls_dilation_iterations=0 +ExportCloudsDialog\mesh_mu=2.5 +ExportCloudsDialog\mesh_decimation_factor=0 +ExportCloudsDialog\mesh_texture=false +ExportCloudsDialog\mesh_angle_tolerance=15 +ExportCloudsDialog\mesh_quad=false +ExportCloudsDialog\mesh_triangle_size=2 +ExportScansDialog\binary=true +ExportScansDialog\normals_k=20 +ExportScansDialog\regenerate=false +ExportScansDialog\regenerate_decimation=1 +ExportScansDialog\filtering=false +ExportScansDialog\filtering_radius=0.02 +ExportScansDialog\filtering_min_neighbors=2 +ExportScansDialog\assemble=true +ExportScansDialog\assemble_voxel=0.01 +PostProcessingDialog\sba=false +PostProcessingDialog\sba_iterations=20 +PostProcessingDialog\sba_epsilon=0 +PostProcessingDialog\sba_type=1 +PostProcessingDialog\sba_variance=1 +graphicsView_graphView\node_radius=0.00999999977648258 +graphicsView_graphView\link_width=0 +graphicsView_graphView\node_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0) +graphicsView_graphView\current_goal_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0) +graphicsView_graphView\neighbor_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0) +graphicsView_graphView\global_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0) +graphicsView_graphView\local_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0) +graphicsView_graphView\user_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0) +graphicsView_graphView\virtual_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0) +graphicsView_graphView\neighbor_merged_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xaa\xaa\0\0\0\0) +graphicsView_graphView\rejected_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0) +graphicsView_graphView\local_path_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0) +graphicsView_graphView\global_path_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0) +graphicsView_graphView\gt_color=@Variant(\0\0\0\x43\x1\xff\xff\xa0\xa0\xa0\xa0\xa4\xa4\0\0) +graphicsView_graphView\intra_session_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0) +graphicsView_graphView\inter_session_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\0\0\0\0) +graphicsView_graphView\intra_inter_session_colors_enabled=false +graphicsView_graphView\grid_visible=true +graphicsView_graphView\origin_visible=true +graphicsView_graphView\referential_visible=true +graphicsView_graphView\local_radius_visible=false +graphicsView_graphView\loop_closure_outlier_thr=@Variant(\0\0\0\x87\0\0\0\0) +graphicsView_graphView\max_link_length=@Variant(\0\0\0\x87<\xa3\xd7\n) +graphicsView_graphView\graph_visible=true +graphicsView_graphView\global_path_visible=true +graphicsView_graphView\local_path_visible=true +graphicsView_graphView\gt_graph_visible=true +General\cloudsKept=true +General\loggerPrintThreadId=false +General\notifyNewGlobalPath=false +General\minDepth0=0 +General\showFeatures0=false +General\downsamplingScan0=1 +General\voxelSizeScan0=0 +General\ptSizeFeatures0=3 +General\minDepth1=0 +General\showFeatures1=true +General\downsamplingScan1=1 +General\voxelSizeScan1=0 +General\ptSizeFeatures1=3 +General\cloudVoxel=0 +General\cloudNoiseRadius=0 +General\cloudNoiseMinNeighbors=5 +General\showLabels=false +General\noFiltering=true +General\subtractFiltering=false +General\subtractFilteringMinPts=5 +General\subtractFilteringRadius=0.02 +General\subtractFilteringAngle=0 +General\normalKSearch=10 +General\gridMapEroded=false +General\projMapFrame=true +General\projMaxGroundAngle=30 +General\projMaxGroundHeight=0 +General\projMinClusterSize=20 +General\projMaxObstaclesHeight=0 +General\projFlatObstaclesDetected=true +General\meshing_angle=15 +General\meshing_quad=false +General\meshing_triangle_size=2 +Figures\counts= +Figures\curves= \ No newline at end of file diff --git a/launch/demo/demo_stereo_outdoor.launch b/launch/demo/demo_stereo_outdoor.launch index 95877fc2..fcc376da 100644 --- a/launch/demo/demo_stereo_outdoor.launch +++ b/launch/demo/demo_stereo_outdoor.launch @@ -55,6 +55,7 @@ + diff --git a/launch/rgbd_mapping.launch b/launch/rgbd_mapping.launch index 2b0a43fc..9cc04de7 100644 --- a/launch/rgbd_mapping.launch +++ b/launch/rgbd_mapping.launch @@ -1,5 +1,7 @@ + + - - + - @@ -41,102 +41,35 @@ - - + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + - - - - - - - - - + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch index d41a611a..3d3f509f 100644 --- a/launch/rtabmap.launch +++ b/launch/rtabmap.launch @@ -2,7 +2,7 @@ - - @@ -37,7 +36,8 @@ - + + @@ -49,7 +49,6 @@ - @@ -80,7 +79,7 @@ - + @@ -91,6 +90,7 @@ + @@ -101,7 +101,7 @@ - + @@ -118,14 +118,15 @@ - + + - + @@ -150,7 +151,8 @@ - + + @@ -189,7 +191,7 @@ - + diff --git a/launch/stereo_mapping.launch b/launch/stereo_mapping.launch index f5d8ad5a..6cbad067 100644 --- a/launch/stereo_mapping.launch +++ b/launch/stereo_mapping.launch @@ -1,5 +1,7 @@ + + - - - + + - - @@ -44,98 +43,40 @@ - - + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + - - - - - - - - - + + + + + - - - - - - - - - - - - - - - - - - + + + + - - - - - - - - - - - - - - - + + + + + + + diff --git a/launch/tests/sensor_fusion.launch b/launch/tests/sensor_fusion.launch new file mode 100644 index 00000000..28c97a15 --- /dev/null +++ b/launch/tests/sensor_fusion.launch @@ -0,0 +1,145 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [true, true, true, + false, false, false, + true, true, true, + false, false, false, + false, false, false] + + [ + false, false, false, + true, true, true, + false, false, false, + true, true, true, + false, false, false] + [ + false, false, false, + true, true, true, + false, false, false, + true, true, true, + true, true, true] + + + + + + + + + + + + + + + + + [0.005, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0.005, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0.006, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0.003, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0.003, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0.006, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0.0025, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0.0025, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.004, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.002, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0015] + + + [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9] + + + \ No newline at end of file diff --git a/launch/tests/sensor_fusion_kinect_brick.launch b/launch/tests/sensor_fusion_kinect_brick.launch new file mode 100644 index 00000000..282b66a1 --- /dev/null +++ b/launch/tests/sensor_fusion_kinect_brick.launch @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml index d2aadf3a..58e29dd7 100644 --- a/nodelet_plugins.xml +++ b/nodelet_plugins.xml @@ -1,4 +1,21 @@ + + + + This is my nodelet. + + + + + + This is my nodelet. + + + diff --git a/package.xml b/package.xml index ea9a1a21..ba16269d 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap_ros - 0.11.7 + 0.11.8 RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe @@ -39,7 +39,6 @@ move_base_msgs costmap_2d octomap_ros - octomap cv_bridge roscpp @@ -69,7 +68,6 @@ move_base_msgs costmap_2d octomap_ros - octomap libpcl-all-dev diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index 21e2dc3f..808430a1 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without @@ -207,7 +207,7 @@ public: ROS_ERROR("Path \"%s\" does not exist (or you don't have the permissions to read)... falling back to usb device...", path.c_str()); } //usb device - camera_ = new rtabmap::CameraVideo(deviceId, frameRate); + camera_ = new rtabmap::CameraVideo(deviceId, false, frameRate); } cameraThread_ = new rtabmap::CameraThread(camera_); init(); diff --git a/src/CoreNode.cpp b/src/CoreNode.cpp index 028cde83..4429c125 100644 --- a/src/CoreNode.cpp +++ b/src/CoreNode.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 2d7b240e..9c82fb72 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without @@ -51,13 +51,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include -#ifdef WITH_OCTOMAP +#ifdef WITH_OCTOMAP_ROS +#ifdef RTABMAP_OCTOMAP #include +#include +#endif #endif #define BAD_COVARIANCE 9999 @@ -98,14 +102,20 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) mapToOdom_(rtabmap::Transform::getIdentity()), mapsManager_(true), depthSync_(0), + depthExactSync_(0), depthScanSync_(0), + depthScan3dSync_(0), stereoScanSync_(0), + stereoScan3dSync_(0), stereoApproxSync_(0), stereoExactSync_(0), depth2Sync_(0), depthTFSync_(0), + depthTFExactSync_(0), depthScanTFSync_(0), + depthScan3dTFSync_(0), stereoScanTFSync_(0), + stereoScan3dTFSync_(0), stereoApproxTFSync_(0), stereoExactTFSync_(0), transformThread_(0), @@ -126,8 +136,9 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) int queueSize = 10; bool publishTf = true; double tfDelay = 0.05; // 20 Hz + double tfTolerance = 0.1; // 100 ms std::string tfPrefix = ""; - bool stereoApproxSync = false; + bool approxSync = true; // ROS related parameters (private) pnh.param("subscribe_depth", subscribeDepth, subscribeDepth); @@ -166,11 +177,22 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_); pnh.param("depth_cameras", depthCameras, depthCameras); pnh.param("queue_size", queueSize, queueSize); - pnh.param("stereo_approx_sync", stereoApproxSync, stereoApproxSync); + if(pnh.hasParam("stereo_approx_sync") && !pnh.hasParam("approx_sync")) + { + ROS_WARN("Parameter \"stereo_approx_sync\" has been renamed " + "to \"approx_sync\"! Your value is still copied to " + "corresponding parameter."); + pnh.param("stereo_approx_sync", approxSync, approxSync); + } + else + { + pnh.param("approx_sync", approxSync, approxSync); + } pnh.param("publish_tf", publishTf, publishTf); pnh.param("tf_delay", tfDelay, tfDelay); pnh.param("tf_prefix", tfPrefix, tfPrefix); + pnh.param("tf_tolerance", tfTolerance, tfTolerance); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_); pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_); @@ -219,7 +241,9 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) ROS_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str()); ROS_INFO("rtabmap: queue_size = %d", queueSize); ROS_INFO("rtabmap: tf_delay = %f", tfDelay); + ROS_INFO("rtabmap: tf_tolerance = %f", tfTolerance); ROS_INFO("rtabmap: depth_cameras = %d", depthCameras); + ROS_INFO("rtabmap: approx_sync = %s", approxSync?"true":"false"); infoPub_ = nh.advertise("info", 1); mapDataPub_ = nh.advertise("mapData", 1); @@ -408,9 +432,11 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) cancelGoalSrv_ = nh.advertiseService("cancel_goal", &CoreWrapper::cancelGoalCallback, this); setLabelSrv_ = nh.advertiseService("set_label", &CoreWrapper::setLabelCallback, this); listLabelsSrv_ = nh.advertiseService("list_labels", &CoreWrapper::listLabelsCallback, this); -#ifdef WITH_OCTOMAP +#ifdef WITH_OCTOMAP_ROS +#ifdef RTABMAP_OCTOMAP octomapBinarySrv_ = nh.advertiseService("octomap_binary", &CoreWrapper::octomapBinaryCallback, this); octomapFullSrv_ = nh.advertiseService("octomap_full", &CoreWrapper::octomapFullCallback, this); +#endif #endif //private services setLogDebugSrv_ = pnh.advertiseService("log_debug", &CoreWrapper::setLogDebug, this); @@ -418,13 +444,13 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) setLogWarnSrv_ = pnh.advertiseService("log_warning", &CoreWrapper::setLogWarn, this); setLogErrorSrv_ = pnh.advertiseService("log_error", &CoreWrapper::setLogError, this); - setupCallbacks(subscribeDepth, subscribeScan2d, subscribeScan3d, subscribeStereo, queueSize, stereoApproxSync, depthCameras); + setupCallbacks(subscribeDepth, subscribeScan2d, subscribeScan3d, subscribeStereo, queueSize, approxSync, depthCameras); int optimizeIterations = 0; Parameters::parse(parameters_, Parameters::kOptimizerIterations(), optimizeIterations); if(publishTf && optimizeIterations != 0) { - transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay)); + transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay, tfTolerance)); } else if(publishTf) { @@ -443,10 +469,16 @@ CoreWrapper::~CoreWrapper() if(depthSync_) delete depthSync_; + if(depthExactSync_) + delete depthExactSync_; if(depthScanSync_) delete depthScanSync_; + if(depthScan3dSync_) + delete depthScan3dSync_; if(stereoScanSync_) delete stereoScanSync_; + if(stereoScan3dSync_) + delete stereoScan3dSync_; if(stereoApproxSync_) delete stereoApproxSync_; if(stereoExactSync_) @@ -455,10 +487,16 @@ CoreWrapper::~CoreWrapper() delete depth2Sync_; if(depthTFSync_) delete depthTFSync_; + if(depthTFExactSync_) + delete depthTFExactSync_; if(depthScanTFSync_) delete depthScanTFSync_; + if(depthScan3dTFSync_) + delete depthScan3dTFSync_; if(stereoScanTFSync_) delete stereoScanTFSync_; + if(stereoScan3dTFSync_) + delete stereoScan3dTFSync_; if(stereoApproxTFSync_) delete stereoApproxTFSync_; if(stereoExactTFSync_) @@ -527,7 +565,7 @@ void CoreWrapper::saveParameters(const std::string & configFile) } } -void CoreWrapper::publishLoop(double tfDelay) +void CoreWrapper::publishLoop(double tfDelay, double tfTolerance) { if(tfDelay == 0) return; @@ -537,7 +575,7 @@ void CoreWrapper::publishLoop(double tfDelay) if(!odomFrameId_.empty()) { mapToOdomMutex_.lock(); - ros::Time tfExpiration = ros::Time::now() + ros::Duration(tfDelay); + ros::Time tfExpiration = ros::Time::now() + ros::Duration(tfTolerance); geometry_msgs::TransformStamped msg; msg.child_frame_id = odomFrameId_; msg.header.frame_id = mapFrameId_; @@ -817,8 +855,19 @@ void CoreWrapper::commonDepthCallback( ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16"); return; } - UASSERT(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight); - UASSERT(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight); + + UASSERT_MSG(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight, + uFormat("imageWidth=%d vs %d imageHeight=%d vs %d", + imageWidth, + imageMsgs[i]->width, + imageHeight, + imageMsgs[i]->height).c_str()); + UASSERT_MSG(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight, + uFormat("imageWidth=%d vs %d imageHeight=%d vs %d", + imageWidth, + depthMsgs[i]->width, + imageHeight, + depthMsgs[i]->height).c_str()); Transform localTransform = getTransform(frameId_, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp); if(localTransform.isNull()) @@ -993,7 +1042,9 @@ void CoreWrapper::commonDepthCallback( if(scanCloudNormalK_ > 0) { //compute normals - pcl::PointCloud::Ptr pclScanNormal = util3d::computeNormals(pclScan, scanCloudNormalK_); + pcl::PointCloud::Ptr normals = util3d::computeNormals(pclScan, scanCloudNormalK_); + pcl::PointCloud::Ptr pclScanNormal(new pcl::PointCloud); + pcl::concatenateFields(*pclScan, *normals, *pclScanNormal); scan = util3d::laserScanFromPointCloud(*pclScanNormal); } else @@ -1156,7 +1207,9 @@ void CoreWrapper::commonStereoCallback( if(scanCloudNormalK_ > 0) { //compute normals - pcl::PointCloud::Ptr pclScanNormal = util3d::computeNormals(pclScan, scanCloudNormalK_); + pcl::PointCloud::Ptr normals = util3d::computeNormals(pclScan, scanCloudNormalK_); + pcl::PointCloud::Ptr pclScanNormal(new pcl::PointCloud); + pcl::concatenateFields(*pclScan, *normals, *pclScanNormal); scan = util3d::laserScanFromPointCloud(*pclScanNormal); } else @@ -1440,6 +1493,8 @@ void CoreWrapper::process( if(rtabmap_.isIDsGenerated() || data.id() > 0) { double timeRtabmap = 0.0; + double timeUpdateMaps = 0.0; + double timePublishMaps = 0.0; if(rtabmap_.process(data, odom, OdometryEvent::generateCovarianceMatrix(odomRotationalVariance, odomTransitionalVariance))) { timeRtabmap = timer.ticks(); @@ -1473,8 +1528,11 @@ void CoreWrapper::process( false, false, false, + false, tmpSignature); + timeUpdateMaps = timer.ticks(); + mapsManager_.publishMaps(filteredPoses, stamp, mapFrameId_); // update goal if planning is enabled @@ -1545,17 +1603,20 @@ void CoreWrapper::process( } } } + + timePublishMaps = timer.ticks(); } } else { timeRtabmap = timer.ticks(); } - ROS_INFO("rtabmap: Rate=%.2fs, Limit=%.3fs, RTAB-Map=%.4fs, Pub=%.4fs (local map=%d, WM=%d)", + ROS_INFO("rtabmap: Rate=%.2fs, Limit=%.3fs, RTAB-Map=%.4fs, Maps update=%.4fs pub=%.4fs (local map=%d, WM=%d)", rate_>0?1.0f/rate_:0, rtabmap_.getTimeThreshold()/1000.0f, timeRtabmap, - timer.ticks(), + timeUpdateMaps, + timePublishMaps, (int)rtabmap_.getLocalOptimizedPoses().size(), rtabmap_.getWMSize()+rtabmap_.getSTMSize()); } @@ -1934,6 +1995,7 @@ bool CoreWrapper::getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs:: false, true, false, + false, false); if(filteredPoses.size()) { @@ -1978,6 +2040,7 @@ bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs:: false, false, true, + false, false); if(filteredPoses.size()) { @@ -2091,6 +2154,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab false, false, false, + false, signatures); } else @@ -2545,7 +2609,8 @@ void CoreWrapper::publishGlobalPath(const ros::Time & stamp) } } -#ifdef WITH_OCTOMAP +#ifdef WITH_OCTOMAP_ROS +#ifdef RTABMAP_OCTOMAP bool CoreWrapper::octomapBinaryCallback( octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res) @@ -2555,14 +2620,10 @@ bool CoreWrapper::octomapBinaryCallback( res.map.header.stamp = ros::Time::now(); std::map poses = rtabmap_.getLocalOptimizedPoses(); - poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false, false, false); + poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, false, false, false, true); - octomap::OcTree * octree = mapsManager_.createOctomap(poses); - bool success = octree != 0 && octree->size() && octomap_msgs::binaryMapToMsg(*octree, res.map); - if(octree) - { - delete octree; - } + const rtabmap::OctoMap * octomap = mapsManager_.getOctomap(); + bool success = octomap->octree()->size() && octomap_msgs::binaryMapToMsg(*octomap->octree(), res.map); return success; } @@ -2575,17 +2636,14 @@ bool CoreWrapper::octomapFullCallback( res.map.header.stamp = ros::Time::now(); std::map poses = rtabmap_.getLocalOptimizedPoses(); - poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false, false, false); + poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, false, false, false, true); - octomap::OcTree * octree = mapsManager_.createOctomap(poses); - bool success = octree != 0 && octree->size() && octomap_msgs::fullMapToMsg(*octree, res.map); - if(octree) - { - delete octree; - } + const rtabmap::OctoMap * octomap = mapsManager_.getOctomap(); + bool success = octomap->octree()->size() && octomap_msgs::fullMapToMsg(*octomap->octree(), res.map); return success; } #endif +#endif /** * exclusive callbacks: @@ -2604,7 +2662,7 @@ void CoreWrapper::setupCallbacks( bool subscribeScan3d, bool subscribeStereo, int queueSize, - bool stereoApproxSync, + bool approxSync, int depthCameras) { ros::NodeHandle nh; // public @@ -2649,7 +2707,6 @@ void CoreWrapper::setupCallbacks( odomSub_.subscribe(nh, "odom", 1); if(subscribeScan2d) { - ROS_INFO("Registering Depth+LaserScan callback..."); scanSub_.subscribe(nh, "scan", 1); depthScanSync_ = new message_filters::Synchronizer( MyDepthScanSyncPolicy(queueSize), @@ -2670,7 +2727,6 @@ void CoreWrapper::setupCallbacks( } else if(subscribeScan3d) { - ROS_INFO("Registering Depth+LaserScan3d callback..."); scan3dSub_.subscribe(nh, "scan_cloud", 1); depthScan3dSync_ = new message_filters::Synchronizer( MyDepthScan3dSyncPolicy(queueSize), @@ -2693,7 +2749,6 @@ void CoreWrapper::setupCallbacks( { if(depthCameras > 1) { - ROS_INFO("Registering Depth2 callback..."); depth2Sync_ = new message_filters::Synchronizer( MyDepth2SyncPolicy(queueSize), odomSub_, @@ -2717,17 +2772,29 @@ void CoreWrapper::setupCallbacks( } else { - ROS_INFO("Registering Depth callback..."); - depthSync_ = new message_filters::Synchronizer( - MyDepthSyncPolicy(queueSize), - *imageSubs_[0], - odomSub_, - *imageDepthSubs_[0], - *cameraInfoSubs_[0]); - depthSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4)); - - ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s", + if(approxSync) + { + depthSync_ = new message_filters::Synchronizer( + MyDepthSyncPolicy(queueSize), + *imageSubs_[0], + odomSub_, + *imageDepthSubs_[0], + *cameraInfoSubs_[0]); + depthSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4)); + } + else + { + depthExactSync_ = new message_filters::Synchronizer( + MyDepthExactSyncPolicy(queueSize), + *imageSubs_[0], + odomSub_, + *imageDepthSubs_[0], + *cameraInfoSubs_[0]); + depthExactSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4)); + } + ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s", ros::this_node::getName().c_str(), + approxSync?"approx":"exact", imageSubs_[0]->getTopic().c_str(), imageDepthSubs_[0]->getTopic().c_str(), cameraInfoSubs_[0]->getTopic().c_str(), @@ -2776,15 +2843,27 @@ void CoreWrapper::setupCallbacks( } else //!subscribeLaserScan { - depthTFSync_ = new message_filters::Synchronizer( - MyDepthTFSyncPolicy(queueSize), - *imageSubs_[0], - *imageDepthSubs_[0], - *cameraInfoSubs_[0]); - depthTFSync_->registerCallback(boost::bind(&CoreWrapper::depthTFCallback, this, _1, _2, _3)); - - ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s", + if(approxSync) + { + depthTFSync_ = new message_filters::Synchronizer( + MyDepthTFSyncPolicy(queueSize), + *imageSubs_[0], + *imageDepthSubs_[0], + *cameraInfoSubs_[0]); + depthTFSync_->registerCallback(boost::bind(&CoreWrapper::depthTFCallback, this, _1, _2, _3)); + } + else + { + depthTFExactSync_ = new message_filters::Synchronizer( + MyDepthTFExactSyncPolicy(queueSize), + *imageSubs_[0], + *imageDepthSubs_[0], + *cameraInfoSubs_[0]); + depthTFExactSync_->registerCallback(boost::bind(&CoreWrapper::depthTFCallback, this, _1, _2, _3)); + } + ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s", ros::this_node::getName().c_str(), + approxSync?"approx":"exact", imageSubs_[0]->getTopic().c_str(), imageDepthSubs_[0]->getTopic().c_str(), cameraInfoSubs_[0]->getTopic().c_str()); @@ -2856,9 +2935,8 @@ void CoreWrapper::setupCallbacks( } else //!subscribeLaserScan { - if(stereoApproxSync) + if(approxSync) { - ROS_INFO("Registering Stereo Approx callback..."); stereoApproxSync_ = new message_filters::Synchronizer( MyStereoApproxSyncPolicy(queueSize), imageRectLeft_, @@ -2870,7 +2948,6 @@ void CoreWrapper::setupCallbacks( } else { - ROS_INFO("Registering Stereo Exact callback..."); stereoExactSync_ = new message_filters::Synchronizer( MyStereoExactSyncPolicy(queueSize), imageRectLeft_, @@ -2881,8 +2958,9 @@ void CoreWrapper::setupCallbacks( stereoExactSync_->registerCallback(boost::bind(&CoreWrapper::stereoCallback, this, _1, _2, _3, _4, _5)); } - ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s", + ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s,\n %s", ros::this_node::getName().c_str(), + approxSync?"approx":"exact", imageRectLeft_.getTopic().c_str(), imageRectRight_.getTopic().c_str(), cameraInfoLeft_.getTopic().c_str(), @@ -2895,7 +2973,6 @@ void CoreWrapper::setupCallbacks( // use odom from TF, so subscribe to sensors only if(subscribeScan2d) { - ROS_INFO("Registering Stereo+LaserScan2d+OdomTF callback..."); scanSub_.subscribe(nh, "scan", 1); stereoScanTFSync_ = new message_filters::Synchronizer( MyStereoScanTFSyncPolicy(queueSize), @@ -2916,7 +2993,6 @@ void CoreWrapper::setupCallbacks( } else if(subscribeScan3d) { - ROS_INFO("Registering Stereo+LaserScan3d+OdomTF callback..."); scan3dSub_.subscribe(nh, "scan_cloud", 1); stereoScan3dTFSync_ = new message_filters::Synchronizer( MyStereoScan3dTFSyncPolicy(queueSize), @@ -2937,9 +3013,8 @@ void CoreWrapper::setupCallbacks( } else //!subscribeLaserScan { - if(stereoApproxSync) + if(approxSync) { - ROS_INFO("Registering Stereo+OdomTF Approx callback..."); stereoApproxTFSync_ = new message_filters::Synchronizer( MyStereoApproxTFSyncPolicy(queueSize), imageRectLeft_, @@ -2950,7 +3025,6 @@ void CoreWrapper::setupCallbacks( } else { - ROS_INFO("Registering Stereo+OdomTF Exact callback..."); stereoExactTFSync_ = new message_filters::Synchronizer( MyStereoExactTFSyncPolicy(queueSize), imageRectLeft_, @@ -2960,8 +3034,9 @@ void CoreWrapper::setupCallbacks( stereoExactTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoTFCallback, this, _1, _2, _3, _4)); } - ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s", + ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s", ros::this_node::getName().c_str(), + approxSync?"approx":"exact", imageRectLeft_.getTopic().c_str(), imageRectRight_.getTopic().c_str(), cameraInfoLeft_.getTopic().c_str(), diff --git a/src/CoreWrapper.h b/src/CoreWrapper.h index 788880b8..e9815ab5 100644 --- a/src/CoreWrapper.h +++ b/src/CoreWrapper.h @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without @@ -65,7 +65,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#ifdef WITH_OCTOMAP +#ifdef WITH_OCTOMAP_ROS #include #endif @@ -90,7 +90,7 @@ private: bool subscribeScan3d, bool subscribeStereo, int queueSize, - bool stereoApproxSync, + bool approxSync, int depthCameras); void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom @@ -234,7 +234,7 @@ private: bool cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); bool setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res); bool listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtabmap_ros::ListLabels::Response& res); -#ifdef WITH_OCTOMAP +#ifdef WITH_OCTOMAP_ROS bool octomapBinaryCallback(octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res); bool octomapFullCallback(octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res); #endif @@ -242,7 +242,7 @@ private: rtabmap::ParametersMap loadParameters(const std::string & configFile); void saveParameters(const std::string & configFile); - void publishLoop(double tfDelay); + void publishLoop(double tfDelay, double tfTolerance); void publishStats(const ros::Time & stamp); void publishCurrentGoal(const ros::Time & stamp); @@ -338,6 +338,12 @@ private: sensor_msgs::Image, sensor_msgs::CameraInfo> MyDepthSyncPolicy; message_filters::Synchronizer * depthSync_; + typedef message_filters::sync_policies::ExactTime< + sensor_msgs::Image, + nav_msgs::Odometry, + sensor_msgs::Image, + sensor_msgs::CameraInfo> MyDepthExactSyncPolicy; + message_filters::Synchronizer * depthExactSync_; typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, @@ -403,6 +409,11 @@ private: sensor_msgs::Image, sensor_msgs::CameraInfo> MyDepthTFSyncPolicy; message_filters::Synchronizer * depthTFSync_; + typedef message_filters::sync_policies::ExactTime< + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo> MyDepthTFExactSyncPolicy; + message_filters::Synchronizer * depthTFExactSync_; typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, @@ -457,7 +468,7 @@ private: ros::ServiceServer cancelGoalSrv_; ros::ServiceServer setLabelSrv_; ros::ServiceServer listLabelsSrv_; -#ifdef WITH_OCTOMAP +#ifdef WITH_OCTOMAP_ROS ros::ServiceServer octomapBinarySrv_; ros::ServiceServer octomapFullSrv_; #endif diff --git a/src/DbPlayerNode.cpp b/src/DbPlayerNode.cpp index 6bbf2e56..a8bbeaf1 100644 --- a/src/DbPlayerNode.cpp +++ b/src/DbPlayerNode.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without @@ -41,8 +41,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include +#include +#include #include #include +#include #include bool paused = false; @@ -118,19 +122,24 @@ int main(int argc, char** argv) ROS_INFO("odom_frame_id = %s", odomFrameId.c_str()); ROS_INFO("camera_frame_id = %s", cameraFrameId.c_str()); ROS_INFO("scan_frame_id = %s", scanFrameId.c_str()); - ROS_INFO("database = %s", databasePath.c_str()); ROS_INFO("rate = %f", rate); ROS_INFO("publish_tf = %s", publishTf?"true":"false"); - - rtabmap::DBReader reader(databasePath, rate); + ROS_INFO("start_id = %d", startId); if(databasePath.empty()) { ROS_ERROR("Parameter \"database\" must be set (path to a RTAB-Map database)."); return -1; } + databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir()); + if(databasePath.size() && databasePath.at(0) != '/') + { + databasePath = UDirectory::currentDir(true) + databasePath; + } + ROS_INFO("database = %s", databasePath.c_str()); - if(!reader.init(startId)) + rtabmap::DBReader reader(databasePath, rate, false, false, false, startId); + if(!reader.init()) { ROS_ERROR("Cannot open database \"%s\".", databasePath.c_str()); return -1; @@ -154,7 +163,9 @@ int main(int argc, char** argv) tf2_ros::TransformBroadcaster tfBroadcaster; UTimer timer; - rtabmap::OdometryEvent odom = reader.getNextData(); + rtabmap::CameraInfo info; + rtabmap::SensorData data = reader.takeImage(&info); + rtabmap::OdometryEvent odom(data, info.odomPose, info.odomCovariance); double acquisitionTime = timer.ticks(); while(ros::ok() && odom.data().id()) { @@ -479,7 +490,9 @@ int main(int argc, char** argv) } timer.restart(); - odom = reader.getNextData(); + info = rtabmap::CameraInfo(); + data = reader.takeImage(&info); + odom = rtabmap::OdometryEvent(data, info.odomPose, info.odomCovariance); acquisitionTime = timer.ticks(); } diff --git a/src/GuiNode.cpp b/src/GuiNode.cpp index c506e314..4190ca3a 100644 --- a/src/GuiNode.cpp +++ b/src/GuiNode.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp index 952de56c..5682ae71 100644 --- a/src/GuiWrapper.cpp +++ b/src/GuiWrapper.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without @@ -529,70 +529,74 @@ void GuiWrapper::commonDepthCallback( const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) { + UASSERT(imageMsgs.size()>0 && + imageMsgs.size() == depthMsgs.size() && + imageMsgs.size() == cameraInfoMsgs.size()); + + std_msgs::Header odomHeader; + if(odomMsg.get()) + { + odomHeader = odomMsg->header; + } + else + { + if(scan2dMsg.get()) + { + odomHeader = scan2dMsg->header; + } + else if(scan3dMsg.get()) + { + odomHeader = scan3dMsg->header; + } + else if(cameraInfoMsgs.size() && cameraInfoMsgs[0].get()) + { + odomHeader = cameraInfoMsgs[0]->header; + } + else if(depthMsgs.size() && depthMsgs[0].get()) + { + odomHeader = depthMsgs[0]->header; + } + else if(imageMsgs.size() && imageMsgs[0].get()) + { + odomHeader = imageMsgs[0]->header; + } + odomHeader.frame_id = odomFrameId_; + } + + Transform odomT = getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp); + cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); + if(odomMsg.get()) + { + UASSERT(odomMsg->pose.covariance.size() == 36); + if(!(odomMsg->pose.covariance[0] == 0 && + odomMsg->pose.covariance[7] == 0 && + odomMsg->pose.covariance[14] == 0 && + odomMsg->pose.covariance[21] == 0 && + odomMsg->pose.covariance[28] == 0 && + odomMsg->pose.covariance[35] == 0)) + { + covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->pose.covariance.data()).clone(); + } + } + if(odomHeader.frame_id.empty()) + { + ROS_ERROR("Odometry frame not set!?"); + return; + } + + cv::Mat rgb; + cv::Mat depth; + std::vector cameraModels; + cv::Mat scan; + rtabmap::OdometryInfo info; + bool ignoreData = false; + if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 && !mainWindow_->isProcessingOdometry() && !mainWindow_->isProcessingStatistics()) { lastOdomInfoUpdateTime_ = UTimer::now(); - UASSERT(imageMsgs.size()>0 && - imageMsgs.size() == depthMsgs.size() && - imageMsgs.size() == cameraInfoMsgs.size()); - - std_msgs::Header odomHeader; - if(odomMsg.get()) - { - odomHeader = odomMsg->header; - } - else - { - if(scan2dMsg.get()) - { - odomHeader = scan2dMsg->header; - } - else if(scan3dMsg.get()) - { - odomHeader = scan3dMsg->header; - } - else if(cameraInfoMsgs.size() && cameraInfoMsgs[0].get()) - { - odomHeader = cameraInfoMsgs[0]->header; - } - else if(depthMsgs.size() && depthMsgs[0].get()) - { - odomHeader = depthMsgs[0]->header; - } - else if(imageMsgs.size() && imageMsgs[0].get()) - { - odomHeader = imageMsgs[0]->header; - } - odomHeader.frame_id = odomFrameId_; - } - - Transform odomT = getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp); - cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); - if(odomMsg.get()) - { - UASSERT(odomMsg->pose.covariance.size() == 36); - if(!(odomMsg->pose.covariance[0] == 0 && - odomMsg->pose.covariance[7] == 0 && - odomMsg->pose.covariance[14] == 0 && - odomMsg->pose.covariance[21] == 0 && - odomMsg->pose.covariance[28] == 0 && - odomMsg->pose.covariance[35] == 0)) - { - covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->pose.covariance.data()).clone(); - } - } - if(odomHeader.frame_id.empty()) - { - ROS_ERROR("Odometry frame not set!?"); - return; - } - - cv::Mat rgb; - cv::Mat depth; - std::vector cameraModels; if(imageMsgs[0].get() && depthMsgs[0].get()) { int imageWidth = imageMsgs[0]->width; @@ -686,7 +690,6 @@ void GuiWrapper::commonDepthCallback( } } - cv::Mat scan; if(scan2dMsg.get() != 0) { // make sure the frame of the laser is updated too @@ -726,28 +729,38 @@ void GuiWrapper::commonDepthCallback( scan = util3d::laserScanFromPointCloud(*pclScan); } - rtabmap::OdometryInfo info; if(odomInfoMsg.get()) { info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg); } - - rtabmap::OdometryEvent odomEvent( - rtabmap::SensorData( - scan, - scan2dMsg.get()?(int)scan2dMsg->ranges.size():0, - scan2dMsg.get()?(int)scan2dMsg->range_max:0, - rgb, - depth, - cameraModels, - odomHeader.seq, - rtabmap_ros::timestampFromROS(odomHeader.stamp)), - odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT, - covariance, - info); - - QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent)); + ignoreData = false; } + else if(odomInfoMsg.get()) + { + info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg).copyWithoutData(); + ignoreData = true; + } + else + { + // don't update GUI odom stuff if we don't use visual odometry + return; + } + + rtabmap::OdometryEvent odomEvent( + rtabmap::SensorData( + scan, + scan2dMsg.get()?(int)scan2dMsg->ranges.size():0, + scan2dMsg.get()?(int)scan2dMsg->range_max:0, + rgb, + depth, + cameraModels, + odomHeader.seq, + rtabmap_ros::timestampFromROS(odomHeader.stamp)), + odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT, + covariance, + info); + + QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData)); } void GuiWrapper::commonStereoCallback( @@ -760,6 +773,91 @@ void GuiWrapper::commonStereoCallback( const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) { + UASSERT(leftImageMsg.get() && rightImageMsg.get()); + UASSERT(leftCamInfoMsg.get() && rightCamInfoMsg.get()); + + if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 || + leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || + leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || + !(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 || + rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || + rightImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)) + { + ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8"); + return; + } + + std_msgs::Header odomHeader; + if(odomMsg.get()) + { + odomHeader = odomMsg->header; + } + else + { + if(scan2dMsg.get()) + { + odomHeader = scan2dMsg->header; + } + else if(scan3dMsg.get()) + { + odomHeader = scan3dMsg->header; + } + else + { + odomHeader = leftCamInfoMsg->header; + } + odomHeader.frame_id = odomFrameId_; + } + + Transform odomT = getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp); + cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); + if(odomMsg.get()) + { + UASSERT(odomMsg->pose.covariance.size() == 36); + if(!(odomMsg->pose.covariance[0] == 0 && + odomMsg->pose.covariance[7] == 0 && + odomMsg->pose.covariance[14] == 0 && + odomMsg->pose.covariance[21] == 0 && + odomMsg->pose.covariance[28] == 0 && + odomMsg->pose.covariance[35] == 0)) + { + covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->pose.covariance.data()).clone(); + } + } + if(odomHeader.frame_id.empty()) + { + ROS_ERROR("Odometry frame not set!?"); + return; + } + + Transform localTransform = getTransform(frameId_, leftCamInfoMsg->header.frame_id, leftCamInfoMsg->header.stamp); + if(localTransform.isNull()) + { + return; + } + // sync with odometry stamp + if(odomHeader.stamp != leftCamInfoMsg->header.stamp) + { + if(!odomT.isNull()) + { + Transform sensorT = getTransform(odomHeader.frame_id, frameId_, leftCamInfoMsg->header.stamp); + if(sensorT.isNull()) + { + return; + } + localTransform = odomT.inverse() * sensorT * localTransform; + } + } + + cv::Mat left; + cv::Mat right; + cv::Mat scan; + rtabmap::StereoCameraModel stereoModel; + rtabmap::OdometryInfo info; + bool ignoreData = false; + // limit 10 Hz max if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 && !mainWindow_->isProcessingOdometry() && @@ -767,85 +865,7 @@ void GuiWrapper::commonStereoCallback( { lastOdomInfoUpdateTime_ = UTimer::now(); - UASSERT(leftImageMsg.get() && rightImageMsg.get()); - UASSERT(leftCamInfoMsg.get() && rightCamInfoMsg.get()); - - if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 || - leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || - !(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 || - rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - rightImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)) - { - ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8"); - return; - } - - std_msgs::Header odomHeader; - if(odomMsg.get()) - { - odomHeader = odomMsg->header; - } - else - { - if(scan2dMsg.get()) - { - odomHeader = scan2dMsg->header; - } - else if(scan3dMsg.get()) - { - odomHeader = scan3dMsg->header; - } - else - { - odomHeader = leftCamInfoMsg->header; - } - odomHeader.frame_id = odomFrameId_; - } - - Transform odomT = getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp); - cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); - if(odomMsg.get()) - { - UASSERT(odomMsg->pose.covariance.size() == 36); - if(!(odomMsg->pose.covariance[0] == 0 && - odomMsg->pose.covariance[7] == 0 && - odomMsg->pose.covariance[14] == 0 && - odomMsg->pose.covariance[21] == 0 && - odomMsg->pose.covariance[28] == 0 && - odomMsg->pose.covariance[35] == 0)) - { - covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->pose.covariance.data()).clone(); - } - } - if(odomHeader.frame_id.empty()) - { - ROS_ERROR("Odometry frame not set!?"); - return; - } - - Transform localTransform = getTransform(frameId_, leftCamInfoMsg->header.frame_id, leftCamInfoMsg->header.stamp); - if(localTransform.isNull()) - { - return; - } - // sync with odometry stamp - if(odomHeader.stamp != leftCamInfoMsg->header.stamp) - { - if(!odomT.isNull()) - { - Transform sensorT = getTransform(odomHeader.frame_id, frameId_, leftCamInfoMsg->header.stamp); - if(sensorT.isNull()) - { - return; - } - localTransform = odomT.inverse() * sensorT * localTransform; - } - } - - rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform); + stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform); if(stereoModel.baseline() > 10.0) { @@ -862,7 +882,6 @@ void GuiWrapper::commonStereoCallback( // left cv_bridge::CvImageConstPtr ptrImage; - cv::Mat left; if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) { @@ -874,9 +893,8 @@ void GuiWrapper::commonStereoCallback( } // right - cv::Mat right = cv_bridge::toCvCopy(rightImageMsg, "mono8")->image; + right = cv_bridge::toCvCopy(rightImageMsg, "mono8")->image; - cv::Mat scan; if(scan2dMsg.get() != 0) { // make sure the frame of the laser is updated too @@ -916,28 +934,38 @@ void GuiWrapper::commonStereoCallback( scan = util3d::laserScanFromPointCloud(*pclScan); } - rtabmap::OdometryInfo info; if(odomInfoMsg.get()) { info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg); } - - rtabmap::OdometryEvent odomEvent( - rtabmap::SensorData( - scan, - scan2dMsg.get()?(int)scan2dMsg->ranges.size():0, - scan2dMsg.get()?(int)scan2dMsg->range_max:0, - left, - right, - stereoModel, - odomHeader.seq, - rtabmap_ros::timestampFromROS(odomHeader.stamp)), - odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT, - covariance, - info); - - QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent)); + ignoreData = false; } + else if(odomInfoMsg.get()) + { + info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg).copyWithoutData(); + ignoreData = true; + } + else + { + // don't update GUI odom stuff if we don't use visual odometry + return; + } + + rtabmap::OdometryEvent odomEvent( + rtabmap::SensorData( + scan, + scan2dMsg.get()?(int)scan2dMsg->ranges.size():0, + scan2dMsg.get()?(int)scan2dMsg->range_max:0, + left, + right, + stereoModel, + odomHeader.seq, + rtabmap_ros::timestampFromROS(odomHeader.stamp)), + odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT, + covariance, + info); + + QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData)); } // With odom msg diff --git a/src/GuiWrapper.h b/src/GuiWrapper.h index 6f39d2d0..b9ba729f 100644 --- a/src/GuiWrapper.h +++ b/src/GuiWrapper.h @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/src/MapAssemblerNode.cpp b/src/MapAssemblerNode.cpp index b6c2ca20..a8055d53 100644 --- a/src/MapAssemblerNode.cpp +++ b/src/MapAssemblerNode.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without @@ -101,6 +101,7 @@ public: false, false, false, + false, nodes_); mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id); diff --git a/src/MapOptimizerNode.cpp b/src/MapOptimizerNode.cpp index ae18bfbd..64e5522a 100644 --- a/src/MapOptimizerNode.cpp +++ b/src/MapOptimizerNode.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp index 7ef71c98..bfaf1977 100644 --- a/src/MapsManager.cpp +++ b/src/MapsManager.cpp @@ -1,9 +1,29 @@ /* - * MapsManager.cpp - * - * Created on: 2015-05-14 - * Author: mathieu - */ +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ #include "MapsManager.h" @@ -17,14 +37,18 @@ #include #include #include +#include #include #include #include -#ifdef WITH_OCTOMAP -#include +#ifdef WITH_OCTOMAP_ROS +#ifdef RTABMAP_OCTOMAP +#include +#include +#endif #endif using namespace rtabmap; @@ -48,6 +72,7 @@ MapsManager::MapsManager(bool usePublicNamespace) : projMaxObstaclesHeight_(2.0), // meters (<=0 disabled) projMaxGroundHeight_(0.0), // meters (<=0 disabled, only works if proj_detect_flat_obstacles is true) projDetectFlatObstacles_(false), + projMapFrame_(false), gridCellSize_(0.05), // meters gridSize_(0), // meters gridEroded_(false), @@ -56,7 +81,10 @@ MapsManager::MapsManager(bool usePublicNamespace) : mapFilterRadius_(0.0), mapFilterAngle_(30.0), // degrees mapCacheCleanup_(true), - negativePosesIgnored(false) + negativePosesIgnored_(false), + octomap_(0), + octomapTreeDepth_(16), + octomapGroundIsObstacle_(false) { ros::NodeHandle nh; @@ -102,6 +130,7 @@ MapsManager::MapsManager(bool usePublicNamespace) : } pnh.param("proj_max_ground_height", projMaxGroundHeight_, projMaxGroundHeight_); pnh.param("proj_detect_flat_obstacles", projDetectFlatObstacles_, projDetectFlatObstacles_); + pnh.param("proj_map_frame", projMapFrame_, projMapFrame_); // common grid map stuff pnh.param("grid_cell_size", gridCellSize_, gridCellSize_); // m @@ -118,7 +147,25 @@ MapsManager::MapsManager(bool usePublicNamespace) : pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_); pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_); pnh.param("map_cleanup", mapCacheCleanup_, mapCacheCleanup_); - pnh.param("map_negative_poses_ignored", negativePosesIgnored, negativePosesIgnored); + pnh.param("map_negative_poses_ignored", negativePosesIgnored_, negativePosesIgnored_); + +#ifdef WITH_OCTOMAP_ROS +#ifdef RTABMAP_OCTOMAP + octomap_ = new OctoMap(gridCellSize_); + pnh.param("octomap_tree_depth", octomapTreeDepth_, octomapTreeDepth_); + if(octomapTreeDepth_ > 16) + { + ROS_WARN("octomap_tree_depth maximum is 16"); + octomapTreeDepth_ = 16; + } + else if(octomapTreeDepth_ < 0) + { + ROS_WARN("octomap_tree_depth cannot be negative, set to 16 instead"); + octomapTreeDepth_ = 16; + } + pnh.param("octomap_ground_is_obstacle", octomapGroundIsObstacle_, octomapGroundIsObstacle_); +#endif +#endif // If true, the last message published on // the map topics will be saved and sent to new subscribers when they @@ -133,6 +180,15 @@ MapsManager::MapsManager(bool usePublicNamespace) : projMapPub_ = nh.advertise("proj_map", 1, latch); gridMapPub_ = nh.advertise("grid_map", 1, latch); scanMapPub_ = nh.advertise("scan_map", 1, latch); +#ifdef WITH_OCTOMAP_ROS +#ifdef RTABMAP_OCTOMAP + octoMapPubBin_ = nh.advertise("octomap_binary", 1, latch); + octoMapPubFull_ = nh.advertise("octomap_full", 1, latch); + octoMapCloud_ = nh.advertise("octomap_cloud", 1, latch); + octoMapEmptySpace_ = nh.advertise("octomap_empty_space", 1, latch); + octoMapProj_ = nh.advertise("octomap_proj", 1, latch); +#endif +#endif } else { @@ -140,11 +196,30 @@ MapsManager::MapsManager(bool usePublicNamespace) : projMapPub_ = pnh.advertise("proj_map", 1, latch); gridMapPub_ = pnh.advertise("grid_map", 1, latch); scanMapPub_ = pnh.advertise("scan_map", 1, latch); +#ifdef WITH_OCTOMAP_ROS +#ifdef RTABMAP_OCTOMAP + octoMapPubBin_ = pnh.advertise("octomap_binary", 1, latch); + octoMapPubFull_ = pnh.advertise("octomap_full", 1, latch); + octoMapCloud_ = pnh.advertise("octomap_cloud", 1, latch); + octoMapEmptySpace_ = pnh.advertise("octomap_cloud_ground", 1, latch); + octoMapProj_ = pnh.advertise("octomap_proj", 1, latch); +#endif +#endif } } MapsManager::~MapsManager() { clear(); + +#ifdef WITH_OCTOMAP_ROS +#ifdef RTABMAP_OCTOMAP + if(octomap_) + { + delete octomap_; + octomap_ = 0; + } +#endif +#endif } void MapsManager::clear() @@ -153,6 +228,11 @@ void MapsManager::clear() cameraModels_.clear(); projMaps_.clear(); gridMaps_.clear(); +#ifdef WITH_OCTOMAP_ROS +#ifdef RTABMAP_OCTOMAP + octomap_->clear(); +#endif +#endif } bool MapsManager::hasSubscribers() const @@ -160,7 +240,12 @@ bool MapsManager::hasSubscribers() const return cloudMapPub_.getNumSubscribers() != 0 || projMapPub_.getNumSubscribers() != 0 || gridMapPub_.getNumSubscribers() != 0 || - scanMapPub_.getNumSubscribers() != 0; + scanMapPub_.getNumSubscribers() != 0 || + octoMapPubBin_.getNumSubscribers() != 0 || + octoMapPubFull_.getNumSubscribers() != 0 || + octoMapCloud_.getNumSubscribers() != 0 || + octoMapEmptySpace_.getNumSubscribers() != 0 || + octoMapProj_.getNumSubscribers() != 0; } std::map MapsManager::getFilteredPoses(const std::map & poses) @@ -181,6 +266,7 @@ std::map MapsManager::updateMapCaches( bool updateProj, bool updateGrid, bool updateScan, + bool updateOctomap, const std::map & signatures) { if(!updateCloud && !updateProj && !updateGrid && !updateScan) @@ -190,8 +276,22 @@ std::map MapsManager::updateMapCaches( updateProj = projMapPub_.getNumSubscribers() != 0; updateGrid = gridMapPub_.getNumSubscribers() != 0; updateScan = scanMapPub_.getNumSubscribers() != 0; + updateOctomap = + octoMapPubBin_.getNumSubscribers() != 0 || + octoMapPubFull_.getNumSubscribers() != 0 || + octoMapCloud_.getNumSubscribers() != 0 || + octoMapEmptySpace_.getNumSubscribers() != 0 || + octoMapProj_.getNumSubscribers() != 0; } +#ifndef WITH_OCTOMAP_ROS + updateOctomap = false; +#endif +#ifndef RTABMAP_OCTOMAP + updateOctomap = false; +#endif + + UDEBUG("Updating map caches..."); if(!memory && signatures.size() == 0) @@ -203,7 +303,7 @@ std::map MapsManager::updateMapCaches( std::map filteredPoses; // update cache - if(updateCloud || updateProj || updateGrid || updateScan) + if(updateCloud || updateProj || updateGrid || updateScan || updateOctomap) { // filter nodes if(mapFilterRadius_ > 0.0) @@ -229,7 +329,7 @@ std::map MapsManager::updateMapCaches( filteredPoses = poses; } - if(negativePosesIgnored) + if(negativePosesIgnored_) { for(std::map::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end();) { @@ -244,6 +344,31 @@ std::map MapsManager::updateMapCaches( } } + bool longUpdate = false; + if(filteredPoses.size() > 20) + { + if(updateCloud && clouds_.size() < 5) + { + ROS_WARN("Many clouds should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-clouds_.size())); + longUpdate = true; + } + else if(updateProj && projMaps_.size() < 5) + { + ROS_WARN("Many occupancy grid map from projections should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-projMaps_.size())); + longUpdate = true; + } + else if(updateGrid && gridMaps_.size() < 5) + { + ROS_WARN("Many occupancy grid map from laser scans should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-gridMaps_.size())); + longUpdate = true; + } + else if(updateScan && scans_.size() < 5) + { + ROS_WARN("Many scans should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-scans_.size())); + longUpdate = true; + } + } + for(std::map::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter) { if(!iter->second.isNull()) @@ -254,6 +379,18 @@ std::map MapsManager::updateMapCaches( bool gridRequired = updateGrid && (iter->first < 0 || !uContains(gridMaps_, iter->first)); bool scanRequired = updateScan && (iter->first < 0 || !uContains(scans_, iter->first)); +#ifdef WITH_OCTOMAP_ROS +#ifdef RTABMAP_OCTOMAP + if(!rgbDepthRequired) + { + rgbDepthRequired = updateOctomap && + (iter->first < 0 || + octomap_->addedNodes().empty() || + iter->first > octomap_->addedNodes().rbegin()->first); + } +#endif +#endif + if(rgbDepthRequired || depthRequired || scanRequired || @@ -373,9 +510,9 @@ std::map MapsManager::updateMapCaches( uInsert(cameraModels_, std::make_pair(iter->first, models)); } - if(depthRequired) + if(depthRequired || updateOctomap) { - UDEBUG("Creating proj map for %d...", iter->first); + UDEBUG("Creating proj map / octomap for %d...", iter->first); cv::Mat ground, obstacles; if(cloudRGB.get()) { @@ -393,12 +530,63 @@ std::map MapsManager::updateMapCaches( // add pose rotation without yaw float roll, pitch, yaw; iter->second.getEulerAngles(roll, pitch, yaw); - cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,0, roll, pitch, 0)); + cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,projMapFrame_?iter->second.z():0, roll, pitch, 0)); - util3d::occupancy2DFromCloud3D(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_, projDetectFlatObstacles_, projMaxGroundHeight_); + pcl::IndicesPtr groundIndices, obstaclesIndices; + util3d::segmentObstaclesFromGround( + cloudClipped, + groundIndices, + obstaclesIndices, + 20, + projMaxGroundAngle_*M_PI/180.0, + gridCellSize_*2.0f, + projMinClusterSize_, + projDetectFlatObstacles_, + projMaxGroundHeight_); + + pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); + pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); + + if(groundIndices->size()) + { + pcl::copyPointCloud(*cloudClipped, *groundIndices, *groundCloud); + } + + if(obstaclesIndices->size()) + { + pcl::copyPointCloud(*cloudClipped, *obstaclesIndices, *obstaclesCloud); + } + + if(updateProj) + { + util3d::occupancy2DFromGroundObstacles( + groundCloud, + obstaclesCloud, + ground, + obstacles, + gridCellSize_); + uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); + } + +#ifdef WITH_OCTOMAP_ROS +#ifdef RTABMAP_OCTOMAP + if(updateOctomap) + { + Transform tinv = Transform(0,0,projMapFrame_?iter->second.z():0, roll, pitch, 0).inverse(); + groundCloud = util3d::transformPointCloud(groundCloud, tinv); + obstaclesCloud = util3d::transformPointCloud(obstaclesCloud, tinv); + if(octomapGroundIsObstacle_) + { + *obstaclesCloud += *groundCloud; + groundCloud->clear(); + } + octomap_->addToCache(iter->first, groundCloud, obstaclesCloud); + } +#endif +#endif } } - else if(cloudXYZ.get()) + else if(updateProj && cloudXYZ.get()) { pcl::PointCloud::Ptr cloudClipped = cloudXYZ; if(cloudClipped->size() && projMaxObstaclesHeight_ > 0) @@ -410,13 +598,30 @@ std::map MapsManager::updateMapCaches( // add pose rotation without yaw float roll, pitch, yaw; iter->second.getEulerAngles(roll, pitch, yaw); - cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,0, roll, pitch, 0)); + cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,projMapFrame_?iter->second.z():0, roll, pitch, 0)); - UDEBUG("util3d::occupancy2DFromCloud3D()"); - util3d::occupancy2DFromCloud3D(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_, projDetectFlatObstacles_, projMaxGroundHeight_); + pcl::IndicesPtr groundIndices, obstaclesIndices; + util3d::segmentObstaclesFromGround( + cloudClipped, + groundIndices, + obstaclesIndices, + 20, + projMaxGroundAngle_*M_PI/180.0, + gridCellSize_*2.0f, + projMinClusterSize_, + projDetectFlatObstacles_, + projMaxGroundHeight_); + + util3d::occupancy2DFromGroundObstacles( + cloudClipped, + groundIndices, + obstaclesIndices, + ground, + obstacles, + gridCellSize_); + uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); } } - uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); } if(scanRequired || gridRequired) @@ -477,6 +682,17 @@ std::map MapsManager::updateMapCaches( } } +#ifdef WITH_OCTOMAP_ROS +#ifdef RTABMAP_OCTOMAP + if(updateOctomap) + { + UTimer time; + octomap_->update(filteredPoses); + ROS_INFO("Octomap update time = %fs", time.ticks()); + } +#endif +#endif + // cleanup not used nodes UDEBUG("Cleanup not used nodes"); for(std::map::Ptr >::iterator iter=clouds_.begin(); @@ -539,6 +755,11 @@ std::map MapsManager::updateMapCaches( ++iter; } } + + if(longUpdate) + { + ROS_WARN("Map(s) updated!"); + } } return filteredPoses; @@ -654,6 +875,101 @@ void MapsManager::publishMaps( clouds_.clear(); cameraModels_.clear(); } +#ifdef WITH_OCTOMAP_ROS +#ifdef RTABMAP_OCTOMAP + if(octoMapPubBin_.getNumSubscribers() || + octoMapPubFull_.getNumSubscribers() || + octoMapCloud_.getNumSubscribers() || + octoMapEmptySpace_.getNumSubscribers() || + octoMapProj_.getNumSubscribers()) + { + if(octoMapPubBin_.getNumSubscribers()) + { + octomap_msgs::Octomap msg; + octomap_msgs::binaryMapToMsg(*octomap_->octree(), msg); + msg.header.frame_id = mapFrameId; + msg.header.stamp = stamp; + octoMapPubBin_.publish(msg); + } + if(octoMapPubFull_.getNumSubscribers()) + { + octomap_msgs::Octomap msg; + octomap_msgs::fullMapToMsg(*octomap_->octree(), msg); + msg.header.frame_id = mapFrameId; + msg.header.stamp = stamp; + octoMapPubFull_.publish(msg); + } + if(octoMapCloud_.getNumSubscribers() || octoMapEmptySpace_.getNumSubscribers()) + { + sensor_msgs::PointCloud2 msg; + pcl::IndicesPtr obstacles(new std::vector); + pcl::IndicesPtr ground(new std::vector); + pcl::PointCloud::Ptr cloud = octomap_->createCloud(octomapTreeDepth_, obstacles.get(), ground.get()); + + if(octoMapCloud_.getNumSubscribers()) + { + pcl::PointCloud cloudObstacles; + pcl::copyPointCloud(*cloud, *obstacles, cloudObstacles); + pcl::toROSMsg(cloudObstacles, msg); + msg.header.frame_id = mapFrameId; + msg.header.stamp = stamp; + octoMapCloud_.publish(msg); + } + if(octoMapEmptySpace_.getNumSubscribers()) + { + pcl::PointCloud cloudGround; + pcl::copyPointCloud(*cloud, *ground, cloudGround); + pcl::toROSMsg(cloudGround, msg); + msg.header.frame_id = mapFrameId; + msg.header.stamp = stamp; + octoMapEmptySpace_.publish(msg); + } + } + if(octoMapProj_.getNumSubscribers()) + { + // create the projection map + float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; + cv::Mat pixels = octomap_->createProjectionMap(xMin, yMin, gridCellSize, gridSize_); + + if(!pixels.empty()) + { + //init + nav_msgs::OccupancyGrid map; + map.info.resolution = gridCellSize; + map.info.origin.position.x = 0.0; + map.info.origin.position.y = 0.0; + map.info.origin.position.z = 0.0; + map.info.origin.orientation.x = 0.0; + map.info.origin.orientation.y = 0.0; + map.info.origin.orientation.z = 0.0; + map.info.origin.orientation.w = 1.0; + + map.info.width = pixels.cols; + map.info.height = pixels.rows; + map.info.origin.position.x = xMin; + map.info.origin.position.y = yMin; + map.data.resize(map.info.width * map.info.height); + + memcpy(map.data.data(), pixels.data, map.info.width * map.info.height); + + map.header.frame_id = mapFrameId; + map.header.stamp = stamp; + + octoMapProj_.publish(map); + } + else if(poses.size()) + { + ROS_WARN("Projection map is empty! (proj maps=%d)", (int)projMaps_.size()); + } + } + } + else + { + octomap_->clear(); + } +#endif +#endif + if(scanMapPub_.getNumSubscribers()) { @@ -820,54 +1136,3 @@ cv::Mat MapsManager::generateGridMap( return map; } -#ifdef WITH_OCTOMAP -// returned OcTree must be deleted -// RTAB-Map optimizes the graph at almost each iteration, an octomap cannot -// be updated online. Only available on service. To have an "online" octomap published as a topic, -// you may want to subscribe an octomap_server to /rtabmap/cloud topic. -// -octomap::OcTree * MapsManager::createOctomap(const std::map & poses) -{ - octomap::OcTree * octree = new octomap::OcTree(gridCellSize_); - UTimer time; - for(std::map::const_iterator posesIter = poses.begin(); posesIter!=poses.end(); ++posesIter) - { - std::map::Ptr >::iterator cloudsIter = clouds_.find(posesIter->first); - if(cloudsIter != clouds_.end() && cloudsIter->second->size()) - { - octomap::Pointcloud * scan = new octomap::Pointcloud(); - - //octomap::pointcloudPCLToOctomap(*cloudsIter->second, *scan); // Not anymore in Indigo! - scan->reserve(cloudsIter->second->size()); - for(pcl::PointCloud::const_iterator it = cloudsIter->second->begin(); - it != cloudsIter->second->end(); - ++it) - { - // Check if the point is invalid - if(pcl::isFinite(*it)) - { - scan->push_back(it->x, it->y, it->z); - } - } - - float x,y,z, r,p,w; - posesIter->second.getTranslationAndEulerAngles(x,y,z,r,p,w); - octomap::ScanNode node(scan, octomap::pose6d(x,y,z, r,p,w), posesIter->first); - octree->insertPointCloud(node, cloudMaxDepth_, true, true); - ROS_INFO("inserted %d pt=%d (%fs)", posesIter->first, (int)scan->size(), time.ticks()); - } - } - - octree->updateInnerOccupancy(); - ROS_INFO("updated inner occupancy (%fs)", time.ticks()); - - // clear memory if no one subscribed - if(mapCacheCleanup_ && cloudMapPub_.getNumSubscribers() == 0) - { - clouds_.clear(); - cameraModels_.clear(); - } - return octree; -} -#endif - diff --git a/src/MapsManager.h b/src/MapsManager.h index 5b4adfb0..def6173c 100644 --- a/src/MapsManager.h +++ b/src/MapsManager.h @@ -1,9 +1,29 @@ /* - * MapsManager.h - * - * Created on: 2015-05-14 - * Author: mathieu - */ +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ #ifndef MAPSMANAGER_H_ #define MAPSMANAGER_H_ @@ -14,12 +34,8 @@ #include #include -namespace octomap{ -class OcTree; -} - namespace rtabmap { - +class OctoMap; class Memory; } // namespace rtabmap @@ -41,6 +57,7 @@ public: bool updateProj, bool updateGrid, bool updateScan, + bool updateOctomap, const std::map & signatures = std::map()); void publishMaps( @@ -60,9 +77,7 @@ public: float & yMin, float & gridCellSize); -#ifdef WITH_OCTOMAP - octomap::OcTree * createOctomap(const std::map & poses); -#endif + rtabmap::OctoMap * getOctomap() const {return octomap_;} private: // mapping stuff @@ -84,6 +99,7 @@ private: double projMaxObstaclesHeight_; double projMaxGroundHeight_; bool projDetectFlatObstacles_; + bool projMapFrame_; double gridCellSize_; double gridSize_; bool gridEroded_; @@ -92,18 +108,27 @@ private: double mapFilterRadius_; double mapFilterAngle_; bool mapCacheCleanup_; - bool negativePosesIgnored; + bool negativePosesIgnored_; ros::Publisher cloudMapPub_; ros::Publisher projMapPub_; ros::Publisher gridMapPub_; ros::Publisher scanMapPub_; + ros::Publisher octoMapPubBin_; + ros::Publisher octoMapPubFull_; + ros::Publisher octoMapCloud_; + ros::Publisher octoMapEmptySpace_; + ros::Publisher octoMapProj_; std::map::Ptr > clouds_; std::map::Ptr > scans_; std::map > cameraModels_; std::map > projMaps_; // std::map > gridMaps_; // + + rtabmap::OctoMap * octomap_; + int octomapTreeDepth_; + bool octomapGroundIsObstacle_; }; #endif /* MAPSMANAGER_H_ */ diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp index 463b1e23..6d1ff691 100644 --- a/src/MsgConversion.cpp +++ b/src/MsgConversion.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without @@ -147,6 +147,7 @@ void infoFromROS(const rtabmap_ros::Info & info, rtabmap::Statistics & stat) stat.setRefImageId(info.refId); stat.setLoopClosureId(info.loopClosureId); stat.setProximityDetectionId(info.proximityDetectionId); + stat.setStamp(info.header.stamp.toSec()); stat.setLoopClosureTransform(rtabmap_ros::transformFromGeometryMsg(info.loopClosureTransform)); @@ -345,19 +346,38 @@ void cameraModelToROS( const rtabmap::CameraModel & model, sensor_msgs::CameraInfo & camInfo) { - UASSERT(model.isValidForRectification()); - camInfo.D = std::vector(model.D_raw().cols); memcpy(camInfo.D.data(), model.D_raw().data, model.D_raw().cols*sizeof(double)); - UASSERT(model.K_raw().total() == 9); - memcpy(camInfo.K.elems, model.K_raw().data, 9*sizeof(double)); + UASSERT(model.K_raw().empty() || model.K_raw().total() == 9); + if(model.K_raw().empty()) + { + memset(camInfo.K.elems, 0.0, 9*sizeof(double)); + } + else + { + memcpy(camInfo.K.elems, model.K_raw().data, 9*sizeof(double)); + } - UASSERT(model.R().total() == 9); - memcpy(camInfo.R.elems, model.R().data, 9*sizeof(double)); + UASSERT(model.R().empty() || model.R().total() == 9); + if(model.R().empty()) + { + memset(camInfo.R.elems, 0.0, 9*sizeof(double)); + } + else + { + memcpy(camInfo.R.elems, model.R().data, 9*sizeof(double)); + } - UASSERT(model.P().total() == 12); - memcpy(camInfo.P.elems, model.P().data, 12*sizeof(double)); + UASSERT(model.P().empty() || model.P().total() == 12); + if(model.P().empty()) + { + memset(camInfo.P.elems, 0.0, 12*sizeof(double)); + } + else + { + memcpy(camInfo.P.elems, model.P().data, 12*sizeof(double)); + } if(camInfo.D.size() > 5) { diff --git a/src/OdomMsgToTFNode.cpp b/src/OdomMsgToTFNode.cpp index b45b14d4..1a014414 100644 --- a/src/OdomMsgToTFNode.cpp +++ b/src/OdomMsgToTFNode.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/src/PreferencesDialogROS.cpp b/src/PreferencesDialogROS.cpp index 4215bc76..dbd7957f 100644 --- a/src/PreferencesDialogROS.cpp +++ b/src/PreferencesDialogROS.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/src/PreferencesDialogROS.h b/src/PreferencesDialogROS.h index 4e73f8a8..8d2a53d7 100644 --- a/src/PreferencesDialogROS.h +++ b/src/PreferencesDialogROS.h @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/src/RGBDOdometryNode.cpp b/src/RGBDOdometryNode.cpp index c27077be..a95d4a95 100644 --- a/src/RGBDOdometryNode.cpp +++ b/src/RGBDOdometryNode.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without @@ -25,358 +25,54 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "OdometryROS.h" - -#include -#include -#include - -#include -#include - -#include - -#include -#include -#include - -#include "rtabmap_ros/MsgConversion.h" - -#include -#include +#include "ros/ros.h" +#include "nodelet/loader.h" #include +#include -using namespace rtabmap; - -class RGBDOdometry : public rtabmap_ros::OdometryROS -{ -public: - RGBDOdometry(int argc, char * argv[]) : - rtabmap_ros::OdometryROS(argc, argv), - sync_(0), - sync2_(0), - queueSize_(5) - { - ros::NodeHandle nh; - ros::NodeHandle pnh("~"); - - int depthCameras = 1; - pnh.param("queue_size", queueSize_, queueSize_); - pnh.param("depth_cameras", depthCameras, depthCameras); - if(depthCameras <= 0) - { - depthCameras = 1; - } - if(depthCameras > 2) - { - ROS_FATAL("Only 2 cameras maximum supported yet."); - } - - if(depthCameras == 2) - { - ros::NodeHandle rgb0_nh(nh, "rgb0"); - ros::NodeHandle depth0_nh(nh, "depth0"); - ros::NodeHandle rgb0_pnh(pnh, "rgb0"); - ros::NodeHandle depth0_pnh(pnh, "depth0"); - image_transport::ImageTransport rgb0_it(rgb0_nh); - image_transport::ImageTransport depth0_it(depth0_nh); - image_transport::TransportHints hintsRgb0("raw", ros::TransportHints(), rgb0_pnh); - image_transport::TransportHints hintsDepth0("raw", ros::TransportHints(), depth0_pnh); - - image_mono_sub_.subscribe(rgb0_it, rgb0_nh.resolveName("image"), 1, hintsRgb0); - image_depth_sub_.subscribe(depth0_it, depth0_nh.resolveName("image"), 1, hintsDepth0); - info_sub_.subscribe(rgb0_nh, "camera_info", 1); - - ros::NodeHandle rgb1_nh(nh, "rgb1"); - ros::NodeHandle depth1_nh(nh, "depth1"); - ros::NodeHandle rgb1_pnh(pnh, "rgb1"); - ros::NodeHandle depth1_pnh(pnh, "depth1"); - image_transport::ImageTransport rgb1_it(rgb1_nh); - image_transport::ImageTransport depth1_it(depth1_nh); - image_transport::TransportHints hintsRgb1("raw", ros::TransportHints(), rgb1_pnh); - image_transport::TransportHints hintsDepth1("raw", ros::TransportHints(), depth1_pnh); - - image_mono2_sub_.subscribe(rgb1_it, rgb1_nh.resolveName("image"), 1, hintsRgb1); - image_depth2_sub_.subscribe(depth1_it, depth1_nh.resolveName("image"), 1, hintsDepth1); - info2_sub_.subscribe(rgb1_nh, "camera_info", 1); - - ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s", - ros::this_node::getName().c_str(), - image_mono_sub_.getTopic().c_str(), - image_depth_sub_.getTopic().c_str(), - info_sub_.getTopic().c_str(), - image_mono2_sub_.getTopic().c_str(), - image_depth2_sub_.getTopic().c_str(), - info2_sub_.getTopic().c_str()); - - sync2_ = new message_filters::Synchronizer( - MySync2Policy(queueSize_), - image_mono_sub_, - image_depth_sub_, - info_sub_, - image_mono2_sub_, - image_depth2_sub_, - info2_sub_); - sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6)); - } - else - { - ros::NodeHandle rgb_nh(nh, "rgb"); - ros::NodeHandle depth_nh(nh, "depth"); - ros::NodeHandle rgb_pnh(pnh, "rgb"); - ros::NodeHandle depth_pnh(pnh, "depth"); - image_transport::ImageTransport rgb_it(rgb_nh); - image_transport::ImageTransport depth_it(depth_nh); - image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); - image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); - - image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); - image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); - info_sub_.subscribe(rgb_nh, "camera_info", 1); - - ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s", - ros::this_node::getName().c_str(), - image_mono_sub_.getTopic().c_str(), - image_depth_sub_.getTopic().c_str(), - info_sub_.getTopic().c_str()); - - sync_ = new message_filters::Synchronizer(MySyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); - sync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3)); - } - } - - virtual ~RGBDOdometry() - { - if(sync_) - { - delete sync_; - } - if(sync2_) - { - delete sync2_; - } - } - - void callback( - const sensor_msgs::ImageConstPtr& image, - const sensor_msgs::ImageConstPtr& depth, - const sensor_msgs::CameraInfoConstPtr& cameraInfo) - { - if(!this->isPaused()) - { - if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || - image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || - image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || - image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || - !(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 || - depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 || - depth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0)) - { - ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 " - "recommended) and image_depth=16UC1,32FC1,mono16. Types detected: %s %s", - image->encoding.c_str(), depth->encoding.c_str()); - return; - } - - ros::Time stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp; - - Transform localTransform = getTransform(this->frameId(), image->header.frame_id, stamp); - if(localTransform.isNull()) - { - return; - } - - if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0) - { - rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform); - cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8"); - cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth); - - rtabmap::SensorData data( - ptrImage->image, - ptrDepth->image, - rtabmapModel, - 0, - rtabmap_ros::timestampFromROS(stamp)); - - this->processData(data, stamp); - } - } - } - - void callback2( - const sensor_msgs::ImageConstPtr& image, - const sensor_msgs::ImageConstPtr& depth, - const sensor_msgs::CameraInfoConstPtr& cameraInfo, - const sensor_msgs::ImageConstPtr& image2, - const sensor_msgs::ImageConstPtr& depth2, - const sensor_msgs::CameraInfoConstPtr& cameraInfo2) - { - if(!this->isPaused()) - { - std::vector imageMsgs; - std::vector depthMsgs; - std::vector infoMsgs; - imageMsgs.push_back(image); - imageMsgs.push_back(image2); - depthMsgs.push_back(depth); - depthMsgs.push_back(depth2); - infoMsgs.push_back(cameraInfo); - infoMsgs.push_back(cameraInfo2); - - ros::Time higherStamp; - int imageWidth = imageMsgs[0]->width; - int imageHeight = imageMsgs[0]->height; - int cameraCount = imageMsgs.size(); - cv::Mat rgb; - cv::Mat depth; - pcl::PointCloud scanCloud; - std::vector cameraModels; - for(unsigned int i=0; iencoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || - !(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 || - depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 || - depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)) - { - ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16"); - return; - } - UASSERT(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight); - UASSERT(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight); - - ros::Time stamp = imageMsgs[i]->header.stamp>depthMsgs[i]->header.stamp?imageMsgs[i]->header.stamp:depthMsgs[i]->header.stamp; - - if(i == 0) - { - higherStamp = stamp; - } - else if(stamp > higherStamp) - { - higherStamp = stamp; - } - - Transform localTransform = getTransform(this->frameId(), imageMsgs[i]->header.frame_id, stamp); - if(localTransform.isNull()) - { - return; - } - - cv_bridge::CvImageConstPtr ptrImage; - if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0) - { - ptrImage = cv_bridge::toCvShare(imageMsgs[i]); - } - else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8"); - } - else - { - ptrImage = cv_bridge::toCvShare(imageMsgs[i], "bgr8"); - } - cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]); - cv::Mat subDepth = ptrDepth->image; - - // initialize - if(rgb.empty()) - { - rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type()); - } - if(depth.empty()) - { - depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type()); - } - - if(ptrImage->image.type() == rgb.type()) - { - ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight))); - } - else - { - ROS_ERROR("Some RGB images are not the same type!"); - return; - } - - if(subDepth.type() == depth.type()) - { - subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight))); - } - else - { - ROS_ERROR("Some Depth images are not the same type!"); - return; - } - - cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*infoMsgs[i], localTransform)); - } - - rtabmap::SensorData data( - rgb, - depth, - cameraModels, - 0, - rtabmap_ros::timestampFromROS(higherStamp)); - - this->processData(data, higherStamp); - } - } - -protected: - virtual void flushCallbacks() - { - // flush callbacks - if(sync_) - { - delete sync_; - sync_ = new message_filters::Synchronizer(MySyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); - sync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3)); - } - if(sync2_) - { - delete sync2_; - sync2_ = new message_filters::Synchronizer( - MySync2Policy(queueSize_), - image_mono_sub_, - image_depth_sub_, - info_sub_, - image_mono2_sub_, - image_depth2_sub_, - info2_sub_); - sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6)); - } - } - -private: - image_transport::SubscriberFilter image_mono_sub_; - image_transport::SubscriberFilter image_depth_sub_; - message_filters::Subscriber info_sub_; - image_transport::SubscriberFilter image_mono2_sub_; - image_transport::SubscriberFilter image_depth2_sub_; - message_filters::Subscriber info2_sub_; - typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; - message_filters::Synchronizer * sync_; - typedef message_filters::sync_policies::ApproximateTime MySync2Policy; - message_filters::Synchronizer * sync2_; - int queueSize_; -}; - -int main(int argc, char *argv[]) +int main(int argc, char **argv) { ULogger::setType(ULogger::kTypeConsole); ULogger::setLevel(ULogger::kWarning); ros::init(argc, argv, "rgbd_odometry"); // process "--params" argument - rtabmap_ros::OdometryROS::processArguments(argc, argv); + nodelet::V_string nargv; + for(int i=1;ifirst + " = \"" + iter->second + "\""; + std::cout << + str << + std::setw(60 - str.size()) << + " [" << + rtabmap::Parameters::getDescription(iter->first).c_str() << + "]" << + std::endl; + } + ROS_WARN("Node will now exit after showing default odometry parameters because " + "argument \"--params\" is detected!"); + exit(0); + } + else if(strcmp(argv[i], "--udebug") == 0) + { + ULogger::setLevel(ULogger::kDebug); + } + else if(strcmp(argv[i], "--uinfo") == 0) + { + ULogger::setLevel(ULogger::kInfo); + } + nargv.push_back(argv[i]); + } - RGBDOdometry odom(argc, argv); + nodelet::Loader nodelet; + nodelet::M_string remap(ros::names::getRemappings()); + std::string nodelet_name = ros::this_node::getName(); + nodelet.load(nodelet_name, "rtabmap_ros/rgbd_odometry", remap, nargv); ros::spin(); return 0; } diff --git a/src/StereoCameraNode.cpp b/src/StereoCameraNode.cpp new file mode 100644 index 00000000..2bd424ef --- /dev/null +++ b/src/StereoCameraNode.cpp @@ -0,0 +1,106 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include "ros/ros.h" +#include +#include +#include +#include +#include +#include +#include +#include + +int main(int argc, char** argv) +{ + ULogger::setType(ULogger::kTypeConsole); + ULogger::setLevel(ULogger::kInfo); + + ros::init(argc, argv, "uvc_stereo_camera"); + + ros::NodeHandle pnh("~"); + double rate = 0.0; + std::string id = "camera"; + std::string frameId = "camera_link"; + double scale = 1.0; + pnh.param("rate", rate, rate); + pnh.param("camera_id", id, id); + pnh.param("frame_id", frameId, frameId); + pnh.param("scale", scale, scale); + + rtabmap::CameraStereoVideo camera(0, false, rate); + + if(camera.init(UDirectory::homeDir() + "/.ros/camera_info", id)) + { + ros::NodeHandle nh; + ros::NodeHandle left_nh(nh, "left"); + ros::NodeHandle right_nh(nh, "right"); + image_transport::ImageTransport left_it(left_nh); + image_transport::ImageTransport right_it(right_nh); + + image_transport::Publisher imageLeftPub = left_it.advertise(left_nh.resolveName("image_raw"), 1); + image_transport::Publisher imageRightPub = right_it.advertise(right_nh.resolveName("image_raw"), 1); + ros::Publisher infoLeftPub = left_nh.advertise(left_nh.resolveName("camera_info"), 1); + ros::Publisher infoRightPub = right_nh.advertise(right_nh.resolveName("camera_info"), 1); + + while(ros::ok()) + { + rtabmap::SensorData data = camera.takeImage(); + + ros::Time currentTime = ros::Time::now(); + + cv_bridge::CvImage imageLeft; + imageLeft.header.frame_id = frameId; + imageLeft.header.stamp = currentTime; + imageLeft.encoding = "bgr8"; + cv::resize(data.imageRaw(), imageLeft.image, cv::Size(0,0), scale, scale, CV_INTER_AREA); + imageLeftPub.publish(imageLeft.toImageMsg()); + + cv_bridge::CvImage imageRight; + imageRight.header = imageLeft.header; + imageRight.encoding = "mono8"; + cv::resize(data.rightRaw(), imageRight.image, cv::Size(0,0), scale, scale, CV_INTER_AREA); + imageRightPub.publish(imageRight.toImageMsg()); + + sensor_msgs::CameraInfo infoLeft, infoRight; + rtabmap_ros::cameraModelToROS(data.stereoCameraModel().left().scaled(scale), infoLeft); + rtabmap_ros::cameraModelToROS(data.stereoCameraModel().right().scaled(scale), infoRight); + infoLeft.header = imageLeft.header; + infoRight.header = imageLeft.header; + infoLeftPub.publish(infoLeft); + infoRightPub.publish(infoRight); + + ros::spinOnce(); + } + } + else + { + ROS_ERROR("Could not initialize the camera!"); + } + + return 0; +} diff --git a/src/StereoOdometryNode.cpp b/src/StereoOdometryNode.cpp index abcac243..30a22997 100644 --- a/src/StereoOdometryNode.cpp +++ b/src/StereoOdometryNode.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without @@ -25,210 +25,55 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "OdometryROS.h" - -#include -#include -#include - -#include -#include - -#include -#include - -#include - -#include - -#include "rtabmap_ros/MsgConversion.h" - +#include "ros/ros.h" +#include "nodelet/loader.h" #include -#include +#include -using namespace rtabmap; - -class StereoOdometry : public rtabmap_ros::OdometryROS -{ -public: - StereoOdometry(int argc, char * argv[]) : - rtabmap_ros::OdometryROS(argc, argv, true), - approxSync_(0), - exactSync_(0), - queueSize_(5) - { - ros::NodeHandle nh; - - ros::NodeHandle pnh("~"); - - bool approxSync = false; - pnh.param("approx_sync", approxSync, approxSync); - pnh.param("queue_size", queueSize_, queueSize_); - ROS_INFO("Approximate time sync = %s", approxSync?"true":"false"); - - ros::NodeHandle left_nh(nh, "left"); - ros::NodeHandle right_nh(nh, "right"); - ros::NodeHandle left_pnh(pnh, "left"); - ros::NodeHandle right_pnh(pnh, "right"); - image_transport::ImageTransport left_it(left_nh); - image_transport::ImageTransport right_it(right_nh); - image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh); - image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh); - - imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft); - imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight); - cameraInfoLeft_.subscribe(left_nh, "camera_info", 1); - cameraInfoRight_.subscribe(right_nh, "camera_info", 1); - - ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s", - ros::this_node::getName().c_str(), - imageRectLeft_.getTopic().c_str(), - imageRectRight_.getTopic().c_str(), - cameraInfoLeft_.getTopic().c_str(), - cameraInfoRight_.getTopic().c_str()); - - if(approxSync) - { - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); - approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4)); - } - else - { - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); - exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4)); - } - } - - virtual ~StereoOdometry() - { - if(approxSync_) - { - delete approxSync_; - } - if(exactSync_) - { - delete exactSync_; - } - } - - void callback( - const sensor_msgs::ImageConstPtr& imageRectLeft, - const sensor_msgs::ImageConstPtr& imageRectRight, - const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft, - const sensor_msgs::CameraInfoConstPtr& cameraInfoRight) - { - if(!this->isPaused()) - { - if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || - imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || - imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || - !(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || - imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || - imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)) - { - ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended)"); - return; - } - - ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp; - - Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp); - if(localTransform.isNull()) - { - return; - } - - ros::WallTime time = ros::WallTime::now(); - - int quality = -1; - if(imageRectLeft->data.size() && imageRectRight->data.size()) - { - rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform); - if(stereoModel.baseline() <= 0) - { - ROS_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo " - "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline()); - return; - } - - if(stereoModel.baseline() > 10.0) - { - static bool shown = false; - if(!shown) - { - ROS_WARN("Detected baseline (%f m) is quite large! Is your " - "right camera_info P(0,3) correctly set? Note that " - "baseline=-P(0,3)/P(0,0). This warning is printed only once.", - stereoModel.baseline()); - shown = true; - } - } - - cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft, "mono8"); - cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight, "mono8"); - - UTimer stepTimer; - // - UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str()); - rtabmap::SensorData data( - ptrImageLeft->image, - ptrImageRight->image, - stereoModel, - 0, - rtabmap_ros::timestampFromROS(stamp)); - - this->processData(data, stamp); - } - else - { - ROS_WARN("Odom: input images empty?!?"); - } - } - } - -protected: - virtual void flushCallbacks() - { - //flush callbacks - if(approxSync_) - { - delete approxSync_; - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); - approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4)); - } - if(exactSync_) - { - delete exactSync_; - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); - exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4)); - } - } - -private: - image_transport::SubscriberFilter imageRectLeft_; - image_transport::SubscriberFilter imageRectRight_; - message_filters::Subscriber cameraInfoLeft_; - message_filters::Subscriber cameraInfoRight_; - typedef message_filters::sync_policies::ApproximateTime MyApproxSyncPolicy; - message_filters::Synchronizer * approxSync_; - typedef message_filters::sync_policies::ExactTime MyExactSyncPolicy; - message_filters::Synchronizer * exactSync_; - int queueSize_; -}; - -int main(int argc, char *argv[]) +int main(int argc, char **argv) { ULogger::setType(ULogger::kTypeConsole); ULogger::setLevel(ULogger::kWarning); - //pcl::console::setVerbosityLevel(pcl::console::L_DEBUG); ros::init(argc, argv, "stereo_odometry"); // process "--params" argument - rtabmap_ros::OdometryROS::processArguments(argc, argv, true); + nodelet::V_string nargv; + for(int i=1;ifirst + " = \"" + iter->second + "\""; + std::cout << + str << + std::setw(60 - str.size()) << + " [" << + rtabmap::Parameters::getDescription(iter->first).c_str() << + "]" << + std::endl; + } + ROS_WARN("Node will now exit after showing default odometry parameters because " + "argument \"--params\" is detected!"); + exit(0); + } + else if(strcmp(argv[i], "--udebug") == 0) + { + ULogger::setLevel(ULogger::kDebug); + } + else if(strcmp(argv[i], "--uinfo") == 0) + { + ULogger::setLevel(ULogger::kInfo); + } + nargv.push_back(argv[i]); - StereoOdometry odom(argc, argv); + } + + nodelet::Loader nodelet; + nodelet::M_string remap(ros::names::getRemappings()); + std::string nodelet_name = ros::this_node::getName(); + nodelet.load(nodelet_name, "rtabmap_ros/stereo_odometry", remap, nargv); ros::spin(); return 0; } diff --git a/src/OdometryROS.cpp b/src/nodelets/OdometryROS.cpp similarity index 78% rename from src/OdometryROS.cpp rename to src/nodelets/OdometryROS.cpp index 6b4ee4b9..317265db 100644 --- a/src/OdometryROS.cpp +++ b/src/nodelets/OdometryROS.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without @@ -55,7 +55,7 @@ using namespace rtabmap; namespace rtabmap_ros { -OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : +OdometryROS::OdometryROS(bool stereo) : odometry_(0), frameId_("base_link"), odomFrameId_("odom"), @@ -64,19 +64,39 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : waitForTransform_(true), waitForTransformDuration_(0.1), // 100 ms publishNullWhenLost_(true), + guessFromTf_(false), paused_(false), resetCountdown_(0), - resetCurrentCount_(0) + resetCurrentCount_(0), + stereo_(stereo) { - ros::NodeHandle nh; + +} + +OdometryROS::~OdometryROS() +{ + ros::NodeHandle & pnh = getPrivateNodeHandle(); + if(pnh.ok()) + { + for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter) + { + pnh.deleteParam(iter->first); + } + } + + delete odometry_; +} + +void OdometryROS::onInit() +{ + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); odomPub_ = nh.advertise("odom", 1); odomInfoPub_ = nh.advertise("odom_info", 1); odomLocalMap_ = nh.advertise("odom_local_map", 1); odomLastFrame_ = nh.advertise("odom_last_frame", 1); - ros::NodeHandle pnh("~"); - Transform initialPose = Transform::getIdentity(); std::string initialPoseStr; std::string tfPrefix; @@ -91,6 +111,13 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_); pnh.param("config_path", configPath, configPath); pnh.param("publish_null_when_lost", publishNullWhenLost_, publishNullWhenLost_); + pnh.param("guess_from_tf", guessFromTf_, guessFromTf_); + + if(publishTf_ && guessFromTf_) + { + NODELET_WARN( "\"publish_tf\" and \"guess_from_tf\" cannot be used at the same time. \"guess_from_tf\" is disabled."); + guessFromTf_ = false; + } configPath = uReplaceChar(configPath, '~', UDirectory::homeDir()); if(configPath.size() && configPath.at(0) != '/') @@ -125,19 +152,19 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : } else { - ROS_ERROR("Wrong initial_pose format: %s (should be \"x y z roll pitch yaw\" with angle in radians). " + NODELET_ERROR( "Wrong initial_pose format: %s (should be \"x y z roll pitch yaw\" with angle in radians). " "Identity will be used...", initialPoseStr.c_str()); } } //parameters - parameters_ = Parameters::getDefaultOdometryParameters(stereo); + parameters_ = Parameters::getDefaultOdometryParameters(stereo_); if(!configPath.empty()) { if(UFile::exists(configPath.c_str())) { - ROS_INFO("Odometry: Loading parameters from %s", configPath.c_str()); + NODELET_INFO( "Odometry: Loading parameters from %s", configPath.c_str()); rtabmap::ParametersMap allParameters; Parameters::readINI(configPath.c_str(), allParameters); // only update odometry parameters @@ -152,7 +179,7 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : } else { - ROS_ERROR("Config file \"%s\" not found!", configPath.c_str()); + NODELET_ERROR( "Config file \"%s\" not found!", configPath.c_str()); } } for(rtabmap::ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter) @@ -163,39 +190,46 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : double vDouble; if(pnh.getParam(iter->first, vStr)) { - ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str()); + NODELET_INFO( "Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str()); iter->second = vStr; } else if(pnh.getParam(iter->first, vBool)) { - ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str()); + NODELET_INFO( "Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str()); iter->second = uBool2Str(vBool); } else if(pnh.getParam(iter->first, vDouble)) { - ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str()); + NODELET_INFO( "Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str()); iter->second = uNumber2Str(vDouble); } else if(pnh.getParam(iter->first, vInt)) { - ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str()); + NODELET_INFO( "Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str()); iter->second = uNumber2Str(vInt); } if(iter->first.compare(Parameters::kVisMinInliers()) == 0 && atoi(iter->second.c_str()) < 8) { - ROS_WARN("Parameter min_inliers must be >= 8, setting to 8..."); + NODELET_WARN( "Parameter min_inliers must be >= 8, setting to 8..."); iter->second = uNumber2Str(8); } } - rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argc, argv); + std::vector argList = getMyArgv(); + char * argv[argList.size()]; + for(unsigned int i=0; ifirst); if(jter!=parameters_.end()) { - ROS_INFO("Update odometry parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str()); + NODELET_INFO( "Update odometry parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str()); jter->second = iter->second; } } @@ -212,19 +246,19 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : { // can be migrated parameters_.at(iter->second.second)= vStr; - ROS_WARN("Odometry: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.", + NODELET_WARN( "Odometry: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.", iter->first.c_str(), iter->second.second.c_str(), vStr.c_str()); } else { if(iter->second.second.empty()) { - ROS_ERROR("Odometry: Parameter \"%s\" doesn't exist anymore!", + NODELET_ERROR( "Odometry: Parameter \"%s\" doesn't exist anymore!", iter->first.c_str()); } else { - ROS_ERROR("Odometry: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", + NODELET_ERROR( "Odometry: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", iter->first.c_str(), iter->second.second.c_str()); } } @@ -248,50 +282,8 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : setLogInfoSrv_ = pnh.advertiseService("log_info", &OdometryROS::setLogInfo, this); setLogWarnSrv_ = pnh.advertiseService("log_warning", &OdometryROS::setLogWarn, this); setLogErrorSrv_ = pnh.advertiseService("log_error", &OdometryROS::setLogError, this); -} -OdometryROS::~OdometryROS() -{ - ros::NodeHandle pnh("~"); - for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter) - { - pnh.deleteParam(iter->first); - } - - delete odometry_; -} - -void OdometryROS::processArguments(int argc, char * argv[], bool stereo) -{ - for(int i=1;ifirst + " = \"" + iter->second + "\""; - std::cout << - str << - std::setw(60 - str.size()) << - " [" << - rtabmap::Parameters::getDescription(iter->first).c_str() << - "]" << - std::endl; - } - ROS_WARN("Node will now exit after showing default odometry parameters because " - "argument \"--params\" is detected!"); - exit(0); - } - else if(strcmp(argv[i], "--udebug") == 0) - { - ULogger::setLevel(ULogger::kDebug); - } - else if(strcmp(argv[i], "--uinfo") == 0) - { - ULogger::setLevel(ULogger::kInfo); - } - } + onOdomInit(); } Transform OdometryROS::getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const @@ -305,7 +297,7 @@ Transform OdometryROS::getTransform(const std::string & fromFrameId, const std:: //if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1))) if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_))) { - ROS_WARN("odometry: Could not get transform from %s to %s (stamp=%f) after %f seconds (\"wait_for_transform_duration\"=%f)!", + NODELET_WARN( "odometry: Could not get transform from %s to %s (stamp=%f) after %f seconds (\"wait_for_transform_duration\"=%f)!", fromFrameId.c_str(), toFrameId.c_str(), stamp.toSec(), waitForTransformDuration_, waitForTransformDuration_); return transform; } @@ -317,14 +309,14 @@ Transform OdometryROS::getTransform(const std::string & fromFrameId, const std:: } catch(tf::TransformException & ex) { - ROS_WARN("%s",ex.what()); + NODELET_WARN( "%s",ex.what()); } return transform; } void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) { - if(odometry_->getPose().isNull() && + if(odometry_->getPose().isIdentity() && !groundTruthFrameId_.empty()) { // sync with the first value of the ground truth @@ -334,18 +326,39 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) return; } - ROS_INFO("Initializing odometry pose to %s (from \"%s\" -> \"%s\")", + NODELET_INFO( "Initializing odometry pose to %s (from \"%s\" -> \"%s\")", initialPose.prettyPrint().c_str(), groundTruthFrameId_.c_str(), frameId_.c_str()); odometry_->reset(initialPose); } + Transform guess; + if(guessFromTf_) + { + Transform previousPose = this->getTransform(odomFrameId_, frameId_, ros::Time(odometry_->previousStamp())); + Transform pose = this->getTransform(odomFrameId_, frameId_, stamp); + if(!previousPose.isNull() && !pose.isNull()) + { + guess = previousPose.inverse() * pose; + + /*if(!odometry_->previousVelocityTransform().isNull()) + { + float dt = rtabmap_ros::timestampFromROS(stamp) - odometry_->previousStamp(); + float vx,vy,vz, vroll,vpitch,vyaw; + odometry_->previousVelocityTransform().getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw); + Transform motionGuess(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt); + NODELET_WARN( "P Guess %s", motionGuess.prettyPrint().c_str()); + } + NODELET_WARN( "TF Guess %s", guess.prettyPrint().c_str());*/ + } + } + // process data ros::WallTime time = ros::WallTime::now(); rtabmap::OdometryInfo info; SensorData dataCpy = data; - rtabmap::Transform pose = odometry_->process(dataCpy, &info); + rtabmap::Transform pose = odometry_->process(dataCpy, guess, &info); if(!pose.isNull()) { resetCurrentCount_ = resetCountdown_; @@ -474,7 +487,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) } else if(publishNullWhenLost_) { - //ROS_WARN("Odometry lost!"); + //NODELET_WARN( "Odometry lost!"); //send null pose to notify that odometry is lost nav_msgs::Odometry odom; @@ -500,7 +513,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) if(pose.isNull() && resetCurrentCount_ > 0) { - ROS_WARN("Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_); + NODELET_WARN( "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_); --resetCurrentCount_; if(resetCurrentCount_ == 0) @@ -509,12 +522,12 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) Transform tfPose = this->getTransform(odomFrameId_, frameId_, stamp); if(tfPose.isNull()) { - ROS_WARN("Odometry automatically reset to latest computed pose!"); + NODELET_WARN( "Odometry automatically reset to latest computed pose!"); odometry_->reset(odometry_->getPose()); } else { - ROS_WARN("Odometry automatically reset to latest odometry pose available from TF (%s->%s)!", + NODELET_WARN( "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!", odomFrameId_.c_str(), frameId_.c_str()); odometry_->reset(tfPose); } @@ -531,7 +544,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) odomInfoPub_.publish(infoMsg); } - ROS_INFO("Odom: quality=%d, std dev=%fm, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.variance), (ros::WallTime::now()-time).toSec()); + NODELET_INFO( "Odom: quality=%d, std dev=%fm, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.variance), (ros::WallTime::now()-time).toSec()); } bool OdometryROS::isOdometryF2M() const @@ -541,7 +554,7 @@ bool OdometryROS::isOdometryF2M() const bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { - ROS_INFO("visual_odometry: reset odom!"); + NODELET_INFO( "visual_odometry: reset odom!"); odometry_->reset(); this->flushCallbacks(); return true; @@ -550,7 +563,7 @@ bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) bool OdometryROS::resetToPose(rtabmap_ros::ResetPose::Request& req, rtabmap_ros::ResetPose::Response&) { Transform pose(req.x, req.y, req.z, req.roll, req.pitch, req.yaw); - ROS_INFO("visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str()); + NODELET_INFO( "visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str()); odometry_->reset(pose); this->flushCallbacks(); return true; @@ -560,12 +573,12 @@ bool OdometryROS::pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { if(paused_) { - ROS_WARN("visual_odometry: Already paused!"); + NODELET_WARN( "visual_odometry: Already paused!"); } else { paused_ = true; - ROS_INFO("visual_odometry: paused!"); + NODELET_INFO( "visual_odometry: paused!"); } return true; } @@ -574,37 +587,37 @@ bool OdometryROS::resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { if(!paused_) { - ROS_WARN("visual_odometry: Already running!"); + NODELET_WARN( "visual_odometry: Already running!"); } else { paused_ = false; - ROS_INFO("visual_odometry: resumed!"); + NODELET_INFO( "visual_odometry: resumed!"); } return true; } bool OdometryROS::setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { - ROS_INFO("visual_odometry: Set log level to Debug"); + NODELET_INFO( "visual_odometry: Set log level to Debug"); ULogger::setLevel(ULogger::kDebug); return true; } bool OdometryROS::setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { - ROS_INFO("visual_odometry: Set log level to Info"); + NODELET_INFO( "visual_odometry: Set log level to Info"); ULogger::setLevel(ULogger::kInfo); return true; } bool OdometryROS::setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { - ROS_INFO("visual_odometry: Set log level to Warning"); + NODELET_INFO( "visual_odometry: Set log level to Warning"); ULogger::setLevel(ULogger::kWarning); return true; } bool OdometryROS::setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { - ROS_INFO("visual_odometry: Set log level to Error"); + NODELET_INFO( "visual_odometry: Set log level to Error"); ULogger::setLevel(ULogger::kError); return true; } diff --git a/src/OdometryROS.h b/src/nodelets/OdometryROS.h similarity index 93% rename from src/OdometryROS.h rename to src/nodelets/OdometryROS.h index 913cf3a3..3432fd26 100644 --- a/src/OdometryROS.h +++ b/src/nodelets/OdometryROS.h @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without @@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define ODOMETRYROS_H_ #include +#include #include #include @@ -46,14 +47,13 @@ class Odometry; namespace rtabmap_ros { -class OdometryROS +class OdometryROS : public nodelet::Nodelet { -public: - static void processArguments(int argc, char * argv[], bool stereo = false); public: - OdometryROS(int argc, char * argv[], bool stereo = false); + OdometryROS(bool stereo); virtual ~OdometryROS(); + void processData(const rtabmap::SensorData & data, const ros::Time & stamp); bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&); @@ -76,6 +76,10 @@ public: protected: virtual void flushCallbacks() = 0; +private: + virtual void onInit(); + virtual void onOdomInit() = 0; + private: rtabmap::Odometry * odometry_; @@ -87,6 +91,7 @@ private: bool waitForTransform_; double waitForTransformDuration_; bool publishNullWhenLost_; + bool guessFromTf_; rtabmap::ParametersMap parameters_; ros::Publisher odomPub_; @@ -107,6 +112,7 @@ private: bool paused_; int resetCountdown_; int resetCurrentCount_; + bool stereo_; }; } diff --git a/src/nodelets/data_odom_sync.cpp b/src/nodelets/data_odom_sync.cpp index 2cfab479..b137044f 100644 --- a/src/nodelets/data_odom_sync.cpp +++ b/src/nodelets/data_odom_sync.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/src/nodelets/data_throttle.cpp b/src/nodelets/data_throttle.cpp index df13547a..47d266ba 100644 --- a/src/nodelets/data_throttle.cpp +++ b/src/nodelets/data_throttle.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/src/nodelets/disparity_to_depth.cpp b/src/nodelets/disparity_to_depth.cpp index 1e97e9fd..3c5628c3 100644 --- a/src/nodelets/disparity_to_depth.cpp +++ b/src/nodelets/disparity_to_depth.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 629c1b7a..0e7dc91e 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/src/nodelets/point_cloud_xyz.cpp b/src/nodelets/point_cloud_xyz.cpp index 6d8ffe72..7274b92a 100644 --- a/src/nodelets/point_cloud_xyz.cpp +++ b/src/nodelets/point_cloud_xyz.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without @@ -214,12 +214,15 @@ private: model.fromCameraInfo(*cameraInfo); pcl::PointCloud::Ptr pclCloud; - pclCloud = rtabmap::util3d::cloudFromDepth( - cv::Mat(imageDepthPtr->image, roi), - model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols), - model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows), + rtabmap::CameraModel m( model.fx(), model.fy(), + model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols), + model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows)); + + pclCloud = rtabmap::util3d::cloudFromDepth( + cv::Mat(imageDepthPtr->image, roi), + m, decimation_); processAndPublish(pclCloud, depth->header); diff --git a/src/nodelets/point_cloud_xyzrgb.cpp b/src/nodelets/point_cloud_xyzrgb.cpp index f8ec9ecb..931cde23 100644 --- a/src/nodelets/point_cloud_xyzrgb.cpp +++ b/src/nodelets/point_cloud_xyzrgb.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without @@ -240,13 +240,16 @@ private: pcl::PointCloud::Ptr pclCloud; cv::Rect roi = rtabmap::Feature2D::computeRoi(imageDepthPtr->image, roiRatios_); + + rtabmap::CameraModel m( + model.fx(), + model.fy(), + model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols), + model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows)); pclCloud = rtabmap::util3d::cloudFromDepthRGB( cv::Mat(imagePtr->image, roi), cv::Mat(imageDepthPtr->image, roi), - model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols), - model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows), - model.fx(), - model.fy(), + m, decimation_); diff --git a/src/nodelets/rgbd_odometry.cpp b/src/nodelets/rgbd_odometry.cpp new file mode 100644 index 00000000..b0f480cf --- /dev/null +++ b/src/nodelets/rgbd_odometry.cpp @@ -0,0 +1,419 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include "OdometryROS.h" + +#include +#include + +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include + +#include "rtabmap_ros/MsgConversion.h" + +#include +#include +#include +#include + +using namespace rtabmap; + +namespace rtabmap_ros +{ + +class RGBDOdometry : public rtabmap_ros::OdometryROS +{ +public: + RGBDOdometry() : + OdometryROS(false), + approxSync_(0), + exactSync_(0), + sync2_(0), + queueSize_(5) + { + } + + virtual ~RGBDOdometry() + { + if(approxSync_) + { + delete approxSync_; + } + if(exactSync_) + { + delete exactSync_; + } + if(sync2_) + { + delete sync2_; + } + } + +private: + + virtual void onOdomInit() + { + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + int depthCameras = 1; + bool approxSync = true; + pnh.param("approx_sync", approxSync, approxSync); + pnh.param("queue_size", queueSize_, queueSize_); + pnh.param("depth_cameras", depthCameras, depthCameras); + if(depthCameras <= 0) + { + depthCameras = 1; + } + if(depthCameras > 2) + { + NODELET_FATAL("Only 2 cameras maximum supported yet."); + } + + if(depthCameras == 2) + { + ros::NodeHandle rgb0_nh(nh, "rgb0"); + ros::NodeHandle depth0_nh(nh, "depth0"); + ros::NodeHandle rgb0_pnh(pnh, "rgb0"); + ros::NodeHandle depth0_pnh(pnh, "depth0"); + image_transport::ImageTransport rgb0_it(rgb0_nh); + image_transport::ImageTransport depth0_it(depth0_nh); + image_transport::TransportHints hintsRgb0("raw", ros::TransportHints(), rgb0_pnh); + image_transport::TransportHints hintsDepth0("raw", ros::TransportHints(), depth0_pnh); + + image_mono_sub_.subscribe(rgb0_it, rgb0_nh.resolveName("image"), 1, hintsRgb0); + image_depth_sub_.subscribe(depth0_it, depth0_nh.resolveName("image"), 1, hintsDepth0); + info_sub_.subscribe(rgb0_nh, "camera_info", 1); + + ros::NodeHandle rgb1_nh(nh, "rgb1"); + ros::NodeHandle depth1_nh(nh, "depth1"); + ros::NodeHandle rgb1_pnh(pnh, "rgb1"); + ros::NodeHandle depth1_pnh(pnh, "depth1"); + image_transport::ImageTransport rgb1_it(rgb1_nh); + image_transport::ImageTransport depth1_it(depth1_nh); + image_transport::TransportHints hintsRgb1("raw", ros::TransportHints(), rgb1_pnh); + image_transport::TransportHints hintsDepth1("raw", ros::TransportHints(), depth1_pnh); + + image_mono2_sub_.subscribe(rgb1_it, rgb1_nh.resolveName("image"), 1, hintsRgb1); + image_depth2_sub_.subscribe(depth1_it, depth1_nh.resolveName("image"), 1, hintsDepth1); + info2_sub_.subscribe(rgb1_nh, "camera_info", 1); + + NODELET_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + image_mono_sub_.getTopic().c_str(), + image_depth_sub_.getTopic().c_str(), + info_sub_.getTopic().c_str(), + image_mono2_sub_.getTopic().c_str(), + image_depth2_sub_.getTopic().c_str(), + info2_sub_.getTopic().c_str()); + + sync2_ = new message_filters::Synchronizer( + MySync2Policy(queueSize_), + image_mono_sub_, + image_depth_sub_, + info_sub_, + image_mono2_sub_, + image_depth2_sub_, + info2_sub_); + sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6)); + } + else + { + ros::NodeHandle rgb_nh(nh, "rgb"); + ros::NodeHandle depth_nh(nh, "depth"); + ros::NodeHandle rgb_pnh(pnh, "rgb"); + ros::NodeHandle depth_pnh(pnh, "depth"); + image_transport::ImageTransport rgb_it(rgb_nh); + image_transport::ImageTransport depth_it(depth_nh); + image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); + image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); + + image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); + image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); + info_sub_.subscribe(rgb_nh, "camera_info", 1); + + if(approxSync) + { + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); + approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3)); + } + else + { + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); + exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3)); + } + + NODELET_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + approxSync?"approx":"exact", + image_mono_sub_.getTopic().c_str(), + image_depth_sub_.getTopic().c_str(), + info_sub_.getTopic().c_str()); + } + } + + void callback( + const sensor_msgs::ImageConstPtr& image, + const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& cameraInfo) + { + if(!this->isPaused()) + { + if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || + image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || + image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || + image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || + image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || + !(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 || + depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 || + depth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0)) + { + NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 " + "recommended) and image_depth=16UC1,32FC1,mono16. Types detected: %s %s", + image->encoding.c_str(), depth->encoding.c_str()); + return; + } + + ros::Time stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp; + + Transform localTransform = getTransform(this->frameId(), image->header.frame_id, stamp); + if(localTransform.isNull()) + { + return; + } + + if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0) + { + rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform); + cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8"); + cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth); + + rtabmap::SensorData data( + ptrImage->image, + ptrDepth->image, + rtabmapModel, + 0, + rtabmap_ros::timestampFromROS(stamp)); + + this->processData(data, stamp); + } + } + } + + void callback2( + const sensor_msgs::ImageConstPtr& image, + const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& cameraInfo, + const sensor_msgs::ImageConstPtr& image2, + const sensor_msgs::ImageConstPtr& depth2, + const sensor_msgs::CameraInfoConstPtr& cameraInfo2) + { + if(!this->isPaused()) + { + std::vector imageMsgs; + std::vector depthMsgs; + std::vector infoMsgs; + imageMsgs.push_back(image); + imageMsgs.push_back(image2); + depthMsgs.push_back(depth); + depthMsgs.push_back(depth2); + infoMsgs.push_back(cameraInfo); + infoMsgs.push_back(cameraInfo2); + + ros::Time higherStamp; + int imageWidth = imageMsgs[0]->width; + int imageHeight = imageMsgs[0]->height; + int cameraCount = imageMsgs.size(); + cv::Mat rgb; + cv::Mat depth; + pcl::PointCloud scanCloud; + std::vector cameraModels; + for(unsigned int i=0; iencoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || + imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || + imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || + imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || + imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || + !(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 || + depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 || + depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)) + { + NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16"); + return; + } + UASSERT_MSG(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight, + uFormat("imageWidth=%d vs %d imageHeight=%d vs %d", + imageWidth, + imageMsgs[i]->width, + imageHeight, + imageMsgs[i]->height).c_str()); + UASSERT_MSG(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight, + uFormat("imageWidth=%d vs %d imageHeight=%d vs %d", + imageWidth, + depthMsgs[i]->width, + imageHeight, + depthMsgs[i]->height).c_str()); + + ros::Time stamp = imageMsgs[i]->header.stamp>depthMsgs[i]->header.stamp?imageMsgs[i]->header.stamp:depthMsgs[i]->header.stamp; + + if(i == 0) + { + higherStamp = stamp; + } + else if(stamp > higherStamp) + { + higherStamp = stamp; + } + + Transform localTransform = getTransform(this->frameId(), imageMsgs[i]->header.frame_id, stamp); + if(localTransform.isNull()) + { + return; + } + + cv_bridge::CvImageConstPtr ptrImage; + if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0) + { + ptrImage = cv_bridge::toCvShare(imageMsgs[i]); + } + else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8"); + } + else + { + ptrImage = cv_bridge::toCvShare(imageMsgs[i], "bgr8"); + } + cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]); + cv::Mat subDepth = ptrDepth->image; + + // initialize + if(rgb.empty()) + { + rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type()); + } + if(depth.empty()) + { + depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type()); + } + + if(ptrImage->image.type() == rgb.type()) + { + ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight))); + } + else + { + NODELET_ERROR("Some RGB images are not the same type!"); + return; + } + + if(subDepth.type() == depth.type()) + { + subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight))); + } + else + { + NODELET_ERROR("Some Depth images are not the same type!"); + return; + } + + cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*infoMsgs[i], localTransform)); + } + + rtabmap::SensorData data( + rgb, + depth, + cameraModels, + 0, + rtabmap_ros::timestampFromROS(higherStamp)); + + this->processData(data, higherStamp); + } + } + +protected: + virtual void flushCallbacks() + { + // flush callbacks + if(approxSync_) + { + delete approxSync_; + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); + approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3)); + } + if(exactSync_) + { + delete exactSync_; + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); + exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3)); + } + if(sync2_) + { + delete sync2_; + sync2_ = new message_filters::Synchronizer( + MySync2Policy(queueSize_), + image_mono_sub_, + image_depth_sub_, + info_sub_, + image_mono2_sub_, + image_depth2_sub_, + info2_sub_); + sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6)); + } + } + +private: + image_transport::SubscriberFilter image_mono_sub_; + image_transport::SubscriberFilter image_depth_sub_; + message_filters::Subscriber info_sub_; + image_transport::SubscriberFilter image_mono2_sub_; + image_transport::SubscriberFilter image_depth2_sub_; + message_filters::Subscriber info2_sub_; + typedef message_filters::sync_policies::ApproximateTime MyApproxSyncPolicy; + message_filters::Synchronizer * approxSync_; + typedef message_filters::sync_policies::ApproximateTime MyExactSyncPolicy; + message_filters::Synchronizer * exactSync_; + typedef message_filters::sync_policies::ApproximateTime MySync2Policy; + message_filters::Synchronizer * sync2_; + int queueSize_; +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDOdometry, nodelet::Nodelet); + +} diff --git a/src/nodelets/stereo_odometry.cpp b/src/nodelets/stereo_odometry.cpp new file mode 100644 index 00000000..de1a1657 --- /dev/null +++ b/src/nodelets/stereo_odometry.cpp @@ -0,0 +1,235 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include "OdometryROS.h" +#include "pluginlib/class_list_macros.h" +#include "nodelet/nodelet.h" + +#include +#include +#include + +#include +#include + +#include +#include + +#include + +#include + +#include "rtabmap_ros/MsgConversion.h" + +#include +#include + +using namespace rtabmap; + +namespace rtabmap_ros +{ + +class StereoOdometry : public rtabmap_ros::OdometryROS +{ +public: + StereoOdometry() : + rtabmap_ros::OdometryROS(true), + approxSync_(0), + exactSync_(0), + queueSize_(5) + { + } + + virtual ~StereoOdometry() + { + if(approxSync_) + { + delete approxSync_; + } + if(exactSync_) + { + delete exactSync_; + } + } + +private: + virtual void onOdomInit() + { + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + bool approxSync = false; + pnh.param("approx_sync", approxSync, approxSync); + pnh.param("queue_size", queueSize_, queueSize_); + NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false"); + + ros::NodeHandle left_nh(nh, "left"); + ros::NodeHandle right_nh(nh, "right"); + ros::NodeHandle left_pnh(pnh, "left"); + ros::NodeHandle right_pnh(pnh, "right"); + image_transport::ImageTransport left_it(left_nh); + image_transport::ImageTransport right_it(right_nh); + image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh); + image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh); + + imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft); + imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight); + cameraInfoLeft_.subscribe(left_nh, "camera_info", 1); + cameraInfoRight_.subscribe(right_nh, "camera_info", 1); + + if(approxSync) + { + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4)); + } + else + { + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4)); + } + + + NODELET_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + approxSync?"approx":"exact", + imageRectLeft_.getTopic().c_str(), + imageRectRight_.getTopic().c_str(), + cameraInfoLeft_.getTopic().c_str(), + cameraInfoRight_.getTopic().c_str()); + } + + void callback( + const sensor_msgs::ImageConstPtr& imageRectLeft, + const sensor_msgs::ImageConstPtr& imageRectRight, + const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft, + const sensor_msgs::CameraInfoConstPtr& cameraInfoRight) + { + if(!this->isPaused()) + { + if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || + imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || + imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || + imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || + !(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || + imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || + imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || + imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)) + { + NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended)"); + return; + } + + ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp; + + Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp); + if(localTransform.isNull()) + { + return; + } + + ros::WallTime time = ros::WallTime::now(); + + int quality = -1; + if(imageRectLeft->data.size() && imageRectRight->data.size()) + { + rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform); + if(stereoModel.baseline() <= 0) + { + NODELET_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo " + "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline()); + return; + } + + if(stereoModel.baseline() > 10.0) + { + static bool shown = false; + if(!shown) + { + NODELET_WARN("Detected baseline (%f m) is quite large! Is your " + "right camera_info P(0,3) correctly set? Note that " + "baseline=-P(0,3)/P(0,0). This warning is printed only once.", + stereoModel.baseline()); + shown = true; + } + } + + cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft, "mono8"); + cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight, "mono8"); + + UTimer stepTimer; + // + UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str()); + rtabmap::SensorData data( + ptrImageLeft->image, + ptrImageRight->image, + stereoModel, + 0, + rtabmap_ros::timestampFromROS(stamp)); + + this->processData(data, stamp); + } + else + { + NODELET_WARN("Odom: input images empty?!?"); + } + } + } + +protected: + virtual void flushCallbacks() + { + //flush callbacks + if(approxSync_) + { + delete approxSync_; + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4)); + } + if(exactSync_) + { + delete exactSync_; + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4)); + } + } + +private: + image_transport::SubscriberFilter imageRectLeft_; + image_transport::SubscriberFilter imageRectRight_; + message_filters::Subscriber cameraInfoLeft_; + message_filters::Subscriber cameraInfoRight_; + typedef message_filters::sync_policies::ApproximateTime MyApproxSyncPolicy; + message_filters::Synchronizer * approxSync_; + typedef message_filters::sync_policies::ExactTime MyExactSyncPolicy; + message_filters::Synchronizer * exactSync_; + int queueSize_; +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::StereoOdometry, nodelet::Nodelet); + +} + diff --git a/src/nodelets/stereo_throttle.cpp b/src/nodelets/stereo_throttle.cpp index 482f70ec..d5db504a 100644 --- a/src/nodelets/stereo_throttle.cpp +++ b/src/nodelets/stereo_throttle.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/src/rviz/InfoDisplay.cpp b/src/rviz/InfoDisplay.cpp index d4da9683..e174f760 100644 --- a/src/rviz/InfoDisplay.cpp +++ b/src/rviz/InfoDisplay.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/src/rviz/InfoDisplay.h b/src/rviz/InfoDisplay.h index 1bd3d00d..7654f2a0 100644 --- a/src/rviz/InfoDisplay.h +++ b/src/rviz/InfoDisplay.h @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/src/rviz/MapCloudDisplay.cpp b/src/rviz/MapCloudDisplay.cpp index ccdf04a7..b178f6bb 100644 --- a/src/rviz/MapCloudDisplay.cpp +++ b/src/rviz/MapCloudDisplay.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/src/rviz/MapCloudDisplay.h b/src/rviz/MapCloudDisplay.h index 00e507e2..938bef97 100644 --- a/src/rviz/MapCloudDisplay.h +++ b/src/rviz/MapCloudDisplay.h @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/src/rviz/MapGraphDisplay.cpp b/src/rviz/MapGraphDisplay.cpp index 84aa2d5e..f672f2eb 100644 --- a/src/rviz/MapGraphDisplay.cpp +++ b/src/rviz/MapGraphDisplay.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/src/rviz/MapGraphDisplay.h b/src/rviz/MapGraphDisplay.h index d499cc76..b17f72f6 100644 --- a/src/rviz/MapGraphDisplay.h +++ b/src/rviz/MapGraphDisplay.h @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/src/rviz/OrbitOrientedViewController.cpp b/src/rviz/OrbitOrientedViewController.cpp index d9f6e480..4df70c07 100644 --- a/src/rviz/OrbitOrientedViewController.cpp +++ b/src/rviz/OrbitOrientedViewController.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/src/rviz/OrbitOrientedViewController.h b/src/rviz/OrbitOrientedViewController.h index 4cd0ead9..9cb0594d 100644 --- a/src/rviz/OrbitOrientedViewController.h +++ b/src/rviz/OrbitOrientedViewController.h @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without