diff --git a/CMakeLists.txt b/CMakeLists.txt index 699bcb70..75c28339 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,23 +7,41 @@ project(rtabmap_ros) find_package(catkin REQUIRED COMPONENTS cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs visualization_msgs image_transport tf tf_conversions tf2_ros eigen_conversions laser_geometry pcl_conversions - pcl_ros nodelet dynamic_reconfigure rviz message_filters class_loader + pcl_ros nodelet dynamic_reconfigure message_filters class_loader genmsg stereo_msgs move_base_msgs ) # Optional components find_package(costmap_2d) find_package(octomap_ros) +find_package(rviz) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) -find_package(RTABMap 0.10.10 REQUIRED) +find_package(RTABMap 0.11.5 REQUIRED) find_package(OpenCV REQUIRED) #Qt stuff -FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED) -INCLUDE(${QT_USE_FILE}) +# If librtabmap_gui.so is found, rtabmapviz will be built +# If rviz is found, plugins will be built +IF(RTABMAP_GUI OR rviz_FOUND) + IF(RTABMAP_QT_VERSION EQUAL 4) + FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED) + INCLUDE(${QT_USE_FILE}) + ELSE() + IF(RTABMAP_GUI) + FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui REQUIRED) + ELSE() + # For rviz plugins, look for Qt5 before Qt4 + FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui QUIET) + IF(NOT Qt5_FOUND) + FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED) + INCLUDE(${QT_USE_FILE}) + ENDIF(NOT Qt5_FOUND) + ENDIF() + ENDIF() +ENDIF(RTABMAP_GUI OR rviz_FOUND) ## We also use Ogre include($ENV{ROS_ROOT}/core/rosbuild/FindPkgConfig.cmake) @@ -52,6 +70,7 @@ add_message_files( Link.msg OdomInfo.msg Point2f.msg + Point3f.msg Goal.msg ) @@ -91,8 +110,9 @@ catkin_package( LIBRARIES rtabmap_ros CATKIN_DEPENDS cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs visualization_msgs image_transport tf tf_conversions tf2_ros eigen_conversions laser_geometry pcl_conversions - pcl_ros nodelet dynamic_reconfigure rviz message_filters class_loader - stereo_msgs move_base_msgs + pcl_ros nodelet dynamic_reconfigure message_filters class_loader + stereo_msgs move_base_msgs + DEPENDS RTABMap OpenCV ) ########### @@ -116,21 +136,7 @@ SET(Libraries ${RTABMap_LIBRARIES} ${rviz_DEFAULT_PLUGIN_LIBRARIES} ) - -## RVIZ plugin -qt4_wrap_cpp(MOC_FILES - src/rviz/MapCloudDisplay.h - src/rviz/MapGraphDisplay.h - src/rviz/InfoDisplay.h - src/rviz/OrbitOrientedViewController.h -) - -# tf:message_filters, mixing boost and Qt signals -set_property( - SOURCE src/rviz/MapCloudDisplay.cpp src/rviz/MapGraphDisplay.cpp src/rviz/InfoDisplay.cpp src/rviz/OrbitOrientedViewController.cpp - PROPERTY COMPILE_DEFINITIONS QT_NO_KEYWORDS - ) - + SET(rtabmap_ros_lib_src src/nodelets/data_throttle.cpp src/nodelets/stereo_throttle.cpp @@ -142,11 +148,7 @@ SET(rtabmap_ros_lib_src src/nodelets/point_cloud_aggregator.cpp src/MsgConversion.cpp src/OdometryROS.cpp - src/rviz/MapCloudDisplay.cpp - src/rviz/MapGraphDisplay.cpp - src/rviz/InfoDisplay.cpp - src/rviz/OrbitOrientedViewController.cpp - ${MOC_FILES} + src/MapsManager.cpp ) # If costmap_2d is found, add the plugin @@ -163,15 +165,72 @@ SET(rtabmap_ros_lib_src ) ENDIF(costmap_2d_FOUND) +IF(QT4_FOUND OR Qt5_FOUND) +SET(Libraries + ${Libraries} + ${QT_LIBRARIES} +) +ENDIF(QT4_FOUND OR Qt5_FOUND) + +# If rviz is found, add plugins +IF(rviz_FOUND) +MESSAGE(STATUS "WITH rviz") +include_directories( + ${rviz_INCLUDE_DIRS} +) +SET(Libraries + ${Libraries} + ${rviz_LIBRARIES} + ${rviz_DEFAULT_PLUGIN_LIBRARIES} +) + +## RVIZ plugin +IF(QT4_FOUND) +qt4_wrap_cpp(MOC_FILES + src/rviz/MapCloudDisplay.h + src/rviz/MapGraphDisplay.h + src/rviz/InfoDisplay.h + src/rviz/OrbitOrientedViewController.h +) +ELSE() +qt5_wrap_cpp(MOC_FILES + src/rviz/MapCloudDisplay.h + src/rviz/MapGraphDisplay.h + src/rviz/InfoDisplay.h + src/rviz/OrbitOrientedViewController.h +) +ENDIF() + +# tf:message_filters, mixing boost and Qt signals +set_property( + SOURCE src/rviz/MapCloudDisplay.cpp src/rviz/MapGraphDisplay.cpp src/rviz/InfoDisplay.cpp src/rviz/OrbitOrientedViewController.cpp + PROPERTY COMPILE_DEFINITIONS QT_NO_KEYWORDS +) + +SET(rtabmap_ros_lib_src + ${rtabmap_ros_lib_src} + src/rviz/MapCloudDisplay.cpp + src/rviz/MapGraphDisplay.cpp + src/rviz/InfoDisplay.cpp + src/rviz/OrbitOrientedViewController.cpp + ${MOC_FILES} +) +ENDIF(rviz_FOUND) + +############################ ## Declare a cpp library +############################ add_library(rtabmap_ros ${rtabmap_ros_lib_src} ) + target_link_libraries(rtabmap_ros ${Libraries} - ${QT_LIBRARIES} ${OGRE_LIBRARIES} ) +IF(Qt5_FOUND) + QT5_USE_MODULES(rtabmap_ros Widgets Core Gui) +ENDIF(Qt5_FOUND) add_dependencies(rtabmap_ros ${${PROJECT_NAME}_EXPORTED_TARGETS}) # If octomap is found, add definition @@ -187,7 +246,7 @@ SET(Libraries add_definitions(-DWITH_OCTOMAP) ENDIF(octomap_ros_FOUND) -add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp src/MapsManager.cpp) +add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp) target_link_libraries(rtabmap rtabmap_ros ${Libraries}) add_executable(rgbd_odometry src/RGBDOdometryNode.cpp) @@ -202,9 +261,6 @@ target_link_libraries(map_optimizer rtabmap_ros ${Libraries}) add_executable(map_assembler src/MapAssemblerNode.cpp) target_link_libraries(map_assembler rtabmap_ros ${Libraries}) -add_executable(grid_map_assembler src/GridMapAssemblerNode.cpp) -target_link_libraries(grid_map_assembler rtabmap_ros ${Libraries}) - add_executable(camera src/CameraNode.cpp) add_dependencies(camera ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_link_libraries(camera ${Libraries}) @@ -212,6 +268,9 @@ target_link_libraries(camera ${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}) + IF(Qt5_FOUND) + QT5_USE_MODULES(rtabmapviz Widgets Core Gui) + ENDIF() ELSE() MESSAGE(WARNING "Found RTAB-Map built without its GUI library. Node rtabmapviz will not be built!") ENDIF() @@ -245,7 +304,6 @@ install(TARGETS rgbd_odometry stereo_odometry map_assembler - grid_map_assembler map_optimizer data_player camera @@ -260,7 +318,6 @@ install(TARGETS rgbd_odometry stereo_odometry map_assembler - grid_map_assembler map_optimizer data_player camera @@ -279,6 +336,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ ## Mark other files for installation (e.g. launch and bag files, etc.) install(FILES + launch/rtabmap.launch launch/rgbd_mapping.launch launch/stereo_mapping.launch launch/data_recorder.launch @@ -299,6 +357,12 @@ install(FILES costmap_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) +IF(costmap_2d_FOUND) +install(FILES + costmap_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) +ENDIF(costmap_2d_FOUND) ############# ## Testing ## diff --git a/README.md b/README.md index 149e6479..b7bade6b 100644 --- a/README.md +++ b/README.md @@ -31,14 +31,19 @@ This section shows how to install RTAB-Map ros-pkg on **ROS Hydro/Indigo/Jade** * The next instructions assume that you have set up your ROS workspace using this [tutorial](http://wiki.ros.org/catkin/Tutorials/create_a_workspace). I will use indigo prefix for convenience, but it should work with hydro and jade. The workspace path is `~/catkin_ws` and your `~/.bashrc` contains: ```bash -source /opt/ros/indigo/setup.bash -source ~/catkin_ws/devel/setup.bash +$ source /opt/ros/indigo/setup.bash +$ source ~/catkin_ws/devel/setup.bash +``` + + * Make sure you don't have the binaries installed too (if you tried them before): + ```bash +$ sudo apt-get remove ros-indigo-rtabmap ``` 0. Optional dependencies * If you want SURF/SIFT on Indigo/Jade (Hydro has already SIFT/SURF), you have to build [OpenCV]([OpenCV](http://opencv.org/)) from source to have access to *nonfree* module. Install it in `/usr/local` (default) and the rtabmap library should link with it instead of the one installed in ROS. I recommend to use latest 2.4 version ([2.4.11](https://github.com/Itseez/opencv/archive/2.4.11.zip)) and build it from source following these [instructions](http://docs.opencv.org/doc/tutorials/introduction/linux_install/linux_install.html#building-opencv-from-source-using-cmake-using-the-command-line). RTAB-Map can build with OpenCV3+[xfeatures2d](https://github.com/Itseez/opencv_contrib/tree/master/modules/xfeatures2d) module, but rtabmap_ros package will have libraries conflict as cv-bridge is depending on OpenCV2. If you want OpenCV3, you should build ros [vision-opencv](https://github.com/ros-perception/vision_opencv) package yourself (and all ros packages depending on it) so it can link on OpenCV3. - * ROS (Qt, PCL, dc1394, OpenNI, OpenNI2, Freenect, g2o, Costmap2d, Rviz, Octomap, CvBridge). Note that I've found that [latest g2o version](https://github.com/RainerKuemmerle/g2o) built from source is faster. + * ROS (Qt, PCL, dc1394, OpenNI, OpenNI2, Freenect, g2o, Costmap2d, Rviz, Octomap, CvBridge). Note that I've found that [latest g2o version](https://github.com/RainerKuemmerle/g2o) built from source is faster (install `libsuitesparse-dev` before building `g2o`) and would be [required to avoid some crashes](http://official-rtab-map-forum.67519.x6.nabble.com/ROS-2D-occupancy-grid-tp1204p1215.html). ```bash $ sudo apt-get install libqt4-dev libpcl-1.7-all-dev libdc1394-dev ros-indigo-openni-launch ros-indigo-openni2-launch ros-indigo-freenect-launch ros-indigo-costmap-2d ros-indigo-octomap-ros ros-indigo-g2o ros-indigo-rviz ros-indigo-cv-bridge ``` diff --git a/include/rtabmap_ros/MsgConversion.h b/include/rtabmap_ros/MsgConversion.h index 4edfaacb..fd4bb016 100644 --- a/include/rtabmap_ros/MsgConversion.h +++ b/include/rtabmap_ros/MsgConversion.h @@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include @@ -40,10 +41,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include +#include #include #include #include @@ -83,6 +86,24 @@ void point2fToROS(const cv::Point2f & kpt, rtabmap_ros::Point2f & msg); std::vector points2fFromROS(const std::vector & msg); void points2fToROS(const std::vector & kpts, std::vector & msg); +cv::Point3f point3fFromROS(const rtabmap_ros::Point3f & msg); +void point3fToROS(const cv::Point3f & kpt, rtabmap_ros::Point3f & msg); + +std::vector points3fFromROS(const std::vector & msg); +void points3fToROS(const std::vector & kpts, std::vector & msg); + +rtabmap::CameraModel cameraModelFromROS( + const sensor_msgs::CameraInfo & camInfo, + const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity()); +void cameraModelToROS( + const rtabmap::CameraModel & model, + sensor_msgs::CameraInfo & camInfo); + +rtabmap::StereoCameraModel stereoCameraModelFromROS( + const sensor_msgs::CameraInfo & leftCamInfo, + const sensor_msgs::CameraInfo & rightCamInfo, + const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity()); + void mapDataFromROS( const rtabmap_ros::MapData & msg, std::map & poses, @@ -110,6 +131,9 @@ void mapGraphToROS( rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg); void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg); +rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData & msg); +void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg); + rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg); void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg); diff --git a/launch/azimut3/az3_mapping_robot_kinect_scan.launch b/launch/azimut3/az3_mapping_robot_kinect_scan.launch index bb5406dd..1c5158b8 100644 --- a/launch/azimut3/az3_mapping_robot_kinect_scan.launch +++ b/launch/azimut3/az3_mapping_robot_kinect_scan.launch @@ -1,69 +1,131 @@ - + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/azimut3/az3_mapping_robot_nav.launch b/launch/azimut3/az3_nav.launch similarity index 58% rename from launch/azimut3/az3_mapping_robot_nav.launch rename to launch/azimut3/az3_nav.launch index c1dec6a7..4628686a 100644 --- a/launch/azimut3/az3_mapping_robot_nav.launch +++ b/launch/azimut3/az3_nav.launch @@ -1,8 +1,10 @@ - - + + + + @@ -13,59 +15,61 @@ - - + + - - - + + + - - + + - - + + - + - + - - + + - + - - - + + + - - + + - - - + + + - - + - - - - - + + + + - - - - + + + + + + + + @@ -79,17 +83,16 @@ - + - - - - + + + @@ -109,7 +112,7 @@ - + @@ -128,7 +131,7 @@ - + @@ -137,10 +140,10 @@ - - - - + + + + diff --git a/launch/azimut3/az3_mapping_client_nav.launch b/launch/azimut3/az3_nav_client.launch similarity index 100% rename from launch/azimut3/az3_mapping_client_nav.launch rename to launch/azimut3/az3_nav_client.launch diff --git a/launch/azimut3/az3_nav_kinect-only.launch b/launch/azimut3/az3_nav_kinect-only.launch new file mode 100644 index 00000000..402d5087 --- /dev/null +++ b/launch/azimut3/az3_nav_kinect-only.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/azimut3/az3_nav_kinect_odom.launch b/launch/azimut3/az3_nav_kinect_odom.launch new file mode 100644 index 00000000..ad6b7f7f --- /dev/null +++ b/launch/azimut3/az3_nav_kinect_odom.launch @@ -0,0 +1,161 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/azimut3/az3_openni.launch b/launch/azimut3/az3_openni.launch index 8982436d..aae74211 100644 --- a/launch/azimut3/az3_openni.launch +++ b/launch/azimut3/az3_openni.launch @@ -2,6 +2,7 @@ + - \ No newline at end of file + diff --git a/launch/azimut3/config/azimut3_nav.rviz b/launch/azimut3/config/azimut3_nav.rviz index b5f7631a..061978da 100644 --- a/launch/azimut3/config/azimut3_nav.rviz +++ b/launch/azimut3/config/azimut3_nav.rviz @@ -7,7 +7,7 @@ Panels: - /Global Options1 - /TF1/Frames1 Splitter Ratio: 0.601881 - Tree Height: 187 + Tree Height: 353 - Class: rviz/Selection Name: Selection - Class: rviz/Views @@ -19,7 +19,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: Info - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 @@ -52,13 +52,73 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: false + base_footprint: + Value: true + base_laser_link: + Value: true + base_link: + Value: true + camera_depth_frame: + Value: true + camera_depth_optical_frame: + Value: true + camera_link: + Value: true + camera_rgb_frame: + Value: true + camera_rgb_optical_frame: + Value: true + map: + Value: true + odom: + Value: true + wheelLB_linkWheel_link: + Value: true + wheelLB_wheel_link: + Value: true + wheelLF_linkWheel_link: + Value: true + wheelLF_wheel_link: + Value: true + wheelRB_linkWheel_link: + Value: true + wheelRB_wheel_link: + Value: true + wheelRF_linkWheel_link: + Value: true + wheelRF_wheel_link: + Value: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: - {} + map: + odom: + base_footprint: + base_link: + base_laser_link: + {} + camera_link: + camera_depth_frame: + camera_depth_optical_frame: + {} + camera_rgb_frame: + camera_rgb_optical_frame: + {} + wheelLB_linkWheel_link: + wheelLB_wheel_link: + {} + wheelLF_linkWheel_link: + wheelLF_wheel_link: + {} + wheelRB_linkWheel_link: + wheelRB_wheel_link: + {} + wheelRF_linkWheel_link: + wheelRF_wheel_link: + {} Update Interval: 0 Value: true - Alpha: 1 @@ -102,18 +162,18 @@ Visualization Manager: Class: rviz/Map Color Scheme: costmap Draw Behind: false - Enabled: false + Enabled: true Name: Global costmap Topic: /planner/move_base/global_costmap/costmap - Value: false + Value: true - Alpha: 0.7 Class: rviz/Map Color Scheme: costmap Draw Behind: false - Enabled: true + Enabled: false Name: Local costmap Topic: /planner/move_base/local_costmap/costmap - Value: true + Value: false - Alpha: 1 Class: rviz/RobotModel Collision Enabled: false @@ -124,6 +184,61 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheelLB_linkWheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheelLB_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheelLF_linkWheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheelLF_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheelRB_linkWheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheelRB_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheelRF_linkWheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheelRF_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Name: RobotModel Robot Description: robot_description TF Prefix: "" @@ -132,7 +247,7 @@ Visualization Manager: Visual Enabled: true - Class: rviz/Image Enabled: true - Image Topic: /camera/data_resized_image_relay + Image Topic: /camera/throttled_image Max Value: 1 Median window: 5 Min Value: 0 @@ -171,7 +286,7 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.01 Style: Points - Topic: /rtabmap/mapData_relay + Topic: /rtabmap/mapData Use Fixed Frame: true Use rainbow: true Value: true @@ -185,7 +300,13 @@ Visualization Manager: Class: rviz/Path Color: 255; 149; 57 Enabled: true + Line Style: Lines + Line Width: 0.03 Name: move_base global plan + Offset: + X: 0 + Y: 0 + Z: 0 Topic: /planner/move_base/NavfnROS/plan Value: true - Alpha: 1 @@ -193,7 +314,13 @@ Visualization Manager: Class: rviz/Path Color: 2; 14; 255 Enabled: true + Line Style: Lines + Line Width: 0.03 Name: move_base local plan + Offset: + X: 0 + Y: 0 + Z: 0 Topic: /planner/move_base/TrajectoryPlannerROS/local_plan Value: true - Alpha: 1 @@ -227,17 +354,28 @@ Visualization Manager: Value: true - Alpha: 1 Class: rtabmap_ros/MapGraph - Color: 0; 0; 255 Enabled: true + Global loop closure: 255; 0; 0 + Local loop closure: 255; 255; 0 + Merged neighbor: 255; 170; 0 Name: MapGraph - Topic: /rtabmap/mapData_relay + Neighbor: 0; 0; 255 + Topic: /rtabmap/mapGraph + User: 255; 0; 0 Value: true + Virtual: 255; 0; 255 - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 255 Enabled: true + Line Style: Lines + Line Width: 0.03 Name: Rtabmap global path + Offset: + X: 0 + Y: 0 + Z: 0 Topic: /rtabmap/global_path Value: true - Alpha: 1 @@ -245,7 +383,13 @@ Visualization Manager: Class: rviz/Path Color: 85; 255; 255 Enabled: true + Line Style: Lines + Line Width: 0.03 Name: Rtabmap local path + Offset: + X: 0 + Y: 0 + Z: 0 Topic: /rtabmap/local_path Value: true - Alpha: 1 @@ -308,10 +452,10 @@ Visualization Manager: Z: -0.0856586 Name: Current View Near Clip Distance: 0.01 - Pitch: 0.884797 + Pitch: 1.2448 Target Frame: base_footprint Value: Orbit (rviz) - Yaw: 3.81544 + Yaw: 3.76044 Saved: ~ Window Geometry: Displays: @@ -321,7 +465,7 @@ Window Geometry: Hide Right Dock: false Image: collapsed: false - QMainWindow State: 000000ff00000000fd000000040000000000000151000002dafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000000fc000000dd00fffffffb0000000a0049006d006100670065010000012a0000012d0000001600fffffffb0000000a0049006d0061006700650000000184000000490000000000000000fb0000000a0049006d006100670065010000027d000000fa0000000000000000fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000025d000000a50000006400ffffff000000010000010f000001b2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000001b2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000375000002da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000151000002dafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001a2000000dd00fffffffb0000000a0049006d00610067006501000001d0000000870000001600fffffffb0000000a0049006d0061006700650000000184000000490000000000000000fb0000000a0049006d006100670065010000027d000000fa0000000000000000fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000025d000000a50000006400ffffff000000010000010f000001b2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000001b2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000375000002da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -331,5 +475,5 @@ Window Geometry: Views: collapsed: false Width: 1228 - X: 406 - Y: 154 + X: 396 + Y: 144 diff --git a/launch/azimut3/config/costmap_common_params_2d.yaml b/launch/azimut3/config/costmap_common_params_2d.yaml index 7c7e531d..9084d0d8 100644 --- a/launch/azimut3/config/costmap_common_params_2d.yaml +++ b/launch/azimut3/config/costmap_common_params_2d.yaml @@ -1,7 +1,5 @@ footprint: [[ 0.3, 0.3], [-0.3, 0.3], [-0.3, -0.3], [ 0.3, -0.3]] -footprint_padding: 0.02 -#robot_radius: 0.38 -#robot_radius: ir_of_robot +footprint_padding: 0.04 inflation_layer: inflation_radius: 0.7 # 2xfootprint, it helps to keep the global planned path farther from obstacles transform_tolerance: 2 @@ -16,8 +14,8 @@ obstacle_layer: laser_scan_sensor: { data_type: LaserScan, - topic: base_scan, - expected_update_rate: 0.2, + topic: scan, + expected_update_rate: 0.1, marking: true, clearing: true } @@ -39,7 +37,7 @@ obstacle_layer: expected_update_rate: 0.5, marking: false, clearing: true, - min_obstacle_height: -1.0 # make usre the ground is not filtered + min_obstacle_height: -1.0 # make sure the ground is not filtered } diff --git a/launch/azimut3/config/global_costmap_params.yaml b/launch/azimut3/config/global_costmap_params.yaml index 643ec7e6..12895d12 100644 --- a/launch/azimut3/config/global_costmap_params.yaml +++ b/launch/azimut3/config/global_costmap_params.yaml @@ -2,7 +2,7 @@ global_frame: map robot_base_frame: base_footprint update_frequency: 1 -publish_frequency: 2 +publish_frequency: 1 always_send_full_costmap: false plugins: - {name: static_layer, type: "rtabmap_ros::StaticLayer"} diff --git a/launch/config/appearance_gui.ini b/launch/config/appearance_gui.ini index e0ecf09c..9e379330 100644 --- a/launch/config/appearance_gui.ini +++ b/launch/config/appearance_gui.ini @@ -6,17 +6,12 @@ General\loggerPauseLevel=4 General\loggerType=1 General\loggerPrintTime=true General\verticalLayoutUsed=false -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\x2\xaf\0\0\x1\xf4\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\x1\xf4\0\0\x1\xcc\0\xff\xff\xff\0\0\0\x1\0\0\x5\0\0\0\x1\xf0\xfc\x2\0\0\0\x3\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\0\0\0\0(\0\0\x1\xf4\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\0\0\xff\xff\xff\xff\0\0\0\xf7\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\0(\0\0\x1\xf0\0\0\0y\0\xff\xff\xff\0\0\0\x3\0\0\x5\0\0\0\0\x9c\xfc\x1\0\0\0\x6\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\x1\0\0\0\0\0\0\x5\0\0\0\0\x8d\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\xff\xff\xff\xff\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\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\0O\0\xff\xff\xff\0\0\0\0\0\0\x1\xf0\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\0\xbd\0\0\0x\0\0\x5\xcc\0\0\x3k\0\0\0\xc5\0\0\0\x94\0\0\x5\xc4\0\0\x3\x63\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\x1\a\0\0\x2\x30\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_\0s\0t\0\x61\0t\0s\0V\0\x32\0\0\0\0(\0\0\x2\x30\0\0\x2\x30\0\xff\xff\xff\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\0\0\0\0(\0\0\x1\xf4\0\0\0\x19\0\xff\xff\xff\0\0\0\x1\0\0\x5\0\0\0\x2\x30\xfc\x2\0\0\0\x3\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\0\0\0\0(\0\0\x1\xf4\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\0\0\xff\xff\xff\xff\0\0\0\xf7\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\0(\0\0\x2\x30\0\0\0+\0\xff\xff\xff\0\0\0\x3\0\0\x5\0\0\0\0\x9c\xfc\x1\0\0\0\x6\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\x1\0\0\0\0\0\0\x5\0\0\0\0\x8d\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\xff\xff\xff\xff\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\0`\0\xff\xff\xff\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\0O\0\xff\xff\xff\0\0\0\0\0\0\x2\x30\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\xc5\0\0\0K\0\0\x5\xd8\0\0\x3t\0\0\0\xcf\0\0\0q\0\0\x5\xce\0\0\x3j\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=2 General\maxDepth1=0 General\showScans1=true @@ -33,12 +27,140 @@ General\opacity1=1 General\ptSize1=1 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=false General\cloudFilteringRadius=0.5 General\cloudFilteringAngle=30 +MainWindow\maximized=false +MainWindow\status_bar=false +PreferencesDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0\0\0\0\0\0\0\0\x3\xd7\0\0\x2\xb4\0\0\0\0\0\0\0\0\0\0\x3\xd7\0\0\x2\xb4\0\0\0\0\0\0) +AboutDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0\0\0\0\0\0\0\0\x3>\0\0\x2\xa8\0\0\0\0\0\0\0\0\0\0\x3>\0\0\x2\xa8\0\0\0\0\0\0) +widget_cloudViewer\camera_pose=@Variant(\0\0\0T\xbf\xf0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0) +widget_cloudViewer\camera_focal=@Variant(\0\0\0T\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\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=1 +widget_cloudViewer\trajectory_shown=true +widget_cloudViewer\trajectory_size=100 +widget_cloudViewer\camera_target_locked=false +widget_cloudViewer\camera_target_follow=true +widget_cloudViewer\camera_free=false +widget_cloudViewer\camera_lockZ=true +widget_cloudViewer\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0) +imageView_source\image_shown=true +imageView_source\depth_shown=false +imageView_source\features_shown=true +imageView_source\lines_shown=true +imageView_source\alpha=50 +imageView_source\graphics_view=false +imageView_source\graphics_view_scale=true +imageView_loopClosure\image_shown=true +imageView_loopClosure\depth_shown=false +imageView_loopClosure\features_shown=true +imageView_loopClosure\lines_shown=true +imageView_loopClosure\alpha=50 +imageView_loopClosure\graphics_view=false +imageView_loopClosure\graphics_view_scale=true +imageView_odometry\image_shown=true +imageView_odometry\depth_shown=false +imageView_odometry\features_shown=true +imageView_odometry\lines_shown=true +imageView_odometry\alpha=200 +imageView_odometry\graphics_view=false +imageView_odometry\graphics_view_scale=true +ExportCloudsDialog\binary=true +ExportCloudsDialog\normals_k=6 +ExportCloudsDialog\regenerate=false +ExportCloudsDialog\regenerate_decimation=1 +ExportCloudsDialog\regenerate_max_depth=4 +ExportCloudsDialog\filtering=false +ExportCloudsDialog\filtering_radius=0.02 +ExportCloudsDialog\filtering_min_neighbors=2 +ExportCloudsDialog\assemble=true +ExportCloudsDialog\assemble_voxel=0.01 +ExportCloudsDialog\subtract=false +ExportCloudsDialog\subtract_point_radius=0.02 +ExportCloudsDialog\subtract_point_angle=45 +ExportCloudsDialog\subtract_min_neighbors=5 +ExportCloudsDialog\mls=false +ExportCloudsDialog\mls_radius=0.04 +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=false +ExportCloudsDialog\mesh_radius=0.04 +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 +PostProcessingDialog\detect_more_lc=true +PostProcessingDialog\cluster_radius=0.5 +PostProcessingDialog\cluster_angle=30 +PostProcessingDialog\iterations=1 +PostProcessingDialog\reextract_features=false +PostProcessingDialog\refine_neigbors=false +PostProcessingDialog\refine_lc=false +PostProcessingDialog\sba=false +PostProcessingDialog\sba_iterations=20 +PostProcessingDialog\sba_epsilon=0.0001 +PostProcessingDialog\sba_inlier_distance=0.05 +PostProcessingDialog\sba_min_inliers=10 +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) +General\loggerPrintThreadId=false +General\notifyNewGlobalPath=false +General\odomQualityThr=50 +General\posteriorGraphView=true +General\showFeatures0=false +General\downsamplingScan0=1 +General\voxelSizeScan0=0 +General\ptSizeFeatures0=3 +General\showFeatures1=true +General\downsamplingScan1=1 +General\voxelSizeScan1=0 +General\ptSizeFeatures1=3 +General\showGraphs=true +General\showLabels=false +General\noFiltering=true +General\subtractFiltering=false +General\subtractFilteringMinPts=5 +General\subtractFilteringRadius=0.02 +General\subtractFilteringAngle=45 +General\gridMapShown=false +General\gridMapResolution=0.05 +General\gridMapOccupancyFrom3DCloud=false +General\gridMapEroded=false +General\gridMapOpacity=0.75 +General\meshing=false +General\meshing_angle=15 +General\meshing_quad=false +General\meshing_triangle_size=2 +Figures\counts=1 +Figures\curves=Loop/Highest_hypothesis_value/ diff --git a/launch/config/demo_stereo_outdoor.rviz b/launch/config/demo_stereo_outdoor.rviz index 1b8076c3..ce7efce9 100644 --- a/launch/config/demo_stereo_outdoor.rviz +++ b/launch/config/demo_stereo_outdoor.rviz @@ -6,7 +6,6 @@ Panels: Expanded: - /Global Options1 - /Status1 - - /Info1 Splitter Ratio: 0.5 Tree Height: 438 - Class: rviz/Selection @@ -134,6 +133,7 @@ Visualization Manager: Download graph: false Download map: false Enabled: true + Filter ceiling (m): 0 Filter floor (m): 0 Invert Rainbow: false Max Color: 255; 255; 255 @@ -147,17 +147,22 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.01 Style: Points - Topic: /rtabmap/mapData_optimized + Topic: /rtabmap/mapData Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 1 Class: rtabmap_ros/MapGraph - Color: 25; 255; 0 Enabled: true + Global loop closure: 255; 0; 0 + Local loop closure: 255; 255; 0 + Merged neighbor: 255; 170; 0 Name: MapGraph - Topic: /rtabmap/mapDataGraph_optimized + Neighbor: 0; 0; 255 + Topic: /rtabmap/mapGraph + User: 255; 0; 0 Value: true + Virtual: 255; 0; 255 - Class: rviz/Image Enabled: true Image Topic: /stereo_camera/left/image_rect_color @@ -173,15 +178,73 @@ Visualization Manager: Class: rviz/Map Color Scheme: map Draw Behind: false - Enabled: false + Enabled: true Name: Map - Topic: /map - Value: false + Topic: /rtabmap/proj_map + Value: true - Class: rtabmap_ros/Info Enabled: true Name: Info Topic: /rtabmap/info Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 1.17745 + Min Value: -1.72299 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: OdomMap + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /rtabmap/odom_local_map + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 85; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: OdomFrame + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /rtabmap/odom_last_frame + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -206,7 +269,7 @@ Visualization Manager: Views: Current: Class: rtabmap_ros/OrbitOriented - Distance: 6.60197 + Distance: 8.28384 Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 @@ -218,10 +281,10 @@ Visualization Manager: Z: 0.113349 Name: Current View Near Clip Distance: 0.01 - Pitch: 0.455398 + Pitch: 0.635398 Target Frame: base_footprint Value: OrbitOriented (rtabmap) - Yaw: 3.1304 + Yaw: 3.0704 Saved: ~ Window Geometry: Displays: @@ -241,5 +304,5 @@ Window Geometry: Views: collapsed: false Width: 1341 - X: 147 - Y: 48 + X: 97 + Y: 14 diff --git a/launch/data_recorder.launch b/launch/data_recorder.launch index c95294fe..4963d293 100644 --- a/launch/data_recorder.launch +++ b/launch/data_recorder.launch @@ -4,7 +4,8 @@ - + + @@ -30,13 +31,17 @@ - - - - - + + + + + + + + + @@ -44,9 +49,10 @@ - + + diff --git a/launch/demo/demo_appearance_localization.launch b/launch/demo/demo_appearance_localization.launch deleted file mode 100644 index 97b6949f..00000000 --- a/launch/demo/demo_appearance_localization.launch +++ /dev/null @@ -1,47 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/demo/demo_appearance_mapping.launch b/launch/demo/demo_appearance_mapping.launch index 78f5f102..5a6d555c 100644 --- a/launch/demo/demo_appearance_mapping.launch +++ b/launch/demo/demo_appearance_mapping.launch @@ -4,27 +4,35 @@ + + + + + - + - + - + - - - - - - - - - - + + + + + + + + + + + + + @@ -40,11 +48,12 @@ - + + - - - - + + + + diff --git a/launch/demo/demo_data_recorder.launch b/launch/demo/demo_data_recorder.launch index a2a93fb3..e0c3cc4e 100644 --- a/launch/demo/demo_data_recorder.launch +++ b/launch/demo/demo_data_recorder.launch @@ -10,7 +10,7 @@ - + @@ -20,7 +20,6 @@ - diff --git a/launch/demo/demo_find_object.launch b/launch/demo/demo_find_object.launch index 04a69264..abe1d058 100644 --- a/launch/demo/demo_find_object.launch +++ b/launch/demo/demo_find_object.launch @@ -1,8 +1,12 @@ - + + + + + @@ -10,39 +14,56 @@ - + - - + + - + - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -51,16 +72,15 @@ --> - + - - + - + @@ -69,16 +89,16 @@ - - + + - + - - + + - + diff --git a/launch/demo/demo_hector_mapping.launch b/launch/demo/demo_hector_mapping.launch index e9a72bcd..40c8ba56 100644 --- a/launch/demo/demo_hector_mapping.launch +++ b/launch/demo/demo_hector_mapping.launch @@ -49,37 +49,39 @@ - + - - + + - - - + + + + + - + - + - - + + - - + + - + @@ -93,7 +95,7 @@ - + diff --git a/launch/demo/demo_multi-session_mapping.launch b/launch/demo/demo_multi-session_mapping.launch index d23aa59e..cbf3fa24 100644 --- a/launch/demo/demo_multi-session_mapping.launch +++ b/launch/demo/demo_multi-session_mapping.launch @@ -17,57 +17,56 @@ - + - - + + - + - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + - - - + + + + - - - + + + - - + + - - + + - + @@ -81,7 +80,7 @@ - + diff --git a/launch/demo/demo_robot_localization.launch b/launch/demo/demo_robot_localization.launch deleted file mode 100644 index 23b8dbe4..00000000 --- a/launch/demo/demo_robot_localization.launch +++ /dev/null @@ -1,83 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/demo/demo_robot_mapping.launch b/launch/demo/demo_robot_mapping.launch index a8fc6c13..0cbd6e56 100644 --- a/launch/demo/demo_robot_mapping.launch +++ b/launch/demo/demo_robot_mapping.launch @@ -10,51 +10,63 @@ + + + + + - - + + - + - - + + - + - - - - - - - - + + + + + + + + + + + + + + + - - - + + + - - + + - - + + - + @@ -67,7 +79,7 @@ - + diff --git a/launch/demo/demo_stereo_outdoor.launch b/launch/demo/demo_stereo_outdoor.launch index ed05a852..95877fc2 100644 --- a/launch/demo/demo_stereo_outdoor.launch +++ b/launch/demo/demo_stereo_outdoor.launch @@ -21,7 +21,7 @@ - + @@ -36,88 +36,69 @@ - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + - + - + - - - + + + - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + - + - - - - - + + + + + + - - - + + + diff --git a/launch/demo/demo_turtlebot_mapping.launch b/launch/demo/demo_turtlebot_mapping.launch index 81d8b6c2..5b29e180 100644 --- a/launch/demo/demo_turtlebot_mapping.launch +++ b/launch/demo/demo_turtlebot_mapping.launch @@ -25,9 +25,13 @@ - - + + + @@ -42,7 +46,7 @@ - + @@ -51,21 +55,22 @@ - - + - + - - - - + + + + - + + + @@ -75,25 +80,21 @@ - - - - + + + + + - - - - - - - - + + + diff --git a/launch/demo/demo_two_kinects.launch b/launch/demo/demo_two_kinects.launch index 63d77cc7..e08f430e 100644 --- a/launch/demo/demo_two_kinects.launch +++ b/launch/demo/demo_two_kinects.launch @@ -26,7 +26,7 @@ diff --git a/launch/rgbd_mapping.launch b/launch/rgbd_mapping.launch index e1e68830..2b0a43fc 100644 --- a/launch/rgbd_mapping.launch +++ b/launch/rgbd_mapping.launch @@ -9,6 +9,9 @@ + + + @@ -29,91 +32,82 @@ + + + - + - - - - - - - - - - - - + - + - - + + - - - - - - - - - - + - - - - - + + + + + + + - - - - - - - + + + + + + + + - - - - + + + + + + + - - - - + + + + + + diff --git a/launch/rgbd_mapping_kinect2.launch b/launch/rgbd_mapping_kinect2.launch index db42349e..55c76c37 100644 --- a/launch/rgbd_mapping_kinect2.launch +++ b/launch/rgbd_mapping_kinect2.launch @@ -30,7 +30,7 @@ diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch new file mode 100644 index 00000000..7fab41d8 --- /dev/null +++ b/launch/rtabmap.launch @@ -0,0 +1,187 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/stereo_mapping.launch b/launch/stereo_mapping.launch index d3cc7f89..f5d8ad5a 100644 --- a/launch/stereo_mapping.launch +++ b/launch/stereo_mapping.launch @@ -9,6 +9,9 @@ + + + @@ -18,6 +21,7 @@ + @@ -26,28 +30,20 @@ + + + + - + - - - - - - - - - - - - @@ -55,7 +51,7 @@ - + @@ -65,57 +61,56 @@ - - - - - - - - - - + - - - - - + + + + + + - - + + + - - - - - - + + + + + + + + - - - - + + + + + + + - - - - - - + + + + + + + @@ -123,6 +118,7 @@ + diff --git a/launch/tests/bumblebee.launch b/launch/tests/bumblebee.launch index fe3eb9f9..467f2e45 100644 --- a/launch/tests/bumblebee.launch +++ b/launch/tests/bumblebee.launch @@ -10,15 +10,16 @@ + - + - - + + @@ -28,13 +29,12 @@ - + + + - - - - \ No newline at end of file + diff --git a/launch/tests/rgbdslam_datasets.launch b/launch/tests/rgbdslam_datasets.launch index a026a383..66a01b20 100644 --- a/launch/tests/rgbdslam_datasets.launch +++ b/launch/tests/rgbdslam_datasets.launch @@ -10,20 +10,7 @@ --> - - - - - - - + @@ -40,13 +27,18 @@ + - - - - - - + + + + + + + + + + @@ -64,15 +56,20 @@ + + + + + + + + - - - - + @@ -89,7 +86,7 @@ - + diff --git a/launch/tests/test_odometry.launch b/launch/tests/test_odometry.launch deleted file mode 100644 index 7cce2a8b..00000000 --- a/launch/tests/test_odometry.launch +++ /dev/null @@ -1,61 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/tests/test_stereo_data_recorder.launch b/launch/tests/test_stereo_data_recorder.launch deleted file mode 100644 index 0f523a58..00000000 --- a/launch/tests/test_stereo_data_recorder.launch +++ /dev/null @@ -1,56 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/launch/tests/test_stereo_odometry.launch b/launch/tests/test_stereo_odometry.launch deleted file mode 100644 index ff12a1c4..00000000 --- a/launch/tests/test_stereo_odometry.launch +++ /dev/null @@ -1,48 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/msg/Info.msg b/msg/Info.msg index 8df59454..ef233374 100644 --- a/msg/Info.msg +++ b/msg/Info.msg @@ -7,7 +7,7 @@ Header header int32 refId int32 loopClosureId -int32 localLoopClosureId +int32 proximityDetectionId geometry_msgs/Transform loopClosureTransform diff --git a/msg/NodeData.msg b/msg/NodeData.msg index de26a52d..2265fd71 100644 --- a/msg/NodeData.msg +++ b/msg/NodeData.msg @@ -8,6 +8,9 @@ string label # Pose from odometry not corrected geometry_msgs/Pose pose +# Ground truth (optional) +geometry_msgs/Pose groundTruthPose + # compressed image in /camera_link frame # use rtabmap::util3d::uncompressImage() from "rtabmap/core/util3d.h" uint8[] image diff --git a/msg/OdomInfo.msg b/msg/OdomInfo.msg index e5e3e09f..e3107457 100644 --- a/msg/OdomInfo.msg +++ b/msg/OdomInfo.msg @@ -42,6 +42,8 @@ int32[] wordsKeys KeyPoint[] wordsValues int32[] wordMatches int32[] wordInliers +int32[] localMapKeys +Point3f[] localMapValues Point2f[] refCorners Point2f[] newCorners diff --git a/msg/Point3f.msg b/msg/Point3f.msg new file mode 100644 index 00000000..eca5d135 --- /dev/null +++ b/msg/Point3f.msg @@ -0,0 +1,10 @@ +#class cv::Point3f +#{ +# float x; +# float y; +# float z; +#} + +float32 x +float32 y +float32 z \ No newline at end of file diff --git a/package.xml b/package.xml index e972177b..cdc2bef6 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap_ros - 0.10.10 + 0.11.5 RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe @@ -37,7 +37,8 @@ class_loader rtabmap move_base_msgs - costmap_2d + + octomap_ros octomap @@ -67,7 +68,7 @@ class_loader rtabmap move_base_msgs - costmap_2d + octomap_ros octomap diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index 98b67868..21e2dc3f 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -193,7 +193,7 @@ public: if(!path.empty() && UDirectory::exists(path)) { //images - camera_ = new rtabmap::CameraImages(path, 1, false, false, false, frameRate); + camera_ = new rtabmap::CameraImages(path, frameRate); } else if(!path.empty() && UFile::exists(path)) { diff --git a/src/CoreNode.cpp b/src/CoreNode.cpp index 66f53b27..028cde83 100644 --- a/src/CoreNode.cpp +++ b/src/CoreNode.cpp @@ -48,14 +48,6 @@ int main(int argc, char** argv) { deleteDbOnStart = true; } - else if(strcmp(argv[i], "--udebug") == 0) - { - ULogger::setLevel(ULogger::kDebug); - } - else if(strcmp(argv[i], "--uinfo") == 0) - { - ULogger::setLevel(ULogger::kInfo); - } else if(strcmp(argv[i], "--params") == 0 || strcmp(argv[i], "--params-all") == 0) { rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters(); @@ -94,14 +86,11 @@ int main(int argc, char** argv) "argument \"--params\" is detected!"); exit(0); } - else - { - ROS_ERROR("Not recognized argument \"%s\"", argv[i]); - exit(-1); - } } - CoreWrapper * rtabmap = new CoreWrapper(deleteDbOnStart); + rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argc, argv); + + CoreWrapper * rtabmap = new CoreWrapper(deleteDbOnStart, parameters); ROS_INFO("rtabmap %s started...", RTABMAP_VERSION); ros::spin(); diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index dd5e9a6a..ed9b1248 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -54,12 +54,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include #ifdef WITH_OCTOMAP #include #endif +#define BAD_COVARIANCE 9999 //msgs #include "rtabmap_ros/Info.h" @@ -72,23 +72,27 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. using namespace rtabmap; -CoreWrapper::CoreWrapper(bool deleteDbOnStart) : +CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) : paused_(false), lastPose_(Transform::getIdentity()), + lastPoseIntermediate_(false), rotVariance_(0), transVariance_(0), latestNodeWasReached_(false), frameId_("base_link"), mapFrameId_("map"), odomFrameId_(""), + groundTruthFrameId_(""), // e.g., "world" configPath_(""), databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()), waitForTransform_(true), - waitForTransformDuration_(0.1), // 100 ms + waitForTransformDuration_(0.2), // 200 ms useActionForGoal_(false), genScan_(false), genScanMaxDepth_(4.0), + genScanMinDepth_(0.0), mapToOdom_(rtabmap::Transform::getIdentity()), + mapsManager_(true), depthSync_(0), depthScanSync_(0), stereoScanSync_(0), @@ -102,13 +106,16 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : stereoExactTFSync_(0), transformThread_(0), rate_(Parameters::defaultRtabmapDetectionRate()), + createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()), time_(ros::Time::now()), + previousStamp_(0), mbClient_("move_base", true) { ros::NodeHandle nh; ros::NodeHandle pnh("~"); - bool subscribeLaserScan = false; + bool subscribeScan2d = false; + bool subscribeScan3d = false; bool subscribeDepth = true; bool subscribeStereo = false; int depthCameras = 1; @@ -120,14 +127,24 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : // ROS related parameters (private) pnh.param("subscribe_depth", subscribeDepth, subscribeDepth); - pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan); + if(pnh.getParam("subscribe_laserScan", subscribeScan2d) && subscribeScan2d) + { + ROS_WARN("rtabmap: \"subscribe_laserScan\" parameter is deprecated, use \"subscribe_scan\" instead. The scan topic is still subscribed."); + } + pnh.param("subscribe_scan", subscribeScan2d, subscribeScan2d); + pnh.param("subscribe_scan_cloud", subscribeScan3d, subscribeScan3d); pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo); if(subscribeDepth && subscribeStereo) { - UWARN("Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false."); + ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false."); subscribeDepth = false; } - if(subscribeLaserScan) + if(subscribeScan2d && subscribeScan3d) + { + ROS_WARN("rtabmap: Parameters subscribe_scan and subscribe_scan_cloud cannot be true at the same time. Parameter subscribe_scan_cloud is set to false."); + subscribeScan3d = false; + } + if(subscribeScan2d || subscribeScan3d) { if(!subscribeDepth && !subscribeStereo) { @@ -142,6 +159,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : pnh.param("frame_id", frameId_, frameId_); pnh.param("map_frame_id", mapFrameId_, mapFrameId_); pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF + 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); @@ -154,6 +172,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_); pnh.param("gen_scan", genScan_, genScan_); pnh.param("gen_scan_max_depth", genScanMaxDepth_, genScanMaxDepth_); + pnh.param("gen_scan_min_depth", genScanMinDepth_, genScanMinDepth_); if(!tfPrefix.empty()) { @@ -169,6 +188,11 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : { odomFrameId_ = tfPrefix+"/"+odomFrameId_; } + if(!groundTruthFrameId_.empty()) + { + groundTruthFrameId_ = tfPrefix+"/"+groundTruthFrameId_; + } + // keep worldFrameId_ without prefix as it should be global } if(depthCameras <= 0 && subscribeDepth) @@ -181,6 +205,10 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : { ROS_INFO("rtabmap: odom_frame_id = %s", odomFrameId_.c_str()); } + if(!groundTruthFrameId_.empty()) + { + ROS_INFO("rtabmap: ground_truth_frame_id = %s", groundTruthFrameId_.c_str()); + } ROS_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str()); ROS_INFO("rtabmap: queue_size = %d", queueSize); ROS_INFO("rtabmap: tf_delay = %f", tfDelay); @@ -256,84 +284,40 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : } } + //update with input arguments + for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) + { + uInsert(parameters_, ParametersPair(iter->first, iter->second)); + ROS_INFO("Update RTAB-Map parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str()); + } + // Backward compatibility - std::list oldParameterNames; - oldParameterNames.push_back("LccReextract/LoopClosureFeatures"); - oldParameterNames.push_back("Rtabmap/DetectorStrategy"); - oldParameterNames.push_back("RGBD/ScanMatchingSize"); - oldParameterNames.push_back("RGBD/LocalLoopDetectionRadius"); - oldParameterNames.push_back("RGBD/ToroIterations"); - oldParameterNames.push_back("Mem/RehearsedNodesKept"); - oldParameterNames.push_back("Odom/PnPEstimation"); - oldParameterNames.push_back("LccBow/MaxDepth"); - oldParameterNames.push_back("GFTT/MaxCorners"); - for(std::list::iterator iter=oldParameterNames.begin(); iter!=oldParameterNames.end(); ++iter) + for(std::map >::const_iterator iter=Parameters::getRemovedParameters().begin(); + iter!=Parameters::getRemovedParameters().end(); + ++iter) { std::string vStr; - if(pnh.getParam(*iter, vStr)) + if(pnh.getParam(iter->first, vStr)) { - if(iter->compare("GFTT/MaxCorners") == 0) + if(iter->second.first) { - ROS_WARN("Parameter name changed: GFTT/MaxCorners -> %s. Please update your launch file accordingly.", - Parameters::kKpWordsPerImage().c_str()); + // can be migrated + parameters_.at(iter->second.second)= vStr; + ROS_WARN("Rtabmap: 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->compare("LccBow/MaxDepth") == 0) + else { - ROS_WARN("Parameter name changed: LccBow/MaxDepth -> %s. Please update your launch file accordingly.", - Parameters::kLccReextractMaxDepth().c_str()); - parameters_.at(Parameters::kLccReextractMaxDepth())= vStr; - } - else if(iter->compare("LccReextract/LoopClosureFeatures") == 0) - { - ROS_WARN("Parameter name changed: LccReextract/LoopClosureFeatures -> %s. Please update your launch file accordingly.", - Parameters::kLccReextractActivated().c_str()); - parameters_.at(Parameters::kLccReextractActivated())= vStr; - } - else if(iter->compare("Rtabmap/DetectorStrategy") == 0) - { - ROS_WARN("Parameter name changed: Rtabmap/DetectorStrategy -> %s. Please update your launch file accordingly.", - Parameters::kKpDetectorStrategy().c_str()); - parameters_.at(Parameters::kKpDetectorStrategy())= vStr; - } - else if(iter->compare("RGBD/ScanMatchingSize") == 0) - { - ROS_WARN("Parameter name changed: RGBD/ScanMatchingSize -> %s. Please update your launch file accordingly.", - Parameters::kRGBDPoseScanMatching().c_str()); - parameters_.at(Parameters::kRGBDPoseScanMatching())= std::atoi(vStr.c_str()) > 0?"true":"false"; - } - else if(iter->compare("RGBD/LocalLoopDetectionRadius") == 0) - { - ROS_WARN("Parameter name changed: RGBD/LocalLoopDetectionRadius -> %s. Please update your launch file accordingly.", - Parameters::kRGBDLocalRadius().c_str()); - parameters_.at(Parameters::kRGBDLocalRadius())= vStr; - } - else if(iter->compare("RGBD/ToroIterations") == 0) - { - ROS_WARN("Parameter name changed: RGBD/ToroIterations -> %s. Please update your launch file accordingly.", - Parameters::kRGBDOptimizeIterations().c_str()); - parameters_.at(Parameters::kRGBDOptimizeIterations())= vStr; - } - else if(iter->compare("Mem/RehearsedNodesKept") == 0) - { - ROS_WARN("Parameter name changed: Mem/RehearsedNodesKept -> %s. Please update your launch file accordingly.", - Parameters::kMemNotLinkedNodesKept().c_str()); - parameters_.at(Parameters::kMemNotLinkedNodesKept())= vStr; - } - else if(iter->compare("RGBD/LocalLoopDetectionMaxDiffID") == 0) - { - ROS_WARN("Parameter name changed: RGBD/LocalLoopDetectionMaxDiffID -> %s. Please update your launch file accordingly.", - Parameters::kRGBDLocalLoopDetectionMaxGraphDepth().c_str()); - parameters_.at(Parameters::kRGBDLocalLoopDetectionMaxGraphDepth())= vStr; - } - else if(iter->compare("RGBD/PlanVirtualLinksMaxDiffID") == 0) - { - ROS_WARN("Parameter \"RGBD/PlanVirtualLinksMaxDiffID\" doesn't exist anymore."); - } - else if(iter->compare("RGBD/LocalLoopDetectionMaxDiffID") == 0) - { - ROS_WARN("Parameter name changed: Odom/PnPEstimation -> %s. Please update your launch file accordingly.", - Parameters::kOdomEstimationType().c_str()); - parameters_.at(Parameters::kOdomEstimationType())= uNumber2Str(1); + if(iter->second.second.empty()) + { + ROS_ERROR("Rtabmap: Parameter \"%s\" doesn't exist anymore!", + iter->first.c_str()); + } + else + { + ROS_ERROR("Rtabmap: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", + iter->first.c_str(), iter->second.second.c_str()); + } } } } @@ -346,8 +330,16 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : } if(parameters_.find(Parameters::kRtabmapDetectionRate()) != parameters_.end()) { - rate_ = uStr2Float(parameters_.at(Parameters::kRtabmapDetectionRate())); - ROS_INFO("RTAB-Map rate detection = %f Hz", rate_); + Parameters::parse(parameters_, Parameters::kRtabmapDetectionRate(), rate_); + ROS_INFO("RTAB-Map detection rate = %f Hz", rate_); + } + if(parameters_.find(Parameters::kRtabmapCreateIntermediateNodes()) != parameters_.end()) + { + Parameters::parse(parameters_, Parameters::kRtabmapCreateIntermediateNodes(), createIntermediateNodes_); + if(createIntermediateNodes_) + { + ROS_INFO("Create intermediate nodes"); + } } bool isRGBD = uStr2Bool(parameters_.at(Parameters::kRGBDEnabled()).c_str()); if(isRGBD) @@ -413,11 +405,16 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : octomapBinarySrv_ = nh.advertiseService("octomap_binary", &CoreWrapper::octomapBinaryCallback, this); octomapFullSrv_ = nh.advertiseService("octomap_full", &CoreWrapper::octomapFullCallback, this); #endif + //private services + setLogDebugSrv_ = pnh.advertiseService("log_debug", &CoreWrapper::setLogDebug, this); + setLogInfoSrv_ = pnh.advertiseService("log_info", &CoreWrapper::setLogInfo, this); + setLogWarnSrv_ = pnh.advertiseService("log_warning", &CoreWrapper::setLogWarn, this); + setLogErrorSrv_ = pnh.advertiseService("log_error", &CoreWrapper::setLogError, this); - setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeStereo, queueSize, stereoApproxSync, depthCameras); + setupCallbacks(subscribeDepth, subscribeScan2d, subscribeScan3d, subscribeStereo, queueSize, stereoApproxSync, depthCameras); int optimizeIterations = 0; - Parameters::parse(parameters_, Parameters::kRGBDOptimizeIterations(), optimizeIterations); + Parameters::parse(parameters_, Parameters::kOptimizerIterations(), optimizeIterations); if(publishTf && optimizeIterations != 0) { transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay)); @@ -425,7 +422,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : else if(publishTf) { UWARN("Graph optimization is disabled (%s=0), the tf between frame \"%s\" and odometry frame will not be published. You can safely ignore this warning if you are using map_optimizer node.", - Parameters::kRGBDOptimizeIterations().c_str(), mapFrameId_.c_str()); + Parameters::kOptimizerIterations().c_str(), mapFrameId_.c_str()); } } @@ -498,7 +495,7 @@ ParametersMap CoreWrapper::loadParameters(const std::string & configFile) { ROS_WARN("Config file doesn't exist! It will be generated..."); } - Rtabmap::readParameters(configFile.c_str(), parameters); + Parameters::readINI(configFile.c_str(), parameters); } // otherwise take default parameters @@ -515,7 +512,7 @@ void CoreWrapper::saveParameters(const std::string & configFile) { printf("Config file doesn't exist, a new one will be created.\n"); } - Rtabmap::writeParameters(configFile.c_str(), parameters_); + Parameters::writeINI(configFile.c_str(), parameters_); } else { @@ -613,18 +610,19 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg) if(!paused_) { Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose); - if(!lastPose_.isIdentity() && odom.isIdentity()) + if(!lastPose_.isIdentity() && !odom.isNull() && (odom.isIdentity() || odomMsg->twist.covariance[0] >= BAD_COVARIANCE)) { - UWARN("Odometry is reset (identity pose detected). Increment map id!"); + UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomMsg->twist.covariance[0]); rtabmap_.triggerNewMap(); rotVariance_ = 0; transVariance_ = 0; } + lastPoseIntermediate_ = false; lastPose_ = odom; lastPoseStamp_ = odomMsg->header.stamp; - double transVariance = uMax3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]); - double rotVariance = uMax3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]); + float transVariance = uMax3(odomMsg->twist.covariance[0], odomMsg->twist.covariance[7], odomMsg->twist.covariance[14]); + float rotVariance = uMax3(odomMsg->twist.covariance[21], odomMsg->twist.covariance[28], odomMsg->twist.covariance[35]); if(uIsFinite(rotVariance) && rotVariance > rotVariance_) { rotVariance_ = rotVariance; @@ -635,14 +633,32 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg) } // Throttle + bool ignoreFrame = false; if(rate_>0.0f) { - if(ros::Time::now() - time_ < ros::Duration(1.0f/rate_)) + if((previousStamp_.toSec() > 0.0 && odomMsg->header.stamp.toSec() > previousStamp_.toSec() && odomMsg->header.stamp - previousStamp_ < ros::Duration(1.0f/rate_)) || + ((previousStamp_.toSec() <= 0.0 || odomMsg->header.stamp.toSec() <= previousStamp_.toSec()) && ros::Time::now() - time_ < ros::Duration(1.0f/rate_))) + { + ignoreFrame = true; + } + } + if(ignoreFrame) + { + if(createIntermediateNodes_) + { + lastPoseIntermediate_ = true; + } + else { return false; } } - time_ = ros::Time::now(); + else if(!ignoreFrame) + { + time_ = ros::Time::now(); + previousStamp_ = odomMsg->header.stamp; + } + return true; } return false; @@ -667,17 +683,36 @@ bool CoreWrapper::commonOdomTFUpdate(const ros::Time & stamp) transVariance_ = 0; } + lastPoseIntermediate_ = false; lastPose_ = odom; lastPoseStamp_ = stamp; - // Throttle + + bool ignoreFrame = false; if(rate_>0.0f) { - if(ros::Time::now() - time_ < ros::Duration(1.0f/rate_)) + if((previousStamp_.toSec() > 0.0 && stamp.toSec() > previousStamp_.toSec() && stamp - previousStamp_ < ros::Duration(1.0f/rate_)) || + ((previousStamp_.toSec() <= 0.0 || stamp.toSec() <= previousStamp_.toSec()) && ros::Time::now() - time_ < ros::Duration(1.0f/rate_))) + { + ignoreFrame = true; + } + } + if(ignoreFrame) + { + if(createIntermediateNodes_) + { + lastPoseIntermediate_ = true; + } + else { return false; } } - time_ = ros::Time::now(); + else if(!ignoreFrame) + { + time_ = ros::Time::now(); + previousStamp_ = stamp; + } + return true; } return false; @@ -694,7 +729,8 @@ Transform CoreWrapper::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("rtabmap: Could not get transform from %s to %s after %f second!", fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_); + ROS_WARN("rtabmap: Could not get transform from %s to %s after %f seconds (for stamp=%f)!", + fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_, stamp.toSec()); return transform; } } @@ -715,7 +751,8 @@ void CoreWrapper::commonDepthCallback( const sensor_msgs::ImageConstPtr& imageMsg, const sensor_msgs::ImageConstPtr& depthMsg, const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, - const sensor_msgs::LaserScanConstPtr& scanMsg) + const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg) { std::vector imageMsgs; std::vector depthMsgs; @@ -723,14 +760,15 @@ void CoreWrapper::commonDepthCallback( imageMsgs.push_back(imageMsg); depthMsgs.push_back(depthMsg); cameraInfoMsgs.push_back(cameraInfoMsg); - commonDepthCallback(odomFrameId, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg); + commonDepthCallback(odomFrameId, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg); } void CoreWrapper::commonDepthCallback( const std::string & odomFrameId, const std::vector & imageMsgs, const std::vector & depthMsgs, const std::vector & cameraInfoMsgs, - const sensor_msgs::LaserScanConstPtr& scanMsg) + const sensor_msgs::LaserScanConstPtr& scan2dMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg) { UASSERT(imageMsgs.size()>0 && imageMsgs.size() == depthMsgs.size() && @@ -749,7 +787,7 @@ void CoreWrapper::commonDepthCallback( int cameraCount = imageMsgs.size(); cv::Mat rgb; cv::Mat depth; - pcl::PointCloud scanCloud; + pcl::PointCloud scanCloud2d; std::vector cameraModels; int genMaxScanPts = 0; for(unsigned int i=0; iheader.frame_id, depthMsgs[i]->header.stamp); if(localTransform.isNull()) { + ROS_ERROR("TF of received depth image %d at time %fs is not set, aborting rtabmap update.", i, depthMsgs[i]->header.stamp.toSec()); return; } // sync with odometry stamp @@ -782,9 +821,13 @@ void CoreWrapper::commonDepthCallback( Transform sensorT = getTransform(odomFrameId, frameId_, depthMsgs[i]->header.stamp); if(sensorT.isNull()) { - return; + ROS_WARN("Could not get odometry value for depth image %d stamp (%fs). Latest odometry " + "stamp is %fs. The depth image pose will not be synchronized with odometry.", i, depthMsgs[i]->header.stamp.toSec(), lastPoseStamp_.toSec()); + } + else + { + localTransform = odomT.inverse() * sensorT * localTransform; } - localTransform = odomT.inverse() * sensorT * localTransform; } } @@ -849,82 +892,100 @@ void CoreWrapper::commonDepthCallback( return; } - image_geometry::PinholeCameraModel model; - model.fromCameraInfo(*cameraInfoMsgs[i]); - cameraModels.push_back(rtabmap::CameraModel( - model.fx(), - model.fy(), - model.cx(), - model.cy(), - localTransform)); + cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*cameraInfoMsgs[i], localTransform)); - if(scanMsg.get() == 0 && genScan_) + if(scan2dMsg.get() == 0 && genScan_) { - scanCloud += util3d::laserScanFromDepthImage( + scanCloud2d += util3d::laserScanFromDepthImage( subDepth, - model.fx(), - model.fy(), - model.cx(), - model.cy(), + cameraModels.back().fx(), + cameraModels.back().fy(), + cameraModels.back().cx(), + cameraModels.back().cy(), genScanMaxDepth_, + genScanMinDepth_, localTransform); genMaxScanPts += subDepth.cols; } } cv::Mat scan; - if(scanMsg.get() != 0) + if(scan2dMsg.get() != 0) { // make sure the frame of the laser is updated too - if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull()) + if(getTransform(frameId_, + scan2dMsg->header.frame_id, + scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)).isNull()) { + ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting rtabmap update.", scan2dMsg->header.stamp.toSec()); return; } //transform in frameId_ frame sensor_msgs::PointCloud2 scanOut; laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_); + projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_); pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(scanOut, *pclScan); // sync with odometry stamp - if(lastPoseStamp_ != scanMsg->header.stamp) + if(lastPoseStamp_ != scan2dMsg->header.stamp) { if(!odomT.isNull()) { - Transform sensorT = getTransform(odomFrameId, frameId_, scanMsg->header.stamp); + Transform sensorT = getTransform(odomFrameId, frameId_, scan2dMsg->header.stamp); if(sensorT.isNull()) { - return; + ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry " + "stamp is %fs. The laser scan pose will not be synchronized with odometry.", scan2dMsg->header.stamp.toSec(), lastPoseStamp_.toSec()); + } + else + { + Transform t = odomT.inverse() * sensorT; + pclScan = util3d::transformPointCloud(pclScan, t); } - Transform t = odomT.inverse() * sensorT; - pclScan = util3d::transformPointCloud(pclScan, t); } } + scan = util3d::laserScan2dFromPointCloud(*pclScan); + } + else if(scan3dMsg.get() != 0) + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*scan3dMsg, *pclScan); scan = util3d::laserScanFromPointCloud(*pclScan); } - else if(scanCloud.size()) + else if(scanCloud2d.size()) { - scan = util3d::laserScanFromPointCloud(scanCloud); + scan = util3d::laserScan2dFromPointCloud(scanCloud2d); } - ros::Time stamp = scanMsg.get() != 0?scanMsg->header.stamp:depthMsgs[0]->header.stamp; + ros::Time stamp = scan2dMsg.get() != 0?scan2dMsg->header.stamp: + scan3dMsg.get() != 0?scan3dMsg->header.stamp: + depthMsgs[0]->header.stamp; + + Transform groundTruthPose; + if(!groundTruthFrameId_.empty()) + { + groundTruthPose = getTransform(groundTruthFrameId_, frameId_, stamp); + } + + SensorData data(scan, + scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():genMaxScanPts, + scan2dMsg.get() != 0?scan2dMsg->range_max:(genScan_?genScanMaxDepth_:0.0f), + rgb, + depth, + cameraModels, + lastPoseIntermediate_?-1:imageMsgs[0]->header.seq, + rtabmap_ros::timestampFromROS(stamp)); + data.setGroundTruth(groundTruthPose); process(stamp, - SensorData(scan, - scanMsg.get() != 0?(int)scanMsg->ranges.size():genMaxScanPts, - scanMsg.get() != 0?scanMsg->range_max:(genScan_?genScanMaxDepth_:0.0f), - rgb, - depth, - cameraModels, - imageMsgs[0]->header.seq, - rtabmap_ros::timestampFromROS(stamp)), + data, lastPose_, odomFrameId, - rotVariance_>0?rotVariance_:1.0, - transVariance_>0?transVariance_:1.0); + uIsFinite(rotVariance_) && rotVariance_>0?rotVariance_:1.0, + uIsFinite(transVariance_) && transVariance_>0?transVariance_:1.0); rotVariance_ = 0; transVariance_ = 0; } @@ -935,7 +996,8 @@ void CoreWrapper::commonStereoCallback( const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, - const sensor_msgs::LaserScanConstPtr& scanMsg) + const sensor_msgs::LaserScanConstPtr& scan2dMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg) { if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 || @@ -979,11 +1041,14 @@ void CoreWrapper::commonStereoCallback( } cv::Mat scan; - if(scanMsg.get() != 0) + if(scan2dMsg.get() != 0) { // make sure the frame of the laser is updated too - if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull()) + if(getTransform(frameId_, + scan2dMsg->header.frame_id, + scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)).isNull()) { + ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting rtabmap update.", scan2dMsg->header.stamp.toSec()); return; } @@ -991,16 +1056,16 @@ void CoreWrapper::commonStereoCallback( sensor_msgs::PointCloud2 scanOut; laser_geometry::LaserProjection projection; //projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfBuffer_); - projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_); + projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_); pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(scanOut, *pclScan); // sync with odometry stamp - if(lastPoseStamp_ != scanMsg->header.stamp) + if(lastPoseStamp_ != scan2dMsg->header.stamp) { if(!odomT.isNull()) { - Transform sensorT = getTransform(odomFrameId, frameId_, scanMsg->header.stamp); + Transform sensorT = getTransform(odomFrameId, frameId_, scan2dMsg->header.stamp); if(sensorT.isNull()) { return; @@ -1011,32 +1076,30 @@ void CoreWrapper::commonStereoCallback( } } + scan = util3d::laserScan2dFromPointCloud(*pclScan); + } + else if(scan3dMsg.get() != 0) + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*scan3dMsg, *pclScan); scan = util3d::laserScanFromPointCloud(*pclScan); } - cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage; + cv_bridge::CvImagePtr ptrLeftImage, ptrRightImage; if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) { - ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "mono8"); + ptrLeftImage = cv_bridge::toCvCopy(leftImageMsg, "mono8"); } else { - ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "bgr8"); + ptrLeftImage = cv_bridge::toCvCopy(leftImageMsg, "bgr8"); } - ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8"); + ptrRightImage = cv_bridge::toCvCopy(rightImageMsg, "mono8"); - image_geometry::StereoCameraModel model; - model.fromCameraInfo(*leftCamInfoMsg, *rightCamInfoMsg); - rtabmap::StereoCameraModel stereoModel( - model.left().fx(), - model.left().fy(), - model.left().cx(), - model.left().cy(), - model.baseline(), - localTransform); + rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform); - if(model.baseline() > 10.0) + if(stereoModel.baseline() > 10.0) { static bool shown = false; if(!shown) @@ -1044,25 +1107,37 @@ void CoreWrapper::commonStereoCallback( 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.", - model.baseline()); + stereoModel.baseline()); shown = true; } } - ros::Time stamp = scanMsg.get() != 0?scanMsg->header.stamp:leftImageMsg->header.stamp; + ros::Time stamp = scan2dMsg.get() != 0?scan2dMsg->header.stamp: + scan3dMsg.get() != 0?scan3dMsg->header.stamp: + leftImageMsg->header.stamp; + + Transform groundTruthPose; + if(!groundTruthFrameId_.empty()) + { + groundTruthPose = getTransform(groundTruthFrameId_, frameId_, stamp); + } + + SensorData data(scan, + scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():0, + scan2dMsg.get() != 0?scan2dMsg->range_max:0, + ptrLeftImage->image, + ptrRightImage->image, + stereoModel, + lastPoseIntermediate_?-1:leftImageMsg->header.seq, + rtabmap_ros::timestampFromROS(stamp)); + data.setGroundTruth(groundTruthPose); + process(stamp, - SensorData(scan, - scanMsg.get() != 0?(int)scanMsg->ranges.size():0, - scanMsg.get() != 0?scanMsg->range_max:0, - ptrLeftImage->image, - ptrRightImage->image, - stereoModel, - leftImageMsg->header.seq, - rtabmap_ros::timestampFromROS(stamp)), + data, lastPose_, odomFrameId, - rotVariance_>0?rotVariance_:1.0, - transVariance_>0?transVariance_:1.0); + uIsFinite(rotVariance_) && rotVariance_>0?rotVariance_:1.0, + uIsFinite(transVariance_) && transVariance_>0?transVariance_:1.0); rotVariance_ = 0; transVariance_ = 0; @@ -1080,7 +1155,8 @@ void CoreWrapper::depthCallback( } sensor_msgs::LaserScanConstPtr scanMsg; // Null - commonDepthCallback(odomMsg->header.frame_id, imageMsg, depthMsg, cameraInfoMsg, scanMsg); + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null + commonDepthCallback(odomMsg->header.frame_id, imageMsg, depthMsg, cameraInfoMsg, scanMsg, scan3dMsg); } void CoreWrapper::depthScanCallback( const sensor_msgs::ImageConstPtr& imageMsg, @@ -1093,7 +1169,22 @@ void CoreWrapper::depthScanCallback( { return; } - commonDepthCallback(odomMsg->header.frame_id, imageMsg, depthMsg, cameraInfoMsg, scanMsg); + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonDepthCallback(odomMsg->header.frame_id, imageMsg, depthMsg, cameraInfoMsg, scanMsg, scan3dMsg); +} +void CoreWrapper::depthScan3dCallback( + const sensor_msgs::ImageConstPtr& imageMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg) +{ + if(!commonOdomUpdate(odomMsg)) + { + return; + } + sensor_msgs::LaserScanConstPtr scan2dMsg; // Null + commonDepthCallback(odomMsg->header.frame_id, imageMsg, depthMsg, cameraInfoMsg, scan2dMsg, scanMsg); } void CoreWrapper::stereoCallback( const sensor_msgs::ImageConstPtr& leftImageMsg, @@ -1108,7 +1199,8 @@ void CoreWrapper::stereoCallback( } sensor_msgs::LaserScanConstPtr scanMsg; // Null - commonStereoCallback(odomMsg->header.frame_id, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg); + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null + commonStereoCallback(odomMsg->header.frame_id, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg); } void CoreWrapper::stereoScanCallback( const sensor_msgs::ImageConstPtr& leftImageMsg, @@ -1122,7 +1214,23 @@ void CoreWrapper::stereoScanCallback( { return; } - commonStereoCallback(odomMsg->header.frame_id, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg); + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonStereoCallback(odomMsg->header.frame_id, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg); +} +void CoreWrapper::stereoScan3dCallback( + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg) +{ + if(!commonOdomUpdate(odomMsg)) + { + return; + } + sensor_msgs::LaserScanConstPtr scan2dMsg; // Null + commonStereoCallback(odomMsg->header.frame_id, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scan2dMsg, scanMsg); } void CoreWrapper::depth2Callback( @@ -1150,7 +1258,8 @@ void CoreWrapper::depth2Callback( cameraInfoMsgs.push_back(cameraInfo2Msg); sensor_msgs::LaserScanConstPtr scanMsg; // Null - commonDepthCallback(odomMsg->header.frame_id, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg); + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonDepthCallback(odomMsg->header.frame_id, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg); } @@ -1164,7 +1273,8 @@ void CoreWrapper::depthTFCallback( return; } sensor_msgs::LaserScanConstPtr scanMsg; // Null - commonDepthCallback(odomFrameId_, imageMsg, depthMsg, cameraInfoMsg, scanMsg); + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonDepthCallback(odomFrameId_, imageMsg, depthMsg, cameraInfoMsg, scanMsg, scan3dMsg); } void CoreWrapper::depthScanTFCallback( const sensor_msgs::ImageConstPtr& imageMsg, @@ -1176,7 +1286,21 @@ void CoreWrapper::depthScanTFCallback( { return; } - commonDepthCallback(odomFrameId_, imageMsg, depthMsg, cameraInfoMsg, scanMsg); + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonDepthCallback(odomFrameId_, imageMsg, depthMsg, cameraInfoMsg, scanMsg, scan3dMsg); +} +void CoreWrapper::depthScan3dTFCallback( + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg) +{ + if(!commonOdomTFUpdate(scanMsg->header.stamp)) + { + return; + } + sensor_msgs::LaserScanConstPtr scan2dMsg; // Null + commonDepthCallback(odomFrameId_, imageMsg, depthMsg, cameraInfoMsg, scan2dMsg, scanMsg); } void CoreWrapper::stereoTFCallback( const sensor_msgs::ImageConstPtr& leftImageMsg, @@ -1190,7 +1314,8 @@ void CoreWrapper::stereoTFCallback( } sensor_msgs::LaserScanConstPtr scanMsg; // null - commonStereoCallback(odomFrameId_, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg); + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null + commonStereoCallback(odomFrameId_, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg); } void CoreWrapper::stereoScanTFCallback( const sensor_msgs::ImageConstPtr& leftImageMsg, @@ -1203,7 +1328,23 @@ void CoreWrapper::stereoScanTFCallback( { return; } - commonStereoCallback(odomFrameId_, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg); + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null + commonStereoCallback(odomFrameId_, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg); +} + +void CoreWrapper::stereoScan3dTFCallback( + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg) +{ + if(!commonOdomTFUpdate(leftImageMsg->header.stamp)) + { + return; + } + sensor_msgs::LaserScanConstPtr scan2dMsg; // null + commonStereoCallback(odomFrameId_, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scan2dMsg, scanMsg); } void CoreWrapper::process( @@ -1211,8 +1352,8 @@ void CoreWrapper::process( const SensorData & data, const Transform & odom, const std::string & odomFrameId, - double odomRotationalVariance, - double odomTransitionalVariance) + float odomRotationalVariance, + float odomTransitionalVariance) { UTimer timer; if(rtabmap_.isIDsGenerated() || data.id() > 0) @@ -1226,95 +1367,103 @@ void CoreWrapper::process( odomFrameId_ = odomFrameId; mapToOdomMutex_.unlock(); - // Publish local graph, info - this->publishStats(stamp); - std::map filteredPoses = rtabmap_.getLocalOptimizedPoses(); - - // create a tmp signature with latest sensory data - std::map tmpSignature; - SensorData tmpData = data; - tmpData.setId(-1); - tmpSignature.insert(std::make_pair(-1, Signature(-1, -1, 0, data.stamp(), "", odom, tmpData))); - filteredPoses.insert(std::make_pair(-1, rtabmap_.getMapCorrection()*odom)); - - // Update maps - filteredPoses = mapsManager_.updateMapCaches( - filteredPoses, - rtabmap_.getMemory(), - false, - false, - false, - tmpSignature); - - mapsManager_.publishMaps(filteredPoses, stamp, mapFrameId_); - - // update goal if planning is enabled - if(!currentMetricGoal_.isNull()) + if(data.id() < 0) { - if(rtabmap_.getPath().size() == 0) + ROS_INFO("Intermediate node added"); + } + else + { + // Publish local graph, info + this->publishStats(stamp); + std::map filteredPoses = rtabmap_.getLocalOptimizedPoses(); + + // create a tmp signature with latest sensory data + std::map tmpSignature; + SensorData tmpData = data; + tmpData.setId(-1); + tmpSignature.insert(std::make_pair(-1, Signature(-1, -1, 0, data.stamp(), "", odom, Transform(), tmpData))); + filteredPoses.insert(std::make_pair(-1, rtabmap_.getMapCorrection()*odom)); + + // Update maps + filteredPoses = mapsManager_.updateMapCaches( + filteredPoses, + rtabmap_.getMemory(), + false, + false, + false, + false, + tmpSignature); + + mapsManager_.publishMaps(filteredPoses, stamp, mapFrameId_); + + // update goal if planning is enabled + if(!currentMetricGoal_.isNull()) { - if(rtabmap_.getPathStatus() > 0) + if(rtabmap_.getPath().size() == 0) { - // Goal reached - ROS_INFO("Planning: Publishing goal reached!"); - } - else - { - ROS_WARN("Planning: Plan failed!"); - if(mbClient_.isServerConnected()) + if(rtabmap_.getPathStatus() > 0) { - mbClient_.cancelGoal(); + // Goal reached + ROS_INFO("Planning: Publishing goal reached!"); } - } - if(goalReachedPub_.getNumSubscribers()) - { - std_msgs::Bool result; - result.data = rtabmap_.getPathStatus() > 0; - goalReachedPub_.publish(result); - } - currentMetricGoal_.setNull(); - latestNodeWasReached_ = false; - } - else - { - currentMetricGoal_ = rtabmap_.getPose(rtabmap_.getPathCurrentGoalId()); - if(!currentMetricGoal_.isNull()) - { - // Adjust the target pose relative to last node - if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size()) + else { - if(latestNodeWasReached_ || - rtabmap_.getLocalOptimizedPoses().rbegin()->second.getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius() || - rtabmap_.getPathTransformToGoal().getNorm() < rtabmap_.getGoalReachedRadius()) + ROS_WARN("Planning: Plan failed!"); + if(mbClient_.isServerConnected()) { - latestNodeWasReached_ = true; - currentMetricGoal_ *= rtabmap_.getPathTransformToGoal(); + mbClient_.cancelGoal(); } } - - // publish next goal with updated currentMetricGoal_ - publishCurrentGoal(stamp); - - // publish local path - publishLocalPath(stamp); - - // publish global path - publishGlobalPath(stamp); - } - else - { - ROS_ERROR("Planning: Local map broken, current goal id=%d (the robot may have moved to far from planned nodes)", - rtabmap_.getPathCurrentGoalId()); - rtabmap_.clearPath(-1); if(goalReachedPub_.getNumSubscribers()) { std_msgs::Bool result; - result.data = false; + result.data = rtabmap_.getPathStatus() > 0; goalReachedPub_.publish(result); } currentMetricGoal_.setNull(); latestNodeWasReached_ = false; } + else + { + currentMetricGoal_ = rtabmap_.getPose(rtabmap_.getPathCurrentGoalId()); + if(!currentMetricGoal_.isNull()) + { + // Adjust the target pose relative to last node + if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size()) + { + if(latestNodeWasReached_ || + rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius() || + rtabmap_.getPathTransformToGoal().getNorm() < rtabmap_.getGoalReachedRadius()) + { + latestNodeWasReached_ = true; + currentMetricGoal_ *= rtabmap_.getPathTransformToGoal(); + } + } + + // publish next goal with updated currentMetricGoal_ + publishCurrentGoal(stamp); + + // publish local path + publishLocalPath(stamp); + + // publish global path + publishGlobalPath(stamp); + } + else + { + ROS_ERROR("Planning: Local map broken, current goal id=%d (the robot may have moved to far from planned nodes)", + rtabmap_.getPathCurrentGoalId()); + rtabmap_.clearPath(-1); + if(goalReachedPub_.getNumSubscribers()) + { + std_msgs::Bool result; + result.data = false; + goalReachedPub_.publish(result); + } + currentMetricGoal_.setNull(); + latestNodeWasReached_ = false; + } + } } } } @@ -1344,7 +1493,8 @@ void CoreWrapper::goalCommonCallback( int id, const std::string & label, const Transform & pose, - const ros::Time & stamp) + const ros::Time & stamp, + double * planningTime) { UTimer timer; @@ -1362,10 +1512,19 @@ void CoreWrapper::goalCommonCallback( ROS_INFO("Planning: set goal %s", pose.prettyPrint().c_str()); } + if(planningTime) + { + *planningTime = 0.0; + } + bool success = false; if((id > 0 && rtabmap_.computePath(id, true)) || (!pose.isNull() && rtabmap_.computePath(pose))) { + if(planningTime) + { + *planningTime = timer.elapsed(); + } ROS_INFO("Planning: Time computing path = %f s", timer.ticks()); const std::vector > & poses = rtabmap_.getPath(); @@ -1394,7 +1553,7 @@ void CoreWrapper::goalCommonCallback( // Adjust the target pose relative to last node if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size()) { - if(rtabmap_.getLocalOptimizedPoses().rbegin()->second.getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius()) + if(rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius()) { latestNodeWasReached_ = true; currentMetricGoal_ *= rtabmap_.getPathTransformToGoal(); @@ -1517,9 +1676,11 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt rotVariance_ = 0; transVariance_ = 0; lastPose_.setIdentity(); + lastPoseIntermediate_ = false; currentMetricGoal_.setNull(); latestNodeWasReached_ = false; mapsManager_.clear(); + previousStamp_ = ros::Time(0); return true; } @@ -1603,6 +1764,31 @@ bool CoreWrapper::setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Em return true; } +bool CoreWrapper::setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +{ + ROS_INFO("rtabmap: Set log level to Debug"); + ULogger::setLevel(ULogger::kDebug); + return true; +} +bool CoreWrapper::setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +{ + ROS_INFO("rtabmap: Set log level to Info"); + ULogger::setLevel(ULogger::kInfo); + return true; +} +bool CoreWrapper::setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +{ + ROS_INFO("rtabmap: Set log level to Warning"); + ULogger::setLevel(ULogger::kWarning); + return true; +} +bool CoreWrapper::setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +{ + ROS_INFO("rtabmap: Set log level to Error"); + ULogger::setLevel(ULogger::kError); + return true; +} + bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res) { ROS_INFO("rtabmap: Getting map (global=%s optimized=%s graphOnly=%s)...", @@ -1636,7 +1822,7 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros: rtabmap_ros::mapDataToROS(poses, constraints, signatures, - Transform::getIdentity(), + mapToOdom_, res.data); res.data.header.stamp = ros::Time::now(); @@ -1653,6 +1839,7 @@ bool CoreWrapper::getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs:: rtabmap_.getMemory(), false, true, + false, false); if(filteredPoses.size()) { @@ -1696,7 +1883,8 @@ bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs:: rtabmap_.getMemory(), false, false, - true); + true, + false); if(filteredPoses.size()) { // create the grid map @@ -1777,7 +1965,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab rtabmap_ros::mapDataToROS(poses, constraints, signatures, - Transform::getIdentity(), + mapToOdom_, *msg); mapDataPub_.publish(msg); @@ -1791,7 +1979,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab rtabmap_ros::mapGraphToROS(poses, constraints, - Transform::getIdentity(), + mapToOdom_, *msg); mapGraphPub_.publish(msg); @@ -1808,6 +1996,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab false, false, false, + false, signatures); } else @@ -1905,10 +2094,12 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab bool CoreWrapper::setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res) { - goalCommonCallback(req.node_id, req.node_label, Transform(), ros::Time::now()); + double planningTime = 0.0; + goalCommonCallback(req.node_id, req.node_label, Transform(), ros::Time::now(), &planningTime); const std::vector > & path = rtabmap_.getPath(); res.path_ids.resize(path.size()); res.path_poses.resize(path.size()); + res.planning_time = planningTime; for(unsigned int i=0; i poses = rtabmap_.getLocalOptimizedPoses(); - poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false, false); + poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false, false, false); octomap::OcTree * octree = mapsManager_.createOctomap(poses); bool success = octree != 0 && octree->size() && octomap_msgs::binaryMapToMsg(*octree, res.map); @@ -2290,7 +2481,7 @@ bool CoreWrapper::octomapFullCallback( res.map.header.stamp = ros::Time::now(); std::map poses = rtabmap_.getLocalOptimizedPoses(); - poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false, false); + poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false, false, false); octomap::OcTree * octree = mapsManager_.createOctomap(poses); bool success = octree != 0 && octree->size() && octomap_msgs::fullMapToMsg(*octree, res.map); @@ -2315,7 +2506,8 @@ bool CoreWrapper::octomapFullCallback( */ void CoreWrapper::setupCallbacks( bool subscribeDepth, - bool subscribeLaserScan, + bool subscribeScan2d, + bool subscribeScan3d, bool subscribeStereo, int queueSize, bool stereoApproxSync, @@ -2327,7 +2519,7 @@ void CoreWrapper::setupCallbacks( if(subscribeDepth) { UASSERT(depthCameras >= 1 && depthCameras <= 2); - UASSERT_MSG(depthCameras == 1 || !(subscribeLaserScan || !odomFrameId_.empty()), "Not yet supported!"); + UASSERT_MSG(depthCameras == 1 || !(subscribeScan2d || subscribeScan3d || !odomFrameId_.empty()), "Not yet supported!"); imageSubs_.resize(depthCameras); imageDepthSubs_.resize(depthCameras); @@ -2361,7 +2553,7 @@ void CoreWrapper::setupCallbacks( if(odomFrameId_.empty()) { odomSub_.subscribe(nh, "odom", 1); - if(subscribeLaserScan) + if(subscribeScan2d) { ROS_INFO("Registering Depth+LaserScan callback..."); scanSub_.subscribe(nh, "scan", 1); @@ -2382,6 +2574,27 @@ void CoreWrapper::setupCallbacks( odomSub_.getTopic().c_str(), scanSub_.getTopic().c_str()); } + else if(subscribeScan3d) + { + ROS_INFO("Registering Depth+LaserScan3d callback..."); + scan3dSub_.subscribe(nh, "scan_cloud", 1); + depthScan3dSync_ = new message_filters::Synchronizer( + MyDepthScan3dSyncPolicy(queueSize), + *imageSubs_[0], + odomSub_, + *imageDepthSubs_[0], + *cameraInfoSubs_[0], + scan3dSub_); + depthScan3dSync_->registerCallback(boost::bind(&CoreWrapper::depthScan3dCallback, this, _1, _2, _3, _4, _5)); + + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageSubs_[0]->getTopic().c_str(), + imageDepthSubs_[0]->getTopic().c_str(), + cameraInfoSubs_[0]->getTopic().c_str(), + odomSub_.getTopic().c_str(), + scan3dSub_.getTopic().c_str()); + } else //!subscribeLaserScan { if(depthCameras > 1) @@ -2431,7 +2644,7 @@ void CoreWrapper::setupCallbacks( else { // use odom from TF, so subscribe to sensors only - if(subscribeLaserScan) + if(subscribeScan2d) { scanSub_.subscribe(nh, "scan", 1); depthScanTFSync_ = new message_filters::Synchronizer( @@ -2449,6 +2662,24 @@ void CoreWrapper::setupCallbacks( cameraInfoSubs_[0]->getTopic().c_str(), scanSub_.getTopic().c_str()); } + else if(subscribeScan3d) + { + scan3dSub_.subscribe(nh, "scan_cloud", 1); + depthScan3dTFSync_ = new message_filters::Synchronizer( + MyDepthScan3dTFSyncPolicy(queueSize), + *imageSubs_[0], + *imageDepthSubs_[0], + *cameraInfoSubs_[0], + scan3dSub_); + depthScan3dTFSync_->registerCallback(boost::bind(&CoreWrapper::depthScan3dTFCallback, this, _1, _2, _3, _4)); + + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageSubs_[0]->getTopic().c_str(), + imageDepthSubs_[0]->getTopic().c_str(), + cameraInfoSubs_[0]->getTopic().c_str(), + scan3dSub_.getTopic().c_str()); + } else //!subscribeLaserScan { depthTFSync_ = new message_filters::Synchronizer( @@ -2485,7 +2716,7 @@ void CoreWrapper::setupCallbacks( if(odomFrameId_.empty()) { odomSub_.subscribe(nh, "odom", 1); - if(subscribeLaserScan) + if(subscribeScan2d) { scanSub_.subscribe(nh, "scan", 1); stereoScanSync_ = new message_filters::Synchronizer( @@ -2507,6 +2738,28 @@ void CoreWrapper::setupCallbacks( odomSub_.getTopic().c_str(), scanSub_.getTopic().c_str()); } + else if(subscribeScan3d) + { + scan3dSub_.subscribe(nh, "scan_cloud", 1); + stereoScan3dSync_ = new message_filters::Synchronizer( + MyStereoScan3dSyncPolicy(queueSize), + imageRectLeft_, + imageRectRight_, + cameraInfoLeft_, + cameraInfoRight_, + scan3dSub_, + odomSub_); + stereoScan3dSync_->registerCallback(boost::bind(&CoreWrapper::stereoScan3dCallback, this, _1, _2, _3, _4, _5, _6)); + + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\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(), + odomSub_.getTopic().c_str(), + scan3dSub_.getTopic().c_str()); + } else //!subscribeLaserScan { if(stereoApproxSync) @@ -2546,9 +2799,9 @@ void CoreWrapper::setupCallbacks( else { // use odom from TF, so subscribe to sensors only - if(subscribeLaserScan) + if(subscribeScan2d) { - ROS_INFO("Registering Stereo+LaserScan+OdomTF callback..."); + ROS_INFO("Registering Stereo+LaserScan2d+OdomTF callback..."); scanSub_.subscribe(nh, "scan", 1); stereoScanTFSync_ = new message_filters::Synchronizer( MyStereoScanTFSyncPolicy(queueSize), @@ -2567,6 +2820,27 @@ void CoreWrapper::setupCallbacks( cameraInfoRight_.getTopic().c_str(), scanSub_.getTopic().c_str()); } + else if(subscribeScan3d) + { + ROS_INFO("Registering Stereo+LaserScan3d+OdomTF callback..."); + scan3dSub_.subscribe(nh, "scan_cloud", 1); + stereoScan3dTFSync_ = new message_filters::Synchronizer( + MyStereoScan3dTFSyncPolicy(queueSize), + imageRectLeft_, + imageRectRight_, + cameraInfoLeft_, + cameraInfoRight_, + scan3dSub_); + stereoScan3dTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoScan3dTFCallback, this, _1, _2, _3, _4, _5)); + + ROS_INFO("\n%s subscribed to:\n %s,\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(), + scan3dSub_.getTopic().c_str()); + } else //!subscribeLaserScan { if(stereoApproxSync) diff --git a/src/CoreWrapper.h b/src/CoreWrapper.h index 1d743d20..5c0e03b4 100644 --- a/src/CoreWrapper.h +++ b/src/CoreWrapper.h @@ -80,13 +80,14 @@ typedef actionlib::SimpleActionClient MoveBaseCl class CoreWrapper { public: - CoreWrapper(bool deleteDbOnStart); + CoreWrapper(bool deleteDbOnStart, const rtabmap::ParametersMap & parameters); virtual ~CoreWrapper(); private: void setupCallbacks( bool subscribeDepth, - bool subscribeLaserScan, + bool subscribeScan2d, + bool subscribeScan3d, bool subscribeStereo, int queueSize, bool stereoApproxSync, @@ -102,20 +103,23 @@ private: const sensor_msgs::ImageConstPtr& imageMsg, const sensor_msgs::ImageConstPtr& depthMsg, const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, - const sensor_msgs::LaserScanConstPtr& scanMsg); + const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg); void commonDepthCallback( const std::string & odomFrameId, const std::vector & imageMsgs, const std::vector & depthMsgs, const std::vector & cameraInfoMsgs, - const sensor_msgs::LaserScanConstPtr& scanMsg); + const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg); void commonStereoCallback( const std::string & odomFrameId, const sensor_msgs::ImageConstPtr& leftImageMsg, const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, - const sensor_msgs::LaserScanConstPtr& scanMsg); + const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg); // with odom msg void depthCallback( @@ -129,6 +133,12 @@ private: const sensor_msgs::ImageConstPtr& imageDepthMsg, const sensor_msgs::CameraInfoConstPtr& camInfoMsg, const sensor_msgs::LaserScanConstPtr& scanMsg); + void depthScan3dCallback( + const sensor_msgs::ImageConstPtr& imageMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& imageDepthMsg, + const sensor_msgs::CameraInfoConstPtr& camInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg); void stereoCallback( const sensor_msgs::ImageConstPtr& leftImageMsg, const sensor_msgs::ImageConstPtr& rightImageMsg, @@ -142,6 +152,13 @@ private: const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, const sensor_msgs::LaserScanConstPtr& scanMsg, const nav_msgs::OdometryConstPtr & odomMsg); + void stereoScan3dCallback( + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg); void depth2Callback( const nav_msgs::OdometryConstPtr & odomMsg, const sensor_msgs::ImageConstPtr& image1Msg, @@ -161,6 +178,11 @@ private: const sensor_msgs::ImageConstPtr& imageDepthMsg, const sensor_msgs::CameraInfoConstPtr& camInfoMsg, const sensor_msgs::LaserScanConstPtr& scanMsg); + void depthScan3dTFCallback( + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& imageDepthMsg, + const sensor_msgs::CameraInfoConstPtr& camInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg); void stereoTFCallback( const sensor_msgs::ImageConstPtr& leftImageMsg, const sensor_msgs::ImageConstPtr& rightImageMsg, @@ -172,8 +194,14 @@ private: const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, const sensor_msgs::LaserScanConstPtr& scanMsg); + void stereoScan3dTFCallback( + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg); - void goalCommonCallback(int id, const std::string & label, const rtabmap::Transform & pose, const ros::Time & stamp); + void goalCommonCallback(int id, const std::string & label, const rtabmap::Transform & pose, const ros::Time & stamp, double * planningTime = 0); void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg); void goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg); void updateGoal(const ros::Time & stamp); @@ -183,8 +211,8 @@ private: const rtabmap::SensorData & data, const rtabmap::Transform & odom = rtabmap::Transform(), const std::string & odomFrameId = "", - double odomRotationalVariance = 1.0, - double odomTransitionalVariance = 1.0); + float odomRotationalVariance = 1.0, + float odomTransitionalVariance = 1.0); bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); @@ -194,6 +222,10 @@ private: bool backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool setModeLocalizationCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); + bool setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&); + bool setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&); + bool setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&); + bool setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res); bool getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res); bool getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res); @@ -225,8 +257,9 @@ private: bool paused_; rtabmap::Transform lastPose_; ros::Time lastPoseStamp_; - double rotVariance_; - double transVariance_; + bool lastPoseIntermediate_; + float rotVariance_; + float transVariance_; rtabmap::Transform currentMetricGoal_; bool latestNodeWasReached_; rtabmap::ParametersMap parameters_; @@ -234,6 +267,7 @@ private: std::string frameId_; std::string mapFrameId_; std::string odomFrameId_; + std::string groundTruthFrameId_; std::string configPath_; std::string databasePath_; bool waitForTransform_; @@ -241,6 +275,7 @@ private: bool useActionForGoal_; bool genScan_; double genScanMaxDepth_; + double genScanMinDepth_; rtabmap::Transform mapToOdom_; boost::mutex mapToOdomMutex_; @@ -276,6 +311,7 @@ private: message_filters::Subscriber odomSub_; message_filters::Subscriber scanSub_; + message_filters::Subscriber scan3dSub_; typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, @@ -285,6 +321,14 @@ private: sensor_msgs::LaserScan> MyDepthScanSyncPolicy; message_filters::Synchronizer * depthScanSync_; + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::Image, + nav_msgs::Odometry, + sensor_msgs::Image, + sensor_msgs::CameraInfo, + sensor_msgs::PointCloud2> MyDepthScan3dSyncPolicy; + message_filters::Synchronizer * depthScan3dSync_; + typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, nav_msgs::Odometry, @@ -301,6 +345,15 @@ private: nav_msgs::Odometry> MyStereoScanSyncPolicy; message_filters::Synchronizer * stereoScanSync_; + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo, + sensor_msgs::CameraInfo, + sensor_msgs::PointCloud2, + nav_msgs::Odometry> MyStereoScan3dSyncPolicy; + message_filters::Synchronizer * stereoScan3dSync_; + typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, @@ -335,6 +388,13 @@ private: sensor_msgs::LaserScan> MyDepthScanTFSyncPolicy; message_filters::Synchronizer * depthScanTFSync_; + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo, + sensor_msgs::PointCloud2> MyDepthScan3dTFSyncPolicy; + message_filters::Synchronizer * depthScan3dTFSync_; + typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, @@ -349,6 +409,14 @@ private: sensor_msgs::LaserScan> MyStereoScanTFSyncPolicy; message_filters::Synchronizer * stereoScanTFSync_; + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo, + sensor_msgs::CameraInfo, + sensor_msgs::PointCloud2> MyStereoScan3dTFSyncPolicy; + message_filters::Synchronizer * stereoScan3dTFSync_; + typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, @@ -374,6 +442,10 @@ private: ros::ServiceServer backupDatabase_; ros::ServiceServer setModeLocalizationSrv_; ros::ServiceServer setModeMappingSrv_; + ros::ServiceServer setLogDebugSrv_; + ros::ServiceServer setLogInfoSrv_; + ros::ServiceServer setLogWarnSrv_; + ros::ServiceServer setLogErrorSrv_; ros::ServiceServer getMapDataSrv_; ros::ServiceServer getProjMapSrv_; ros::ServiceServer getGridMapSrv_; @@ -392,7 +464,9 @@ private: boost::thread* transformThread_; float rate_; + bool createIntermediateNodes_; ros::Time time_; + ros::Time previousStamp_; }; #endif /* COREWRAPPER_H_ */ diff --git a/src/DbPlayerNode.cpp b/src/DbPlayerNode.cpp index 6135b2cc..6bbf2e56 100644 --- a/src/DbPlayerNode.cpp +++ b/src/DbPlayerNode.cpp @@ -90,7 +90,7 @@ int main(int argc, char** argv) std::string odomFrameId = "odom"; std::string cameraFrameId = "camera_optical_link"; std::string scanFrameId = "base_laser_link"; - double rate = 1.0f; + double rate = -1.0f; std::string databasePath = ""; bool publishTf = true; int startId = 0; @@ -214,7 +214,7 @@ int main(int argc, char** argv) else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U) { //stereo - if(odom.data().stereoCameraModel().isValid()) + if(odom.data().stereoCameraModel().isValidForProjection()) { camInfoA.D.resize(8,0); @@ -257,14 +257,12 @@ int main(int argc, char** argv) // publish transforms first if(publishTf) { - ros::Time tfExpiration = time + ros::Duration(rate>0?1.0/rate:acquisitionTime); - rtabmap::Transform localTransform; if(odom.data().cameraModels().size() == 1) { localTransform = odom.data().cameraModels()[0].localTransform(); } - else if(odom.data().stereoCameraModel().isValid()) + else if(odom.data().stereoCameraModel().isValidForProjection()) { localTransform = odom.data().stereoCameraModel().left().localTransform(); } @@ -273,7 +271,7 @@ int main(int argc, char** argv) geometry_msgs::TransformStamped baseToCamera; baseToCamera.child_frame_id = cameraFrameId; baseToCamera.header.frame_id = frameId; - baseToCamera.header.stamp = tfExpiration; + baseToCamera.header.stamp = time; rtabmap_ros::transformToGeometryMsg(localTransform, baseToCamera.transform); tfBroadcaster.sendTransform(baseToCamera); } @@ -283,7 +281,7 @@ int main(int argc, char** argv) geometry_msgs::TransformStamped odomToBase; odomToBase.child_frame_id = frameId; odomToBase.header.frame_id = odomFrameId; - odomToBase.header.stamp = tfExpiration; + odomToBase.header.stamp = time; rtabmap_ros::transformToGeometryMsg(odom.pose(), odomToBase.transform); tfBroadcaster.sendTransform(odomToBase); } @@ -293,7 +291,7 @@ int main(int argc, char** argv) geometry_msgs::TransformStamped baseToLaserScan; baseToLaserScan.child_frame_id = scanFrameId; baseToLaserScan.header.frame_id = frameId; - baseToLaserScan.header.stamp = tfExpiration; + baseToLaserScan.header.stamp = time; rtabmap_ros::transformToGeometryMsg(rtabmap::Transform(0,0,scanHeight,0,0,0), baseToLaserScan.transform); tfBroadcaster.sendTransform(baseToLaserScan); } diff --git a/src/GridMapAssemblerNode.cpp b/src/GridMapAssemblerNode.cpp deleted file mode 100644 index 70c1b10b..00000000 --- a/src/GridMapAssemblerNode.cpp +++ /dev/null @@ -1,199 +0,0 @@ -/* -Copyright (c) 2010-2014, 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 -#include "rtabmap_ros/MapData.h" -#include "rtabmap_ros/MsgConversion.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace rtabmap; - -class GridMapAssembler -{ - -public: - GridMapAssembler() : - gridCellSize_(0.05), // meters - mapSize_(0), // meters - eroded_(false), - filterRadius_(0.5), - filterAngle_(30.0) // degrees - { - ros::NodeHandle pnh("~"); - pnh.param("cell_size", gridCellSize_, gridCellSize_); // m - pnh.param("map_size", mapSize_, mapSize_); // m - pnh.param("filter_radius", filterRadius_, filterRadius_); - pnh.param("filter_angle", filterAngle_, filterAngle_); - pnh.param("eroded", eroded_, eroded_); - - UASSERT(gridCellSize_ > 0.0); - UASSERT(mapSize_ >= 0.0); - - ros::NodeHandle nh; - mapDataTopic_ = nh.subscribe("mapData", 1, &GridMapAssembler::mapDataReceivedCallback, this); - - gridMap_ = nh.advertise("grid_map", 1); - - //private service - getMapService_ = pnh.advertiseService("get_map", &GridMapAssembler::getGridMapCallback, this); - resetService_ = pnh.advertiseService("reset", &GridMapAssembler::reset, this); - } - - ~GridMapAssembler() - { - } - - void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg) - { - UTimer timer; - for(unsigned int i=0; inodes.size(); ++i) - { - if(!uContains(gridMaps_, msg->nodes[i].id) && msg->nodes[i].laserScan.size()) - { - cv::Mat laserScan = rtabmap::uncompressData(msg->nodes[i].laserScan); - if(!laserScan.empty()) - { - cv::Mat ground, obstacles; - util3d::occupancy2DFromLaserScan(laserScan, ground, obstacles, gridCellSize_); - - if(!ground.empty() || !obstacles.empty()) - { - gridMaps_.insert(std::make_pair(msg->nodes[i].id, std::make_pair(ground, obstacles))); - } - } - } - } - - std::map poses; - UASSERT(msg->graph.posesId.size() == msg->graph.poses.size()); - for(unsigned int i=0; igraph.posesId.size(); ++i) - { - poses.insert(std::make_pair(msg->graph.posesId[i], rtabmap_ros::transformFromPoseMsg(msg->graph.poses[i]))); - } - - if(filterRadius_ > 0.0 && filterAngle_ > 0.0) - { - poses = rtabmap::graph::radiusPosesFiltering(poses, filterRadius_, filterAngle_*CV_PI/180.0); - } - - if(gridMap_.getNumSubscribers()) - { - // create the map - float xMin=0.0f, yMin=0.0f; - //cv::Mat pixels = util3d::create2DMap(poses, scans_, gridCellSize_, gridUnknownSpaceFilled_, xMin, yMin, mapSize_); - cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps( - poses, - gridMaps_, - gridCellSize_, - xMin, yMin, - mapSize_, - eroded_); - - if(!pixels.empty()) - { - //init - 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 = msg->header.frame_id; - map_.header.stamp = ros::Time::now(); - - gridMap_.publish(map_); - ROS_INFO("Grid Map published [%d,%d] (%fs)", pixels.cols, pixels.rows, timer.ticks()); - } - } - } - - bool getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res) - { - if(map_.data.size()) - { - res.map = map_; - return true; - } - return false; - } - - bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) - { - ROS_INFO("grid_map_assembler: reset!"); - gridMaps_.clear(); - map_ = nav_msgs::OccupancyGrid(); - return true; - } - -private: - double gridCellSize_; - double mapSize_; - bool eroded_; - double filterRadius_; - double filterAngle_; - - ros::Subscriber mapDataTopic_; - - ros::Publisher gridMap_; - - ros::ServiceServer getMapService_; - ros::ServiceServer resetService_; - - std::map > gridMaps_; // - - nav_msgs::OccupancyGrid map_; -}; - - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "grid_map_assembler"); - GridMapAssembler assembler; - ros::spin(); - return 0; -} diff --git a/src/GuiNode.cpp b/src/GuiNode.cpp index 8ac60d89..c506e314 100644 --- a/src/GuiNode.cpp +++ b/src/GuiNode.cpp @@ -33,8 +33,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +QApplication * app = 0; +ros::AsyncSpinner * spinner = 0; + void my_handler(int s){ - QApplication::exit(); + ROS_INFO("rtabmapviz: ctrl-c catched! Exiting Qt app..."); + spinner->stop(); + exit(-1); } int main(int argc, char** argv) @@ -46,7 +51,10 @@ int main(int argc, char** argv) ros::init(argc, argv, "rtabmapviz"); - GuiWrapper gui(argc, argv); + app = new QApplication(argc, argv); + app->connect( app, SIGNAL( lastWindowClosed() ), app, SLOT( quit() ) ); + + GuiWrapper * gui = new GuiWrapper(argc, argv); // Catch ctrl-c to close the gui // (Place this after QApplication's constructor) @@ -57,15 +65,18 @@ int main(int argc, char** argv) sigaction(SIGINT, &sigIntHandler, NULL); // Here start the ROS events loop - ros::AsyncSpinner spinner(4); // Use 4 threads - spinner.start(); + spinner = new ros::AsyncSpinner(1); // Use 1 thread + spinner->start(); ROS_INFO("rtabmapviz started."); // Now wait for application to finish - int r = gui.exec();// MUST be called by the Main Thread + int r = app->exec();// MUST be called by the Main Thread - spinner.stop(); + spinner->stop(); + delete spinner; + delete gui; + delete app; ROS_INFO("rtabmapviz: All done! Closing..."); return r; } diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp index ac81e48c..952de56c 100644 --- a/src/GuiWrapper.cpp +++ b/src/GuiWrapper.cpp @@ -26,8 +26,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include "GuiWrapper.h" -#include -#include +#include +#include #include #include @@ -40,9 +40,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include -#include - #include #include #include @@ -71,11 +68,10 @@ float max3( const float& a, const float& b, const float& c) } GuiWrapper::GuiWrapper(int & argc, char** argv) : - app_(0), mainWindow_(0), frameId_("base_link"), waitForTransform_(true), - waitForTransformDuration_(0.1), // 100 ms + waitForTransformDuration_(0.2), // 200 ms cameraNodeName_(""), lastOdomInfoUpdateTime_(0), depthScanSync_(0), @@ -88,7 +84,6 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) : depthOdomInfo2Sync_(0) { ros::NodeHandle nh; - app_ = new QApplication(argc, argv); QString configFile = QDir::homePath()+"/.ros/rtabmapGUI.ini"; for(int i=1; isetMonitoringState(paused); - app_->connect( app_, SIGNAL( lastWindowClosed() ), app_, SLOT( quit() ) ); ros::NodeHandle pnh("~"); // To receive odometry events - bool subscribeLaserScan = false; + bool subscribeLaserScan2d = false; + bool subscribeLaserScan3d = false; bool subscribeDepth = false; bool subscribeOdomInfo = false; bool subscribeStereo = false; @@ -130,7 +125,12 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) : pnh.param("frame_id", frameId_, frameId_); pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF pnh.param("subscribe_depth", subscribeDepth, subscribeDepth); - pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan); + if(pnh.getParam("subscribe_laserScan", subscribeLaserScan2d) && subscribeLaserScan2d) + { + ROS_WARN("rtabmapviz: \"subscribe_laserScan\" parameter is deprecated, use \"subscribe_scan\" instead. The scan topic is still subscribed."); + } + pnh.param("subscribe_scan", subscribeLaserScan2d, subscribeLaserScan2d); + pnh.param("subscribe_scan_cloud", subscribeLaserScan3d, subscribeLaserScan3d); pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo); pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo); pnh.param("depth_cameras", depthCameras, depthCameras); @@ -181,7 +181,8 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) : this->setupCallbacks( subscribeDepth, - subscribeLaserScan, + subscribeLaserScan2d, + subscribeLaserScan3d, subscribeOdomInfo, subscribeStereo, queueSize, @@ -210,6 +211,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) : GuiWrapper::~GuiWrapper() { + UDEBUG(""); if(depthSync_) delete depthSync_; if(depth2Sync_) @@ -245,12 +247,6 @@ GuiWrapper::~GuiWrapper() delete infoMapSync_; delete mainWindow_; - delete app_; -} - -int GuiWrapper::exec() -{ - return app_->exec(); } void GuiWrapper::infoMapCallback( @@ -292,7 +288,7 @@ void GuiWrapper::goalPathCallback( poses[i].first = -int(i)-1; poses[i].second = rtabmap_ros::transformFromPoseMsg(pathMsg->poses[i].pose); } - this->post(new RtabmapGlobalPathEvent(goalMsg->node_id, goalMsg->node_label, poses)); + this->post(new RtabmapGlobalPathEvent(goalMsg->node_id, goalMsg->node_label, poses, 0.0)); } void GuiWrapper::goalReachedCallback( @@ -441,7 +437,7 @@ void GuiWrapper::handleEvent(UEvent * anEvent) poses[i].first = setGoalSrv.response.path_ids[i]; poses[i].second = rtabmap_ros::transformFromPoseMsg(setGoalSrv.response.path_poses[i]); } - this->post(new RtabmapGlobalPathEvent(setGoalSrv.request.node_id, setGoalSrv.request.node_label, poses)); + this->post(new RtabmapGlobalPathEvent(setGoalSrv.request.node_id, setGoalSrv.request.node_label, poses, setGoalSrv.response.planning_time)); } } else if(cmd == rtabmap::RtabmapEventCmd::kCmdCancelGoal) @@ -489,7 +485,8 @@ Transform GuiWrapper::getTransform(const std::string & fromFrameId, const std::s //if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1))) if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_))) { - ROS_WARN("rtabmapviz: Could not get transform from %s to %s after %f seconds!", fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_); + ROS_WARN("rtabmapviz: Could not get transform from %s to %s after %f seconds (for stamp=%f)!", + fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_, stamp.toSec()); return transform; } } @@ -510,7 +507,8 @@ void GuiWrapper::commonDepthCallback( const sensor_msgs::ImageConstPtr& imageMsg, const sensor_msgs::ImageConstPtr& depthMsg, const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, - const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::LaserScanConstPtr& scan2dMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) { std::vector imageMsgs; @@ -519,7 +517,7 @@ void GuiWrapper::commonDepthCallback( imageMsgs.push_back(imageMsg); depthMsgs.push_back(depthMsg); cameraInfoMsgs.push_back(cameraInfoMsg); - commonDepthCallback(odomMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, odomInfoMsg); + commonDepthCallback(odomMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scan2dMsg, scan3dMsg, odomInfoMsg); } void GuiWrapper::commonDepthCallback( @@ -527,7 +525,8 @@ void GuiWrapper::commonDepthCallback( const std::vector & imageMsgs, const std::vector & depthMsgs, const std::vector & cameraInfoMsgs, - const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::LaserScanConstPtr& scan2dMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) { if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 && @@ -547,9 +546,13 @@ void GuiWrapper::commonDepthCallback( } else { - if(scanMsg.get()) + if(scan2dMsg.get()) { - odomHeader = scanMsg->header; + odomHeader = scan2dMsg->header; + } + else if(scan3dMsg.get()) + { + odomHeader = scan3dMsg->header; } else if(cameraInfoMsgs.size() && cameraInfoMsgs[0].get()) { @@ -679,22 +682,15 @@ void GuiWrapper::commonDepthCallback( return; } - image_geometry::PinholeCameraModel model; - model.fromCameraInfo(*cameraInfoMsgs[i]); - cameraModels.push_back(rtabmap::CameraModel( - model.fx(), - model.fy(), - model.cx(), - model.cy(), - localTransform)); + cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*cameraInfoMsgs[i], localTransform)); } } cv::Mat scan; - if(scanMsg.get() != 0) + if(scan2dMsg.get() != 0) { // make sure the frame of the laser is updated too - if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull()) + if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull()) { return; } @@ -702,16 +698,16 @@ void GuiWrapper::commonDepthCallback( //transform in frameId_ frame sensor_msgs::PointCloud2 scanOut; laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_); + projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_); pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(scanOut, *pclScan); // sync with odometry stamp - if(odomHeader.stamp != scanMsg->header.stamp) + if(odomHeader.stamp != scan2dMsg->header.stamp) { if(!odomT.isNull()) { - Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scanMsg->header.stamp); + Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scan2dMsg->header.stamp); if(sensorT.isNull()) { return; @@ -723,6 +719,12 @@ void GuiWrapper::commonDepthCallback( } scan = util3d::laserScanFromPointCloud(*pclScan); } + else if(scan3dMsg.get() != 0) + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*scan3dMsg, *pclScan); + scan = util3d::laserScanFromPointCloud(*pclScan); + } rtabmap::OdometryInfo info; if(odomInfoMsg.get()) @@ -733,8 +735,8 @@ void GuiWrapper::commonDepthCallback( rtabmap::OdometryEvent odomEvent( rtabmap::SensorData( scan, - scanMsg.get()?(int)scanMsg->ranges.size():0, - scanMsg.get()?(int)scanMsg->range_max:0, + scan2dMsg.get()?(int)scan2dMsg->ranges.size():0, + scan2dMsg.get()?(int)scan2dMsg->range_max:0, rgb, depth, cameraModels, @@ -754,7 +756,8 @@ void GuiWrapper::commonStereoCallback( const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, - const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::LaserScanConstPtr& scan2dMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) { // limit 10 Hz max @@ -787,9 +790,13 @@ void GuiWrapper::commonStereoCallback( } else { - if(scanMsg.get()) + if(scan2dMsg.get()) { - odomHeader = scanMsg->header; + odomHeader = scan2dMsg->header; + } + else if(scan3dMsg.get()) + { + odomHeader = scan3dMsg->header; } else { @@ -838,17 +845,9 @@ void GuiWrapper::commonStereoCallback( } } - image_geometry::StereoCameraModel model; - model.fromCameraInfo(*leftCamInfoMsg, *rightCamInfoMsg); - rtabmap::StereoCameraModel stereoModel( - model.left().fx(), - model.left().fy(), - model.left().cx(), - model.left().cy(), - model.baseline(), - localTransform); + rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform); - if(model.baseline() > 10.0) + if(stereoModel.baseline() > 10.0) { static bool shown = false; if(!shown) @@ -856,7 +855,7 @@ void GuiWrapper::commonStereoCallback( 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.", - model.baseline()); + stereoModel.baseline()); shown = true; } } @@ -878,10 +877,10 @@ void GuiWrapper::commonStereoCallback( cv::Mat right = cv_bridge::toCvCopy(rightImageMsg, "mono8")->image; cv::Mat scan; - if(scanMsg.get() != 0) + if(scan2dMsg.get() != 0) { // make sure the frame of the laser is updated too - if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull()) + if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull()) { return; } @@ -889,16 +888,16 @@ void GuiWrapper::commonStereoCallback( //transform in frameId_ frame sensor_msgs::PointCloud2 scanOut; laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_); + projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_); pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(scanOut, *pclScan); // sync with odometry stamp - if(odomHeader.stamp != scanMsg->header.stamp) + if(odomHeader.stamp != scan2dMsg->header.stamp) { if(!odomT.isNull()) { - Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scanMsg->header.stamp); + Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scan2dMsg->header.stamp); if(sensorT.isNull()) { return; @@ -908,6 +907,12 @@ void GuiWrapper::commonStereoCallback( } } + scan = util3d::laserScan2dFromPointCloud(*pclScan); + } + else if(scan3dMsg.get() != 0) + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*scan3dMsg, *pclScan); scan = util3d::laserScanFromPointCloud(*pclScan); } @@ -920,8 +925,8 @@ void GuiWrapper::commonStereoCallback( rtabmap::OdometryEvent odomEvent( rtabmap::SensorData( scan, - scanMsg.get()?(int)scanMsg->ranges.size():0, - scanMsg.get()?(int)scanMsg->range_max:0, + scan2dMsg.get()?(int)scan2dMsg->ranges.size():0, + scan2dMsg.get()?(int)scan2dMsg->range_max:0, left, right, stereoModel, @@ -944,6 +949,7 @@ void GuiWrapper::defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg) sensor_msgs::ImageConstPtr(), sensor_msgs::CameraInfoConstPtr(), sensor_msgs::LaserScanConstPtr(), + sensor_msgs::PointCloud2ConstPtr(), rtabmap_ros::OdomInfoConstPtr()); } @@ -959,6 +965,7 @@ void GuiWrapper::depthCallback( depthMsg, cameraInfoMsg, sensor_msgs::LaserScanConstPtr(), + sensor_msgs::PointCloud2ConstPtr(), rtabmap_ros::OdomInfoConstPtr()); } @@ -987,6 +994,7 @@ void GuiWrapper::depth2Callback( depthMsgs, cameraInfoMsgs, sensor_msgs::LaserScanConstPtr(), + sensor_msgs::PointCloud2ConstPtr(), rtabmap_ros::OdomInfoConstPtr()); } @@ -1003,6 +1011,7 @@ void GuiWrapper::depthOdomInfoCallback( depthMsg, cameraInfoMsg, sensor_msgs::LaserScanConstPtr(), + sensor_msgs::PointCloud2ConstPtr(), odomInfoMsg); } @@ -1032,6 +1041,7 @@ void GuiWrapper::depthOdomInfo2Callback( depthMsgs, cameraInfoMsgs, sensor_msgs::LaserScanConstPtr(), + sensor_msgs::PointCloud2ConstPtr(), odomInfoMsg); } @@ -1048,9 +1058,63 @@ void GuiWrapper::depthScanCallback( depthMsg, cameraInfoMsg, scanMsg, + sensor_msgs::PointCloud2ConstPtr(), rtabmap_ros::OdomInfoConstPtr()); } +void GuiWrapper::depthScanOdomInfoCallback( + const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, + const sensor_msgs::LaserScanConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg) +{ + commonDepthCallback( + odomMsg, + imageMsg, + depthMsg, + cameraInfoMsg, + scanMsg, + sensor_msgs::PointCloud2ConstPtr(), + odomInfoMsg); +} + +void GuiWrapper::depthScan3dCallback( + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg) +{ + commonDepthCallback( + odomMsg, + imageMsg, + depthMsg, + cameraInfoMsg, + sensor_msgs::LaserScanConstPtr(), + scanMsg, + rtabmap_ros::OdomInfoConstPtr()); +} + +void GuiWrapper::depthScan3dOdomInfoCallback( + const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg) +{ + commonDepthCallback( + odomMsg, + imageMsg, + depthMsg, + cameraInfoMsg, + sensor_msgs::LaserScanConstPtr(), + scanMsg, + odomInfoMsg); +} + void GuiWrapper::stereoScanCallback( const sensor_msgs::LaserScanConstPtr& scanMsg, const nav_msgs::OdometryConstPtr & odomMsg, @@ -1066,9 +1130,69 @@ void GuiWrapper::stereoScanCallback( leftCameraInfoMsg, rightCameraInfoMsg, scanMsg, + sensor_msgs::PointCloud2ConstPtr(), rtabmap_ros::OdomInfoConstPtr()); } +void GuiWrapper::stereoScanOdomInfoCallback( + const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, + const sensor_msgs::LaserScanConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg) +{ + commonStereoCallback( + odomMsg, + leftImageMsg, + rightImageMsg, + leftCameraInfoMsg, + rightCameraInfoMsg, + scanMsg, + sensor_msgs::PointCloud2ConstPtr(), + odomInfoMsg); +} + +void GuiWrapper::stereoScan3dCallback( + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg) +{ + commonStereoCallback( + odomMsg, + leftImageMsg, + rightImageMsg, + leftCameraInfoMsg, + rightCameraInfoMsg, + sensor_msgs::LaserScanConstPtr(), + scanMsg, + rtabmap_ros::OdomInfoConstPtr()); +} + +void GuiWrapper::stereoScan3dOdomInfoCallback( + const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg) +{ + commonStereoCallback( + odomMsg, + leftImageMsg, + rightImageMsg, + leftCameraInfoMsg, + rightCameraInfoMsg, + sensor_msgs::LaserScanConstPtr(), + scanMsg, + odomInfoMsg); +} + void GuiWrapper::stereoOdomInfoCallback( const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, const nav_msgs::OdometryConstPtr & odomMsg, @@ -1084,6 +1208,7 @@ void GuiWrapper::stereoOdomInfoCallback( leftCameraInfoMsg, rightCameraInfoMsg, sensor_msgs::LaserScanConstPtr(), + sensor_msgs::PointCloud2ConstPtr(), odomInfoMsg); } @@ -1101,6 +1226,7 @@ void GuiWrapper::stereoCallback( leftCameraInfoMsg, rightCameraInfoMsg, sensor_msgs::LaserScanConstPtr(), + sensor_msgs::PointCloud2ConstPtr(), rtabmap_ros::OdomInfoConstPtr()); } @@ -1116,6 +1242,7 @@ void GuiWrapper::depthTFCallback( depthMsg, cameraInfoMsg, sensor_msgs::LaserScanConstPtr(), + sensor_msgs::PointCloud2ConstPtr(), rtabmap_ros::OdomInfoConstPtr()); } @@ -1131,6 +1258,7 @@ void GuiWrapper::depthOdomInfoTFCallback( depthMsg, cameraInfoMsg, sensor_msgs::LaserScanConstPtr(), + sensor_msgs::PointCloud2ConstPtr(), odomInfoMsg); } @@ -1146,6 +1274,23 @@ void GuiWrapper::depthScanTFCallback( depthMsg, cameraInfoMsg, scanMsg, + sensor_msgs::PointCloud2ConstPtr(), + rtabmap_ros::OdomInfoConstPtr()); +} + +void GuiWrapper::depthScan3dTFCallback( + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg) +{ + commonDepthCallback( + nav_msgs::OdometryConstPtr(), + imageMsg, + depthMsg, + cameraInfoMsg, + sensor_msgs::LaserScanConstPtr(), + scanMsg, rtabmap_ros::OdomInfoConstPtr()); } @@ -1163,6 +1308,25 @@ void GuiWrapper::stereoScanTFCallback( leftCameraInfoMsg, rightCameraInfoMsg, scanMsg, + sensor_msgs::PointCloud2ConstPtr(), + rtabmap_ros::OdomInfoConstPtr()); +} + +void GuiWrapper::stereoScan3dTFCallback( + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg) +{ + commonStereoCallback( + nav_msgs::OdometryConstPtr(), + leftImageMsg, + rightImageMsg, + leftCameraInfoMsg, + rightCameraInfoMsg, + sensor_msgs::LaserScanConstPtr(), + scanMsg, rtabmap_ros::OdomInfoConstPtr()); } @@ -1180,6 +1344,7 @@ void GuiWrapper::stereoOdomInfoTFCallback( leftCameraInfoMsg, rightCameraInfoMsg, sensor_msgs::LaserScanConstPtr(), + sensor_msgs::PointCloud2ConstPtr(), odomInfoMsg); } @@ -1196,12 +1361,14 @@ void GuiWrapper::stereoTFCallback( leftCameraInfoMsg, rightCameraInfoMsg, sensor_msgs::LaserScanConstPtr(), + sensor_msgs::PointCloud2ConstPtr(), rtabmap_ros::OdomInfoConstPtr()); } void GuiWrapper::setupCallbacks( bool subscribeDepth, - bool subscribeLaserScan, + bool subscribeLaserScan2d, + bool subscribeLaserScan3d, bool subscribeOdomInfo, bool subscribeStereo, int queueSize, @@ -1212,9 +1379,11 @@ void GuiWrapper::setupCallbacks( if(subscribeDepth && subscribeStereo) { - ROS_WARN("\"subscribe_depth\" already true, ignoring \"subscribe_stereo\"."); + ROS_WARN("rtabmapviz: Parameters subscribe_depth and subscribe_stereo cannot be true at the " + "same time. Parameter subscribe_depth is set to false."); + subscribeDepth = false; } - if(!subscribeDepth && !subscribeStereo && subscribeLaserScan) + if(!subscribeDepth && !subscribeStereo && (subscribeLaserScan2d || subscribeLaserScan3d)) { ROS_WARN("Cannot subscribe to laser scan without depth or stereo subscription..."); } @@ -1231,7 +1400,7 @@ void GuiWrapper::setupCallbacks( if(subscribeDepth) { UASSERT(depthCameras >= 1 && depthCameras <= 2); - UASSERT_MSG(depthCameras == 1 || !(subscribeLaserScan || !odomFrameId_.empty()), "Not yet supported!"); + UASSERT_MSG(depthCameras == 1 || !(subscribeLaserScan2d || subscribeLaserScan3d || !odomFrameId_.empty()), "Not yet supported!"); imageSubs_.resize(depthCameras); imageDepthSubs_.resize(depthCameras); @@ -1265,25 +1434,95 @@ void GuiWrapper::setupCallbacks( if(odomFrameId_.empty()) { odomSub_.subscribe(nh, "odom", 1); - if(subscribeLaserScan) + if(subscribeLaserScan2d) { scanSub_.subscribe(nh, "scan", 1); - depthScanSync_ = new message_filters::Synchronizer( - MyDepthScanSyncPolicy(queueSize), - scanSub_, - odomSub_, - *imageSubs_[0], - *imageDepthSubs_[0], - *cameraInfoSubs_[0]); - depthScanSync_->registerCallback(boost::bind(&GuiWrapper::depthScanCallback, this, _1, _2, _3, _4, _5)); + if(subscribeOdomInfo) + { + odomInfoSub_.subscribe(nh, "odom_info", 1); + depthScanOdomInfoSync_ = new message_filters::Synchronizer( + MyDepthScanOdomInfoSyncPolicy(queueSize), + odomInfoSub_, + scanSub_, + odomSub_, + *imageSubs_[0], + *imageDepthSubs_[0], + *cameraInfoSubs_[0]); + depthScanOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::depthScanOdomInfoCallback, this, _1, _2, _3, _4, _5, _6)); - ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s", - ros::this_node::getName().c_str(), - imageSubs_[0]->getTopic().c_str(), - imageDepthSubs_[0]->getTopic().c_str(), - cameraInfoSubs_[0]->getTopic().c_str(), - odomSub_.getTopic().c_str(), - scanSub_.getTopic().c_str()); + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageSubs_[0]->getTopic().c_str(), + imageDepthSubs_[0]->getTopic().c_str(), + cameraInfoSubs_[0]->getTopic().c_str(), + odomSub_.getTopic().c_str(), + scanSub_.getTopic().c_str(), + odomInfoSub_.getTopic().c_str()); + } + else + { + depthScanSync_ = new message_filters::Synchronizer( + MyDepthScanSyncPolicy(queueSize), + scanSub_, + odomSub_, + *imageSubs_[0], + *imageDepthSubs_[0], + *cameraInfoSubs_[0]); + depthScanSync_->registerCallback(boost::bind(&GuiWrapper::depthScanCallback, this, _1, _2, _3, _4, _5)); + + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageSubs_[0]->getTopic().c_str(), + imageDepthSubs_[0]->getTopic().c_str(), + cameraInfoSubs_[0]->getTopic().c_str(), + odomSub_.getTopic().c_str(), + scanSub_.getTopic().c_str()); + } + } + else if(subscribeLaserScan3d) + { + scan3dSub_.subscribe(nh, "scan_cloud", 1); + if(subscribeOdomInfo) + { + odomInfoSub_.subscribe(nh, "odom_info", 1); + depthScan3dOdomInfoSync_ = new message_filters::Synchronizer( + MyDepthScan3dOdomInfoSyncPolicy(queueSize), + odomInfoSub_, + scan3dSub_, + odomSub_, + *imageSubs_[0], + *imageDepthSubs_[0], + *cameraInfoSubs_[0]); + depthScan3dOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::depthScan3dOdomInfoCallback, this, _1, _2, _3, _4, _5, _6)); + + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageSubs_[0]->getTopic().c_str(), + imageDepthSubs_[0]->getTopic().c_str(), + cameraInfoSubs_[0]->getTopic().c_str(), + odomSub_.getTopic().c_str(), + scan3dSub_.getTopic().c_str(), + odomInfoSub_.getTopic().c_str()); + } + else + { + depthScan3dSync_ = new message_filters::Synchronizer( + MyDepthScan3dSyncPolicy(queueSize), + scan3dSub_, + odomSub_, + *imageSubs_[0], + *imageDepthSubs_[0], + *cameraInfoSubs_[0]); + depthScan3dSync_->registerCallback(boost::bind(&GuiWrapper::depthScan3dCallback, this, _1, _2, _3, _4, _5)); + + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageSubs_[0]->getTopic().c_str(), + imageDepthSubs_[0]->getTopic().c_str(), + cameraInfoSubs_[0]->getTopic().c_str(), + odomSub_.getTopic().c_str(), + scan3dSub_.getTopic().c_str()); + } } else if(subscribeOdomInfo) { @@ -1380,7 +1619,7 @@ void GuiWrapper::setupCallbacks( else { // use TF as odom - if(subscribeLaserScan) + if(subscribeLaserScan2d) { scanSub_.subscribe(nh, "scan", 1); depthScanTFSync_ = new message_filters::Synchronizer( @@ -1398,6 +1637,24 @@ void GuiWrapper::setupCallbacks( cameraInfoSubs_[0]->getTopic().c_str(), scanSub_.getTopic().c_str()); } + else if(subscribeLaserScan3d) + { + scan3dSub_.subscribe(nh, "scan_cloud", 1); + depthScan3dTFSync_ = new message_filters::Synchronizer( + MyDepthScan3dTFSyncPolicy(queueSize), + scan3dSub_, + *imageSubs_[0], + *imageDepthSubs_[0], + *cameraInfoSubs_[0]); + depthScan3dTFSync_->registerCallback(boost::bind(&GuiWrapper::depthScan3dTFCallback, this, _1, _2, _3, _4)); + + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageSubs_[0]->getTopic().c_str(), + imageDepthSubs_[0]->getTopic().c_str(), + cameraInfoSubs_[0]->getTopic().c_str(), + scan3dSub_.getTopic().c_str()); + } else if(subscribeOdomInfo) { odomInfoSub_.subscribe(nh, "odom_info", 1); @@ -1452,27 +1709,103 @@ void GuiWrapper::setupCallbacks( if(odomFrameId_.empty()) { odomSub_.subscribe(nh, "odom", 1); - if(subscribeLaserScan) + if(subscribeLaserScan2d) { scanSub_.subscribe(nh, "scan", 1); - stereoScanSync_ = new message_filters::Synchronizer( - MyStereoScanSyncPolicy(queueSize), - scanSub_, - odomSub_, - imageRectLeft_, - imageRectRight_, - cameraInfoLeft_, - cameraInfoRight_); - stereoScanSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanCallback, this, _1, _2, _3, _4, _5, _6)); + if(subscribeOdomInfo) + { + odomInfoSub_.subscribe(nh, "odom_info", 1); + stereoScanOdomInfoSync_ = new message_filters::Synchronizer( + MyStereoScanOdomInfoSyncPolicy(queueSize), + odomInfoSub_, + scanSub_, + odomSub_, + imageRectLeft_, + imageRectRight_, + cameraInfoLeft_, + cameraInfoRight_); + stereoScanOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanOdomInfoCallback, this, _1, _2, _3, _4, _5, _6, _7)); - ROS_INFO("\n%s subscribed to:\n %s,\n %s,\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(), - odomSub_.getTopic().c_str(), - scanSub_.getTopic().c_str()); + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\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(), + odomSub_.getTopic().c_str(), + scanSub_.getTopic().c_str(), + odomInfoSub_.getTopic().c_str()); + } + else + { + stereoScanSync_ = new message_filters::Synchronizer( + MyStereoScanSyncPolicy(queueSize), + scanSub_, + odomSub_, + imageRectLeft_, + imageRectRight_, + cameraInfoLeft_, + cameraInfoRight_); + stereoScanSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanCallback, this, _1, _2, _3, _4, _5, _6)); + + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\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(), + odomSub_.getTopic().c_str(), + scanSub_.getTopic().c_str()); + } + } + else if(subscribeLaserScan3d) + { + scan3dSub_.subscribe(nh, "scan_cloud", 1); + if(subscribeOdomInfo) + { + odomInfoSub_.subscribe(nh, "odom_info", 1); + stereoScan3dOdomInfoSync_ = new message_filters::Synchronizer( + MyStereoScan3dOdomInfoSyncPolicy(queueSize), + odomInfoSub_, + scan3dSub_, + odomSub_, + imageRectLeft_, + imageRectRight_, + cameraInfoLeft_, + cameraInfoRight_); + stereoScan3dOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::stereoScan3dOdomInfoCallback, this, _1, _2, _3, _4, _5, _6, _7)); + + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\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(), + odomSub_.getTopic().c_str(), + scan3dSub_.getTopic().c_str(), + odomInfoSub_.getTopic().c_str()); + } + else + { + stereoScan3dSync_ = new message_filters::Synchronizer( + MyStereoScan3dSyncPolicy(queueSize), + scan3dSub_, + odomSub_, + imageRectLeft_, + imageRectRight_, + cameraInfoLeft_, + cameraInfoRight_); + stereoScan3dSync_->registerCallback(boost::bind(&GuiWrapper::stereoScan3dCallback, this, _1, _2, _3, _4, _5, _6)); + + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\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(), + odomSub_.getTopic().c_str(), + scan3dSub_.getTopic().c_str()); + } } else if(subscribeOdomInfo) { @@ -1519,7 +1852,7 @@ void GuiWrapper::setupCallbacks( else { //use odom TF - if(subscribeLaserScan) + if(subscribeLaserScan2d) { scanSub_.subscribe(nh, "scan", 1); stereoScanTFSync_ = new message_filters::Synchronizer( @@ -1539,6 +1872,26 @@ void GuiWrapper::setupCallbacks( cameraInfoRight_.getTopic().c_str(), scanSub_.getTopic().c_str()); } + else if(subscribeLaserScan3d) + { + scan3dSub_.subscribe(nh, "scan_cloud", 1); + stereoScan3dTFSync_ = new message_filters::Synchronizer( + MyStereoScan3dTFSyncPolicy(queueSize), + scan3dSub_, + imageRectLeft_, + imageRectRight_, + cameraInfoLeft_, + cameraInfoRight_); + stereoScan3dTFSync_->registerCallback(boost::bind(&GuiWrapper::stereoScan3dTFCallback, this, _1, _2, _3, _4, _5)); + + ROS_INFO("\n%s subscribed to:\n %s,\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(), + scan3dSub_.getTopic().c_str()); + } else if(subscribeOdomInfo) { odomInfoSub_.subscribe(nh, "odom_info", 1); diff --git a/src/GuiWrapper.h b/src/GuiWrapper.h index bb93a46d..6f39d2d0 100644 --- a/src/GuiWrapper.h +++ b/src/GuiWrapper.h @@ -68,8 +68,6 @@ public: GuiWrapper(int & argc, char** argv); virtual ~GuiWrapper(); - int exec(); - protected: virtual void handleEvent(UEvent * anEvent); @@ -80,7 +78,8 @@ private: void setupCallbacks( bool subscribeDepth, - bool subscribeLaserScan, + bool subscribeLaserScan2d, + bool subscribeLaserScan3d, bool subscribeOdomInfo, bool subscribeStereo, int queueSize, @@ -91,14 +90,16 @@ private: const sensor_msgs::ImageConstPtr& imageMsg, const sensor_msgs::ImageConstPtr& depthMsg, const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, - const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::LaserScanConstPtr& scan2dMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg); void commonDepthCallback( const nav_msgs::OdometryConstPtr & odomMsg, const std::vector & imageMsgs, const std::vector & depthMsgs, const std::vector & cameraInfoMsgs, - const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::LaserScanConstPtr& scan2dMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg); void commonStereoCallback( const nav_msgs::OdometryConstPtr & odomMsg, @@ -106,7 +107,8 @@ private: const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, - const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::LaserScanConstPtr& scan2dMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg); void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg); @@ -146,6 +148,26 @@ private: const sensor_msgs::ImageConstPtr& imageMsg, const sensor_msgs::ImageConstPtr& imageDepthMsg, const sensor_msgs::CameraInfoConstPtr& camInfoMsg); + void depthScanOdomInfoCallback( + const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, + const sensor_msgs::LaserScanConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& imageDepthMsg, + const sensor_msgs::CameraInfoConstPtr& camInfoMsg); + void depthScan3dCallback( + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& imageDepthMsg, + const sensor_msgs::CameraInfoConstPtr& camInfoMsg); + void depthScan3dOdomInfoCallback( + const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& imageDepthMsg, + const sensor_msgs::CameraInfoConstPtr& camInfoMsg); void stereoScanCallback( const sensor_msgs::LaserScanConstPtr& scanMsg, @@ -154,6 +176,29 @@ private: const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg); + void stereoScanOdomInfoCallback( + const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, + const sensor_msgs::LaserScanConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg); + void stereoScan3dCallback( + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg); + void stereoScan3dOdomInfoCallback( + const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg); void stereoOdomInfoCallback( const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, const nav_msgs::OdometryConstPtr & odomMsg, @@ -182,6 +227,11 @@ private: const sensor_msgs::ImageConstPtr& imageMsg, const sensor_msgs::ImageConstPtr& imageDepthMsg, const sensor_msgs:: CameraInfoConstPtr& camInfoMsg); + void depthScan3dTFCallback( + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& imageDepthMsg, + const sensor_msgs:: CameraInfoConstPtr& camInfoMsg); void stereoScanTFCallback( const sensor_msgs::LaserScanConstPtr& scanMsg, @@ -189,6 +239,12 @@ private: const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg); + void stereoScan3dTFCallback( + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg); void stereoOdomInfoTFCallback( const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, const sensor_msgs::ImageConstPtr& leftImageMsg, @@ -205,7 +261,6 @@ private: rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const; private: - QApplication * app_; rtabmap::MainWindow * mainWindow_; std::string cameraNodeName_; double lastOdomInfoUpdateTime_; @@ -231,6 +286,7 @@ private: message_filters::Subscriber odomSub_; message_filters::Subscriber odomInfoSub_; message_filters::Subscriber scanSub_; + message_filters::Subscriber scan3dSub_; image_transport::SubscriberFilter imageRectLeft_; image_transport::SubscriberFilter imageRectRight_; @@ -256,6 +312,32 @@ private: sensor_msgs::CameraInfo> MyDepthScanSyncPolicy; message_filters::Synchronizer * depthScanSync_; + typedef message_filters::sync_policies::ApproximateTime< + rtabmap_ros::OdomInfo, + sensor_msgs::LaserScan, + nav_msgs::Odometry, + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo> MyDepthScanOdomInfoSyncPolicy; + message_filters::Synchronizer * depthScanOdomInfoSync_; + + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::PointCloud2, + nav_msgs::Odometry, + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo> MyDepthScan3dSyncPolicy; + message_filters::Synchronizer * depthScan3dSync_; + + typedef message_filters::sync_policies::ApproximateTime< + rtabmap_ros::OdomInfo, + sensor_msgs::PointCloud2, + nav_msgs::Odometry, + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo> MyDepthScan3dOdomInfoSyncPolicy; + message_filters::Synchronizer * depthScan3dOdomInfoSync_; + typedef message_filters::sync_policies::ApproximateTime< nav_msgs::Odometry, sensor_msgs::Image, @@ -288,6 +370,35 @@ private: sensor_msgs::CameraInfo> MyStereoScanSyncPolicy; message_filters::Synchronizer * stereoScanSync_; + typedef message_filters::sync_policies::ApproximateTime< + rtabmap_ros::OdomInfo, + sensor_msgs::LaserScan, + nav_msgs::Odometry, + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo, + sensor_msgs::CameraInfo> MyStereoScanOdomInfoSyncPolicy; + message_filters::Synchronizer * stereoScanOdomInfoSync_; + + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::PointCloud2, + nav_msgs::Odometry, + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo, + sensor_msgs::CameraInfo> MyStereoScan3dSyncPolicy; + message_filters::Synchronizer * stereoScan3dSync_; + + typedef message_filters::sync_policies::ApproximateTime< + rtabmap_ros::OdomInfo, + sensor_msgs::PointCloud2, + nav_msgs::Odometry, + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo, + sensor_msgs::CameraInfo> MyStereoScan3dOdomInfoSyncPolicy; + message_filters::Synchronizer * stereoScan3dOdomInfoSync_; + typedef message_filters::sync_policies::ApproximateTime< rtabmap_ros::OdomInfo, nav_msgs::Odometry, @@ -326,6 +437,13 @@ private: sensor_msgs::CameraInfo> MyDepthScanTFSyncPolicy; message_filters::Synchronizer * depthScanTFSync_; + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::PointCloud2, + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo> MyDepthScan3dTFSyncPolicy; + message_filters::Synchronizer * depthScan3dTFSync_; + typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, @@ -354,6 +472,14 @@ private: sensor_msgs::CameraInfo> MyStereoScanTFSyncPolicy; message_filters::Synchronizer * stereoScanTFSync_; + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::PointCloud2, + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo, + sensor_msgs::CameraInfo> MyStereoScan3dTFSyncPolicy; + message_filters::Synchronizer * stereoScan3dTFSync_; + typedef message_filters::sync_policies::ApproximateTime< rtabmap_ros::OdomInfo, sensor_msgs::Image, diff --git a/src/MapAssemblerNode.cpp b/src/MapAssemblerNode.cpp index d75b33bf..b6c2ca20 100644 --- a/src/MapAssemblerNode.cpp +++ b/src/MapAssemblerNode.cpp @@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "rtabmap_ros/MapData.h" #include "rtabmap_ros/MsgConversion.h" +#include "MapsManager.h" #include #include #include @@ -49,54 +50,13 @@ class MapAssembler public: MapAssembler() : - cloudDecimation_(4), - cloudMaxDepth_(4.0), - cloudVoxelSize_(0.02), - scanVoxelSize_(0.01), - nodeFilteringAngle_(30), // degrees - nodeFilteringRadius_(0.5), - noiseFilterRadius_(0.0), - noiseFilterMinNeighbors_(5), - computeOccupancyGrid_(false), - gridCellSize_(0.05), - groundMaxAngle_(M_PI_4), - clusterMinSize_(20), - maxHeight_(0), - occupancyMapSize_(0.0) + mapsManager_(false) { ros::NodeHandle pnh("~"); - pnh.param("cloud_decimation", cloudDecimation_, cloudDecimation_); - pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_); - pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_); - pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_); - - pnh.param("filter_radius", nodeFilteringRadius_, nodeFilteringRadius_); - pnh.param("filter_angle", nodeFilteringAngle_, nodeFilteringAngle_); - - pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_); - pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_); - - pnh.param("occupancy_grid", computeOccupancyGrid_, computeOccupancyGrid_); - pnh.param("occupancy_cell_size", gridCellSize_, gridCellSize_); - pnh.param("occupancy_ground_max_angle", groundMaxAngle_, groundMaxAngle_); - pnh.param("occupancy_cluster_min_size", clusterMinSize_, clusterMinSize_); - pnh.param("occupancy_max_height", maxHeight_, maxHeight_); - pnh.param("occupancy_map_size", occupancyMapSize_, occupancyMapSize_); - - UASSERT(gridCellSize_ > 0); - UASSERT(maxHeight_ >= 0); - UASSERT(occupancyMapSize_ >=0.0); ros::NodeHandle nh; mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this); - assembledMapClouds_ = nh.advertise("assembled_clouds", 1); - assembledMapScans_ = nh.advertise("assembled_scans", 1); - if(computeOccupancyGrid_) - { - occupancyMapPub_ = nh.advertise("grid_projection_map", 1); - } - // private service resetService_ = pnh.advertiseService("reset", &MapAssembler::reset, this); } @@ -108,239 +68,60 @@ public: void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg) { UTimer timer; + + std::map poses; + std::multimap constraints; + Transform mapOdom; + rtabmap_ros::mapGraphFromROS(msg->graph, poses, constraints, mapOdom); for(unsigned int i=0; inodes.size(); ++i) { - int id = msg->nodes[i].id; - if(!uContains(rgbClouds_, id)) + if(msg->nodes[i].image.size() || + msg->nodes[i].depth.size() || + msg->nodes[i].laserScan.size()) { - rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(msg->nodes[i]); - if(!s.sensorData().imageCompressed().empty() && - !s.sensorData().depthOrRightCompressed().empty() && - (s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValid())) - { - cv::Mat image, depth; - s.sensorData().uncompressData(&image, &depth, 0); - - - if(!s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty()) - { - pcl::PointCloud::Ptr cloud; - cloud = rtabmap::util3d::cloudRGBFromSensorData( - s.sensorData(), - cloudDecimation_, - cloudMaxDepth_); - - if(cloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0) - { - pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloud, noiseFilterRadius_, noiseFilterMinNeighbors_); - pcl::PointCloud::Ptr tmp(new pcl::PointCloud); - pcl::copyPointCloud(*cloud, *indices, *tmp); - cloud = tmp; - } - if(cloud->size() && cloudVoxelSize_ > 0) - { - cloud = util3d::voxelize(cloud, cloudVoxelSize_); - } - - if(cloud->size()) - { - rgbClouds_.insert(std::make_pair(id, cloud)); - - if(computeOccupancyGrid_) - { - pcl::PointCloud::Ptr cloudClipped = cloud; - if(cloudClipped->size() && maxHeight_ > 0) - { - cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits::min(), maxHeight_); - } - if(cloudClipped->size()) - { - cloudClipped = util3d::voxelize(cloudClipped, gridCellSize_); - - cv::Mat ground, obstacles; - util3d::occupancy2DFromCloud3D(cloudClipped, ground, obstacles, gridCellSize_, groundMaxAngle_, clusterMinSize_); - if(!ground.empty() || !obstacles.empty()) - { - occupancyLocalMaps_.insert(std::make_pair(id, std::make_pair(ground, obstacles))); - } - } - - } - } - } - } - } - - if(!uContains(scans_, id) && msg->nodes[i].laserScan.size()) - { - cv::Mat laserScan = rtabmap::uncompressData(msg->nodes[i].laserScan); - if(!laserScan.empty()) - { - pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloud(laserScan); - if(cloud->size() && scanVoxelSize_ > 0) - { - cloud = util3d::voxelize(cloud, scanVoxelSize_); - } - if(cloud->size()) - { - scans_.insert(std::make_pair(id, cloud)); - } - } + uInsert(nodes_, std::make_pair(msg->nodes[i].id, rtabmap_ros::nodeDataFromROS(msg->nodes[i]))); } } - // filter poses - std::map poses; - UASSERT(msg->graph.posesId.size() == msg->graph.poses.size()); - for(unsigned int i=0; igraph.posesId.size(); ++i) + // create a tmp signature with latest sensory data + if(poses.size() && nodes_.find(poses.rbegin()->first) != nodes_.end()) { - poses.insert(std::make_pair(msg->graph.posesId[i], rtabmap_ros::transformFromPoseMsg(msg->graph.poses[i]))); - } - if(nodeFilteringAngle_ > 0.0 && nodeFilteringRadius_ > 0.0) - { - poses = rtabmap::graph::radiusPosesFiltering(poses, nodeFilteringRadius_, nodeFilteringAngle_*CV_PI/180.0); + Signature tmpS = nodes_.at(poses.rbegin()->first); + SensorData tmpData = tmpS.sensorData(); + tmpData.setId(-1); + uInsert(nodes_, std::make_pair(-1, Signature(-1, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), Transform(), tmpData))); + poses.insert(std::make_pair(-1, poses.rbegin()->second)); } - if(assembledMapClouds_.getNumSubscribers()) - { - // generate the assembled cloud! - pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); + // Update maps + poses = mapsManager_.updateMapCaches( + poses, + 0, + false, + false, + false, + false, + nodes_); - for(std::map::iterator iter = poses.begin(); iter!=poses.end(); ++iter) - { - std::map::Ptr >::iterator jter = rgbClouds_.find(iter->first); - if(jter != rgbClouds_.end()) - { - pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); - *assembledCloud+=*transformed; - } - } + mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id); - if(assembledCloud->size()) - { - if(cloudVoxelSize_ > 0) - { - assembledCloud = util3d::voxelize(assembledCloud,cloudVoxelSize_); - } - - sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); - pcl::toROSMsg(*assembledCloud, *cloudMsg); - cloudMsg->header.stamp = ros::Time::now(); - cloudMsg->header.frame_id = msg->header.frame_id; - assembledMapClouds_.publish(cloudMsg); - } - } - - if(assembledMapScans_.getNumSubscribers()) - { - // generate the assembled scan! - pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); - - for(std::map::iterator iter = poses.begin(); iter!=poses.end(); ++iter) - { - std::map::Ptr >::iterator jter = scans_.find(iter->first); - if(jter != scans_.end()) - { - pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); - *assembledCloud+=*transformed; - } - } - - if(assembledCloud->size()) - { - if(scanVoxelSize_ > 0) - { - assembledCloud = util3d::voxelize(assembledCloud, scanVoxelSize_); - } - - sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); - pcl::toROSMsg(*assembledCloud, *cloudMsg); - cloudMsg->header.stamp = ros::Time::now(); - cloudMsg->header.frame_id = msg->header.frame_id; - assembledMapScans_.publish(cloudMsg); - } - } - - if(occupancyMapPub_.getNumSubscribers()) - { - // create the map - float xMin=0.0f, yMin=0.0f; - cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps( - poses, - occupancyLocalMaps_, - gridCellSize_, xMin, yMin, - occupancyMapSize_); - - 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 = msg->header.frame_id; - map.header.stamp = ros::Time::now(); - - occupancyMapPub_.publish(map); - } - } - ROS_INFO("Processing data %fs", timer.ticks()); + ROS_INFO("map_assembler: Publishing data = %fs", timer.ticks()); } bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { ROS_INFO("map_assembler: reset!"); - occupancyLocalMaps_.clear(); - rgbClouds_.clear(); - scans_.clear(); + mapsManager_.clear(); return true; } private: - int cloudDecimation_; - double cloudMaxDepth_; - double cloudVoxelSize_; - double scanVoxelSize_; - - double nodeFilteringAngle_; - double nodeFilteringRadius_; - - double noiseFilterRadius_; - double noiseFilterMinNeighbors_; - - bool computeOccupancyGrid_; - double gridCellSize_; - double groundMaxAngle_; - int clusterMinSize_; - double maxHeight_; - double occupancyMapSize_; - - std::map > occupancyLocalMaps_; // + MapsManager mapsManager_; + std::map nodes_; ros::Subscriber mapDataTopic_; - ros::Publisher assembledMapClouds_; - ros::Publisher assembledMapScans_; - ros::Publisher occupancyMapPub_; - ros::ServiceServer resetService_; - - std::map::Ptr > rgbClouds_; - std::map::Ptr > scans_; }; diff --git a/src/MapOptimizerNode.cpp b/src/MapOptimizerNode.cpp index 96a809a5..ae18bfbd 100644 --- a/src/MapOptimizerNode.cpp +++ b/src/MapOptimizerNode.cpp @@ -31,9 +31,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap_ros/MsgConversion.h" #include #include +#include #include #include #include +#include #include #include #include @@ -48,8 +50,6 @@ public: MapOptimizer() : mapFrameId_("map"), odomFrameId_("odom"), - iterations_(100), - ignoreVariance_(false), globalOptimization_(true), optimizeFromLastNode_(false), mapToOdom_(rtabmap::Transform::getIdentity()), @@ -58,14 +58,35 @@ public: ros::NodeHandle nh; ros::NodeHandle pnh("~"); + double epsilon = 0.0; + bool robust = true; + bool slam2d =false; + int strategy = 0; // 0=TORO, 1=g2o, 2=GTSAM + int iterations = 100; + bool ignoreVariance = false; + pnh.param("map_frame_id", mapFrameId_, mapFrameId_); pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); - pnh.param("iterations", iterations_, iterations_); - pnh.param("ignore_variance", ignoreVariance_, ignoreVariance_); + pnh.param("iterations", iterations, iterations); + pnh.param("ignore_variance", ignoreVariance, ignoreVariance); pnh.param("global_optimization", globalOptimization_, globalOptimization_); pnh.param("optimize_from_last_node", optimizeFromLastNode_, optimizeFromLastNode_); + pnh.param("epsilon", epsilon, epsilon); + pnh.param("robust", robust, robust); + pnh.param("slam_2d", slam2d, slam2d); + pnh.param("strategy", strategy, strategy); - UASSERT(iterations_ > 0); + + UASSERT(iterations > 0); + + ParametersMap parameters; + parameters.insert(ParametersPair(Parameters::kOptimizerStrategy(), uNumber2Str(strategy))); + parameters.insert(ParametersPair(Parameters::kOptimizerEpsilon(), uNumber2Str(epsilon))); + parameters.insert(ParametersPair(Parameters::kOptimizerIterations(), uNumber2Str(iterations))); + parameters.insert(ParametersPair(Parameters::kOptimizerRobust(), uBool2Str(robust))); + parameters.insert(ParametersPair(Parameters::kOptimizerSlam2D(), uBool2Str(slam2d))); + parameters.insert(ParametersPair(Parameters::kOptimizerVarianceIgnored(), uBool2Str(ignoreVariance))); + optimizer_ = Optimizer::create(parameters); double tfDelay = 0.05; // 20 Hz bool publishTf = true; @@ -137,8 +158,10 @@ public: if(iter->second.to() == link.to()) { edgeAlreadyAdded = true; - if(iter->second.transform() != link.transform()) + if(iter->second.transform().getDistanceSquared(link.transform()) > 0.0001) { + ROS_WARN("%d ->%d (%s vs %s)",iter->second.from(), iter->second.to(), iter->second.transform().prettyPrint().c_str(), + link.transform().prettyPrint().c_str()); dataChanged = true; } } @@ -149,16 +172,17 @@ public: } } - std::map newPoses; + std::map newNodeInfos; // add new odometry poses for(unsigned int i=0; inodes.size(); ++i) { int id = msg->nodes[i].id; Transform pose = rtabmap_ros::transformFromPoseMsg(msg->nodes[i].pose); - newPoses.insert(std::make_pair(id, pose)); + Signature s = rtabmap_ros::nodeInfoFromROS(msg->nodes[i]); + newNodeInfos.insert(std::make_pair(id, s)); - std::pair::iterator, bool> p = cachedPoses_.insert(std::make_pair(id, pose)); - if(!p.second && pose != cachedPoses_.at(id)) + std::pair::iterator, bool> p = cachedNodeInfos_.insert(std::make_pair(id, s)); + if(!p.second && pose.getDistanceSquared(cachedNodeInfos_.at(id).getPose()) > 0.0001) { dataChanged = true; } @@ -167,27 +191,27 @@ public: if(dataChanged) { ROS_WARN("Graph data has changed! Reset cache..."); - cachedPoses_ = newPoses; cachedConstraints_ = newConstraints; + cachedNodeInfos_ = newNodeInfos; } //match poses in the graph - std::map poses; std::multimap constraints; + std::map nodeInfos; if(globalOptimization_) { - poses = cachedPoses_; constraints = cachedConstraints_; + nodeInfos = cachedNodeInfos_; } else { constraints = newConstraints; for(unsigned int i=0; igraph.posesId.size(); ++i) { - std::map::iterator iter = cachedPoses_.find(msg->graph.posesId[i]); - if(iter != cachedPoses_.end()) + std::map::iterator iter = cachedNodeInfos_.find(msg->graph.posesId[i]); + if(iter != cachedNodeInfos_.end()) { - poses.insert(*iter); + nodeInfos.insert(*iter); } else { @@ -196,25 +220,31 @@ public: } } } + + std::map poses; + for(std::map::iterator iter=nodeInfos.begin(); iter!=nodeInfos.end(); ++iter) + { + poses.insert(std::make_pair(iter->first, iter->second.getPose())); + } + // Optimize only if there is a subscriber if(mapDataPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers()) { UTimer timer; std::map optimizedPoses; Transform mapCorrection = Transform::getIdentity(); + std::map posesOut; std::multimap linksOut; if(poses.size() > 1 && constraints.size() > 0) { - graph::TOROOptimizer optimizer(iterations_, false, ignoreVariance_); int fromId = optimizeFromLastNode_?poses.rbegin()->first:poses.begin()->first; - std::map posesOut; - optimizer.getConnectedGraph( + optimizer_->getConnectedGraph( fromId, poses, constraints, posesOut, linksOut); - optimizedPoses = optimizer.optimize(fromId, posesOut, linksOut); + optimizedPoses = optimizer_->optimize(fromId, posesOut, linksOut); mapToOdomMutex_.lock(); mapCorrection = optimizedPoses.at(posesOut.rbegin()->first) * posesOut.rbegin()->second.inverse(); mapToOdom_ = mapCorrection; @@ -249,6 +279,33 @@ public: outputDataMsg.header = msg->header; outputDataMsg.graph = outputGraphMsg; outputDataMsg.nodes = msg->nodes; + if(posesOut.size() > msg->nodes.size()) + { + std::set addedNodes; + for(unsigned int i=0; inodes.size(); ++i) + { + addedNodes.insert(msg->nodes[i].id); + } + std::list toAdd; + for(std::map::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter) + { + if(addedNodes.find(iter->first) == addedNodes.end()) + { + toAdd.push_back(iter->first); + } + } + if(toAdd.size()) + { + int oi = outputDataMsg.nodes.size(); + outputDataMsg.nodes.resize(outputDataMsg.nodes.size()+toAdd.size()); + for(std::list::iterator iter=toAdd.begin(); iter!=toAdd.end(); ++iter) + { + UASSERT(cachedNodeInfos_.find(*iter) != cachedNodeInfos_.end()); + rtabmap_ros::nodeDataToROS(cachedNodeInfos_.at(*iter), outputDataMsg.nodes[oi]); + ++oi; + } + } + } mapDataPub_.publish(outputDataMsg); } @@ -259,10 +316,9 @@ public: private: std::string mapFrameId_; std::string odomFrameId_; - int iterations_; - bool ignoreVariance_; bool globalOptimization_; bool optimizeFromLastNode_; + Optimizer * optimizer_; rtabmap::Transform mapToOdom_; boost::mutex mapToOdomMutex_; @@ -272,8 +328,8 @@ private: ros::Publisher mapDataPub_; ros::Publisher mapGraphPub_; - std::map cachedPoses_; std::multimap cachedConstraints_; + std::map cachedNodeInfos_; tf2_ros::TransformBroadcaster tfBroadcaster_; boost::thread* transformThread_; diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp index c29785ab..d00b339d 100644 --- a/src/MapsManager.cpp +++ b/src/MapsManager.cpp @@ -29,23 +29,34 @@ using namespace rtabmap; -MapsManager::MapsManager() : +MapsManager::MapsManager(bool usePublicNamespace) : cloudDecimation_(4), cloudMaxDepth_(4.0), // meters + cloudMinDepth_(0.0), // meters cloudVoxelSize_(0.05), // meters cloudFloorCullingHeight_(0.0), + cloudCeilingCullingHeight_(0.0), cloudOutputVoxelized_(false), cloudFrustumCulling_(false), + cloudNoiseFilteringRadius_(0.0), + cloudNoiseFilteringMinNeighbors_(5), + scanDecimation_(0), + scanVoxelSize_(0.0), + scanOutputVoxelized_(false), projMaxGroundAngle_(45.0), // degrees projMinClusterSize_(20), - projMaxHeight_(2.0), // meters + projMaxObstaclesHeight_(2.0), // meters (<=0 disabled) + projMaxGroundHeight_(0.0), // meters (<=0 disabled, only works if proj_detect_flat_obstacles is true) + projDetectFlatObstacles_(false), gridCellSize_(0.05), // meters gridSize_(0), // meters gridEroded_(false), gridUnknownSpaceFilled_(false), + gridMaxUnknownSpaceFilledRange_(6.0), mapFilterRadius_(0.5), mapFilterAngle_(30.0), // degrees - mapCacheCleanup_(true) + mapCacheCleanup_(true), + negativePosesIgnored(false) { ros::NodeHandle nh; @@ -54,31 +65,82 @@ MapsManager::MapsManager() : // cloud map stuff pnh.param("cloud_decimation", cloudDecimation_, cloudDecimation_); pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_); + pnh.param("cloud_min_depth", cloudMinDepth_, cloudMinDepth_); pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_); pnh.param("cloud_floor_culling_height", cloudFloorCullingHeight_, cloudFloorCullingHeight_); + pnh.param("cloud_ceiling_culling_height", cloudCeilingCullingHeight_, cloudCeilingCullingHeight_); + if(cloudFloorCullingHeight_ > 0 && + cloudCeilingCullingHeight_ > 0 && + cloudCeilingCullingHeight_ < cloudFloorCullingHeight_) + { + ROS_WARN("\"cloud_floor_culling_height\" should be lower than \"cloud_ceiling_culling_height\", setting \"cloud_ceiling_culling_height\" to 0 (disabled)."); + cloudCeilingCullingHeight_ = 0; + } pnh.param("cloud_output_voxelized", cloudOutputVoxelized_, cloudOutputVoxelized_); pnh.param("cloud_frustum_culling", cloudFrustumCulling_, cloudFrustumCulling_); + pnh.param("cloud_noise_filtering_radius", cloudNoiseFilteringRadius_, cloudNoiseFilteringRadius_); + pnh.param("cloud_noise_filtering_min_neighbors", cloudNoiseFilteringMinNeighbors_, cloudNoiseFilteringMinNeighbors_); + + // scan map stuff + pnh.param("scan_decimation", scanDecimation_, scanDecimation_); + pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_); + pnh.param("scan_output_voxelized", scanOutputVoxelized_, scanOutputVoxelized_); //projection map stuff pnh.param("proj_max_ground_angle", projMaxGroundAngle_, projMaxGroundAngle_); pnh.param("proj_min_cluster_size", projMinClusterSize_, projMinClusterSize_); - pnh.param("proj_max_height", projMaxHeight_, projMaxHeight_); + if(pnh.hasParam("proj_max_height") && !pnh.hasParam("proj_max_obstacles_height")) + { + ROS_WARN("Parameter \"proj_max_height\" has been renamed " + "to \"proj_max_obstacles_height\"! Your value is still copied to " + "corresponding parameter."); + pnh.param("proj_max_height", projMaxObstaclesHeight_, projMaxObstaclesHeight_); + } + else + { + pnh.param("proj_max_obstacles_height", projMaxObstaclesHeight_, projMaxObstaclesHeight_); + } + pnh.param("proj_max_ground_height", projMaxGroundHeight_, projMaxGroundHeight_); + pnh.param("proj_detect_flat_obstacles", projDetectFlatObstacles_, projDetectFlatObstacles_); // common grid map stuff pnh.param("grid_cell_size", gridCellSize_, gridCellSize_); // m + if(gridCellSize_ <= 0) + { + ROS_FATAL("\"grid_cell_size\" (%f) should be greater than 0!", gridCellSize_); + } pnh.param("grid_size", gridSize_, gridSize_); // m pnh.param("grid_eroded", gridEroded_, gridEroded_); pnh.param("grid_unknown_space_filled", gridUnknownSpaceFilled_, gridUnknownSpaceFilled_); + pnh.param("grid_unknown_space_filled_max_range", gridMaxUnknownSpaceFilledRange_, gridMaxUnknownSpaceFilledRange_); // common map stuff 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); + + // If true, the last message published on + // the map topics will be saved and sent to new subscribers when they + // connect + bool latch = true; + pnh.param("latch", latch, latch); // mapping topics - cloudMapPub_ = nh.advertise("cloud_map", 1); - projMapPub_ = nh.advertise("proj_map", 1); - gridMapPub_ = nh.advertise("grid_map", 1); + if(usePublicNamespace) + { + cloudMapPub_ = nh.advertise("cloud_map", 1, latch); + projMapPub_ = nh.advertise("proj_map", 1, latch); + gridMapPub_ = nh.advertise("grid_map", 1, latch); + scanMapPub_ = nh.advertise("scan_map", 1, latch); + } + else + { + cloudMapPub_ = pnh.advertise("cloud_map", 1, latch); + projMapPub_ = pnh.advertise("proj_map", 1, latch); + gridMapPub_ = pnh.advertise("grid_map", 1, latch); + scanMapPub_ = pnh.advertise("scan_map", 1, latch); + } } MapsManager::~MapsManager() { @@ -97,7 +159,8 @@ bool MapsManager::hasSubscribers() const { return cloudMapPub_.getNumSubscribers() != 0 || projMapPub_.getNumSubscribers() != 0 || - gridMapPub_.getNumSubscribers() != 0; + gridMapPub_.getNumSubscribers() != 0 || + scanMapPub_.getNumSubscribers() != 0; } std::map MapsManager::getFilteredPoses(const std::map & poses) @@ -117,32 +180,35 @@ std::map MapsManager::updateMapCaches( bool updateCloud, bool updateProj, bool updateGrid, + bool updateScan, const std::map & signatures) { - if(!updateCloud && !updateProj && !updateGrid) + if(!updateCloud && !updateProj && !updateGrid && !updateScan) { // all false, udpate only those where we have subscribers updateCloud = cloudMapPub_.getNumSubscribers() != 0; updateProj = projMapPub_.getNumSubscribers() != 0; updateGrid = gridMapPub_.getNumSubscribers() != 0; + updateScan = scanMapPub_.getNumSubscribers() != 0; } UDEBUG("Updating map caches..."); if(!memory && signatures.size() == 0) { - ROS_FATAL("Memory should not be null!?"); + ROS_ERROR("Memory and signatures should not be both null!?"); return std::map(); } std::map filteredPoses; // update cache - if(updateCloud || updateProj || updateGrid) + if(updateCloud || updateProj || updateGrid || updateScan) { // filter nodes if(mapFilterRadius_ > 0.0) { + UDEBUG("Filter nodes..."); double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0; filteredPoses = rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle); for(std::map::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) @@ -163,6 +229,21 @@ std::map MapsManager::updateMapCaches( filteredPoses = poses; } + if(negativePosesIgnored) + { + for(std::map::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end();) + { + if(iter->first <= 0) + { + filteredPoses.erase(iter++); + } + else + { + ++iter; + } + } + } + for(std::map::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter) { if(!iter->second.isNull()) @@ -170,12 +251,15 @@ std::map MapsManager::updateMapCaches( rtabmap::SensorData data; bool rgbDepthRequired = updateCloud && (iter->first < 0 || !uContains(clouds_, iter->first)); bool depthRequired = updateProj && (iter->first < 0 || !uContains(projMaps_, iter->first)); - bool scanRequired = updateGrid && (iter->first < 0 || !uContains(gridMaps_, iter->first)); + bool gridRequired = updateGrid && (iter->first < 0 || !uContains(gridMaps_, iter->first)); + bool scanRequired = updateScan && (iter->first < 0 || !uContains(scans_, iter->first)); if(rgbDepthRequired || depthRequired || - scanRequired) + scanRequired || + gridRequired) { + UDEBUG("Data required for %d", iter->first); std::map::const_iterator findIter = signatures.find(iter->first); if(findIter != signatures.end()) { @@ -191,26 +275,40 @@ std::map MapsManager::updateMapCaches( { if(!(data.imageCompressed().empty() && data.imageRaw().empty()) && !(data.depthOrRightCompressed().empty() && data.depthOrRightRaw().empty()) && - (data.cameraModels().size() || data.stereoCameraModel().isValid())) + (data.cameraModels().size() || data.stereoCameraModel().isValidForProjection())) { // Which data should we decompress? cv::Mat image, depth, scan; data.uncompressData( - (rgbDepthRequired||data.stereoCameraModel().isValid()) ? &image:0, + (rgbDepthRequired||data.stereoCameraModel().isValidForProjection()) ? &image:0, (rgbDepthRequired||depthRequired) ? &depth:0, - scanRequired?&scan:0); + scanRequired||gridRequired?&scan:0); pcl::PointCloud::Ptr cloudRGB; pcl::PointCloud::Ptr cloudXYZ; if(rgbDepthRequired) { + UDEBUG("rgbDepthRequired"); if(!image.empty() && !depth.empty()) { + pcl::IndicesPtr validIndices(new std::vector); cloudRGB = util3d::cloudRGBFromSensorData( data, cloudDecimation_, cloudMaxDepth_, - cloudVoxelSize_); + cloudMinDepth_, + validIndices.get()); + if(cloudVoxelSize_) + { + cloudRGB = util3d::voxelize(cloudRGB, validIndices, cloudVoxelSize_); + } + if(cloudRGB->size() && cloudNoiseFilteringRadius_ > 0.0 && cloudNoiseFilteringMinNeighbors_ > 0) + { + pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloudRGB, cloudNoiseFilteringRadius_, cloudNoiseFilteringMinNeighbors_); + pcl::PointCloud::Ptr tmp(new pcl::PointCloud); + pcl::copyPointCloud(*cloudRGB, *indices, *tmp); + cloudRGB = tmp; + } } else { @@ -219,13 +317,25 @@ std::map MapsManager::updateMapCaches( } else if(depthRequired) { + UDEBUG("depthRequired"); if( !depth.empty()) { + pcl::IndicesPtr validIndices(new std::vector); cloudXYZ = util3d::cloudFromSensorData( data, cloudDecimation_, cloudMaxDepth_, - gridCellSize_); // use gridCellSize since this cloud is only for the projection map + cloudMinDepth_, + validIndices.get()); // use gridCellSize since this cloud is only for the projection map + UASSERT(gridCellSize_ > 0); + cloudXYZ = util3d::voxelize(cloudXYZ, validIndices, gridCellSize_); + if(cloudXYZ->size() && cloudNoiseFilteringRadius_ > 0.0 && cloudNoiseFilteringMinNeighbors_ > 0) + { + pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloudXYZ, cloudNoiseFilteringRadius_, cloudNoiseFilteringMinNeighbors_); + pcl::PointCloud::Ptr tmp(new pcl::PointCloud); + pcl::copyPointCloud(*cloudXYZ, *indices, *tmp); + cloudXYZ = tmp; + } } else { @@ -240,7 +350,7 @@ std::map MapsManager::updateMapCaches( // Make sure that image size is set in camera models. // The camera models are used when cloud_frustum_culling=true. std::vector models; - if(data.stereoCameraModel().isValid()) + if(data.stereoCameraModel().isValidForProjection()) { //insert only the left camera model rtabmap::CameraModel model = data.stereoCameraModel().left(); @@ -265,40 +375,90 @@ std::map MapsManager::updateMapCaches( if(depthRequired) { + UDEBUG("Creating proj map for %d...", iter->first); cv::Mat ground, obstacles; if(cloudRGB.get()) { pcl::PointCloud::Ptr cloudClipped = cloudRGB; - if(cloudClipped->size() && projMaxHeight_ > 0) + if(cloudClipped->size() && projMaxObstaclesHeight_ > 0) { - cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits::min(), projMaxHeight_); + cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits::min(), projMaxObstaclesHeight_); + } + if(cloudClipped->size() && gridCellSize_ > cloudVoxelSize_) + { + cloudClipped = util3d::voxelize(cloudClipped, gridCellSize_); } if(cloudClipped->size()) { - cloudClipped = util3d::voxelize(cloudClipped, gridCellSize_); - util3d::occupancy2DFromCloud3D(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_); + // 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)); + + util3d::occupancy2DFromCloud3D(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_, projDetectFlatObstacles_, projMaxGroundHeight_); } } else if(cloudXYZ.get()) { pcl::PointCloud::Ptr cloudClipped = cloudXYZ; - if(cloudClipped->size() && projMaxHeight_ > 0) + if(cloudClipped->size() && projMaxObstaclesHeight_ > 0) { - cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits::min(), projMaxHeight_); + cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits::min(), projMaxObstaclesHeight_); } if(cloudClipped->size()) { - util3d::occupancy2DFromCloud3D(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_); + // 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)); + + UDEBUG("util3d::occupancy2DFromCloud3D()"); + util3d::occupancy2DFromCloud3D(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_, projDetectFlatObstacles_, projMaxGroundHeight_); } } uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); } - if(scanRequired) + if(scanRequired || gridRequired) { - cv::Mat ground, obstacles; - util3d::occupancy2DFromLaserScan(scan, ground, obstacles, gridCellSize_, data.id() < 0 || gridUnknownSpaceFilled_, data.laserScanMaxRange()); - uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); + if(scan.cols && (gridRequired || scanVoxelSize_ > 0.0)) + { + if(scanDecimation_ > 1) + { + scan = util3d::downsample(scan, scanDecimation_); + } + + if(scanRequired || scanVoxelSize_ > 0.0) + { + pcl::PointCloud::Ptr scanCloud = util3d::laserScanToPointCloud(scan); + if(scanVoxelSize_ > 0.0) + { + scanCloud = util3d::voxelize(scanCloud, scanVoxelSize_); + if(gridRequired && scan.type() == CV_32FC2) + { + scan = util3d::laserScan2dFromPointCloud(*scanCloud); + } + } + + if(scanRequired) + { + uInsert(scans_, std::make_pair(iter->first, scanCloud)); + } + } + } + + if(gridRequired && scan.type() == CV_32FC2) + { + cv::Mat ground, obstacles; + util3d::occupancy2DFromLaserScan( + scan, + ground, + obstacles, + gridCellSize_, + data.id() < 0 || gridUnknownSpaceFilled_, + data.laserScanMaxRange()>gridMaxUnknownSpaceFilledRange_?gridMaxUnknownSpaceFilledRange_:data.laserScanMaxRange()); + uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); + } } } else @@ -307,7 +467,7 @@ std::map MapsManager::updateMapCaches( iter->first, !(data.imageCompressed().empty() && data.imageRaw().empty())?1:0, !(data.depthOrRightCompressed().empty() && data.depthOrRightRaw().empty())?1:0, - (data.cameraModels().size() || data.stereoCameraModel().isValid())?1:0); + (data.cameraModels().size() || data.stereoCameraModel().isValidForProjection())?1:0); } } } @@ -318,6 +478,7 @@ std::map MapsManager::updateMapCaches( } // cleanup not used nodes + UDEBUG("Cleanup not used nodes"); for(std::map::Ptr >::iterator iter=clouds_.begin(); iter!=clouds_.end();) { @@ -416,32 +577,37 @@ void MapsManager::publishMaps( { for(unsigned int i=0; isecond.size(); ++i) { - if(kter->second[i].isValid()) + if(kter->second[i].isValidForProjection()) { int size = assembledCloud->size(); assembledCloud = util3d::frustumFiltering( assembledCloud, - iter->second, + iter->second, // FIXME: should include camera local transform kter->second[i].horizontalFOV(), kter->second[i].verticalFOV(), 0.0f, cloudMaxDepth_>0.0?cloudMaxDepth_:999999., true); //ROS_INFO("Frustum culling %d ->%d", size, (int)assembledCloud->size()); - pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); - *assembledCloud+=*transformed; + if(jter->second->size()) + { + pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); + *assembledCloud+=*transformed; + } } } } } } - if(cloudFloorCullingHeight_ > 0.0) + if(assembledCloud->size() && (cloudFloorCullingHeight_ > 0.0 || cloudCeilingCullingHeight_ > 0.0)) { - assembledCloud = util3d::passThrough(assembledCloud, "z", cloudFloorCullingHeight_, 99999.0f); + assembledCloud = util3d::passThrough(assembledCloud, "z", + cloudFloorCullingHeight_>0.0?cloudFloorCullingHeight_:-999.0, + cloudCeilingCullingHeight_>0.0 && (cloudFloorCullingHeight_<=0.0 || cloudCeilingCullingHeight_>cloudFloorCullingHeight_)?cloudCeilingCullingHeight_:999.0); } - if(cloudVoxelSize_ > 0 && cloudOutputVoxelized_) + if(assembledCloud->size() && cloudVoxelSize_ > 0 && cloudOutputVoxelized_) { assembledCloud = util3d::voxelize(assembledCloud, cloudVoxelSize_); } @@ -456,7 +622,7 @@ void MapsManager::publishMaps( } else if(poses.size()) { - ROS_WARN("Cloud map is empty! (clouds=%d)", (int)clouds_.size()); + ROS_WARN("Cloud map is empty! (poses=%d clouds=%d)", (int)poses.size(), (int)clouds_.size()); } } else if(mapCacheCleanup_) @@ -465,6 +631,53 @@ void MapsManager::publishMaps( cameraModels_.clear(); } + if(scanMapPub_.getNumSubscribers()) + { + // generate the assembled scan cloud! + UTimer time; + pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); + int count = 0; + std::list > negativePoses; + for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) + { + if(iter->first > 0) + { + std::map::Ptr >::iterator jter = scans_.find(iter->first); + if(jter != scans_.end() && jter->second->size()) + { + pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); + *assembledCloud+=*transformed; + ++count; + } + } + // negative poses are not used + } + + if(assembledCloud->size()) + { + if(assembledCloud->size() && scanVoxelSize_ > 0 && scanOutputVoxelized_) + { + assembledCloud = util3d::voxelize(assembledCloud, scanVoxelSize_); + } + + ROS_INFO("Assembled %d scans (%fs)", count, time.ticks()); + + sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); + pcl::toROSMsg(*assembledCloud, *cloudMsg); + cloudMsg->header.stamp = stamp; + cloudMsg->header.frame_id = mapFrameId; + scanMapPub_.publish(cloudMsg); + } + else if(poses.size()) + { + ROS_WARN("Scan map is empty! (poses=%d, scans=%d)", (int)poses.size(), (int)scans_.size()); + } + } + else if(mapCacheCleanup_) + { + scans_.clear(); + } + if(projMapPub_.getNumSubscribers()) { // create the projection map diff --git a/src/MapsManager.h b/src/MapsManager.h index 9e24816d..5b4adfb0 100644 --- a/src/MapsManager.h +++ b/src/MapsManager.h @@ -26,7 +26,7 @@ class Memory; class MapsManager { public: - MapsManager(); + MapsManager(bool usePublicNamespace); virtual ~MapsManager(); void clear(); bool hasSubscribers() const; @@ -40,6 +40,7 @@ public: bool updateCloud, bool updateProj, bool updateGrid, + bool updateScan, const std::map & signatures = std::map()); void publishMaps( @@ -67,26 +68,39 @@ private: // mapping stuff int cloudDecimation_; double cloudMaxDepth_; + double cloudMinDepth_; double cloudVoxelSize_; double cloudFloorCullingHeight_; + double cloudCeilingCullingHeight_; bool cloudOutputVoxelized_; bool cloudFrustumCulling_; + double cloudNoiseFilteringRadius_; + int cloudNoiseFilteringMinNeighbors_; + int scanDecimation_; + double scanVoxelSize_; + bool scanOutputVoxelized_; double projMaxGroundAngle_; int projMinClusterSize_; - double projMaxHeight_; + double projMaxObstaclesHeight_; + double projMaxGroundHeight_; + bool projDetectFlatObstacles_; double gridCellSize_; double gridSize_; bool gridEroded_; bool gridUnknownSpaceFilled_; + double gridMaxUnknownSpaceFilledRange_; double mapFilterRadius_; double mapFilterAngle_; bool mapCacheCleanup_; + bool negativePosesIgnored; ros::Publisher cloudMapPub_; ros::Publisher projMapPub_; ros::Publisher gridMapPub_; + ros::Publisher scanMapPub_; std::map::Ptr > clouds_; + std::map::Ptr > scans_; std::map > cameraModels_; std::map > projMaps_; // std::map > gridMaps_; // diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp index c19a9e34..463b1e23 100644 --- a/src/MsgConversion.cpp +++ b/src/MsgConversion.cpp @@ -36,6 +36,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include +#include namespace rtabmap_ros { @@ -144,7 +146,7 @@ void infoFromROS(const rtabmap_ros::Info & info, rtabmap::Statistics & stat) // rtabmap_ros::Info stat.setRefImageId(info.refId); stat.setLoopClosureId(info.loopClosureId); - stat.setLocalLoopClosureId(info.localLoopClosureId); + stat.setProximityDetectionId(info.proximityDetectionId); stat.setLoopClosureTransform(rtabmap_ros::transformFromGeometryMsg(info.loopClosureTransform)); @@ -188,7 +190,7 @@ void infoToROS(const rtabmap::Statistics & stats, rtabmap_ros::Info & info) { info.refId = stats.refImageId(); info.loopClosureId = stats.loopClosureId(); - info.localLoopClosureId = stats.localLoopClosureId(); + info.proximityDetectionId = stats.proximityDetectionId(); rtabmap_ros::transformToGeometryMsg(stats.loopClosureTransform(), info.loopClosureTransform); @@ -293,6 +295,103 @@ void points2fToROS(const std::vector & kpts, std::vector points3fFromROS(const std::vector & msg) +{ + std::vector v(msg.size()); + for(unsigned int i=0; i & kpts, std::vector & msg) +{ + msg.resize(kpts.size()); + for(unsigned int i=0; i(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.R().total() == 9); + 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)); + + if(camInfo.D.size() > 5) + { + camInfo.distortion_model = "rational_polynomial"; + } + else + { + camInfo.distortion_model = "plumb_bob"; + } + camInfo.binning_x = 1; + camInfo.binning_y = 1; + camInfo.roi.width = model.imageWidth(); + camInfo.roi.height = model.imageHeight(); + + camInfo.width = model.imageWidth(); + camInfo.height = model.imageHeight(); +} +rtabmap::StereoCameraModel stereoCameraModelFromROS( + const sensor_msgs::CameraInfo & leftCamInfo, + const sensor_msgs::CameraInfo & rightCamInfo, + const rtabmap::Transform & localTransform) +{ + image_geometry::StereoCameraModel model; + model.fromCameraInfo(leftCamInfo, rightCamInfo); + return rtabmap::StereoCameraModel( + model.left().fx(), + model.left().fy(), + model.left().cx(), + model.left().cy(), + model.baseline(), + localTransform, + cv::Size(model.left().fullResolution().width, model.left().fullResolution().height)); +} + void mapDataFromROS( const rtabmap_ros::MapData & msg, std::map & poses, @@ -384,13 +483,14 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg) { //Features stuff... std::multimap words; - std::multimap words3D; + std::multimap words3D; pcl::PointCloud cloud; if(msg.wordPts.data.size() && - msg.wordPts.data.size() == msg.wordIds.size()) + msg.wordPts.height*msg.wordPts.width == msg.wordIds.size()) { pcl::fromROSMsg(msg.wordPts, cloud); } + for(unsigned int i=0; i cloud; cloud.resize(signature.getWords3().size()); index = 0; - for(std::multimap::const_iterator jter=signature.getWords3().begin(); + for(std::multimap::const_iterator jter=signature.getWords3().begin(); jter!=signature.getWords3().end(); ++jter) { - cloud[index++] = jter->second; + cloud[index].x = jter->second.x; + cloud[index].y = jter->second.y; + cloud[index++].z = jter->second.z; } pcl::toROSMsg(cloud, msg.wordPts); } @@ -555,6 +659,30 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & } } +rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData & msg) +{ + rtabmap::Signature s( + msg.id, + msg.mapId, + msg.weight, + msg.stamp, + msg.label, + transformFromPoseMsg(msg.pose), + transformFromPoseMsg(msg.groundTruthPose)); + return s; +} +void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg) +{ + // add data + msg.id = signature.id(); + msg.mapId = signature.mapId(); + msg.weight = signature.getWeight(); + msg.stamp = signature.getStamp(); + msg.label = signature.getLabel(); + transformToPoseMsg(signature.getPose(), msg.pose); + transformToPoseMsg(signature.getGroundTruthPose(), msg.groundTruthPose); +} + rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg) { rtabmap::OdometryInfo info; @@ -588,6 +716,12 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg) info.transform = transformFromGeometryMsg(msg.transform); info.transformFiltered = transformFromGeometryMsg(msg.transformFiltered); + UASSERT(msg.localMapKeys.size() == msg.localMapValues.size()); + for(unsigned int i=0; i #include -#include +#include +#include #include #include #include @@ -48,6 +49,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UStl.h" #include "rtabmap/utilite/UFile.h" +#define BAD_COVARIANCE 9999 + using namespace rtabmap; namespace rtabmap_ros { @@ -60,7 +63,10 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : publishTf_(true), waitForTransform_(true), waitForTransformDuration_(0.1), // 100 ms - paused_(false) + publishNullWhenLost_(true), + paused_(false), + resetCountdown_(0), + resetCurrentCount_(0) { ros::NodeHandle nh; @@ -84,6 +90,8 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : pnh.param("initial_pose", initialPoseStr, initialPoseStr); // "x y z roll pitch yaw" pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_); pnh.param("config_path", configPath, configPath); + pnh.param("publish_null_when_lost", publishNullWhenLost_, publishNullWhenLost_); + configPath = uReplaceChar(configPath, '~', UDirectory::homeDir()); if(configPath.size() && configPath.at(0) != '/') { @@ -124,14 +132,14 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : //parameters - parameters_ = this->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()); rtabmap::ParametersMap allParameters; - Rtabmap::readParameters(configPath.c_str(), allParameters); + Parameters::readINI(configPath.c_str(), allParameters); // only update odometry parameters for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter) { @@ -174,78 +182,58 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : iter->second = uNumber2Str(vInt); } - if(iter->first.compare(Parameters::kOdomMinInliers()) == 0 && atoi(iter->second.c_str()) < 8) + if(iter->first.compare(Parameters::kVisMinInliers()) == 0 && atoi(iter->second.c_str()) < 8) { ROS_WARN("Parameter min_inliers must be >= 8, setting to 8..."); iter->second = uNumber2Str(8); } } + rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argc, argv); + for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) + { + rtabmap::ParametersMap::iterator jter = parameters_.find(iter->first); + if(jter!=parameters_.end()) + { + ROS_INFO("Update odometry parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str()); + jter->second = iter->second; + } + } + // Backward compatibility - std::list oldParameterNames; - oldParameterNames.push_back("Odom/Type"); - oldParameterNames.push_back("Odom/MaxWords"); - oldParameterNames.push_back("Odom/WordsRatio"); - oldParameterNames.push_back("Odom/LocalHistory"); - oldParameterNames.push_back("Odom/NearestNeighbor"); - oldParameterNames.push_back("Odom/NNDR"); - oldParameterNames.push_back("GFTT/MaxCorners"); - for(std::list::iterator iter=oldParameterNames.begin(); iter!=oldParameterNames.end(); ++iter) + for(std::map >::const_iterator iter=Parameters::getRemovedParameters().begin(); + iter!=Parameters::getRemovedParameters().end(); + ++iter) { std::string vStr; - if(pnh.getParam(*iter, vStr)) + if(pnh.getParam(iter->first, vStr)) { - if(iter->compare("Odom/Type") == 0) + if(iter->second.first) { - ROS_WARN("Parameter name changed: Odom/Type -> %s. Please update your launch file accordingly.", - Parameters::kOdomFeatureType().c_str()); - parameters_.at(Parameters::kOdomFeatureType())= vStr; + // 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.", + iter->first.c_str(), iter->second.second.c_str(), vStr.c_str()); } - else if(iter->compare("Odom/MaxWords") == 0) + else { - ROS_WARN("Parameter name changed: Odom/MaxWords -> %s. Please update your launch file accordingly.", - Parameters::kOdomMaxFeatures().c_str()); - parameters_.at(Parameters::kOdomMaxFeatures())= vStr; - } - else if(iter->compare("Odom/LocalHistory") == 0) - { - ROS_WARN("Parameter name changed: Odom/LocalHistory -> %s. Please update your launch file accordingly.", - Parameters::kOdomBowLocalHistorySize().c_str()); - parameters_.at(Parameters::kOdomBowLocalHistorySize())= vStr; - } - else if(iter->compare("Odom/NearestNeighbor") == 0) - { - ROS_WARN("Parameter name changed: Odom/NearestNeighbor -> %s. Please update your launch file accordingly.", - Parameters::kOdomBowNNType().c_str()); - parameters_.at(Parameters::kOdomBowNNType())= vStr; - } - else if(iter->compare("Odom/NNDR") == 0) - { - ROS_WARN("Parameter name changed: Odom/NNDR -> %s. Please update your launch file accordingly.", - Parameters::kOdomBowNNDR().c_str()); - parameters_.at(Parameters::kOdomBowNNDR())= vStr; - } - else if(iter->compare("GFTT/MaxCorners") == 0) - { - ROS_WARN("Parameter GFTT/MaxCorners doesn't exist anymore, use %s. Please update your launch file accordingly.", - Parameters::kOdomMaxFeatures().c_str()); - parameters_.at(Parameters::kOdomMaxFeatures())= vStr; + if(iter->second.second.empty()) + { + ROS_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\"", + iter->first.c_str(), iter->second.second.c_str()); + } } } } - int odomStrategy = 0; // BOW - Parameters::parse(parameters_, Parameters::kOdomStrategy(), odomStrategy); - if(odomStrategy == 1) - { - ROS_INFO("Using OdometryOpticalFlow"); - odometry_ = new rtabmap::OdometryOpticalFlow(parameters_); - } - else - { - ROS_INFO("Using OdometryBOW"); - odometry_ = new rtabmap::OdometryBOW(parameters_); - } + Parameters::parse(parameters_, Parameters::kOdomResetCountdown(), resetCountdown_); + parameters_.at(Parameters::kOdomResetCountdown()) = "0"; // use modified reset countdown here + odometry_ = Odometry::create(parameters_); if(!initialPose.isIdentity()) { odometry_->reset(initialPose); @@ -255,6 +243,11 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : resetToPoseSrv_ = nh.advertiseService("reset_odom_to_pose", &OdometryROS::resetToPose, this); pauseSrv_ = nh.advertiseService("pause_odom", &OdometryROS::pause, this); resumeSrv_ = nh.advertiseService("resume_odom", &OdometryROS::resume, this); + + setLogDebugSrv_ = pnh.advertiseService("log_debug", &OdometryROS::setLogDebug, this); + 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() @@ -268,48 +261,13 @@ OdometryROS::~OdometryROS() delete odometry_; } -rtabmap::ParametersMap OdometryROS::getDefaultOdometryParameters(bool stereo) -{ - rtabmap::ParametersMap odomParameters; - rtabmap::ParametersMap defaultParameters = rtabmap::Parameters::getDefaultParameters(); - for(rtabmap::ParametersMap::iterator iter=defaultParameters.begin(); iter!=defaultParameters.end(); ++iter) - { - std::string group = uSplit(iter->first, '/').front(); - if(uStrContains(group, "Odom") || - group.compare("Stereo") || - group.compare("SURF") == 0 || - group.compare("SIFT") == 0 || - group.compare("ORB") == 0 || - group.compare("FAST") == 0 || - group.compare("FREAK") == 0 || - group.compare("BRIEF") == 0 || - group.compare("GFTT") == 0 || - group.compare("BRISK") == 0) - { - if(stereo) - { - if(iter->first.compare(Parameters::kOdomMaxDepth()) == 0) - { - iter->second = "0"; // infinity - } - else if(iter->first.compare(Parameters::kOdomEstimationType()) == 0) - { - iter->second = "1"; // 3D->2D (PNP) - } - } - odomParameters.insert(*iter); - } - } - return odomParameters; -} - void OdometryROS::processArguments(int argc, char * argv[], bool stereo) { for(int i=1;ifirst + " = \"" + iter->second + "\""; @@ -325,6 +283,14 @@ void OdometryROS::processArguments(int argc, char * argv[], bool stereo) "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); + } } } @@ -378,9 +344,12 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) // process data ros::WallTime time = ros::WallTime::now(); rtabmap::OdometryInfo info; - rtabmap::Transform pose = odometry_->process(data, &info); + SensorData dataCpy = data; + rtabmap::Transform pose = odometry_->process(dataCpy, &info); if(!pose.isNull()) { + resetCurrentCount_ = resetCountdown_; + //********************* // Update odometry //********************* @@ -410,24 +379,47 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) odom.pose.pose.orientation = poseMsg.transform.rotation; //set covariance - odom.pose.covariance.at(0) = info.variance; // xx - odom.pose.covariance.at(7) = info.variance; // yy - odom.pose.covariance.at(14) = info.variance; // zz - odom.pose.covariance.at(21) = info.variance; // rr - odom.pose.covariance.at(28) = info.variance; // pp - odom.pose.covariance.at(35) = info.variance; // yawyaw + // libviso2 uses approximately vel variance * 2 + odom.pose.covariance.at(0) = info.variance*2; // xx + odom.pose.covariance.at(7) = info.variance*2; // yy + odom.pose.covariance.at(14) = info.variance*2; // zz + odom.pose.covariance.at(21) = info.variance*2; // rr + odom.pose.covariance.at(28) = info.variance*2; // pp + odom.pose.covariance.at(35) = info.variance*2; // yawyaw + + //set velocity + bool setTwist = !odometry_->previousVelocityTransform().isNull(); + if(setTwist) + { + float x,y,z,roll,pitch,yaw; + odometry_->previousVelocityTransform().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + odom.twist.twist.linear.x = x; + odom.twist.twist.linear.y = y; + odom.twist.twist.linear.z = z; + odom.twist.twist.angular.x = roll; + odom.twist.twist.angular.y = pitch; + odom.twist.twist.angular.z = yaw; + } + + odom.twist.covariance.at(0) = setTwist?info.variance:BAD_COVARIANCE; // xx + odom.twist.covariance.at(7) = setTwist?info.variance:BAD_COVARIANCE; // yy + odom.twist.covariance.at(14) = setTwist?info.variance:BAD_COVARIANCE; // zz + odom.twist.covariance.at(21) = setTwist?info.variance:BAD_COVARIANCE; // rr + odom.twist.covariance.at(28) = setTwist?info.variance:BAD_COVARIANCE; // pp + odom.twist.covariance.at(35) = setTwist?info.variance:BAD_COVARIANCE; // yawyaw //publish the message odomPub_.publish(odom); } - if(odomLocalMap_.getNumSubscribers() && dynamic_cast(odometry_)) + // local map / reference frame + if(odomLocalMap_.getNumSubscribers() && dynamic_cast(odometry_)) { - const std::map & map = ((OdometryBOW*)odometry_)->getLocalMap(); pcl::PointCloud cloud; - for(std::map::const_iterator iter=map.begin(); iter!=map.end(); ++iter) + const std::multimap & map = ((OdometryF2M*)odometry_)->getMap().getWords3(); + for(std::multimap::const_iterator iter=map.begin(); iter!=map.end(); ++iter) { - cloud.push_back(iter->second); + cloud.push_back(pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z)); } sensor_msgs::PointCloud2 cloudMsg; pcl::toROSMsg(cloud, cloudMsg); @@ -438,18 +430,17 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) if(odomLastFrame_.getNumSubscribers()) { - if(dynamic_cast(odometry_)) + if(dynamic_cast(odometry_)) { - const rtabmap::Signature * s = ((OdometryBOW*)odometry_)->getMemory()->getLastWorkingSignature(); - if(s) + const std::multimap & words3 = ((OdometryF2M*)odometry_)->getLastFrame().getWords3(); + if(words3.size()) { - const std::multimap & words3 = s->getWords3(); pcl::PointCloud cloud; - for(std::multimap::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter) + for(std::multimap::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter) { // transform to odom frame - pcl::PointXYZ pt = util3d::transformPoint(iter->second, pose); - cloud.push_back(pt); + cv::Point3f pt = util3d::transformPoint(iter->second, pose); + cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z)); } sensor_msgs::PointCloud2 cloudMsg; @@ -461,14 +452,19 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) } else { - //Optical flow - const pcl::PointCloud::Ptr & cloud = ((OdometryOpticalFlow*)odometry_)->getLastCorners3D(); - if(cloud->size()) + //Frame to Frame + const Signature & refFrame = ((OdometryF2F*)odometry_)->getRefFrame(); + if(refFrame.getWords3().size()) { - pcl::PointCloud::Ptr cloudTransformed; - cloudTransformed = util3d::transformPointCloud(cloud, pose); + pcl::PointCloud cloud; + for(std::multimap::const_iterator iter=refFrame.getWords3().begin(); iter!=refFrame.getWords3().end(); ++iter) + { + // transform to odom frame + cv::Point3f pt = util3d::transformPoint(iter->second, pose); + cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z)); + } sensor_msgs::PointCloud2 cloudMsg; - pcl::toROSMsg(*cloudTransformed, cloudMsg); + pcl::toROSMsg(cloud, cloudMsg); cloudMsg.header.stamp = stamp; // use corresponding time stamp to image cloudMsg.header.frame_id = odomFrameId_; odomLastFrame_.publish(cloudMsg); @@ -476,7 +472,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) } } } - else + else if(publishNullWhenLost_) { //ROS_WARN("Odometry lost!"); @@ -485,11 +481,47 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) odom.header.stamp = stamp; // use corresponding time stamp to image odom.header.frame_id = odomFrameId_; odom.child_frame_id = frameId_; + odom.pose.covariance.at(0) = BAD_COVARIANCE; // xx + odom.pose.covariance.at(7) = BAD_COVARIANCE; // yy + odom.pose.covariance.at(14) = BAD_COVARIANCE; // zz + odom.pose.covariance.at(21) = BAD_COVARIANCE; // rr + odom.pose.covariance.at(28) = BAD_COVARIANCE; // pp + odom.pose.covariance.at(35) = BAD_COVARIANCE; // yawyaw + odom.twist.covariance.at(0) = BAD_COVARIANCE; // xx + odom.twist.covariance.at(7) = BAD_COVARIANCE; // yy + odom.twist.covariance.at(14) = BAD_COVARIANCE; // zz + odom.twist.covariance.at(21) = BAD_COVARIANCE; // rr + odom.twist.covariance.at(28) = BAD_COVARIANCE; // pp + odom.twist.covariance.at(35) = BAD_COVARIANCE; // yawyaw //publish the message odomPub_.publish(odom); } + if(pose.isNull() && resetCurrentCount_ > 0) + { + ROS_WARN("Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_); + + --resetCurrentCount_; + if(resetCurrentCount_ == 0) + { + // Check TF to see if sensor fusion is used (e.g., the output of robot_localization) + Transform tfPose = this->getTransform(odomFrameId_, frameId_, stamp); + if(tfPose.isNull()) + { + ROS_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)!", + odomFrameId_.c_str(), frameId_.c_str()); + odometry_->reset(tfPose); + } + + } + } + if(odomInfoPub_.getNumSubscribers()) { rtabmap_ros::OdomInfo infoMsg; @@ -502,9 +534,9 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) 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()); } -bool OdometryROS::isOdometryBOW() const +bool OdometryROS::isOdometryF2M() const { - return dynamic_cast(odometry_) != 0; + return dynamic_cast(odometry_) != 0; } bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) @@ -550,4 +582,30 @@ bool OdometryROS::resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&) return true; } +bool OdometryROS::setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +{ + ROS_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"); + 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"); + 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"); + ULogger::setLevel(ULogger::kError); + return true; +} + + } diff --git a/src/OdometryROS.h b/src/OdometryROS.h index 5d37cc9b..ec705d41 100644 --- a/src/OdometryROS.h +++ b/src/OdometryROS.h @@ -49,7 +49,6 @@ namespace rtabmap_ros { class OdometryROS { public: - static rtabmap::ParametersMap getDefaultOdometryParameters(bool stereo = false); static void processArguments(int argc, char * argv[], bool stereo = false); public: @@ -61,13 +60,17 @@ public: bool resetToPose(rtabmap_ros::ResetPose::Request&, rtabmap_ros::ResetPose::Response&); bool pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&); + bool setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&); + bool setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&); + bool setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&); + bool setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&); const std::string & frameId() const {return frameId_;} const std::string & odomFrameId() const {return odomFrameId_;} const rtabmap::ParametersMap & parameters() const {return parameters_;} const tf::TransformListener & tfListener() const {return tfListener_;} bool isPaused() const {return paused_;} - bool isOdometryBOW() const; + bool isOdometryF2M() const; rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const; private: @@ -80,6 +83,7 @@ private: bool publishTf_; bool waitForTransform_; double waitForTransformDuration_; + bool publishNullWhenLost_; rtabmap::ParametersMap parameters_; ros::Publisher odomPub_; @@ -90,10 +94,16 @@ private: ros::ServiceServer resetToPoseSrv_; ros::ServiceServer pauseSrv_; ros::ServiceServer resumeSrv_; + ros::ServiceServer setLogDebugSrv_; + ros::ServiceServer setLogInfoSrv_; + ros::ServiceServer setLogWarnSrv_; + ros::ServiceServer setLogErrorSrv_; tf2_ros::TransformBroadcaster tfBroadcaster_; tf::TransformListener tfListener_; bool paused_; + int resetCountdown_; + int resetCurrentCount_; }; } diff --git a/src/PreferencesDialogROS.cpp b/src/PreferencesDialogROS.cpp index baf8807b..4215bc76 100644 --- a/src/PreferencesDialogROS.cpp +++ b/src/PreferencesDialogROS.cpp @@ -27,15 +27,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "PreferencesDialogROS.h" #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include -#include +#include #include +#include using namespace rtabmap; @@ -76,19 +77,42 @@ QString PreferencesDialogROS::getParamMessage() bool PreferencesDialogROS::readCoreSettings(const QString & filePath) { - if(filePath.isEmpty() || filePath.compare(getTmpIniFilePath()) == 0) + QString path = getIniFilePath(); + if(!filePath.isEmpty()) { - ros::NodeHandle nh; - ROS_INFO("%s", this->getParamMessage().toStdString().c_str()); - bool validParameters = true; - int readCount = 0; - rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters(); - for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i) + path = filePath; + } + + ros::NodeHandle nh; + ROS_INFO("%s", this->getParamMessage().toStdString().c_str()); + bool validParameters = true; + int readCount = 0; + rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters(); + for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i) + { + if(i->first.compare(rtabmap::Parameters::kRtabmapWorkingDirectory()) == 0) + { + // use working directory of the GUI, not the one on rosparam server + QSettings settings(path, QSettings::IniFormat); + settings.beginGroup("Core"); + QString value = settings.value(rtabmap::Parameters::kRtabmapWorkingDirectory().c_str(), "").toString(); + if(!value.isEmpty() && QDir(value).exists()) + { + this->setParameter(rtabmap::Parameters::kRtabmapWorkingDirectory(), value.toStdString()); + } + else + { + // use default one + this->setParameter(rtabmap::Parameters::kRtabmapWorkingDirectory(), (QDir::homePath()+"/.ros").toStdString()); + } + settings.endGroup(); + } + else { std::string value; - if(nh.getParam((*i).first,value)) + if(nh.getParam(i->first,value)) { - PreferencesDialog::setParameter((*i).first, value); + PreferencesDialog::setParameter(i->first, value); ++readCount; } else @@ -96,51 +120,50 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath) validParameters = false; } } + } - ROS_INFO("Parameters read = %d", readCount); + ROS_INFO("Parameters read = %d", readCount); - if(validParameters) - { - ROS_INFO("Parameters successfully read."); - } - else - { - if(this->isVisible()) - { - QString warning = tr("Failed to get some RTAB-Map parameters from ROS server, the rtabmap node may be not started or some parameters won't work..."); - ROS_WARN("%s", warning.toStdString().c_str()); - QMessageBox::warning(this, tr("Can't read parameters from ROS server."), warning); - } - return false; - } - return true; + if(validParameters) + { + ROS_INFO("Parameters successfully read."); } else { - return PreferencesDialog::readCoreSettings(filePath); + if(this->isVisible()) + { + QString warning = tr("Failed to get some RTAB-Map parameters from ROS server, the rtabmap node may be not started or some parameters won't work..."); + ROS_WARN("%s", warning.toStdString().c_str()); + QMessageBox::warning(this, tr("Can't read parameters from ROS server."), warning); + } + return false; } - + return true; } -void PreferencesDialogROS::writeSettings(const QString & filePath) +void PreferencesDialogROS::writeCoreSettings(const QString & filePath) const { - writeGuiSettings(filePath); - - // This will tell the MainWindow that the - //parameters are updated. The MainWindow will send an Event that - // will be handled by the GuiWrapper where we will write - // parameters in ROS and the rtabmap_node will be notified. - if(_parameters.size()) + QString path = getIniFilePath(); + if(!filePath.isEmpty()) { - emit settingsChanged(_parameters); + path = filePath; } - if(_obsoletePanels) + if(QFile::exists(path)) { - emit settingsChanged(_obsoletePanels); - } + rtabmap::ParametersMap parameters = this->getAllParameters(); - _parameters = rtabmap::ParametersMap(); - _obsoletePanels = kPanelDummy; + std::string workingDir = uValue(parameters, Parameters::kRtabmapWorkingDirectory(), std::string("")); + + if(!workingDir.empty()) + { + //Just update GUI working directory + QSettings settings(path, QSettings::IniFormat); + settings.beginGroup("Core"); + settings.remove(""); + settings.setValue(Parameters::kRtabmapWorkingDirectory().c_str(), workingDir.c_str()); + settings.endGroup(); + } + } } diff --git a/src/PreferencesDialogROS.h b/src/PreferencesDialogROS.h index 2453f290..4e73f8a8 100644 --- a/src/PreferencesDialogROS.h +++ b/src/PreferencesDialogROS.h @@ -40,15 +40,15 @@ public: virtual ~PreferencesDialogROS(); virtual QString getIniFilePath() const; + virtual QString getTmpIniFilePath() const; protected: virtual QString getParamMessage(); virtual void readCameraSettings(const QString & filePath); virtual bool readCoreSettings(const QString & filePath); - virtual void writeSettings(const QString & filePath); - - virtual QString getTmpIniFilePath() const; + virtual void writeCameraSettings(const QString & filePath) const {} + virtual void writeCoreSettings(const QString & filePath) const; private: QString configFile_; diff --git a/src/RGBDOdometryNode.cpp b/src/RGBDOdometryNode.cpp index 9874ea89..c1c07958 100644 --- a/src/RGBDOdometryNode.cpp +++ b/src/RGBDOdometryNode.cpp @@ -189,16 +189,9 @@ public: if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0) { - image_geometry::PinholeCameraModel model; - model.fromCameraInfo(*cameraInfo); - rtabmap::CameraModel rtabmapModel( - model.fx(), - model.fy(), - model.cx(), - model.cy(), - localTransform); - cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8"); - cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depth); + 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, @@ -321,14 +314,7 @@ public: return; } - image_geometry::PinholeCameraModel model; - model.fromCameraInfo(*infoMsgs[i]); - cameraModels.push_back(rtabmap::CameraModel( - model.fx(), - model.fy(), - model.cx(), - model.cy(), - localTransform)); + cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*infoMsgs[i], localTransform)); } rtabmap::SensorData data( diff --git a/src/StereoOdometryNode.cpp b/src/StereoOdometryNode.cpp index e138a634..28f0dcac 100644 --- a/src/StereoOdometryNode.cpp +++ b/src/StereoOdometryNode.cpp @@ -145,24 +145,15 @@ public: int quality = -1; if(imageRectLeft->data.size() && imageRectRight->data.size()) { - image_geometry::StereoCameraModel model; - model.fromCameraInfo(*cameraInfoLeft, *cameraInfoRight); - if(model.baseline() <= 0) + 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.", model.baseline()); + "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline()); return; } - rtabmap::StereoCameraModel stereoModel( - model.left().fx(), - model.left().fy(), - model.left().cx(), - model.left().cy(), - model.baseline(), - localTransform); - - if(model.baseline() > 10.0) + if(stereoModel.baseline() > 10.0) { static bool shown = false; if(!shown) @@ -170,13 +161,13 @@ public: 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.", - model.baseline()); + stereoModel.baseline()); shown = true; } } - cv_bridge::CvImageConstPtr ptrImageLeft = cv_bridge::toCvShare(imageRectLeft, "mono8"); - cv_bridge::CvImageConstPtr ptrImageRight = cv_bridge::toCvShare(imageRectRight, "mono8"); + cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft, "mono8"); + cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight, "mono8"); UTimer stepTimer; // diff --git a/src/nodelets/data_throttle.cpp b/src/nodelets/data_throttle.cpp index 923aac31..df13547a 100644 --- a/src/nodelets/data_throttle.cpp +++ b/src/nodelets/data_throttle.cpp @@ -91,16 +91,16 @@ private: bool approxSync = true; if(private_nh.getParam("max_rate", rate_)) { - ROS_WARN("\"max_rate\" is now known as \"rate\"."); + NODELET_WARN("\"max_rate\" is now known as \"rate\"."); } private_nh.param("rate", rate_, rate_); private_nh.param("queue_size", queueSize, queueSize); private_nh.param("approx_sync", approxSync, approxSync); private_nh.param("decimation", decimation_, decimation_); ROS_ASSERT(decimation_ >= 1); - ROS_INFO("Rate=%f Hz", rate_); - ROS_INFO("Decimation=%d", decimation_); - ROS_INFO("Approximate time sync = %s", approxSync?"true":"false"); + NODELET_INFO("Rate=%f Hz", rate_); + NODELET_INFO("Decimation=%d", decimation_); + NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false"); if(approxSync) { diff --git a/src/nodelets/disparity_to_depth.cpp b/src/nodelets/disparity_to_depth.cpp index b90fbc37..1e97e9fd 100644 --- a/src/nodelets/disparity_to_depth.cpp +++ b/src/nodelets/disparity_to_depth.cpp @@ -63,7 +63,7 @@ private: { if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0) { - ROS_ERROR("Input type must be disparity=32FC1"); + NODELET_ERROR("Input type must be disparity=32FC1"); return; } diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 62dac3ab..ad21b45a 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -67,10 +67,13 @@ class ObstaclesDetection : public nodelet::Nodelet public: ObstaclesDetection() : frameId_("base_link"), - normalEstimationRadius_(0.05), + normalKSearch_(20), groundNormalAngle_(M_PI_4), + clusterRadius_(0.05), minClusterSize_(20), maxObstaclesHeight_(0.0), // if<=0.0 -> disabled + maxGroundHeight_(0.0), // if<=0.0 -> disabled, used only if detect_flat_obstacles is true + segmentFlatObstacles_(false), waitForTransform_(false), optimizeForCloseObjects_(false) {} @@ -87,10 +90,24 @@ private: int queueSize = 10; pnh.param("queue_size", queueSize, queueSize); pnh.param("frame_id", frameId_, frameId_); - pnh.param("normal_estimation_radius", normalEstimationRadius_, normalEstimationRadius_); + pnh.param("normal_k", normalKSearch_, normalKSearch_); pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_); + if(pnh.hasParam("normal_estimation_radius") && !pnh.hasParam("cluster_radius")) + { + NODELET_WARN("Parameter \"normal_estimation_radius\" has been renamed " + "to \"cluster_radius\"! Your value is still copied to " + "corresponding parameter. Instead of normal radius, nearest neighbors count " + "\"normal_k\" is used instead (default 20)."); + pnh.param("normal_estimation_radius", clusterRadius_, clusterRadius_); + } + else + { + pnh.param("cluster_radius", clusterRadius_, clusterRadius_); + } pnh.param("min_cluster_size", minClusterSize_, minClusterSize_); pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_); + pnh.param("max_ground_height", maxGroundHeight_, maxGroundHeight_); + pnh.param("detect_flat_obstacles", segmentFlatObstacles_, segmentFlatObstacles_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); pnh.param("optimize_for_close_objects", optimizeForCloseObjects_, optimizeForCloseObjects_); @@ -104,7 +121,7 @@ private: void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) { - ros::Time time = ros::Time::now(); + ros::WallTime time = ros::WallTime::now(); if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0) { @@ -119,7 +136,7 @@ private: { if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1))) { - ROS_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str()); + NODELET_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str()); return; } } @@ -129,7 +146,7 @@ private: } catch(tf::TransformException & ex) { - ROS_ERROR("%s",ex.what()); + NODELET_ERROR("%s",ex.what()); return; } @@ -158,9 +175,12 @@ private: originalCloud, ground, obstacles, - normalEstimationRadius_, + normalKSearch_, groundNormalAngle_, - minClusterSize_); + clusterRadius_, + minClusterSize_, + segmentFlatObstacles_, + maxGroundHeight_); if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) { @@ -190,9 +210,12 @@ private: originalCloud_near, ground, obstacles, - normalEstimationRadius_, + normalKSearch_, groundNormalAngle_, - minClusterSize_); + clusterRadius_, + minClusterSize_, + segmentFlatObstacles_, + maxGroundHeight_); if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) { @@ -211,9 +234,12 @@ private: originalCloud_far, ground, obstacles, - 3.*normalEstimationRadius_, + normalKSearch_, 2.*groundNormalAngle_, - minClusterSize_); + 3.*clusterRadius_, + minClusterSize_, + segmentFlatObstacles_, + maxGroundHeight_); if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) { @@ -255,15 +281,18 @@ private: obstaclesPub_.publish(rosCloud); } - //ROS_INFO("Obstacles segmentation time = %f s", (ros::Time::now() - time).toSec()); + //NODELET_INFO("Obstacles segmentation time = %f s", (ros::WallTime::now() - time).toSec()); } private: std::string frameId_; - double normalEstimationRadius_; + int normalKSearch_; double groundNormalAngle_; + double clusterRadius_; int minClusterSize_; double maxObstaclesHeight_; + double maxGroundHeight_; + bool segmentFlatObstacles_; bool waitForTransform_; bool optimizeForCloseObjects_; diff --git a/src/nodelets/point_cloud_xyz.cpp b/src/nodelets/point_cloud_xyz.cpp index 6bed0638..eabe22ff 100644 --- a/src/nodelets/point_cloud_xyz.cpp +++ b/src/nodelets/point_cloud_xyz.cpp @@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include + #include #include #include @@ -62,6 +64,7 @@ class PointCloudXYZ : public nodelet::Nodelet public: PointCloudXYZ() : maxDepth_(0.0), + minDepth_(0.0), voxelSize_(0.0), decimation_(1), noiseFilterRadius_(0.0), @@ -98,6 +101,7 @@ private: pnh.param("approx_sync", approxSync, approxSync); pnh.param("queue_size", queueSize, queueSize); pnh.param("max_depth", maxDepth_, maxDepth_); + pnh.param("min_depth", minDepth_, minDepth_); pnh.param("voxel_size", voxelSize_, voxelSize_); pnh.param("decimation", decimation_, decimation_); pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_); @@ -106,7 +110,7 @@ private: pnh.param("cut_right", cut_right_, cut_right_); pnh.param("special_filter_close_object", create_close_obstacle_if_depth_is_missing_, create_close_obstacle_if_depth_is_missing_); - ROS_INFO("Approximate time sync = %s", approxSync?"true":"false"); + NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false"); if(approxSync) { @@ -147,7 +151,7 @@ private: depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 && depth->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0) { - ROS_ERROR("Input type depth=32FC1,16UC1,MONO16"); + NODELET_ERROR("Input type depth=32FC1,16UC1,MONO16"); return; } @@ -222,7 +226,7 @@ private: if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 && disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0) { - ROS_ERROR("Input type must be disparity=32FC1 or 16SC1"); + NODELET_ERROR("Input type must be disparity=32FC1 or 16SC1"); return; } @@ -238,18 +242,12 @@ private: if(cloudPub_.getNumSubscribers()) { - image_geometry::PinholeCameraModel model; - model.fromCameraInfo(*cameraInfo); - float cx = model.cx(); - float cy = model.cy(); - pcl::PointCloud::Ptr pclCloud; + rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo); + rtabmap::StereoCameraModel stereoModel(disparityMsg->f, disparityMsg->f, leftModel.cx(), leftModel.cy(), disparityMsg->T); pclCloud = rtabmap::util3d::cloudFromDisparity( disparity, - cx, - cy, - disparityMsg->f, - disparityMsg->T, + stereoModel, decimation_); processAndPublish(pclCloud, disparityMsg->header); @@ -258,9 +256,9 @@ private: void processAndPublish(pcl::PointCloud::Ptr & pclCloud, const std_msgs::Header & header) { - if(pclCloud->size() && maxDepth_ > 0) + if(pclCloud->size() && (minDepth_ != 0.0 || maxDepth_ > minDepth_)) { - pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", 0, maxDepth_); + pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", minDepth_, maxDepth_>minDepth_?maxDepth_:std::numeric_limits::max()); } if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0) @@ -288,6 +286,7 @@ private: private: double maxDepth_; + double minDepth_; double voxelSize_; int decimation_; double noiseFilterRadius_; diff --git a/src/nodelets/point_cloud_xyzrgb.cpp b/src/nodelets/point_cloud_xyzrgb.cpp index f58c9cfb..d762106f 100644 --- a/src/nodelets/point_cloud_xyzrgb.cpp +++ b/src/nodelets/point_cloud_xyzrgb.cpp @@ -33,6 +33,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include + #include #include #include @@ -62,6 +64,7 @@ class PointCloudXYZRGB : public nodelet::Nodelet public: PointCloudXYZRGB() : maxDepth_(0.0), + minDepth_(0.0), voxelSize_(0.0), decimation_(1), noiseFilterRadius_(0.0), @@ -95,12 +98,13 @@ private: pnh.param("approx_sync", approxSync, approxSync); pnh.param("queue_size", queueSize, queueSize); pnh.param("max_depth", maxDepth_, maxDepth_); + pnh.param("min_depth", minDepth_, minDepth_); pnh.param("voxel_size", voxelSize_, voxelSize_); pnh.param("decimation", decimation_, decimation_); pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_); pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_); - ROS_INFO("Approximate time sync = %s", approxSync?"true":"false"); + NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false"); cloudPub_ = nh.advertise("cloud", 1); @@ -165,13 +169,27 @@ private: imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 || imageDepth->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"); + NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16"); return; } if(cloudPub_.getNumSubscribers()) { - cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image); + cv_bridge::CvImageConstPtr imagePtr; + if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0) + { + imagePtr = cv_bridge::toCvShare(image); + } + else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + imagePtr = cv_bridge::toCvShare(image, "mono8"); + } + else + { + imagePtr = cv_bridge::toCvShare(image, "bgr8"); + } + cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth); image_geometry::PinholeCameraModel model; @@ -210,7 +228,7 @@ private: imageRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || imageRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)) { - ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (enc=%s)", imageLeft->encoding.c_str()); + NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (enc=%s)", imageLeft->encoding.c_str()); return; } @@ -228,22 +246,11 @@ private: } ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8"); - image_geometry::StereoCameraModel model; - model.fromCameraInfo(*camInfoLeft, *camInfoRight); - - float fx = model.left().fx(); - float cx = model.left().cx(); - float cy = model.left().cy(); - float baseline = model.baseline(); - pcl::PointCloud::Ptr pclCloud; pclCloud = rtabmap::util3d::cloudFromStereoImages( ptrLeftImage->image, ptrRightImage->image, - cx, - cy, - fx, - baseline, + rtabmap_ros::stereoCameraModelFromROS(*camInfoLeft, *camInfoRight), decimation_); processAndPublish(pclCloud, imageLeft->header); @@ -252,9 +259,9 @@ private: void processAndPublish(pcl::PointCloud::Ptr & pclCloud, const std_msgs::Header & header) { - if(pclCloud->size() && maxDepth_ > 0) + if(pclCloud->size() && (minDepth_ != 0.0 || maxDepth_ > minDepth_)) { - pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", 0, maxDepth_); + pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", minDepth_, maxDepth_>minDepth_?maxDepth_:std::numeric_limits::max()); } if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0) @@ -282,6 +289,7 @@ private: private: double maxDepth_; + double minDepth_; double voxelSize_; int decimation_; double noiseFilterRadius_; diff --git a/src/nodelets/stereo_throttle.cpp b/src/nodelets/stereo_throttle.cpp index 629e8841..482f70ec 100644 --- a/src/nodelets/stereo_throttle.cpp +++ b/src/nodelets/stereo_throttle.cpp @@ -93,9 +93,9 @@ private: pnh.param("queue_size", queueSize, queueSize); pnh.param("decimation", decimation_, decimation_); ROS_ASSERT(decimation_ >= 1); - ROS_INFO("Rate=%f Hz", rate_); - ROS_INFO("Decimation=%d", decimation_); - ROS_INFO("Approximate time sync = %s", approxSync?"true":"false"); + NODELET_INFO("Rate=%f Hz", rate_); + NODELET_INFO("Decimation=%d", decimation_); + NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false"); if(approxSync) { diff --git a/src/rviz/InfoDisplay.cpp b/src/rviz/InfoDisplay.cpp index bbe6e052..d4da9683 100644 --- a/src/rviz/InfoDisplay.cpp +++ b/src/rviz/InfoDisplay.cpp @@ -51,8 +51,8 @@ void InfoDisplay::onInitialize() this->setStatusStd(rviz::StatusProperty::Ok, "Info", ""); this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", ""); this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", ""); - this->setStatusStd(rviz::StatusProperty::Ok, "Global", "0"); - this->setStatusStd(rviz::StatusProperty::Ok, "Local", "0"); + this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", "0"); + this->setStatusStd(rviz::StatusProperty::Ok, "Proximity detections", "0"); spinner_.start(); } @@ -63,12 +63,12 @@ void InfoDisplay::processMessage( const rtabmap_ros::InfoConstPtr& msg ) boost::mutex::scoped_lock lock(info_mutex_); if(msg->loopClosureId) { - info_ = QString("%1->%2 [Global]").arg(msg->refId).arg(msg->loopClosureId); + info_ = QString("%1->%2").arg(msg->refId).arg(msg->loopClosureId); globalCount_ += 1; } - else if(msg->localLoopClosureId) + else if(msg->proximityDetectionId) { - info_ = QString("%1->%2 [Local]").arg(msg->refId).arg(msg->localLoopClosureId); + info_ = QString("%1->%2 [Proximity]").arg(msg->refId).arg(msg->proximityDetectionId); localCount_ += 1; } else @@ -103,8 +103,8 @@ void InfoDisplay::update( float wall_dt, float ros_dt ) this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", tr("%1;%2;%3").arg(x).arg(y).arg(z).toStdString()); this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", tr("%1;%2;%3").arg(roll).arg(pitch).arg(yaw).toStdString()); } - this->setStatusStd(rviz::StatusProperty::Ok, "Global", tr("%1").arg(globalCount_).toStdString()); - this->setStatusStd(rviz::StatusProperty::Ok, "Local", tr("%1").arg(localCount_).toStdString()); + this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", tr("%1").arg(globalCount_).toStdString()); + this->setStatusStd(rviz::StatusProperty::Ok, "Proximity detections", tr("%1").arg(localCount_).toStdString()); for(std::map::const_iterator iter=statistics_.begin(); iter!=statistics_.end(); ++iter) { diff --git a/src/rviz/MapCloudDisplay.cpp b/src/rviz/MapCloudDisplay.cpp index 0bf52c0f..60a0ee16 100644 --- a/src/rviz/MapCloudDisplay.cpp +++ b/src/rviz/MapCloudDisplay.cpp @@ -25,9 +25,9 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include -#include -#include +#include +#include +#include #include #include @@ -143,6 +143,12 @@ MapCloudDisplay::MapCloudDisplay() cloud_max_depth_->setMin( 0.0f ); cloud_max_depth_->setMax( 999.0f ); + cloud_min_depth_ = new rviz::FloatProperty( "Cloud min depth (m)", 0.0f, + "Minimum depth of the generated clouds.", + this, SLOT( updateCloudParameters() ), this ); + cloud_min_depth_->setMin( 0.0f ); + cloud_min_depth_->setMax( 999.0f ); + cloud_voxel_size_ = new rviz::FloatProperty( "Cloud voxel size (m)", 0.01f, "Voxel size of the generated clouds.", this, SLOT( updateCloudParameters() ), this ); @@ -156,6 +162,13 @@ MapCloudDisplay::MapCloudDisplay() cloud_filter_floor_height_->setMin( 0.0f ); cloud_filter_floor_height_->setMax( 999.0f ); + cloud_filter_ceiling_height_ = new rviz::FloatProperty( "Filter ceiling (m)", 0.0f, + "Filter the ceiling at the specified height set here " + "(only appropriate for 2D mapping).", + this, SLOT( updateCloudParameters() ), this ); + cloud_filter_ceiling_height_->setMin( 0.0f ); + cloud_filter_ceiling_height_->setMax( 999.0f ); + node_filtering_radius_ = new rviz::FloatProperty( "Node filtering radius (m)", 0.2f, "(Disabled=0) Only keep one node in the specified radius.", this, SLOT( updateCloudParameters() ), this ); @@ -249,17 +262,24 @@ void MapCloudDisplay::processMessage( const rtabmap_ros::MapDataConstPtr& msg ) void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map) { + std::map poses; + for(unsigned int i=0; i::Ptr cloud; + pcl::IndicesPtr validIndices(new std::vector); cloud = rtabmap::util3d::cloudRGBFromSensorData( s.sensorData(), cloud_decimation_->getInt(), cloud_max_depth_->getFloat(), - cloud_voxel_size_->getFloat()); + cloud_min_depth_->getFloat(), + validIndices.get()); + + if(cloud_voxel_size_->getFloat()) + { + cloud = rtabmap::util3d::voxelize(cloud, validIndices, cloud_voxel_size_->getFloat()); + } if(cloud->size()) { - if(cloud_filter_floor_height_->getFloat() > 0.0f) + if(cloud_filter_floor_height_->getFloat() > 0.0f || cloud_filter_ceiling_height_->getFloat() > 0.0f) { - cloud = rtabmap::util3d::passThrough(cloud, "z", cloud_filter_floor_height_->getFloat(), 999.0f); + cloud = rtabmap::util3d::passThrough(cloud, "z", + cloud_filter_floor_height_->getFloat()>0.0f?cloud_filter_floor_height_->getFloat():-999.0f, + cloud_filter_ceiling_height_->getFloat()>0.0f && (cloud_filter_floor_height_->getFloat()<=0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f); } sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); @@ -302,12 +331,6 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map) } // Update graph - std::map poses; - for(unsigned int i=0; igetFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f) { poses = rtabmap::graph::radiusPosesFiltering(poses, diff --git a/src/rviz/MapCloudDisplay.h b/src/rviz/MapCloudDisplay.h index db289a23..00e507e2 100644 --- a/src/rviz/MapCloudDisplay.h +++ b/src/rviz/MapCloudDisplay.h @@ -28,6 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef MAP_CLOUD_DISPLAY_H #define MAP_CLOUD_DISPLAY_H +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 + #include #include #include @@ -42,6 +44,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#endif + namespace rviz { class IntProperty; class BoolProperty; @@ -106,8 +110,10 @@ public: rviz::EnumProperty* style_property_; rviz::IntProperty* cloud_decimation_; rviz::FloatProperty* cloud_max_depth_; + rviz::FloatProperty* cloud_min_depth_; rviz::FloatProperty* cloud_voxel_size_; rviz::FloatProperty* cloud_filter_floor_height_; + rviz::FloatProperty* cloud_filter_ceiling_height_; rviz::FloatProperty* node_filtering_radius_; rviz::FloatProperty* node_filtering_angle_; rviz::BoolProperty* download_map_; diff --git a/src/rviz/MapGraphDisplay.cpp b/src/rviz/MapGraphDisplay.cpp index 4b472e3d..84aa2d5e 100644 --- a/src/rviz/MapGraphDisplay.cpp +++ b/src/rviz/MapGraphDisplay.cpp @@ -52,7 +52,9 @@ namespace rtabmap_ros MapGraphDisplay::MapGraphDisplay() { color_neighbor_property_ = new rviz::ColorProperty( "Neighbor", Qt::blue, - "Color to draw neighbor links.", this ); + "Color to draw neighbor links.", this ); + color_neighbor_merged_property_ = new rviz::ColorProperty( "Merged neighbor", QColor(255,170,0), + "Color to draw merged neighbor links.", this ); color_global_property_ = new rviz::ColorProperty( "Global loop closure", Qt::red, "Color to draw global loop closure links.", this ); color_local_property_ = new rviz::ColorProperty( "Local loop closure", Qt::yellow, @@ -139,6 +141,10 @@ void MapGraphDisplay::processMessage( const rtabmap_ros::MapGraph::ConstPtr& msg { color = color_neighbor_property_->getOgreColor(); } + else if(iter->second.type() == rtabmap::Link::kNeighborMerged) + { + color = color_neighbor_merged_property_->getOgreColor(); + } else if(iter->second.type() == rtabmap::Link::kVirtualClosure) { color = color_virtual_property_->getOgreColor(); diff --git a/src/rviz/MapGraphDisplay.h b/src/rviz/MapGraphDisplay.h index da9a3e29..d499cc76 100644 --- a/src/rviz/MapGraphDisplay.h +++ b/src/rviz/MapGraphDisplay.h @@ -76,6 +76,7 @@ private: std::vector manual_objects_; ColorProperty* color_neighbor_property_; + ColorProperty* color_neighbor_merged_property_; ColorProperty* color_global_property_; ColorProperty* color_local_property_; ColorProperty* color_user_property_; diff --git a/srv/SetGoal.srv b/srv/SetGoal.srv index 9de0acb4..8127f359 100644 --- a/srv/SetGoal.srv +++ b/srv/SetGoal.srv @@ -5,4 +5,5 @@ string node_label --- #response int32[] path_ids -geometry_msgs/Pose[] path_poses \ No newline at end of file +geometry_msgs/Pose[] path_poses +float32 planning_time \ No newline at end of file