diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 00000000..28abe882 --- /dev/null +++ b/.travis.yml @@ -0,0 +1,46 @@ +sudo: true +dist: trusty +language: cpp + +compiler: + - gcc + - clang + +addons: + apt: + packages: + - cmake + - libopencv-dev + - libqt4-dev + - libsqlite3-dev + +install: + - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list' + - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - + - sudo apt-get update + - sudo apt-get -y install ros-indigo-ros-base libpcl-1.7-all libfreenect-dev ros-indigo-libg2o ros-indigo-octomap libopenni2-dev ros-indigo-costmap-2d ros-indigo-octomap-ros ros-indigo-rviz ros-indigo-cv-bridge ros-indigo-move-base-msgs ros-indigo-costmap-2d ros-indigo-image-geometry ros-indigo-message-filters ros-indigo-image-transport ros-indigo-eigen-conversions ros-indigo-stereo-msgs ros-indigo-nav-msgs ros-indigo-sensor-msgs ros-indigo-tf-conversions ros-indigo-laser-geometry ros-indigo-pcl-conversions ros-indigo-pcl-ros ros-indigo-dynamic-reconfigure ros-indigo-nodelet + +script: + - source /opt/ros/indigo/setup.bash + - export PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages + - cd .. + - mkdir -p catkin_ws/src + - cd catkin_ws/src + - catkin_init_workspace + - cd .. + - catkin_make + - cd .. + - mv rtabmap_ros catkin_ws/src/. + - git clone https://github.com/introlab/rtabmap.git + - cd rtabmap + - mkdir -p build && cd build + - cmake -DCMAKE_INSTALL_PREFIX=~/build/introlab/catkin_ws/devel .. + - make + - make install + - cd ../../catkin_ws + - source devel/setup.bash + - catkin_make + +notifications: + email: + - matlabbe@gmail.com diff --git a/CMakeLists.txt b/CMakeLists.txt index a003957b..fecf15b3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,7 +18,7 @@ find_package(rviz) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) -find_package(RTABMap 0.11.8 REQUIRED) +find_package(RTABMap 0.11.13 REQUIRED) find_package(OpenCV REQUIRED) @@ -72,6 +72,8 @@ add_message_files( Point2f.msg Point3f.msg Goal.msg + RGBDImage.msg + UserData.msg ) ## Generate services in the 'srv' folder @@ -151,6 +153,8 @@ SET(Libraries SET(rtabmap_ros_lib_src src/nodelets/rgbd_odometry.cpp src/nodelets/stereo_odometry.cpp + src/nodelets/rgbdicp_odometry.cpp + src/nodelets/icp_odometry.cpp src/nodelets/data_throttle.cpp src/nodelets/stereo_throttle.cpp src/nodelets/data_odom_sync.cpp @@ -158,10 +162,19 @@ SET(rtabmap_ros_lib_src src/nodelets/point_cloud_xyz.cpp src/nodelets/disparity_to_depth.cpp src/nodelets/obstacles_detection.cpp + src/nodelets/obstacles_detection_old.cpp src/nodelets/point_cloud_aggregator.cpp - src/nodelets/OdometryROS.cpp + src/nodelets/undistort_depth.cpp + src/nodelets/rgbd_sync.cpp + src/OdometryROS.cpp src/MsgConversion.cpp src/MapsManager.cpp + src/CommonDataSubscriber.cpp + src/CoreWrapper.cpp + src/impl/CommonDataSubscriberDepth.cpp + src/impl/CommonDataSubscriberStereo.cpp + src/impl/CommonDataSubscriberRGBD.cpp + src/impl/CommonDataSubscriberRGBD2.cpp ) # If costmap_2d is found, add the plugin @@ -259,8 +272,8 @@ IF(Qt5_FOUND) ENDIF(Qt5_FOUND) add_dependencies(rtabmap_ros ${${PROJECT_NAME}_EXPORTED_TARGETS}) -add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp) -target_link_libraries(rtabmap rtabmap_ros ${Libraries}) +add_executable(rtabmap src/CoreNode.cpp) +target_link_libraries(rtabmap ${Libraries}) add_executable(rgbd_odometry src/RGBDOdometryNode.cpp) target_link_libraries(rgbd_odometry ${Libraries}) @@ -268,6 +281,12 @@ target_link_libraries(rgbd_odometry ${Libraries}) add_executable(stereo_odometry src/StereoOdometryNode.cpp) target_link_libraries(stereo_odometry ${Libraries}) +add_executable(rgbdicp_odometry src/RGBDICPOdometryNode.cpp) +target_link_libraries(rgbdicp_odometry ${Libraries}) + +add_executable(icp_odometry src/ICPOdometryNode.cpp) +target_link_libraries(icp_odometry ${Libraries}) + add_executable(map_optimizer src/MapOptimizerNode.cpp) target_link_libraries(map_optimizer rtabmap_ros ${Libraries}) @@ -318,6 +337,7 @@ install(TARGETS rtabmap rtabmapviz rgbd_odometry + rgbdicp_odometry stereo_odometry map_assembler map_optimizer @@ -332,6 +352,7 @@ install(TARGETS rtabmap_ros rtabmap rgbd_odometry + rgbdicp_odometry stereo_odometry map_assembler map_optimizer @@ -392,3 +413,4 @@ ENDIF(costmap_2d_FOUND) ## Add folders to be run by python nosetests # catkin_add_nosetests(test) + diff --git a/README.md b/README.md index 29df0d5e..9dcdec52 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -rtabmap_ros +rtabmap_ros [![Build Status](https://travis-ci.org/introlab/rtabmap_ros.svg?branch=master)](https://travis-ci.org/introlab/rtabmap_ros) =========== RTAB-Map's ROS package. @@ -35,7 +35,7 @@ $ export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/kinetic/lib/x86_64-linux-gnu ``` ### Build from source -This section shows how to install RTAB-Map ros-pkg on **ROS Hydro/Indigo/Jade/Kinetic** (Catkin build). RTAB-Map works only with the PCL 1.7, which is the default version installed with ROS Hydro/Indigo/Jade/Kinetic (**Fuerte and Groovy are not supported**). +This section shows how to install RTAB-Map ros-pkg on **ROS Hydro/Indigo/Jade/Kinetic** (Catkin build). RTAB-Map works only with the PCL >=1.7, which is the default version installed with ROS Hydro/Indigo/Jade/Kinetic (**Fuerte and Groovy are not supported**). * 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 kinetic prefix for convenience, but it should work with Hydro, Indigo and Jade. The workspace path is `~/catkin_ws` and your `~/.bashrc` contains: @@ -57,7 +57,7 @@ $ sudo apt-get remove ros-kinetic-rtabmap $ sudo apt-get install libqt4-dev libpcl-1.7-all-dev libdc1394-dev ros-kinetic-openni-launch ros-kinetic-openni2-launch ros-kinetic-freenect-launch ros-kinetic-costmap-2d ros-kinetic-octomap-ros ros-kinetic-g2o ros-kinetic-rviz ros-kinetic-cv-bridge ``` - * [GTSAM](https://collab.cc.gatech.edu/borg/gtsam): Follow installation instructions from [here](https://collab.cc.gatech.edu/borg/gtsam/#quickstart). RTAB-Map needs latest version from source (`git clone https://bitbucket.org/gtborg/gtsam.git`), it will not build with 3.2.1. + * [GTSAM](https://collab.cc.gatech.edu/borg/gtsam): Follow installation instructions from [here](https://collab.cc.gatech.edu/borg/gtsam/#quickstart). RTAB-Map needs latest version from source (`git clone https://bitbucket.org/gtborg/gtsam.git`), it will **not build** with 3.2.1. * [cvsba](http://www.uco.es/investiga/grupos/ava/node/39): Follow installation instructions from [here](http://www.uco.es/investiga/grupos/ava/node/39). Their installation is not standard CMake, you need these extra steps so RTAB-Map can find it: ```bash @@ -94,6 +94,7 @@ $ git pull origin master $ cd build $ make $ make install +# Do "sudo make install" if you installed rtabmap in "/usr/local" $ roscd rtabmap_ros $ git pull origin master diff --git a/include/rtabmap_ros/CommonDataSubscriber.h b/include/rtabmap_ros/CommonDataSubscriber.h new file mode 100644 index 00000000..9b50bc0d --- /dev/null +++ b/include/rtabmap_ros/CommonDataSubscriber.h @@ -0,0 +1,264 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBER_H_ +#define INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBER_H_ + +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include + +namespace rtabmap_ros { + +class CommonDataSubscriber { +public: + CommonDataSubscriber(bool gui); + virtual ~CommonDataSubscriber(); + + bool isSubscribedToDepth() const {return subscribedToDepth_;} + bool isSubscribedToStereo() const {return subscribedToStereo_;} + bool isSubscribedToRGBD() const {return subscribedToRGBD_;} + bool isSubscribedToScan2d() const {return subscribedToScan2d_;} + bool isSubscribedToScan3d() const {return subscribedToScan3d_;} + bool isSubscribedToOdomInfo() const {return subscribedToOdomInfo_;} + bool isDataSubscribed() const {return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD();} + int getQueueSize() const {return queueSize_;} + +protected: + void setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::string & name); + virtual void commonDepthCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const std::vector & imageMsgs, + const std::vector & depthMsgs, + const std::vector & cameraInfoMsgs, + const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) = 0; + virtual void commonStereoCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + 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::PointCloud2ConstPtr& scan3dMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) = 0; + + void commonSingleDepthCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const cv_bridge::CvImageConstPtr & imageMsg, + const cv_bridge::CvImageConstPtr & depthMsg, + const sensor_msgs::CameraInfo & cameraInfoMsg, + const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg); + +private: + void warningLoop(); + void callbackCalled() {callbackCalled_ = true;} + void setupDepthCallbacks( + ros::NodeHandle & nh, + ros::NodeHandle & pnh, + bool subscribeOdom, + bool subscribeUserData, + bool subscribeScan2d, + bool subscribeScan3d, + bool subscribeOdomInfo, + int queueSize, + bool approxSync); + void setupStereoCallbacks( + ros::NodeHandle & nh, + ros::NodeHandle & pnh, + bool subscribeOdom, + bool subscribeOdomInfo, + int queueSize, + bool approxSync); + void setupRGBDCallbacks( + ros::NodeHandle & nh, + ros::NodeHandle & pnh, + bool subscribeOdom, + bool subscribeUserData, + bool subscribeScan2d, + bool subscribeScan3d, + bool subscribeOdomInfo, + int queueSize, + bool approxSync); + void setupRGBD2Callbacks( + ros::NodeHandle & nh, + ros::NodeHandle & pnh, + bool subscribeOdom, + bool subscribeUserData, + bool subscribeScan2d, + bool subscribeScan3d, + bool subscribeOdomInfo, + int queueSize, + bool approxSync); + +protected: + std::string subscribedTopicsMsg_; + int queueSize_; + +private: + bool approxSync_; + boost::thread* warningThread_; + bool callbackCalled_; + bool subscribedToDepth_; + bool subscribedToStereo_; + bool subscribedToRGBD_; + bool subscribedToScan2d_; + bool subscribedToScan3d_; + bool subscribedToOdomInfo_; + std::string name_; + + //for depth callback + image_transport::SubscriberFilter imageSub_; + image_transport::SubscriberFilter imageDepthSub_; + message_filters::Subscriber cameraInfoSub_; + + //for rgbd callback + ros::Subscriber rgbdSub_; + std::vector*> rgbdSubs_; + + //stereo callback + image_transport::SubscriberFilter imageRectLeft_; + image_transport::SubscriberFilter imageRectRight_; + message_filters::Subscriber cameraInfoLeft_; + message_filters::Subscriber cameraInfoRight_; + + message_filters::Subscriber odomSub_; + message_filters::Subscriber userDataSub_; + message_filters::Subscriber scanSub_; + message_filters::Subscriber scan3dSub_; + message_filters::Subscriber odomInfoSub_; + + // RGB + Depth + DATA_SYNCS3(depth, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo); + DATA_SYNCS4(depthScan2d, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan); + DATA_SYNCS4(depthScan3d, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2); + DATA_SYNCS4(depthInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo); + + // RGB + Depth + Odom + DATA_SYNCS4(depthOdom, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo); + DATA_SYNCS5(depthOdomScan2d, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan); + DATA_SYNCS5(depthOdomScan3d, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2); + DATA_SYNCS5(depthOdomInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo); + + // RGB + Depth + User Data + DATA_SYNCS4(depthData, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo); + DATA_SYNCS5(depthDataScan2d, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan); + DATA_SYNCS5(depthDataScan3d, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2); + DATA_SYNCS5(depthDataInfo, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo); + + // RGB + Depth + Odom + User Data + DATA_SYNCS5(depthOdomData, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo); + DATA_SYNCS6(depthOdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan); + DATA_SYNCS6(depthOdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2); + DATA_SYNCS6(depthOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo); + + // Stereo + DATA_SYNCS4(stereo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo); + DATA_SYNCS5(stereoInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo); + + // Stereo + Odom + DATA_SYNCS5(stereoOdom, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo); + DATA_SYNCS6(stereoOdomInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo); + + // 1 RGBD + void rgbdCallback(const rtabmap_ros::RGBDImageConstPtr&); + DATA_SYNCS2(rgbdScan2d, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan); + DATA_SYNCS2(rgbdScan3d, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2); + DATA_SYNCS2(rgbdInfo, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo); + + // 1 RGBD + Odom + DATA_SYNCS2(rgbdOdom, nav_msgs::Odometry, rtabmap_ros::RGBDImage); + DATA_SYNCS3(rgbdOdomScan2d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan); + DATA_SYNCS3(rgbdOdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2); + DATA_SYNCS3(rgbdOdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo); + + // 1 RGBD + User Data + DATA_SYNCS2(rgbdData, rtabmap_ros::UserData, rtabmap_ros::RGBDImage); + DATA_SYNCS3(rgbdDataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan); + DATA_SYNCS3(rgbdDataScan3d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2); + DATA_SYNCS3(rgbdDataInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo); + + // 1 RGBD + Odom + User Data + DATA_SYNCS3(rgbdOdomData, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage); + DATA_SYNCS4(rgbdOdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan); + DATA_SYNCS4(rgbdOdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2); + DATA_SYNCS4(rgbdOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo); + + // 2 RGBD + DATA_SYNCS2(rgbd2, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage); + DATA_SYNCS3(rgbd2Scan2d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan); + DATA_SYNCS3(rgbd2Scan3d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2); + DATA_SYNCS3(rgbd2Info, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo); + + // 2 RGBD + Odom + DATA_SYNCS3(rgbd2Odom, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage); + DATA_SYNCS4(rgbd2OdomScan2d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan); + DATA_SYNCS4(rgbd2OdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2); + DATA_SYNCS4(rgbd2OdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo); + + // 2 RGBD + User Data + DATA_SYNCS3(rgbd2Data, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage); + DATA_SYNCS4(rgbd2DataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan); + DATA_SYNCS4(rgbd2DataScan3d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2); + DATA_SYNCS4(rgbd2DataInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo); + + // 2 RGBD + Odom + User Data + DATA_SYNCS4(rgbd2OdomData, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage); + DATA_SYNCS5(rgbd2OdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan); + DATA_SYNCS5(rgbd2OdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2); + DATA_SYNCS5(rgbd2OdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo); + +}; + +} /* namespace rtabmap_ros */ + +#endif /* INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBER_H_ */ diff --git a/include/rtabmap_ros/CommonDataSubscriberDefines.h b/include/rtabmap_ros/CommonDataSubscriberDefines.h new file mode 100644 index 00000000..a88aa5ae --- /dev/null +++ b/include/rtabmap_ros/CommonDataSubscriberDefines.h @@ -0,0 +1,195 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_ +#define INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_ + +#include + +#define DATA_SYNC2(PREFIX, SYNC_NAME, MSG0, MSG1) \ + typedef message_filters::sync_policies::SYNC_NAME##Time PREFIX##SYNC_NAME##SyncPolicy; \ + message_filters::Synchronizer * PREFIX##SYNC_NAME##Sync_; + +#define DATA_SYNCS2(PREFIX, MSG0, MSG1) \ + DATA_SYNC2(PREFIX, Approximate, MSG0, MSG1) \ + DATA_SYNC2(PREFIX, Exact, MSG0, MSG1) \ + void PREFIX##Callback(const MSG0##ConstPtr&, const MSG1##ConstPtr&); + +#define DATA_SYNC3(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2) \ + typedef message_filters::sync_policies::SYNC_NAME##Time PREFIX##SYNC_NAME##SyncPolicy; \ + message_filters::Synchronizer * PREFIX##SYNC_NAME##Sync_; + +#define DATA_SYNCS3(PREFIX, MSG0, MSG1, MSG2) \ + DATA_SYNC3(PREFIX, Approximate, MSG0, MSG1, MSG2) \ + DATA_SYNC3(PREFIX, Exact, MSG0, MSG1, MSG2) \ + void PREFIX##Callback(const MSG0##ConstPtr&, const MSG1##ConstPtr&, const MSG2##ConstPtr&); \ + +#define DATA_SYNC4(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3) \ + typedef message_filters::sync_policies::SYNC_NAME##Time PREFIX##SYNC_NAME##SyncPolicy; \ + message_filters::Synchronizer * PREFIX##SYNC_NAME##Sync_; + +#define DATA_SYNCS4(PREFIX, MSG0, MSG1, MSG2, MSG3) \ + DATA_SYNC4(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3) \ + DATA_SYNC4(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3) \ + void PREFIX##Callback(const MSG0##ConstPtr&, const MSG1##ConstPtr&, const MSG2##ConstPtr&, const MSG3##ConstPtr&); \ + +#define DATA_SYNC5(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3, MSG4) \ + typedef message_filters::sync_policies::SYNC_NAME##Time PREFIX##SYNC_NAME##SyncPolicy; \ + message_filters::Synchronizer * PREFIX##SYNC_NAME##Sync_; + +#define DATA_SYNCS5(PREFIX, MSG0, MSG1, MSG2, MSG3, MSG4) \ + DATA_SYNC5(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3, MSG4) \ + DATA_SYNC5(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3, MSG4) \ + void PREFIX##Callback(const MSG0##ConstPtr&, const MSG1##ConstPtr&, const MSG2##ConstPtr&, const MSG3##ConstPtr&, const MSG4##ConstPtr&); \ + +#define DATA_SYNC6(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \ + typedef message_filters::sync_policies::SYNC_NAME##Time PREFIX##SYNC_NAME##SyncPolicy; \ + message_filters::Synchronizer * PREFIX##SYNC_NAME##Sync_; + +#define DATA_SYNCS6(PREFIX, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \ + DATA_SYNC6(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \ + DATA_SYNC6(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \ + void PREFIX##Callback(const MSG0##ConstPtr&, const MSG1##ConstPtr&, const MSG2##ConstPtr&, const MSG3##ConstPtr&, const MSG4##ConstPtr&, const MSG5##ConstPtr&); \ + +// Constructor +#define SYNC_INIT(PREFIX) \ + PREFIX##ApproximateSync_(0), \ + PREFIX##ExactSync_(0) + +// Destructor +#define SYNC_DEL(PREFIX) \ + if(PREFIX##ApproximateSync_) delete PREFIX##ApproximateSync_; \ + if(PREFIX##ExactSync_) delete PREFIX##ExactSync_; + +// Sync declarations +#define SYNC_DECL2(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1) \ + if(APPROX) \ + { \ + PREFIX##ApproximateSync_ = new message_filters::Synchronizer( \ + PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1); \ + PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2)); \ + } \ + else \ + { \ + PREFIX##ExactSync_ = new message_filters::Synchronizer( \ + PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1); \ + PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2)); \ + } \ + subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s", \ + name_.c_str(), \ + APPROX?"approx":"exact", \ + SUB0.getTopic().c_str(), \ + SUB1.getTopic().c_str()); + +#define SYNC_DECL3(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2) \ + if(APPROX) \ + { \ + PREFIX##ApproximateSync_ = new message_filters::Synchronizer( \ + PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2); \ + PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3)); \ + } \ + else \ + { \ + PREFIX##ExactSync_ = new message_filters::Synchronizer( \ + PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2); \ + PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3)); \ + } \ + subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s", \ + name_.c_str(), \ + APPROX?"approx":"exact", \ + SUB0.getTopic().c_str(), \ + SUB1.getTopic().c_str(), \ + SUB2.getTopic().c_str()); + +#define SYNC_DECL4(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3) \ + if(APPROX) \ + { \ + PREFIX##ApproximateSync_ = new message_filters::Synchronizer( \ + PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3); \ + PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4)); \ + } \ + else \ + { \ + PREFIX##ExactSync_ = new message_filters::Synchronizer( \ + PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3); \ + PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4)); \ + } \ + subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s", \ + name_.c_str(), \ + APPROX?"approx":"exact", \ + SUB0.getTopic().c_str(), \ + SUB1.getTopic().c_str(), \ + SUB2.getTopic().c_str(), \ + SUB3.getTopic().c_str()); + +#define SYNC_DECL5(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4) \ + if(APPROX) \ + { \ + PREFIX##ApproximateSync_ = new message_filters::Synchronizer( \ + PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4); \ + PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5)); \ + } \ + else \ + { \ + PREFIX##ExactSync_ = new message_filters::Synchronizer( \ + PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4); \ + PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5)); \ + } \ + subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s,\n %s", \ + name_.c_str(), \ + approxSync?"approx":"exact", \ + SUB0.getTopic().c_str(), \ + SUB1.getTopic().c_str(), \ + SUB2.getTopic().c_str(), \ + SUB3.getTopic().c_str(), \ + SUB4.getTopic().c_str()); + +#define SYNC_DECL6(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5) \ + if(APPROX) \ + { \ + PREFIX##ApproximateSync_ = new message_filters::Synchronizer( \ + PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5); \ + PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6)); \ + } \ + else \ + { \ + PREFIX##ExactSync_ = new message_filters::Synchronizer( \ + PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5); \ + PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6)); \ + } \ + subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s,\n %s,\n %s", \ + name_.c_str(), \ + APPROX?"approx":"exact", \ + SUB0.getTopic().c_str(), \ + SUB1.getTopic().c_str(), \ + SUB2.getTopic().c_str(), \ + SUB3.getTopic().c_str(), \ + SUB4.getTopic().c_str(), \ + SUB5.getTopic().c_str()); + + +#endif /* INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_ */ diff --git a/include/rtabmap_ros/CoreWrapper.h b/include/rtabmap_ros/CoreWrapper.h new file mode 100644 index 00000000..6855aaf7 --- /dev/null +++ b/include/rtabmap_ros/CoreWrapper.h @@ -0,0 +1,264 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef COREWRAPPER_H_ +#define COREWRAPPER_H_ + + +#include +#include + +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include "rtabmap_ros/GetMap.h" +#include "rtabmap_ros/ListLabels.h" +#include "rtabmap_ros/PublishMap.h" +#include "rtabmap_ros/SetGoal.h" +#include "rtabmap_ros/SetLabel.h" +#include "rtabmap_ros/Goal.h" +#include "rtabmap_ros/CommonDataSubscriber.h" + +#include "MapsManager.h" + +#ifdef WITH_OCTOMAP_ROS +#include +#endif + +#include +#include +#include +#include +#include +#include +typedef actionlib::SimpleActionClient MoveBaseClient; + +namespace rtabmap { +class StereoDense; +} + +namespace rtabmap_ros { + +class CoreWrapper : public CommonDataSubscriber, public nodelet::Nodelet +{ +public: + CoreWrapper(); + virtual ~CoreWrapper(); + +private: + + virtual void onInit(); + + bool odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg); + bool odomTFUpdate(const ros::Time & stamp); // TF odom + + virtual void commonDepthCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const std::vector & imageMsgs, + const std::vector & depthMsgs, + const std::vector & cameraInfoMsgs, + const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg); + void commonDepthCallbackImpl( + const std::string & odomFrameId, + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const std::vector & imageMsgs, + const std::vector & depthMsgs, + const std::vector & cameraInfoMsgs, + const sensor_msgs::LaserScanConstPtr& scan2dMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg); + virtual void commonStereoCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + 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::PointCloud2ConstPtr& scan3dMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg); + + void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom + + 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); + + void process( + const ros::Time & stamp, + const rtabmap::SensorData & data, + const rtabmap::Transform & odom = rtabmap::Transform(), + const std::string & odomFrameId = "", + 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&); + bool pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); + bool resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); + bool triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); + 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 getMapDataCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res); + bool getMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::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); + bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&); + bool setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res); + bool cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); + bool setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res); + bool listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtabmap_ros::ListLabels::Response& res); +#ifdef WITH_OCTOMAP_ROS + bool octomapBinaryCallback(octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res); + bool octomapFullCallback(octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res); +#endif + + void loadParameters(const std::string & configFile, rtabmap::ParametersMap & parameters); + void saveParameters(const std::string & configFile); + + void publishLoop(double tfDelay, double tfTolerance); + + void publishStats(const ros::Time & stamp); + void publishCurrentGoal(const ros::Time & stamp); + void goalDoneCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result); + void goalActiveCb(); + void goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback); + void publishLocalPath(const ros::Time & stamp); + void publishGlobalPath(const ros::Time & stamp); + +private: + rtabmap::Rtabmap rtabmap_; + bool paused_; + rtabmap::Transform lastPose_; + ros::Time lastPoseStamp_; + bool lastPoseIntermediate_; + float rotVariance_; + float transVariance_; + rtabmap::Transform currentMetricGoal_; + bool latestNodeWasReached_; + rtabmap::ParametersMap parameters_; + + std::string frameId_; + std::string odomFrameId_; + std::string mapFrameId_; + std::string groundTruthFrameId_; + std::string groundTruthBaseFrameId_; + std::string configPath_; + std::string databasePath_; + bool waitForTransform_; + double waitForTransformDuration_; + bool useActionForGoal_; + bool genScan_; + double genScanMaxDepth_; + double genScanMinDepth_; + int scanCloudMaxPoints_; + int scanCloudNormalK_; + + rtabmap::Transform mapToOdom_; + boost::mutex mapToOdomMutex_; + + MapsManager mapsManager_; + + ros::Publisher infoPub_; + ros::Publisher mapDataPub_; + ros::Publisher mapGraphPub_; + ros::Publisher labelsPub_; + + //Planning stuff + ros::Subscriber goalSub_; + ros::Subscriber goalNodeSub_; + ros::Publisher nextMetricGoalPub_; + ros::Publisher goalReachedPub_; + ros::Publisher globalPathPub_; + ros::Publisher localPathPub_; + + tf2_ros::TransformBroadcaster tfBroadcaster_; + tf::TransformListener tfListener_; + + ros::ServiceServer updateSrv_; + ros::ServiceServer resetSrv_; + ros::ServiceServer pauseSrv_; + ros::ServiceServer resumeSrv_; + ros::ServiceServer triggerNewMapSrv_; + 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 getMapSrv_; + ros::ServiceServer getGridMapSrv_; + ros::ServiceServer publishMapDataSrv_; + ros::ServiceServer setGoalSrv_; + ros::ServiceServer cancelGoalSrv_; + ros::ServiceServer setLabelSrv_; + ros::ServiceServer listLabelsSrv_; +#ifdef WITH_OCTOMAP_ROS + ros::ServiceServer octomapBinarySrv_; + ros::ServiceServer octomapFullSrv_; +#endif + + MoveBaseClient mbClient_; + + boost::thread* transformThread_; + bool tfThreadRunning_; + + // for loop closure detection only + image_transport::Subscriber defaultSub_; + + bool stereoToDepth_; + bool odomSensorSync_; + float rate_; + bool createIntermediateNodes_; + ros::Time time_; + ros::Time previousStamp_; +}; + +} + +#endif /* COREWRAPPER_H_ */ + diff --git a/include/rtabmap_ros/GuiWrapper.h b/include/rtabmap_ros/GuiWrapper.h new file mode 100644 index 00000000..07483638 --- /dev/null +++ b/include/rtabmap_ros/GuiWrapper.h @@ -0,0 +1,128 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef GUIWRAPPER_H_ +#define GUIWRAPPER_H_ + +#include +#include "rtabmap_ros/Info.h" +#include "rtabmap_ros/MapData.h" +#include "rtabmap_ros/OdomInfo.h" +#include "rtabmap_ros/Goal.h" +#include "rtabmap/utilite/UEventsHandler.h" +#include "rtabmap/core/Transform.h" + +#include + +#include +#include +#include + +#include + +namespace rtabmap +{ + class MainWindow; +} + +class QApplication; + +namespace rtabmap_ros { + +class GuiWrapper : public UEventsHandler, public CommonDataSubscriber +{ +public: + GuiWrapper(int & argc, char** argv); + virtual ~GuiWrapper(); + +protected: + virtual void handleEvent(UEvent * anEvent); + +private: + void infoMapCallback(const rtabmap_ros::InfoConstPtr & infoMsg, const rtabmap_ros::MapDataConstPtr & mapMsg); + void goalPathCallback(const rtabmap_ros::GoalConstPtr & goalMsg, const nav_msgs::PathConstPtr & pathMsg); + void goalReachedCallback(const std_msgs::BoolConstPtr & value); + + virtual void commonDepthCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const std::vector & imageMsgs, + const std::vector & depthMsgs, + const std::vector & cameraInfoMsgs, + const sensor_msgs::LaserScanConstPtr& scan2dMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg); + virtual void commonStereoCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, + const sensor_msgs::LaserScanConstPtr& scan2dMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg); + + void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg); + + void processRequestedMap(const rtabmap_ros::MapData & map); + +private: + rtabmap::MainWindow * mainWindow_; + std::string cameraNodeName_; + double lastOdomInfoUpdateTime_; + + // odometry subscription stuffs + std::string frameId_; + std::string odomFrameId_; + bool waitForTransform_; + double waitForTransformDuration_; + bool odomSensorSync_; + tf::TransformListener tfListener_; + + message_filters::Subscriber infoTopic_; + message_filters::Subscriber mapDataTopic_; + + message_filters::Subscriber goalTopic_; + message_filters::Subscriber pathTopic_; + ros::Subscriber goalReachedTopic_; + + ros::Subscriber defaultSub_; // odometry only + + typedef message_filters::sync_policies::ExactTime< + rtabmap_ros::Info, + rtabmap_ros::MapData> MyInfoMapSyncPolicy; + message_filters::Synchronizer * infoMapSync_; + + typedef message_filters::sync_policies::ExactTime< + rtabmap_ros::Goal, + nav_msgs::Path> MyGoalPathSyncPolicy; + message_filters::Synchronizer * goalPathSync_; +}; + +} + +#endif /* GUIWRAPPER_H_ */ diff --git a/src/MapsManager.h b/include/rtabmap_ros/MapsManager.h similarity index 69% rename from src/MapsManager.h rename to include/rtabmap_ros/MapsManager.h index def6173c..5ca0c9d2 100644 --- a/src/MapsManager.h +++ b/include/rtabmap_ros/MapsManager.h @@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define MAPSMANAGER_H_ #include +#include +#include #include #include #include @@ -37,15 +39,19 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { class OctoMap; class Memory; +class OccupancyGrid; } // namespace rtabmap class MapsManager { public: - MapsManager(bool usePublicNamespace); + MapsManager(); virtual ~MapsManager(); + void init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::string & name, bool usePublicNamespace); void clear(); bool hasSubscribers() const; + void backwardCompatibilityParameters(ros::NodeHandle & pnh, rtabmap::ParametersMap & parameters) const; + void setParameters(const rtabmap::ParametersMap & parameters); std::map getFilteredPoses( const std::map & poses); @@ -53,10 +59,7 @@ public: std::map updateMapCaches( const std::map & poses, const rtabmap::Memory * memory, - bool updateCloud, - bool updateProj, bool updateGrid, - bool updateScan, bool updateOctomap, const std::map & signatures = std::map()); @@ -65,52 +68,33 @@ public: const ros::Time & stamp, const std::string & mapFrameId); - cv::Mat generateProjMap( - const std::map & filteredPoses, - float & xMin, - float & yMin, - float & gridCellSize); - cv::Mat generateGridMap( const std::map & filteredPoses, float & xMin, float & yMin, float & gridCellSize); - rtabmap::OctoMap * getOctomap() const {return octomap_;} + const rtabmap::OctoMap * getOctomap() const {return octomap_;} + const rtabmap::OccupancyGrid * getOccupancyGrid() const {return occupancyGrid_;} 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 projMaxObstaclesHeight_; - double projMaxGroundHeight_; - bool projDetectFlatObstacles_; - bool projMapFrame_; + bool cloudSubtractFiltering_; + int cloudSubtractFilteringMinNeighbors_; double gridCellSize_; + bool gridIncremental_; double gridSize_; bool gridEroded_; - bool gridUnknownSpaceFilled_; - double gridMaxUnknownSpaceFilledRange_; + double footprintRadius_; double mapFilterRadius_; double mapFilterAngle_; bool mapCacheCleanup_; bool negativePosesIgnored_; ros::Publisher cloudMapPub_; + ros::Publisher cloudGroundPub_; + ros::Publisher cloudObstaclesPub_; ros::Publisher projMapPub_; ros::Publisher gridMapPub_; ros::Publisher scanMapPub_; @@ -120,15 +104,27 @@ private: ros::Publisher octoMapEmptySpace_; ros::Publisher octoMapProj_; - std::map::Ptr > clouds_; - std::map::Ptr > scans_; - std::map > cameraModels_; - std::map > projMaps_; // + std::map assembledGroundPoses_; + std::map assembledObstaclePoses_; + pcl::PointCloud::Ptr assembledObstacles_; + pcl::PointCloud::Ptr assembledGround_; + rtabmap::FlannIndex assembledGroundIndex_; + rtabmap::FlannIndex assembledObstacleIndex_; + std::map::Ptr > groundClouds_; + std::map::Ptr > obstacleClouds_; + + std::map gridPoses_; + cv::Mat gridMap_; std::map > gridMaps_; // + std::map gridMapsViewpoints_; + + rtabmap::OccupancyGrid * occupancyGrid_; rtabmap::OctoMap * octomap_; int octomapTreeDepth_; - bool octomapGroundIsObstacle_; + double octomapOccupancyThr_; + + rtabmap::ParametersMap parameters_; }; #endif /* MAPSMANAGER_H_ */ diff --git a/include/rtabmap_ros/MsgConversion.h b/include/rtabmap_ros/MsgConversion.h index 9af81a23..64449815 100644 --- a/include/rtabmap_ros/MsgConversion.h +++ b/include/rtabmap_ros/MsgConversion.h @@ -29,12 +29,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define MSGCONVERSION_H_ #include +#include #include #include #include +#include +#include #include #include +#include #include #include @@ -52,6 +56,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include +#include namespace rtabmap_ros { @@ -64,6 +70,9 @@ rtabmap::Transform transformFromGeometryMsg(const geometry_msgs::Transform & msg void transformToPoseMsg(const rtabmap::Transform & transform, geometry_msgs::Pose & msg); rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg); +void toCvCopy(const rtabmap_ros::RGBDImage & image, cv_bridge::CvImagePtr & rgb, cv_bridge::CvImagePtr & depth); +void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth); + // copy data void compressedMatToBytes(const cv::Mat & compressed, std::vector & bytes); cv::Mat compressedMatFromBytes(const std::vector & bytes, bool copy = true); @@ -137,8 +146,78 @@ void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg); void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg); +cv::Mat userDataFromROS(const rtabmap_ros::UserData & dataMsg); +void userDataToROS(const cv::Mat & data, rtabmap_ros::UserData & dataMsg, bool compress); + inline double timestampFromROS(const ros::Time & stamp) {return double(stamp.sec) + double(stamp.nsec)/1000000000.0;} +// common stuff +rtabmap::Transform getTransform( + const std::string & fromFrameId, + const std::string & toFrameId, + const ros::Time & stamp, + tf::TransformListener & listener, + double waitForTransform); + + +// get moving transform accordingly to a fixed frame. For example get +// transform of /base_link between two stamps accordingly to /odom frame. +rtabmap::Transform getTransform( + const std::string & sourceTargetFrame, + const std::string & fixedFrame, + const ros::Time & stampSource, + const ros::Time & stampTarget, + tf::TransformListener & listener, + double waitForTransform); + +bool convertRGBDMsgs( + const std::vector & imageMsgs, + const std::vector & depthMsgs, + const std::vector & cameraInfoMsgs, + const std::string & frameId, + const std::string & odomFrameId, + const ros::Time & odomStamp, + cv::Mat & rgb, + cv::Mat & depth, + std::vector & cameraModels, + tf::TransformListener & listener, + double waitForTransform); + +bool convertStereoMsg( + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, + const std::string & frameId, + const std::string & odomFrameId, + const ros::Time & odomStamp, + cv::Mat & left, + cv::Mat & right, + rtabmap::StereoCameraModel & stereoModel, + tf::TransformListener & listener, + double waitForTransform); + +bool convertScanMsg( + const sensor_msgs::LaserScanConstPtr& scan2dMsg, + const std::string & frameId, + const std::string & odomFrameId, + const ros::Time & odomStamp, + cv::Mat & scan, + rtabmap::Transform & scanLocalTransform, + tf::TransformListener & listener, + double waitForTransform); + +bool convertScan3dMsg( + const sensor_msgs::PointCloud2ConstPtr & scan3dMsg, + const std::string & frameId, + const std::string & odomFrameId, + const ros::Time & odomStamp, + int scanCloudNormalK, + cv::Mat & scan, + rtabmap::Transform & scanLocalTransform, + tf::TransformListener & listener, + double waitForTransform); + } #endif /* MSGCONVERSION_H_ */ diff --git a/src/nodelets/OdometryROS.h b/include/rtabmap_ros/OdometryROS.h similarity index 86% rename from src/nodelets/OdometryROS.h rename to include/rtabmap_ros/OdometryROS.h index 3432fd26..f674f0c8 100644 --- a/src/nodelets/OdometryROS.h +++ b/include/rtabmap_ros/OdometryROS.h @@ -41,6 +41,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include + namespace rtabmap { class Odometry; } @@ -51,7 +53,7 @@ class OdometryROS : public nodelet::Nodelet { public: - OdometryROS(bool stereo); + OdometryROS(bool stereoParams, bool visParams, bool icpParams); virtual ~OdometryROS(); void processData(const rtabmap::SensorData & data, const ros::Time & stamp); @@ -68,25 +70,32 @@ public: 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 isOdometryF2M() const; rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const; protected: + void startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync); + void callbackCalled() {callbackCalled_ = true;} + virtual void flushCallbacks() = 0; + tf::TransformListener & tfListener() {return tfListener_;} private: + void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync); virtual void onInit(); virtual void onOdomInit() = 0; + virtual void updateParameters(rtabmap::ParametersMap & parameters) {} private: rtabmap::Odometry * odometry_; + boost::thread * warningThread_; + bool callbackCalled_; // parameters std::string frameId_; std::string odomFrameId_; std::string groundTruthFrameId_; + std::string guessFrameId_; bool publishTf_; bool waitForTransform_; double waitForTransformDuration_; @@ -97,6 +106,7 @@ private: ros::Publisher odomPub_; ros::Publisher odomInfoPub_; ros::Publisher odomLocalMap_; + ros::Publisher odomLocalScanMap_; ros::Publisher odomLastFrame_; ros::ServiceServer resetSrv_; ros::ServiceServer resetToPoseSrv_; @@ -112,7 +122,9 @@ private: bool paused_; int resetCountdown_; int resetCurrentCount_; - bool stereo_; + bool stereoParams_; + bool visParams_; + bool icpParams_; }; } diff --git a/src/PreferencesDialogROS.h b/include/rtabmap_ros/PreferencesDialogROS.h similarity index 100% rename from src/PreferencesDialogROS.h rename to include/rtabmap_ros/PreferencesDialogROS.h diff --git a/launch/azimut3/az3_mapping_robot_stereo_nav.launch b/launch/azimut3/az3_mapping_robot_stereo_nav.launch index 63e9f486..9deae37a 100644 --- a/launch/azimut3/az3_mapping_robot_stereo_nav.launch +++ b/launch/azimut3/az3_mapping_robot_stereo_nav.launch @@ -28,7 +28,7 @@ - + diff --git a/launch/calibration/distortion_model_PS1080.bin b/launch/calibration/distortion_model_PS1080.bin new file mode 100644 index 00000000..54ee5d35 Binary files /dev/null and b/launch/calibration/distortion_model_PS1080.bin differ diff --git a/launch/calibration/distortion_model_PS1080.png b/launch/calibration/distortion_model_PS1080.png new file mode 100644 index 00000000..3c9ab89c Binary files /dev/null and b/launch/calibration/distortion_model_PS1080.png differ diff --git a/launch/config/demo_stereo_outdoor.rviz b/launch/config/demo_stereo_outdoor.rviz index ce7efce9..c9eb5966 100644 --- a/launch/config/demo_stereo_outdoor.rviz +++ b/launch/config/demo_stereo_outdoor.rviz @@ -180,7 +180,7 @@ Visualization Manager: Draw Behind: false Enabled: true Name: Map - Topic: /rtabmap/proj_map + Topic: /rtabmap/grid_map Value: true - Class: rtabmap_ros/Info Enabled: true diff --git a/launch/data_recorder.launch b/launch/data_recorder.launch index 4a210253..ddf02002 100644 --- a/launch/data_recorder.launch +++ b/launch/data_recorder.launch @@ -5,7 +5,8 @@ - + + @@ -41,7 +42,8 @@ - + + @@ -52,7 +54,7 @@ - + diff --git a/launch/demo/demo_multi-session_mapping.launch b/launch/demo/demo_multi-session_mapping.launch index cbf3fa24..710172ce 100644 --- a/launch/demo/demo_multi-session_mapping.launch +++ b/launch/demo/demo_multi-session_mapping.launch @@ -6,6 +6,7 @@ + @@ -13,7 +14,7 @@ - + @@ -33,7 +34,7 @@ - + @@ -49,8 +50,12 @@ - + + + + + diff --git a/launch/demo/demo_robot_mapping.launch b/launch/demo/demo_robot_mapping.launch index 0cbd6e56..efbf952c 100644 --- a/launch/demo/demo_robot_mapping.launch +++ b/launch/demo/demo_robot_mapping.launch @@ -33,19 +33,18 @@ - - + + - - + diff --git a/launch/demo/demo_stereo_outdoor.launch b/launch/demo/demo_stereo_outdoor.launch index fcc376da..0a8d2115 100644 --- a/launch/demo/demo_stereo_outdoor.launch +++ b/launch/demo/demo_stereo_outdoor.launch @@ -77,6 +77,8 @@ + + diff --git a/launch/demo/demo_two_kinects.launch b/launch/demo/demo_two_kinects.launch index e08f430e..0f582860 100644 --- a/launch/demo/demo_two_kinects.launch +++ b/launch/demo/demo_two_kinects.launch @@ -1,7 +1,7 @@ - + @@ -15,7 +15,7 @@ - + + + + + + + + + + + + + + + + + - - - - - - - + + + - + @@ -75,65 +87,38 @@ - - + + + + - - - - - - - + + + - + + - + - - - - - - - + + - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/rgbd_mapping.launch b/launch/rgbd_mapping.launch index 9cc04de7..ade39606 100644 --- a/launch/rgbd_mapping.launch +++ b/launch/rgbd_mapping.launch @@ -15,8 +15,8 @@ - - + + @@ -37,6 +37,7 @@ + @@ -68,7 +69,8 @@ - + + diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch index 3d3f509f..b421cd23 100644 --- a/launch/rtabmap.launch +++ b/launch/rtabmap.launch @@ -27,7 +27,7 @@ - + @@ -36,7 +36,10 @@ - + + + + @@ -51,9 +54,8 @@ - - + @@ -63,6 +65,7 @@ + @@ -81,7 +84,7 @@ - + @@ -124,6 +127,7 @@ + @@ -158,8 +162,10 @@ + + @@ -178,7 +184,7 @@ - + diff --git a/launch/stereo_mapping.launch b/launch/stereo_mapping.launch index 6cbad067..71487894 100644 --- a/launch/stereo_mapping.launch +++ b/launch/stereo_mapping.launch @@ -15,8 +15,8 @@ - - + + @@ -39,12 +39,12 @@ + - @@ -75,7 +75,8 @@ - + + diff --git a/launch/tests/rgbdslam_datasets.launch b/launch/tests/rgbdslam_datasets.launch index 66a01b20..7af0a7f4 100644 --- a/launch/tests/rgbdslam_datasets.launch +++ b/launch/tests/rgbdslam_datasets.launch @@ -62,6 +62,8 @@ + + diff --git a/launch/tests/sensor_fusion_kinect_brick.launch b/launch/tests/sensor_fusion_kinect_brick.launch index 282b66a1..ba585de5 100644 --- a/launch/tests/sensor_fusion_kinect_brick.launch +++ b/launch/tests/sensor_fusion_kinect_brick.launch @@ -1,6 +1,7 @@ + @@ -12,7 +13,7 @@ - + diff --git a/launch/tests/test_icp_odometry.launch b/launch/tests/test_icp_odometry.launch new file mode 100644 index 00000000..bb4f2a79 --- /dev/null +++ b/launch/tests/test_icp_odometry.launch @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tests/test_map_optimizer.launch b/launch/tests/test_map_optimizer.launch new file mode 100644 index 00000000..466a6c7d --- /dev/null +++ b/launch/tests/test_map_optimizer.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/launch/tests/test_obstacles_detection.launch b/launch/tests/test_obstacles_detection.launch index acaf83bc..fba3ca09 100644 --- a/launch/tests/test_obstacles_detection.launch +++ b/launch/tests/test_obstacles_detection.launch @@ -1,8 +1,6 @@ - - @@ -23,7 +21,6 @@ - diff --git a/launch/tests/test_rgbd_image.launch b/launch/tests/test_rgbd_image.launch new file mode 100644 index 00000000..72a2eb83 --- /dev/null +++ b/launch/tests/test_rgbd_image.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tests/test_rtabmap_nodelets.launch b/launch/tests/test_rtabmap_nodelets.launch new file mode 100644 index 00000000..c5a31fc6 --- /dev/null +++ b/launch/tests/test_rtabmap_nodelets.launch @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tests/test_undistort_depth.launch b/launch/tests/test_undistort_depth.launch new file mode 100644 index 00000000..45382886 --- /dev/null +++ b/launch/tests/test_undistort_depth.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/msg/NodeData.msg b/msg/NodeData.msg index 2265fd71..c817e062 100644 --- a/msg/NodeData.msg +++ b/msg/NodeData.msg @@ -24,6 +24,8 @@ float32[] fx float32[] fy float32[] cx float32[] cy +float32[] width +float32[] height float32 baseline # local transform (/base_link -> /camera_link) geometry_msgs/Transform[] localTransform @@ -33,13 +35,25 @@ geometry_msgs/Transform[] localTransform uint8[] laserScan int32 laserScanMaxPts float32 laserScanMaxRange +geometry_msgs/Transform laserScanLocalTransform # compressed user data # use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h" uint8[] userData +# compressed occupancy grid +# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h" +uint8[] grid_ground +uint8[] grid_obstacles +float32 grid_cell_size +Point3f grid_view_point + # std::multimap # std::multimap int32[] wordIds KeyPoint[] wordKpts -sensor_msgs/PointCloud2 wordPts \ No newline at end of file +sensor_msgs/PointCloud2 wordPts + +# compressed descriptors +# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h" +uint8[] descriptors diff --git a/msg/OdomInfo.msg b/msg/OdomInfo.msg index e3107457..4cace8f1 100644 --- a/msg/OdomInfo.msg +++ b/msg/OdomInfo.msg @@ -6,7 +6,8 @@ Header header # bool lost; # int matches; # int inliers; -# float variance; +# float varianceLin; +# float varianceAng; # int features; # int localMapSize; # float time; @@ -27,9 +28,11 @@ Header header bool lost int32 matches int32 inliers -float32 variance +float32 varianceLin +float32 varianceAng int32 features int32 localMapSize +int32 localScanMapSize float32 timeEstimation float32 timeParticleFiltering float32 stamp @@ -52,3 +55,7 @@ int32[] cornerInliers geometry_msgs/Transform transform geometry_msgs/Transform transformFiltered +# compressed local scan map data +# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h" +uint8[] localScanMap + diff --git a/msg/RGBDImage.msg b/msg/RGBDImage.msg new file mode 100644 index 00000000..f88bafdc --- /dev/null +++ b/msg/RGBDImage.msg @@ -0,0 +1,12 @@ + +Header header + +sensor_msgs/CameraInfo cameraInfo + +# Raw +sensor_msgs/Image rgb +sensor_msgs/Image depth + +# Compressed +sensor_msgs/CompressedImage rgbCompressed +sensor_msgs/CompressedImage depthCompressed \ No newline at end of file diff --git a/msg/UserData.msg b/msg/UserData.msg new file mode 100644 index 00000000..005d9b66 --- /dev/null +++ b/msg/UserData.msg @@ -0,0 +1,12 @@ + +Header header + +# OpenCV matrix containing the user data. A matrix of type CV_8UC1 +# with 1 row is considered to be compressed (with rtabmap::compressData() method). +# If you have one dimension unsigned 8 bits uncompressed data, make sure to transpose it +# (to have multiple rows instead of multiple columns) in order to be detected as +# not compressed. +uint32 rows +uint32 cols +uint32 type +uint8[] data \ No newline at end of file diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml index 58e29dd7..98b339b2 100644 --- a/nodelet_plugins.xml +++ b/nodelet_plugins.xml @@ -1,5 +1,13 @@ + + + This is my nodelet. + + + @@ -15,6 +23,22 @@ This is my nodelet. + + + + This is my nodelet. + + + + + + This is my nodelet. + + + + + + This is my nodelet. + + + + + + This is my nodelet. + + + + + + This is my nodelet. + + + diff --git a/package.xml b/package.xml index 372846a8..1fa8a3e8 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap_ros - 0.11.8 + 0.11.13 RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe diff --git a/src/CommonDataSubscriber.cpp b/src/CommonDataSubscriber.cpp new file mode 100644 index 00000000..fc01376d --- /dev/null +++ b/src/CommonDataSubscriber.cpp @@ -0,0 +1,411 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include + +namespace rtabmap_ros { + +CommonDataSubscriber::CommonDataSubscriber(bool gui) : + queueSize_(10), + approxSync_(true), + warningThread_(0), + callbackCalled_(false), + subscribedToDepth_(!gui), + subscribedToStereo_(false), + subscribedToRGBD_(false), + subscribedToScan2d_(false), + subscribedToScan3d_(false), + subscribedToOdomInfo_(false), + + // RGB + Depth + SYNC_INIT(depth), + SYNC_INIT(depthScan2d), + SYNC_INIT(depthScan3d), + SYNC_INIT(depthInfo), + + // RGB + Depth + Odom + SYNC_INIT(depthOdom), + SYNC_INIT(depthOdomScan2d), + SYNC_INIT(depthOdomScan3d), + SYNC_INIT(depthOdomInfo), + + // RGB + Depth + User Data + SYNC_INIT(depthData), + SYNC_INIT(depthDataScan2d), + SYNC_INIT(depthDataScan3d), + SYNC_INIT(depthDataInfo), + + // RGB + Depth + Odom + User Data + SYNC_INIT(depthOdomData), + SYNC_INIT(depthOdomDataScan2d), + SYNC_INIT(depthOdomDataScan3d), + SYNC_INIT(depthOdomDataInfo), + + // Stereo + SYNC_INIT(stereo), + SYNC_INIT(stereoInfo), + + // Stereo + Odom + SYNC_INIT(stereoOdom), + SYNC_INIT(stereoOdomInfo), + + // 1 RGBD + SYNC_INIT(rgbdScan2d), + SYNC_INIT(rgbdScan3d), + SYNC_INIT(rgbdInfo), + + // 1 RGBD + Odom + SYNC_INIT(rgbdOdom), + SYNC_INIT(rgbdOdomScan2d), + SYNC_INIT(rgbdOdomScan3d), + SYNC_INIT(rgbdOdomInfo), + + // 1 RGBD + User Data + SYNC_INIT(rgbdData), + SYNC_INIT(rgbdDataScan2d), + SYNC_INIT(rgbdDataScan3d), + SYNC_INIT(rgbdDataInfo), + + // 1 RGBD + Odom + User Data + SYNC_INIT(rgbdOdomData), + SYNC_INIT(rgbdOdomDataScan2d), + SYNC_INIT(rgbdOdomDataScan3d), + SYNC_INIT(rgbdOdomDataInfo), + + // 2 RGBD + SYNC_INIT(rgbd2), + SYNC_INIT(rgbd2Scan2d), + SYNC_INIT(rgbd2Scan3d), + SYNC_INIT(rgbd2Info), + + // 2 RGBD + Odom + SYNC_INIT(rgbd2Odom), + SYNC_INIT(rgbd2OdomScan2d), + SYNC_INIT(rgbd2OdomScan3d), + SYNC_INIT(rgbd2OdomInfo), + + // 2 RGBD + User Data + SYNC_INIT(rgbd2Data), + SYNC_INIT(rgbd2DataScan2d), + SYNC_INIT(rgbd2DataScan3d), + SYNC_INIT(rgbd2DataInfo), + + // 2 RGBD + Odom + User Data + SYNC_INIT(rgbd2OdomData), + SYNC_INIT(rgbd2OdomDataScan2d), + SYNC_INIT(rgbd2OdomDataScan3d), + SYNC_INIT(rgbd2OdomDataInfo) + +{ +} + +void CommonDataSubscriber::setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::string & name) +{ + bool subscribeScan2d = false; + bool subscribeScan3d = false; + bool subscribeOdomInfo = false; + bool subscribeUserData = false; + int rgbdCameras = 1; + name_ = name; + + // ROS related parameters (private) + pnh.param("subscribe_depth", subscribedToDepth_, subscribedToDepth_); + 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", subscribedToStereo_, subscribedToStereo_); + pnh.param("subscribe_rgbd", subscribedToRGBD_, subscribedToRGBD_); + pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo); + pnh.param("subscribe_user_data", subscribeUserData, subscribeUserData); + if(subscribedToDepth_ && subscribedToStereo_) + { + ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false."); + subscribedToDepth_ = false; + } + if(subscribedToDepth_ && subscribedToRGBD_) + { + ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_rgbd cannot be true at the same time. Parameter subscribe_depth is set to false."); + subscribedToDepth_ = false; + } + if(subscribedToStereo_ && subscribedToRGBD_) + { + ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_rgbd cannot be true at the same time. Parameter subscribe_stereo is set to false."); + subscribedToStereo_ = false; + } + 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(!subscribedToDepth_ && !subscribedToStereo_ && !subscribedToRGBD_) + { + ROS_WARN("When subscribing to laser scan, you should subscribe to depth, stereo or rgbd too. Subscribing to depth by default..."); + subscribedToDepth_ = true; + } + } + if(subscribedToStereo_) + { + approxSync_ = false; // default for stereo: exact sync + } + + std::string odomFrameId; + pnh.getParam("odom_frame_id", odomFrameId); + pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras); + if(pnh.hasParam("depth_cameras")) + { + ROS_ERROR("\"depth_cameras\" parameter doesn't exist anymore. It is replaced by \"rgbd_cameras\" used when \"subscribe_rgbd\" is true."); + } + pnh.param("queue_size", queueSize_, queueSize_); + if(pnh.hasParam("stereo_approx_sync") && !pnh.hasParam("approx_sync")) + { + ROS_WARN("Parameter \"stereo_approx_sync\" has been renamed " + "to \"approx_sync\"! Your value is still copied to " + "corresponding parameter."); + pnh.param("stereo_approx_sync", approxSync_, approxSync_); + } + else + { + pnh.param("approx_sync", approxSync_, approxSync_); + } + + if(rgbdCameras <= 0 && subscribedToRGBD_) + { + rgbdCameras = 1; + } + + ROS_INFO("%s: queue_size = %d", name.c_str(), queueSize_); + ROS_INFO("%s: rgbd_cameras = %d", name.c_str(), rgbdCameras); + ROS_INFO("%s: approx_sync = %s", name.c_str(), approxSync_?"true":"false"); + + bool subscribeOdom = odomFrameId.empty(); + if(subscribedToDepth_) + { + setupDepthCallbacks( + nh, + pnh, + subscribeOdom, + subscribeUserData, + subscribeScan2d, + subscribeScan3d, + subscribeOdomInfo, + queueSize_, + approxSync_); + } + else if(subscribedToStereo_) + { + setupStereoCallbacks( + nh, + pnh, + subscribeOdom, + subscribeOdomInfo, + queueSize_, + approxSync_); + } + else if(subscribedToRGBD_) + { + if(rgbdCameras == 2) + { + setupRGBD2Callbacks( + nh, + pnh, + subscribeOdom, + subscribeUserData, + subscribeScan2d, + subscribeScan3d, + subscribeOdomInfo, + queueSize_, + approxSync_); + } + else + { + setupRGBDCallbacks( + nh, + pnh, + subscribeOdom, + subscribeUserData, + subscribeScan2d, + subscribeScan3d, + subscribeOdomInfo, + queueSize_, + approxSync_); + } + } + if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_) + { + warningThread_ = new boost::thread(boost::bind(&CommonDataSubscriber::warningLoop, this)); + ROS_INFO("%s", subscribedTopicsMsg_.c_str()); + } +} + +CommonDataSubscriber::~CommonDataSubscriber() +{ + if(warningThread_) + { + callbackCalled(); + warningThread_->join(); + delete warningThread_; + } + + // RGB + Depth + SYNC_DEL(depth); + SYNC_DEL(depthScan2d); + SYNC_DEL(depthScan3d); + SYNC_DEL(depthInfo); + + // RGB + Depth + Odom + SYNC_DEL(depthOdom); + SYNC_DEL(depthOdomScan2d); + SYNC_DEL(depthOdomScan3d); + SYNC_DEL(depthOdomInfo); + + // RGB + Depth + User Data + SYNC_DEL(depthData); + SYNC_DEL(depthDataScan2d); + SYNC_DEL(depthDataScan3d); + SYNC_DEL(depthDataInfo); + + // RGB + Depth + Odom + User Data + SYNC_DEL(depthOdomData); + SYNC_DEL(depthOdomDataScan2d); + SYNC_DEL(depthOdomDataScan3d); + SYNC_DEL(depthOdomDataInfo); + + // Stereo + SYNC_DEL(stereo); + SYNC_DEL(stereoInfo); + + // Stereo + Odom + SYNC_DEL(stereoOdom); + SYNC_DEL(stereoOdomInfo); + + // 1 RGBD + SYNC_DEL(rgbdScan2d); + SYNC_DEL(rgbdScan3d); + SYNC_DEL(rgbdInfo); + + // 1 RGBD + Odom + SYNC_DEL(rgbdOdom); + SYNC_DEL(rgbdOdomScan2d); + SYNC_DEL(rgbdOdomScan3d); + SYNC_DEL(rgbdOdomInfo); + + // 1 RGBD + User Data + SYNC_DEL(rgbdData); + SYNC_DEL(rgbdDataScan2d); + SYNC_DEL(rgbdDataScan3d); + SYNC_DEL(rgbdDataInfo); + + // 1 RGBD + Odom + User Data + SYNC_DEL(rgbdOdomData); + SYNC_DEL(rgbdOdomDataScan2d); + SYNC_DEL(rgbdOdomDataScan3d); + SYNC_DEL(rgbdOdomDataInfo); + + // 2 RGBD + SYNC_DEL(rgbd2); + SYNC_DEL(rgbd2Scan2d); + SYNC_DEL(rgbd2Scan3d); + SYNC_DEL(rgbd2Info); + + // 2 RGBD + Odom + SYNC_DEL(rgbd2Odom); + SYNC_DEL(rgbd2OdomScan2d); + SYNC_DEL(rgbd2OdomScan3d); + SYNC_DEL(rgbd2OdomInfo); + + // 2 RGBD + User Data + SYNC_DEL(rgbd2Data); + SYNC_DEL(rgbd2DataScan2d); + SYNC_DEL(rgbd2DataScan3d); + SYNC_DEL(rgbd2DataInfo); + + // 2 RGBD + Odom + User Data + SYNC_DEL(rgbd2OdomData); + SYNC_DEL(rgbd2OdomDataScan2d); + SYNC_DEL(rgbd2OdomDataScan3d); + SYNC_DEL(rgbd2OdomDataInfo); + + for(unsigned int i=0; i imageMsgs; + std::vector depthMsgs; + std::vector cameraInfoMsgs; + if(imageMsg.get()) + { + imageMsgs.push_back(imageMsg); + } + if(depthMsg.get()) + { + depthMsgs.push_back(depthMsg); + } + cameraInfoMsgs.push_back(cameraInfoMsg); + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} + +} /* namespace rtabmap_ros */ diff --git a/src/CoreNode.cpp b/src/CoreNode.cpp index 4429c125..99d3ad95 100644 --- a/src/CoreNode.cpp +++ b/src/CoreNode.cpp @@ -25,12 +25,14 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "CoreWrapper.h" +#include "ros/ros.h" +#include #include #include #include #include #include +#include "nodelet/loader.h" int main(int argc, char** argv) { @@ -41,19 +43,14 @@ int main(int argc, char** argv) ros::init(argc, argv, "rtabmap"); - bool deleteDbOnStart = false; + nodelet::V_string nargv; for(int i=1;i #include -#include -#include -#include +#include "pluginlib/class_list_macros.h" + #include #include #include +#include +#include +#include #include @@ -48,14 +50,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include #include #include #include - -#include - -#include +#include +#include #ifdef WITH_OCTOMAP_ROS #ifdef RTABMAP_OCTOMAP @@ -77,7 +76,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. using namespace rtabmap; -CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) : +namespace rtabmap_ros { + +CoreWrapper::CoreWrapper() : + CommonDataSubscriber(false), paused_(false), lastPose_(Transform::getIdentity()), lastPoseIntermediate_(false), @@ -85,9 +87,10 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) transVariance_(0), latestNodeWasReached_(false), frameId_("base_link"), - mapFrameId_("map"), odomFrameId_(""), + mapFrameId_("map"), groundTruthFrameId_(""), // e.g., "world" + groundTruthBaseFrameId_(""), // e.g., "base_link_gt" configPath_(""), databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()), waitForTransform_(true), @@ -98,95 +101,44 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) genScanMinDepth_(0.0), scanCloudMaxPoints_(0), scanCloudNormalK_(0), - flipScan_(false), mapToOdom_(rtabmap::Transform::getIdentity()), - mapsManager_(true), - depthSync_(0), - depthExactSync_(0), - depthScanSync_(0), - depthScan3dSync_(0), - stereoScanSync_(0), - stereoScan3dSync_(0), - stereoApproxSync_(0), - stereoExactSync_(0), - depth2Sync_(0), - depthTFSync_(0), - depthTFExactSync_(0), - depthScanTFSync_(0), - depthScan3dTFSync_(0), - stereoScanTFSync_(0), - stereoScan3dTFSync_(0), - stereoApproxTFSync_(0), - stereoExactTFSync_(0), transformThread_(0), + tfThreadRunning_(false), + stereoToDepth_(false), + odomSensorSync_(false), rate_(Parameters::defaultRtabmapDetectionRate()), createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()), time_(ros::Time::now()), previousStamp_(0), mbClient_("move_base", true) { - ros::NodeHandle nh; - ros::NodeHandle pnh("~"); +} + +void CoreWrapper::onInit() +{ + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + mapsManager_.init(nh, pnh, getName(), true); - bool subscribeScan2d = false; - bool subscribeScan3d = false; - bool subscribeDepth = true; - bool subscribeStereo = false; - int depthCameras = 1; - int queueSize = 10; bool publishTf = true; double tfDelay = 0.05; // 20 Hz double tfTolerance = 0.1; // 100 ms std::string tfPrefix = ""; - bool approxSync = true; - - // ROS related parameters (private) - pnh.param("subscribe_depth", subscribeDepth, subscribeDepth); - 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) - { - 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(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) - { - ROS_WARN("When subscribing to laser scan, you should subscribe to depth or stereo too. Subscribing to depth by default..."); - subscribeDepth = true; - } - } pnh.param("config_path", configPath_, configPath_); pnh.param("database_path", databasePath_, databasePath_); 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("map_frame_id", mapFrameId_, mapFrameId_); pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_); - pnh.param("depth_cameras", depthCameras, depthCameras); - pnh.param("queue_size", queueSize, queueSize); - if(pnh.hasParam("stereo_approx_sync") && !pnh.hasParam("approx_sync")) + pnh.param("ground_truth_base_frame_id", groundTruthBaseFrameId_, frameId_); + if(pnh.hasParam("depth_cameras") && !pnh.hasParam("depth_cameras")) { - ROS_WARN("Parameter \"stereo_approx_sync\" has been renamed " - "to \"approx_sync\"! Your value is still copied to " - "corresponding parameter."); - pnh.param("stereo_approx_sync", approxSync, approxSync); - } - else - { - pnh.param("approx_sync", approxSync, approxSync); + NODELET_ERROR("\"depth_cameras\" parameter doesn't exist " + "anymore! It is replaced by \"rgbd_cameras\" parameter " + "used when \"subscribe_rgbd\" is true"); } pnh.param("publish_tf", publishTf, publishTf); @@ -201,7 +153,14 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) pnh.param("gen_scan_min_depth", genScanMinDepth_, genScanMinDepth_); pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_); pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_); - pnh.param("flip_scan", flipScan_, flipScan_); + pnh.param("stereo_to_depth", stereoToDepth_, stereoToDepth_); + pnh.param("odom_sensor_sync", odomSensorSync_, odomSensorSync_); + if(pnh.hasParam("flip_scan")) + { + NODELET_WARN("Parameter \"flip_scan\" doesn't exist anymore. Rtabmap now " + "detects automatically if the laser is upside down with /tf, then if so, it " + "switches scan values."); + } if(!tfPrefix.empty()) { @@ -221,29 +180,34 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) { groundTruthFrameId_ = tfPrefix+"/"+groundTruthFrameId_; } + if(!groundTruthBaseFrameId_.empty()) + { + groundTruthBaseFrameId_ = tfPrefix+"/"+groundTruthBaseFrameId_; + } // keep worldFrameId_ without prefix as it should be global } - if(depthCameras <= 0 && subscribeDepth) - { - depthCameras = 1; - } - - ROS_INFO("rtabmap: frame_id = %s", frameId_.c_str()); + NODELET_INFO("rtabmap: frame_id = %s", frameId_.c_str()); if(!odomFrameId_.empty()) { - ROS_INFO("rtabmap: odom_frame_id = %s", odomFrameId_.c_str()); + NODELET_INFO("rtabmap: odom_frame_id = %s", odomFrameId_.c_str()); } if(!groundTruthFrameId_.empty()) { - ROS_INFO("rtabmap: ground_truth_frame_id = %s", groundTruthFrameId_.c_str()); + NODELET_INFO("rtabmap: ground_truth_frame_id = %s -> ground_truth_base_frame_id = %s", + groundTruthFrameId_.c_str(), + groundTruthBaseFrameId_.c_str()); + } + NODELET_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str()); + NODELET_INFO("rtabmap: tf_delay = %f", tfDelay); + NODELET_INFO("rtabmap: tf_tolerance = %f", tfTolerance); + NODELET_INFO("rtabmap: odom_sensor_sync = %s", odomSensorSync_?"true":"false"); + bool subscribeStereo = false; + pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo); + if(subscribeStereo) + { + NODELET_INFO("rtabmap: stereo_to_depth = %s", stereoToDepth_?"true":"false"); } - ROS_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str()); - ROS_INFO("rtabmap: queue_size = %d", queueSize); - ROS_INFO("rtabmap: tf_delay = %f", tfDelay); - ROS_INFO("rtabmap: tf_tolerance = %f", tfTolerance); - ROS_INFO("rtabmap: depth_cameras = %d", depthCameras); - ROS_INFO("rtabmap: approx_sync = %s", approxSync?"true":"false"); infoPub_ = nh.advertise("info", 1); mapDataPub_ = nh.advertise("mapData", 1); @@ -273,12 +237,15 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) databasePath_ = UDirectory::currentDir(true) + databasePath_; } + ParametersMap allParameters = Parameters::getDefaultParameters(); + uInsert(allParameters, ParametersPair(Parameters::kRGBDCreateOccupancyGrid(), "true")); // default true in ROS + uInsert(allParameters, ParametersPair(Parameters::kRtabmapWorkingDirectory(), UDirectory::homeDir()+"/.ros")); // change default to ~/.ros + // load parameters - parameters_ = loadParameters(configPath_); + loadParameters(configPath_, parameters_); // update parameters with user input parameters (private) - uInsert(parameters_, std::make_pair(Parameters::kRtabmapWorkingDirectory(), UDirectory::homeDir()+"/.ros")); // change default to ~/.ros - for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter) + for(ParametersMap::iterator iter=allParameters.begin(); iter!=allParameters.end(); ++iter) { std::string vStr; bool vBool; @@ -286,40 +253,52 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) double vDouble; if(pnh.getParam(iter->first, vStr)) { - ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str()); - iter->second = vStr; + NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str()); if(iter->first.compare(Parameters::kRtabmapWorkingDirectory()) == 0) { - iter->second = uReplaceChar(iter->second, '~', UDirectory::homeDir()); + vStr = uReplaceChar(vStr, '~', UDirectory::homeDir()); } else if(iter->first.compare(Parameters::kKpDictionaryPath()) == 0) { - iter->second = uReplaceChar(iter->second, '~', UDirectory::homeDir()); + vStr = uReplaceChar(vStr, '~', UDirectory::homeDir()); } + uInsert(parameters_, ParametersPair(iter->first, vStr)); } else if(pnh.getParam(iter->first, vBool)) { - ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str()); - iter->second = uBool2Str(vBool); + NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str()); + uInsert(parameters_, ParametersPair(iter->first, uBool2Str(vBool))); } else if(pnh.getParam(iter->first, vDouble)) { - ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str()); - iter->second = uNumber2Str(vDouble); + NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str()); + uInsert(parameters_, ParametersPair(iter->first, uNumber2Str(vDouble))); } else if(pnh.getParam(iter->first, vInt)) { - ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str()); - iter->second = uNumber2Str(vInt); + NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str()); + uInsert(parameters_, ParametersPair(iter->first, uNumber2Str(vInt))); } } - //update with input arguments + //parse input arguments + std::vector argList = getMyArgv(); + char * argv[argList.size()]; + bool deleteDbOnStart = false; + for(unsigned int i=0; ifirst, iter->second)); - ROS_INFO("Update RTAB-Map parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str()); + NODELET_INFO("Update RTAB-Map parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str()); } // Backward compatibility @@ -333,26 +312,76 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) if(iter->second.first) { // 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.", + uInsert(parameters_, ParametersPair(iter->second.second, vStr)); + NODELET_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->second.second.empty()) { - ROS_ERROR("Rtabmap: Parameter \"%s\" doesn't exist anymore!", + NODELET_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\"", + NODELET_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()); } } } } + // Backward compatibility (MapsManager) + mapsManager_.backwardCompatibilityParameters(pnh, parameters_); + + bool subscribeScan2d = false; + bool subscribeScan3d = false; + pnh.param("subscribe_scan", subscribeScan2d, subscribeScan2d); + pnh.param("subscribe_scan_cloud", subscribeScan3d, subscribeScan3d); + if((subscribeScan2d || subscribeScan3d) && parameters_.find(Parameters::kGridFromDepth()) == parameters_.end()) + { + NODELET_WARN("Setting \"%s\" parameter to false (default true) as \"subscribe_scan\" or \"subscribe_scan_cloud\" is " + "true. The occupancy grid map will be constructed from " + "laser scans. To get occupancy grid map from cloud projection, set \"%s\" " + "to true. To suppress this warning, " + "add ", + Parameters::kGridFromDepth().c_str(), + Parameters::kGridFromDepth().c_str(), + Parameters::kGridFromDepth().c_str()); + parameters_.insert(ParametersPair(Parameters::kGridFromDepth(), "false")); + } + + // modify default parameters with those in the database + if(!deleteDbOnStart) + { + ParametersMap dbParameters; + rtabmap::DBDriver * driver = rtabmap::DBDriver::create(); + if(driver->openConnection(databasePath_)) + { + dbParameters = driver->getLastParameters(); // parameter migration is already done + } + delete driver; + for(ParametersMap::iterator iter=dbParameters.begin(); iter!=dbParameters.end(); ++iter) + { + if(iter->first.compare(Parameters::kRtabmapWorkingDirectory()) == 0) + { + // ignore working directory + continue; + } + if(parameters_.find(iter->first) == parameters_.end() && + allParameters.find(iter->first) != allParameters.end() && + allParameters.find(iter->first)->second.compare(iter->second) !=0) + { + NODELET_INFO("Update RTAB-Map parameter \"%s\"=\"%s\" from database", iter->first.c_str(), iter->second.c_str()); + parameters_.insert(*iter); + } + } + } + + // Add all other parameters (not copied if already exists) + parameters_.insert(allParameters.begin(), allParameters.end()); + // set public parameters nh.setParam("is_rtabmap_paused", paused_); for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter) @@ -362,57 +391,47 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) if(parameters_.find(Parameters::kRtabmapDetectionRate()) != parameters_.end()) { Parameters::parse(parameters_, Parameters::kRtabmapDetectionRate(), rate_); - ROS_INFO("RTAB-Map detection rate = %f Hz", rate_); + NODELET_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) - { - // RGBD SLAM - if(!subscribeDepth && !subscribeStereo) - { - ROS_WARN("ROS param subscribe_depth and subscribe_stereo are false, but RTAB-Map " - "parameter \"RGBD/Enabled\" is true! Please set subscribe_depth or subscribe_stereo " - "to true to use rtabmap node for RGB-D SLAM, or set \"RGBD/Enabled\" to false for loop closure " - "detection on images-only."); + NODELET_INFO("Create intermediate nodes"); } } if(paused_) { - ROS_WARN("Node paused... don't forget to call service \"resume\" to start rtabmap."); + NODELET_WARN("Node paused... don't forget to call service \"resume\" to start rtabmap."); } if(deleteDbOnStart) { if(UFile::erase(databasePath_) == 0) { - ROS_INFO("rtabmap: Deleted database \"%s\" (--delete_db_on_start is set).", databasePath_.c_str()); + NODELET_INFO("rtabmap: Deleted database \"%s\" (--delete_db_on_start is set).", databasePath_.c_str()); } } if(databasePath_.size()) { - ROS_INFO("rtabmap: Using database from \"%s\".", databasePath_.c_str()); + NODELET_INFO("rtabmap: Using database from \"%s\".", databasePath_.c_str()); } else { - ROS_INFO("rtabmap: database_path parameter not set, the map will not be saved."); + NODELET_INFO("rtabmap: database_path parameter not set, the map will not be saved."); } + mapsManager_.setParameters(parameters_); + // Init RTAB-Map rtabmap_.init(parameters_, databasePath_); if(databasePath_.size() && rtabmap_.getMemory()) { - ROS_INFO("rtabmap: Database version = \"%s\".", rtabmap_.getMemory()->getDatabaseVersion().c_str()); + NODELET_INFO("rtabmap: Database version = \"%s\".", rtabmap_.getMemory()->getDatabaseVersion().c_str()); } // setup services @@ -424,7 +443,8 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) backupDatabase_ = nh.advertiseService("backup", &CoreWrapper::backupDatabaseCallback, this); setModeLocalizationSrv_ = nh.advertiseService("set_mode_localization", &CoreWrapper::setModeLocalizationCallback, this); setModeMappingSrv_ = nh.advertiseService("set_mode_mapping", &CoreWrapper::setModeMappingCallback, this); - getMapDataSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this); + getMapDataSrv_ = nh.advertiseService("get_map_data", &CoreWrapper::getMapDataCallback, this); + getMapSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this); getGridMapSrv_ = nh.advertiseService("get_grid_map", &CoreWrapper::getGridMapCallback, this); getProjMapSrv_ = nh.advertiseService("get_proj_map", &CoreWrapper::getProjMapCallback, this); publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this); @@ -444,12 +464,11 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) setLogWarnSrv_ = pnh.advertiseService("log_warning", &CoreWrapper::setLogWarn, this); setLogErrorSrv_ = pnh.advertiseService("log_error", &CoreWrapper::setLogError, this); - setupCallbacks(subscribeDepth, subscribeScan2d, subscribeScan3d, subscribeStereo, queueSize, approxSync, depthCameras); - int optimizeIterations = 0; Parameters::parse(parameters_, Parameters::kOptimizerIterations(), optimizeIterations); if(publishTf && optimizeIterations != 0) { + tfThreadRunning_ = true; transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay, tfTolerance)); } else if(publishTf) @@ -457,67 +476,38 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) 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::kOptimizerIterations().c_str(), mapFrameId_.c_str()); } + + setupCallbacks(nh, pnh, getName()); // do it at the end + if(!this->isDataSubscribed()) + { + bool isRGBD = uStr2Bool(parameters_.at(Parameters::kRGBDEnabled()).c_str()); + if(isRGBD) + { + NODELET_WARN("ROS param subscribe_depth, subscribe_stereo and subscribe_rgbd are false, but RTAB-Map " + "parameter \"%s\" is true! Please set subscribe_depth, subscribe_stereo or subscribe_rgbd " + "to true to use rtabmap node for RGB-D SLAM, or set \"%s\" to false for loop closure " + "detection on images-only.", Parameters::kRGBDEnabled().c_str(), Parameters::kRGBDEnabled().c_str()); + } + + ros::NodeHandle rgb_nh(nh, "rgb"); + ros::NodeHandle rgb_pnh(pnh, "rgb"); + image_transport::ImageTransport rgb_it(rgb_nh); + image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); + defaultSub_ = rgb_it.subscribe("image", 1, &CoreWrapper::defaultCallback, this); + + NODELET_INFO("\n%s subscribed to:\n %s", getName().c_str(), defaultSub_.getTopic().c_str()); + } } CoreWrapper::~CoreWrapper() { if(transformThread_) { + tfThreadRunning_ = false; transformThread_->join(); delete transformThread_; } - if(depthSync_) - delete depthSync_; - if(depthExactSync_) - delete depthExactSync_; - if(depthScanSync_) - delete depthScanSync_; - if(depthScan3dSync_) - delete depthScan3dSync_; - if(stereoScanSync_) - delete stereoScanSync_; - if(stereoScan3dSync_) - delete stereoScan3dSync_; - if(stereoApproxSync_) - delete stereoApproxSync_; - if(stereoExactSync_) - delete stereoExactSync_; - if(depth2Sync_) - delete depth2Sync_; - if(depthTFSync_) - delete depthTFSync_; - if(depthTFExactSync_) - delete depthTFExactSync_; - if(depthScanTFSync_) - delete depthScanTFSync_; - if(depthScan3dTFSync_) - delete depthScan3dTFSync_; - if(stereoScanTFSync_) - delete stereoScanTFSync_; - if(stereoScan3dTFSync_) - delete stereoScan3dTFSync_; - if(stereoApproxTFSync_) - delete stereoApproxTFSync_; - if(stereoExactTFSync_) - delete stereoExactTFSync_; - - for(unsigned int i=0; isaveParameters(configPath_); ros::NodeHandle nh; @@ -530,21 +520,17 @@ CoreWrapper::~CoreWrapper() printf("rtabmap: Saving database/long-term memory... (located at %s)\n", databasePath_.c_str()); } -ParametersMap CoreWrapper::loadParameters(const std::string & configFile) +void CoreWrapper::loadParameters(const std::string & configFile, ParametersMap & parameters) { - ParametersMap parameters = Parameters::getDefaultParameters(); if(!configFile.empty()) { - ROS_INFO("Loading parameters from %s", configFile.c_str()); + NODELET_INFO("Loading parameters from %s", configFile.c_str()); if(!UFile::exists(configFile.c_str())) { - ROS_WARN("Config file doesn't exist! It will be generated..."); + NODELET_WARN("Config file doesn't exist! It will be generated..."); } Parameters::readINI(configFile.c_str(), parameters); } - // otherwise take default parameters - - return parameters; } void CoreWrapper::saveParameters(const std::string & configFile) @@ -561,7 +547,7 @@ void CoreWrapper::saveParameters(const std::string & configFile) } else { - ROS_INFO("Parameters are not saved! (No configuration file provided...)"); + NODELET_INFO("Parameters are not saved! (No configuration file provided...)"); } } @@ -570,7 +556,7 @@ void CoreWrapper::publishLoop(double tfDelay, double tfTolerance) if(tfDelay == 0) return; ros::Rate r(1.0 / tfDelay); - while(ros::ok()) + while(tfThreadRunning_) { if(!odomFrameId_.empty()) { @@ -606,7 +592,7 @@ void CoreWrapper::defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg) imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || imageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)) { - ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8"); + NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8"); return; } @@ -627,7 +613,7 @@ void CoreWrapper::defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg) { if(!rtabmap_.process(ptrImage->image.clone(), ptrImage->header.seq)) { - ROS_WARN("RTAB-Map could not process the data received! (ROS id = %d)", ptrImage->header.seq); + NODELET_WARN("RTAB-Map could not process the data received! (ROS id = %d)", ptrImage->header.seq); } else { @@ -636,13 +622,13 @@ void CoreWrapper::defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg) } else if(!rtabmap_.isIDsGenerated()) { - ROS_WARN("Ignoring received image because its sequence ID=0. Please " + NODELET_WARN("Ignoring received image because its sequence ID=0. Please " "set \"Mem/GenerateIds\"=\"true\" to ignore ros generated sequence id. " "Use only \"Mem/GenerateIds\"=\"false\" for once-time run of RTAB-Map and " "when you need to have IDs output of RTAB-map synchronised with the source " "image sequence ID."); } - ROS_INFO("rtabmap: Update rate=%fs, Limit=%fs, Processing time = %fs (%d local nodes)", + NODELET_INFO("rtabmap: Update rate=%fs, Limit=%fs, Processing time = %fs (%d local nodes)", 1.0f/rate_, rtabmap_.getTimeThreshold()/1000.0f, timer.ticks(), @@ -650,7 +636,7 @@ void CoreWrapper::defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg) } } -bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg) +bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg) { if(!paused_) { @@ -670,16 +656,28 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg) // Only update variance if odom is not null if(!odom.isNull()) { - // using MIN in case of 3DoF mapping (maybe not parameters are set, except x and yaw for the twist) + // using MIN in case of 3DoF mapping (maybe no parameters are set, except x and yaw for the twist) float transVariance = uMax3(odomMsg->twist.covariance[0], MIN(odomMsg->twist.covariance[7], BAD_COVARIANCE), MIN(odomMsg->twist.covariance[14], BAD_COVARIANCE)); float rotVariance = uMax3(MIN(odomMsg->twist.covariance[21],BAD_COVARIANCE), MIN(odomMsg->twist.covariance[28], BAD_COVARIANCE), odomMsg->twist.covariance[35]); - if(uIsFinite(rotVariance) && rotVariance > rotVariance_) + + if(transVariance == BAD_COVARIANCE) { - rotVariance_ = rotVariance; + //use the one of the pose + transVariance = uMax3(odomMsg->pose.covariance[0]/2.0, MIN(odomMsg->pose.covariance[7]/2.0, BAD_COVARIANCE), MIN(odomMsg->pose.covariance[14]/2.0, BAD_COVARIANCE)); } - if(uIsFinite(transVariance) && transVariance > transVariance_) + if(rotVariance == BAD_COVARIANCE) { - transVariance_ = transVariance; + //use the one of the pose + rotVariance = uMax3(MIN(odomMsg->pose.covariance[21]/2.0,BAD_COVARIANCE), MIN(odomMsg->pose.covariance[28]/2.0, BAD_COVARIANCE), odomMsg->pose.covariance[35]/2.0); + } + + if(uIsFinite(rotVariance) && rotVariance != 1.0f) + { + rotVariance_ += rotVariance; + } + if(uIsFinite(transVariance) && transVariance != 1.0f) + { + transVariance_ += transVariance; } } @@ -715,12 +713,12 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg) return false; } -bool CoreWrapper::commonOdomTFUpdate(const ros::Time & stamp) +bool CoreWrapper::odomTFUpdate(const ros::Time & stamp) { if(!paused_) { // Odom TF ready? - Transform odom = getTransform(odomFrameId_, frameId_, stamp); + Transform odom = rtabmap_ros::getTransform(odomFrameId_, frameId_, stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0); if(odom.isNull()) { return false; @@ -769,316 +767,187 @@ bool CoreWrapper::commonOdomTFUpdate(const ros::Time & stamp) return false; } -Transform CoreWrapper::getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const -{ - // TF ready? - Transform transform; - try - { - if(waitForTransform_ && !stamp.isZero() && waitForTransformDuration_>0.0) - { - //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 seconds (for stamp=%f)!", - fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_, stamp.toSec()); - return transform; - } - } - - tf::StampedTransform tmp; - tfListener_.lookupTransform(fromFrameId, toFrameId, stamp, tmp); - transform = rtabmap_ros::transformFromTF(tmp); - } - catch(tf::TransformException & ex) - { - ROS_WARN("%s",ex.what()); - } - return transform; -} - void CoreWrapper::commonDepthCallback( - const std::string & odomFrameId, - const sensor_msgs::ImageConstPtr& imageMsg, - const sensor_msgs::ImageConstPtr& depthMsg, - const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, - const sensor_msgs::LaserScanConstPtr& scanMsg, - const sensor_msgs::PointCloud2ConstPtr& scan3dMsg) -{ - std::vector imageMsgs; - std::vector depthMsgs; - std::vector cameraInfoMsgs; - imageMsgs.push_back(imageMsg); - depthMsgs.push_back(depthMsg); - cameraInfoMsgs.push_back(cameraInfoMsg); - 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 nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const std::vector & imageMsgs, + const std::vector & depthMsgs, + const std::vector & cameraInfoMsgs, const sensor_msgs::LaserScanConstPtr& scan2dMsg, - const sensor_msgs::PointCloud2ConstPtr& scan3dMsg) + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) { - UASSERT(imageMsgs.size()>0 && - imageMsgs.size() == depthMsgs.size() && - imageMsgs.size() == cameraInfoMsgs.size()); - - //for sync transform - Transform odomT = getTransform(odomFrameId, frameId_, lastPoseStamp_); - if(odomT.isNull() && !odomFrameId_.empty()) + std::string odomFrameId = odomFrameId_; + if(odomMsg.get()) { - ROS_WARN("Could not get TF transform from %s to %s, sensors will not be synchronized with odometry pose.", - odomFrameId.c_str(), frameId_.c_str()); + odomFrameId = odomMsg->header.frame_id; + if(!odomUpdate(odomMsg)) + { + return; + } } + else if(scan2dMsg.get()) + { + if(!odomTFUpdate(scan2dMsg->header.stamp)) + { + return; + } + } + else if(scan3dMsg.get()) + { + if(!odomTFUpdate(scan3dMsg->header.stamp)) + { + return; + } + } + else + { + if(imageMsgs.size() == 0 || imageMsgs[0].get() == 0 || !odomTFUpdate(imageMsgs[0]->header.stamp)) + { + return; + } + } + commonDepthCallbackImpl(odomFrameId, + userDataMsg, + imageMsgs, + depthMsgs, + cameraInfoMsgs, + scan2dMsg, + scan3dMsg, + odomInfoMsg); +} - int imageWidth = imageMsgs[0]->width; - int imageHeight = imageMsgs[0]->height; - int cameraCount = imageMsgs.size(); +void CoreWrapper::commonDepthCallbackImpl( + const std::string & odomFrameId, + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const std::vector & imageMsgs, + const std::vector & depthMsgs, + const std::vector & cameraInfoMsgs, + const sensor_msgs::LaserScanConstPtr& scan2dMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ cv::Mat rgb; cv::Mat depth; - pcl::PointCloud scanCloud2d; - std::vector cameraModels; - int genMaxScanPts = 0; - for(unsigned int i=0; i cameraModels; + if(!rtabmap_ros::convertRGBDMsgs( + imageMsgs, + depthMsgs, + cameraInfoMsgs, + frameId_, + odomSensorSync_?odomFrameId:"", + lastPoseStamp_, + rgb, + depth, + cameraModels, + tfListener_, + waitForTransform_?waitForTransformDuration_:0.0)) { - if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || - !(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 || - depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 || - depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)) - { - ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16"); - return; - } + NODELET_ERROR("Could not convert rgb/depth msgs! Aborting rtabmap update..."); + return; + } - UASSERT_MSG(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight, - uFormat("imageWidth=%d vs %d imageHeight=%d vs %d", - imageWidth, - imageMsgs[i]->width, - imageHeight, - imageMsgs[i]->height).c_str()); - UASSERT_MSG(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight, - uFormat("imageWidth=%d vs %d imageHeight=%d vs %d", - imageWidth, - depthMsgs[i]->width, - imageHeight, - depthMsgs[i]->height).c_str()); - - Transform localTransform = getTransform(frameId_, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp); - if(localTransform.isNull()) + UASSERT(uContains(parameters_, rtabmap::Parameters::kMemSaveDepth16Format())); + if(depth.type() == CV_32FC1 && uStr2Bool(parameters_.at(Parameters::kMemSaveDepth16Format()))) + { + depth = rtabmap::util2d::cvtDepthFromFloat(depth); + static bool shown = false; + if(!shown) { - 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 - if(lastPoseStamp_ != depthMsgs[i]->header.stamp) - { - if(!odomT.isNull()) - { - Transform sensorT = getTransform(odomFrameId, frameId_, depthMsgs[i]->header.stamp); - if(sensorT.isNull()) - { - 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; - } - } - } - - cv_bridge::CvImageConstPtr ptrImage; - if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0) - { - ptrImage = cv_bridge::toCvShare(imageMsgs[i]); - } - else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8"); - } - else - { - ptrImage = cv_bridge::toCvShare(imageMsgs[i], "bgr8"); - } - cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]); - cv::Mat subDepth = ptrDepth->image; - UASSERT(uContains(parameters_, Parameters::kMemSaveDepth16Format())); - if(subDepth.type() == CV_32FC1 && uStr2Bool(parameters_.at(Parameters::kMemSaveDepth16Format()))) - { - subDepth = util2d::cvtDepthFromFloat(subDepth); - static bool shown = false; - if(!shown) - { - ROS_WARN("Save depth data to 16 bits format: depth type detected is " - "32FC1, use 16UC1 depth format to avoid this conversion " - "(or set parameter \"Mem/SaveDepth16Format=false\" to use " - "32bits format). This message is only printed once..."); - shown = true; - } - } - - // initialize - if(rgb.empty()) - { - rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type()); - } - if(depth.empty()) - { - depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type()); - } - - if(ptrImage->image.type() == rgb.type()) - { - ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight))); - } - else - { - ROS_ERROR("Some RGB images are not the same type!"); - return; - } - - if(subDepth.type() == depth.type()) - { - subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight))); - } - else - { - ROS_ERROR("Some Depth images are not the same type!"); - return; - } - - cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*cameraInfoMsgs[i], localTransform)); - - if(scan2dMsg.get() == 0 && genScan_) - { - scanCloud2d += util3d::laserScanFromDepthImage( - subDepth, - cameraModels.back().fx(), - cameraModels.back().fy(), - cameraModels.back().cx(), - cameraModels.back().cy(), - genScanMaxDepth_, - genScanMinDepth_, - localTransform); - genMaxScanPts += subDepth.cols; + NODELET_WARN("Save depth data to 16 bits format: depth type detected is " + "32FC1, use 16UC1 depth format to avoid this conversion " + "(or set parameter \"Mem/SaveDepth16Format=false\" to use " + "32bits format). This message is only printed once..."); + shown = true; } } cv::Mat scan; - if(scan2dMsg.get() != 0) + Transform scanLocalTransform = Transform::getIdentity(); + pcl::PointCloud scanCloud2d; + bool genMaxScanPts = 0; + if(scan2dMsg.get() == 0 && scan3dMsg.get() == 0 && genScan_) { - // make sure the frame of the laser is updated too - if(getTransform(frameId_, - scan2dMsg->header.frame_id, - scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)).isNull()) + scanCloud2d = util3d::laserScanFromDepthImages( + depth, + cameraModels, + genScanMaxDepth_, + genScanMinDepth_); + genMaxScanPts += depth.cols; + scan = util3d::laserScan2dFromPointCloud(scanCloud2d); + } + else if(scan2dMsg.get() != 0) + { + if(!rtabmap_ros::convertScanMsg( + scan2dMsg, + frameId_, + odomSensorSync_?odomFrameId:"", + lastPoseStamp_, + scan, + scanLocalTransform, + tfListener_, + waitForTransform_?waitForTransformDuration_:0)) { - ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting rtabmap update.", scan2dMsg->header.stamp.toSec()); + NODELET_ERROR("Could not convert laser scan msg! Aborting rtabmap update..."); return; } - - //transform in frameId_ frame - sensor_msgs::PointCloud2 scanOut; - laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_); - pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(scanOut, *pclScan); - - // sync with odometry stamp - if(lastPoseStamp_ != scan2dMsg->header.stamp) - { - if(!odomT.isNull()) - { - Transform sensorT = getTransform(odomFrameId, frameId_, scan2dMsg->header.stamp); - if(sensorT.isNull()) - { - 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); - } - - } - } - scan = util3d::laserScan2dFromPointCloud(*pclScan); - if(flipScan_) + Transform zAxis(0,0,1,0,0,0); + if((scanLocalTransform.rotation()*zAxis).z() < 0) { cv::Mat flipScan; cv::flip(scan, flipScan, 1); scan = flipScan; } + if(rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0) + { + // backward compatibility, project 2D scan in /base_link frame + scan = util3d::transformLaserScan(scan, scanLocalTransform); + scanLocalTransform = Transform::getIdentity(); + } } else if(scan3dMsg.get() != 0) { - bool containNormals = false; - for(unsigned int i=0; ifields.size(); ++i) + if(!rtabmap_ros::convertScan3dMsg( + scan3dMsg, + frameId_, + odomSensorSync_?odomFrameId:"", + lastPoseStamp_, + scanCloudNormalK_, + scan, + scanLocalTransform, + tfListener_, + waitForTransform_?waitForTransformDuration_:0)) { - if(scan3dMsg->fields[i].name.compare("normal_x") == 0) - { - containNormals = true; - break; - } - } - if(containNormals) - { - pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(*scan3dMsg, *pclScan); - scan = util3d::laserScanFromPointCloud(*pclScan); - } - else - { - pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(*scan3dMsg, *pclScan); - - if(scanCloudNormalK_ > 0) - { - //compute normals - pcl::PointCloud::Ptr normals = util3d::computeNormals(pclScan, scanCloudNormalK_); - pcl::PointCloud::Ptr pclScanNormal(new pcl::PointCloud); - pcl::concatenateFields(*pclScan, *normals, *pclScanNormal); - scan = util3d::laserScanFromPointCloud(*pclScanNormal); - } - else - { - scan = util3d::laserScanFromPointCloud(*pclScan); - } + NODELET_ERROR("Could not convert 3d laser scan msg! Aborting rtabmap update..."); + return; } } - else if(scanCloud2d.size()) - { - scan = util3d::laserScan2dFromPointCloud(scanCloud2d); - } - - 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); + groundTruthPose = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, tfListener_, waitForTransform_?waitForTransformDuration_:0.0); } + cv::Mat userData; + if(userDataMsg.get()) + { + userData = rtabmap_ros::userDataFromROS(*userDataMsg); + } SensorData data(scan, - scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():(genScan_?genMaxScanPts:scan3dMsg.get() != 0?scanCloudMaxPoints_:0), - scan2dMsg.get() != 0?scan2dMsg->range_max:(genScan_?genScanMaxDepth_:0.0f), + LaserScanInfo( + scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():(genScan_?genMaxScanPts:scan3dMsg.get() != 0?scanCloudMaxPoints_:0), + scan2dMsg.get() != 0?scan2dMsg->range_max:(genScan_?genScanMaxDepth_:0.0f), + scanLocalTransform), rgb, depth, cameraModels, lastPoseIntermediate_?-1:imageMsgs[0]->header.seq, - rtabmap_ros::timestampFromROS(stamp)); + rtabmap_ros::timestampFromROS(lastPoseStamp_), + userData); data.setGroundTruth(groundTruthPose); - process(stamp, + process(lastPoseStamp_, data, lastPose_, odomFrameId, @@ -1089,184 +958,182 @@ void CoreWrapper::commonDepthCallback( } void CoreWrapper::commonStereoCallback( - const std::string & odomFrameId, + const nav_msgs::OdometryConstPtr & odomMsg, const sensor_msgs::ImageConstPtr& leftImageMsg, const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, const sensor_msgs::LaserScanConstPtr& scan2dMsg, - const sensor_msgs::PointCloud2ConstPtr& scan3dMsg) + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) { - if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 || - leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || - !(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 || - rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - rightImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)) + std::string odomFrameId = odomFrameId_; + if(odomMsg.get()) { - ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8"); - return; - } - - //for sync transform - Transform odomT = getTransform(odomFrameId, frameId_, lastPoseStamp_); - if(odomT.isNull() && !odomFrameId_.empty()) - { - ROS_WARN("Could not get TF transform from %s to %s, sensors will not be synchronized with odometry pose.", - odomFrameId.c_str(), frameId_.c_str()); - } - - Transform localTransform = getTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp); - if(localTransform.isNull()) - { - return; - } - - // sync with odometry stamp - if(lastPoseStamp_ != leftImageMsg->header.stamp) - { - if(!odomT.isNull()) + odomFrameId = odomMsg->header.frame_id; + if(!odomUpdate(odomMsg)) { - Transform sensorT = getTransform(odomFrameId, frameId_, leftImageMsg->header.stamp); - if(sensorT.isNull()) - { - return; - } - localTransform = odomT.inverse() * sensorT * localTransform; + return; } } + else if(scan2dMsg.get()) + { + if(!odomTFUpdate(scan2dMsg->header.stamp)) + { + return; + } + } + else if(scan3dMsg.get()) + { + if(!odomTFUpdate(scan3dMsg->header.stamp)) + { + return; + } + } + else + { + if(leftImageMsg.get() == 0 || !odomTFUpdate(leftImageMsg->header.stamp)) + { + return; + } + } + + cv::Mat left; + cv::Mat right; + StereoCameraModel stereoModel; + if(!rtabmap_ros::convertStereoMsg( + leftImageMsg, + rightImageMsg, + leftCamInfoMsg, + rightCamInfoMsg, + frameId_, + odomSensorSync_?odomFrameId:"", + lastPoseStamp_, + left, + right, + stereoModel, + tfListener_, + waitForTransform_?waitForTransformDuration_:0.0)) + { + NODELET_ERROR("Could not convert stereo msgs! Aborting rtabmap update..."); + return; + } + + if(stereoToDepth_) + { + // cv::stereoBM() see "$ rosrun rtabmap_ros rtabmap --params | grep StereoBM" for parameters + cv::Mat disparity = rtabmap::util2d::disparityFromStereoImages( + left, + right, + parameters_); + if(disparity.empty()) + { + NODELET_ERROR("Could not compute disparity image (\"stereo_to_depth\" is true)!"); + return; + } + cv::Mat depth = rtabmap::util2d::depthFromDisparity( + disparity, + stereoModel.left().fx(), + stereoModel.baseline()); + + if(depth.empty()) + { + NODELET_ERROR("Could not compute depth image (\"stereo_to_depth\" is true)!"); + return; + } + UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1); + + // move to common depth callback + cv_bridge::CvImagePtr imgDepth(new cv_bridge::CvImage); + if(depth.type() == CV_16UC1) + { + imgDepth->encoding = sensor_msgs::image_encodings::TYPE_16UC1; + } + else // CV_32FC1 + { + imgDepth->encoding = sensor_msgs::image_encodings::TYPE_32FC1; + } + imgDepth->image = depth; + imgDepth->header = leftImageMsg->header; + std::vector rgbImages(1); + std::vector depthImages(1); + std::vector cameraInfos(1); + rgbImages[0] = cv_bridge::toCvShare(leftImageMsg); + depthImages[0] = imgDepth; + cameraInfos[0] = *leftCamInfoMsg; + + commonDepthCallbackImpl(odomFrameId, rtabmap_ros::UserDataConstPtr(), rgbImages, depthImages, cameraInfos, scan2dMsg, scan3dMsg, odomInfoMsg); + return; + } cv::Mat scan; + Transform scanLocalTransform = Transform::getIdentity(); if(scan2dMsg.get() != 0) { - // make sure the frame of the laser is updated too - if(getTransform(frameId_, - scan2dMsg->header.frame_id, - scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)).isNull()) + if(!rtabmap_ros::convertScanMsg( + scan2dMsg, + frameId_, + odomSensorSync_?odomFrameId:"", + lastPoseStamp_, + scan, + scanLocalTransform, + tfListener_, + waitForTransform_?waitForTransformDuration_:0)) { - ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting rtabmap update.", scan2dMsg->header.stamp.toSec()); + NODELET_ERROR("Could not convert laser scan msg! Aborting rtabmap update..."); return; } - - //transform in frameId_ frame - sensor_msgs::PointCloud2 scanOut; - laser_geometry::LaserProjection projection; - //projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfBuffer_); - projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_); - pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(scanOut, *pclScan); - - // sync with odometry stamp - if(lastPoseStamp_ != scan2dMsg->header.stamp) - { - if(!odomT.isNull()) - { - Transform sensorT = getTransform(odomFrameId, frameId_, scan2dMsg->header.stamp); - if(sensorT.isNull()) - { - return; - } - Transform t = odomT.inverse() * sensorT; - pclScan = util3d::transformPointCloud(pclScan, t); - - } - } - - scan = util3d::laserScan2dFromPointCloud(*pclScan); - if(flipScan_) + Transform zAxis(0,0,1,0,0,0); + if((scanLocalTransform.rotation()*zAxis).z() < 0) { cv::Mat flipScan; cv::flip(scan, flipScan, 1); scan = flipScan; } + if(rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0) + { + // backward compatibility, project 2D scan in /base_link frame + scan = util3d::transformLaserScan(scan, scanLocalTransform); + scanLocalTransform = Transform::getIdentity(); + } } else if(scan3dMsg.get() != 0) { - bool containNormals = false; - for(unsigned int i=0; ifields.size(); ++i) + if(!rtabmap_ros::convertScan3dMsg( + scan3dMsg, + frameId_, + odomSensorSync_?odomFrameId:"", + lastPoseStamp_, + scanCloudNormalK_, + scan, + scanLocalTransform, + tfListener_, + waitForTransform_?waitForTransformDuration_:0)) { - if(scan3dMsg->fields[i].name.compare("normal_x") == 0) - { - containNormals = true; - break; - } - } - if(containNormals) - { - pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(*scan3dMsg, *pclScan); - scan = util3d::laserScanFromPointCloud(*pclScan); - } - else - { - pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(*scan3dMsg, *pclScan); - - if(scanCloudNormalK_ > 0) - { - //compute normals - pcl::PointCloud::Ptr normals = util3d::computeNormals(pclScan, scanCloudNormalK_); - pcl::PointCloud::Ptr pclScanNormal(new pcl::PointCloud); - pcl::concatenateFields(*pclScan, *normals, *pclScanNormal); - scan = util3d::laserScanFromPointCloud(*pclScanNormal); - } - else - { - scan = util3d::laserScanFromPointCloud(*pclScan); - } + NODELET_ERROR("Could not convert 3d laser scan msg! Aborting rtabmap update..."); + return; } } - 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::toCvCopy(leftImageMsg, "mono8"); - } - else - { - ptrLeftImage = cv_bridge::toCvCopy(leftImageMsg, "bgr8"); - } - ptrRightImage = cv_bridge::toCvCopy(rightImageMsg, "mono8"); - - rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform); - - if(stereoModel.baseline() > 10.0) - { - static bool shown = false; - if(!shown) - { - ROS_WARN("Detected baseline (%f m) is quite large! Is your " - "right camera_info P(0,3) correctly set? Note that " - "baseline=-P(0,3)/P(0,0). This warning is printed only once.", - stereoModel.baseline()); - shown = true; - } - } - - 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); + groundTruthPose = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, tfListener_, waitForTransform_?waitForTransformDuration_:0.0); } SensorData data(scan, - scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():scan3dMsg.get() != 0?scanCloudMaxPoints_:0, - scan2dMsg.get() != 0?scan2dMsg->range_max:0, - ptrLeftImage->image, - ptrRightImage->image, + LaserScanInfo( + scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():scan3dMsg.get() != 0?scanCloudMaxPoints_:0, + scan2dMsg.get() != 0?scan2dMsg->range_max:0, + scanLocalTransform), + left, + right, stereoModel, lastPoseIntermediate_?-1:leftImageMsg->header.seq, - rtabmap_ros::timestampFromROS(stamp)); + rtabmap_ros::timestampFromROS(lastPoseStamp_)); data.setGroundTruth(groundTruthPose); - process(stamp, + process(lastPoseStamp_, data, lastPose_, odomFrameId, @@ -1277,210 +1144,6 @@ void CoreWrapper::commonStereoCallback( transVariance_ = 0; } -void CoreWrapper::depthCallback( - const sensor_msgs::ImageConstPtr& imageMsg, - const nav_msgs::OdometryConstPtr & odomMsg, - const sensor_msgs::ImageConstPtr& depthMsg, - const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg) -{ - if(!commonOdomUpdate(odomMsg)) - { - return; - } - - sensor_msgs::LaserScanConstPtr scanMsg; // Null - sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null - commonDepthCallback(odomMsg->header.frame_id, imageMsg, depthMsg, cameraInfoMsg, scanMsg, scan3dMsg); -} -void CoreWrapper::depthScanCallback( - const sensor_msgs::ImageConstPtr& imageMsg, - const nav_msgs::OdometryConstPtr & odomMsg, - const sensor_msgs::ImageConstPtr& depthMsg, - const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, - const sensor_msgs::LaserScanConstPtr& scanMsg) -{ - if(!commonOdomUpdate(odomMsg)) - { - return; - } - 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, - const sensor_msgs::ImageConstPtr& rightImageMsg, - const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, - const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, - const nav_msgs::OdometryConstPtr & odomMsg) -{ - if(!commonOdomUpdate(odomMsg)) - { - return; - } - - sensor_msgs::LaserScanConstPtr scanMsg; // Null - sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null - commonStereoCallback(odomMsg->header.frame_id, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg); -} -void CoreWrapper::stereoScanCallback( - 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 nav_msgs::OdometryConstPtr & odomMsg) -{ - if(!commonOdomUpdate(odomMsg)) - { - return; - } - 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( - const nav_msgs::OdometryConstPtr & odomMsg, - const sensor_msgs::ImageConstPtr& image1Msg, - const sensor_msgs::ImageConstPtr& depth1Msg, - const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg, - const sensor_msgs::ImageConstPtr& image2Msg, - const sensor_msgs::ImageConstPtr& depth2Msg, - const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg) -{ - if(!commonOdomUpdate(odomMsg)) - { - return; - } - - std::vector imageMsgs; - std::vector depthMsgs; - std::vector cameraInfoMsgs; - imageMsgs.push_back(image1Msg); - imageMsgs.push_back(image2Msg); - depthMsgs.push_back(depth1Msg); - depthMsgs.push_back(depth2Msg); - cameraInfoMsgs.push_back(cameraInfo1Msg); - cameraInfoMsgs.push_back(cameraInfo2Msg); - - sensor_msgs::LaserScanConstPtr scanMsg; // Null - sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null - commonDepthCallback(odomMsg->header.frame_id, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg); -} - - -void CoreWrapper::depthTFCallback( - const sensor_msgs::ImageConstPtr& imageMsg, - const sensor_msgs::ImageConstPtr& depthMsg, - const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg) -{ - if(!commonOdomTFUpdate(depthMsg->header.stamp)) - { - return; - } - sensor_msgs::LaserScanConstPtr scanMsg; // Null - sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null - commonDepthCallback(odomFrameId_, imageMsg, depthMsg, cameraInfoMsg, scanMsg, scan3dMsg); -} -void CoreWrapper::depthScanTFCallback( - const sensor_msgs::ImageConstPtr& imageMsg, - const sensor_msgs::ImageConstPtr& depthMsg, - const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, - const sensor_msgs::LaserScanConstPtr& scanMsg) -{ - if(!commonOdomTFUpdate(scanMsg->header.stamp)) - { - return; - } - 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, - const sensor_msgs::ImageConstPtr& rightImageMsg, - const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, - const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg) -{ - if(!commonOdomTFUpdate(leftImageMsg->header.stamp)) - { - return; - } - - sensor_msgs::LaserScanConstPtr scanMsg; // null - sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null - commonStereoCallback(odomFrameId_, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg); -} -void CoreWrapper::stereoScanTFCallback( - 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) -{ - if(!commonOdomTFUpdate(leftImageMsg->header.stamp)) - { - return; - } - 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( const ros::Time & stamp, const SensorData & data, @@ -1505,7 +1168,7 @@ void CoreWrapper::process( if(data.id() < 0) { - ROS_INFO("Intermediate node added"); + NODELET_INFO("Intermediate node added"); } else { @@ -1513,12 +1176,20 @@ void CoreWrapper::process( this->publishStats(stamp); std::map filteredPoses = rtabmap_.getLocalOptimizedPoses(); - // create a tmp signature with latest sensory data + // create a tmp signature with latest sensory data if latest signature was ignored 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)); + if(rtabmap_.getMemory() == 0 || + filteredPoses.size() == 0 || + rtabmap_.getMemory()->getLastSignatureId() != filteredPoses.rbegin()->first || + rtabmap_.getMemory()->getLastWorkingSignature() == 0 || + rtabmap_.getMemory()->getLastWorkingSignature()->sensorData().gridCellSize() == 0 || + (!mapsManager_.getOccupancyGrid()->isGridFromDepth() && data.laserScanRaw().channels() == 2)) // 2d laser scan would fill empty space for latest data + { + 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( @@ -1526,9 +1197,6 @@ void CoreWrapper::process( rtabmap_.getMemory(), false, false, - false, - false, - false, tmpSignature); timeUpdateMaps = timer.ticks(); @@ -1543,11 +1211,11 @@ void CoreWrapper::process( if(rtabmap_.getPathStatus() > 0) { // Goal reached - ROS_INFO("Planning: Publishing goal reached!"); + NODELET_INFO("Planning: Publishing goal reached!"); } else { - ROS_WARN("Planning: Plan failed!"); + NODELET_WARN("Planning: Plan failed!"); if(mbClient_.isServerConnected()) { mbClient_.cancelGoal(); @@ -1589,7 +1257,7 @@ void CoreWrapper::process( } else { - ROS_ERROR("Planning: Local map broken, current goal id=%d (the robot may have moved to far from planned nodes)", + NODELET_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()) @@ -1611,7 +1279,8 @@ void CoreWrapper::process( { timeRtabmap = timer.ticks(); } - ROS_INFO("rtabmap: Rate=%.2fs, Limit=%.3fs, RTAB-Map=%.4fs, Maps update=%.4fs pub=%.4fs (local map=%d, WM=%d)", + NODELET_INFO("rtabmap (%d): Rate=%.2fs, Limit=%.3fs, RTAB-Map=%.4fs, Maps update=%.4fs pub=%.4fs (local map=%d, WM=%d)", + rtabmap_.getLastLocationId(), rate_>0?1.0f/rate_:0, rtabmap_.getTimeThreshold()/1000.0f, timeRtabmap, @@ -1622,7 +1291,7 @@ void CoreWrapper::process( } else if(!rtabmap_.isIDsGenerated()) { - ROS_WARN("Ignoring received image because its sequence ID=0. Please " + NODELET_WARN("Ignoring received image because its sequence ID=0. Please " "set \"Mem/GenerateIds\"=\"true\" to ignore ros generated sequence id. " "Use only \"Mem/GenerateIds\"=\"false\" for once-time run of RTAB-Map and " "when you need to have IDs output of RTAB-map synchronised with the source " @@ -1646,11 +1315,11 @@ void CoreWrapper::goalCommonCallback( if(id > 0) { - ROS_INFO("Planning: set goal %d", id); + NODELET_INFO("Planning: set goal %d", id); } else if(!pose.isNull()) { - ROS_INFO("Planning: set goal %s", pose.prettyPrint().c_str()); + NODELET_INFO("Planning: set goal %s", pose.prettyPrint().c_str()); } if(planningTime) @@ -1666,14 +1335,14 @@ void CoreWrapper::goalCommonCallback( { *planningTime = timer.elapsed(); } - ROS_INFO("Planning: Time computing path = %f s", timer.ticks()); + NODELET_INFO("Planning: Time computing path = %f s", timer.ticks()); const std::vector > & poses = rtabmap_.getPath(); currentMetricGoal_.setNull(); latestNodeWasReached_ = false; if(poses.size() == 0) { - ROS_WARN("Planning: Goal already reached (RGBD/GoalReachedRadius=%fm).", + NODELET_WARN("Planning: Goal already reached (RGBD/GoalReachedRadius=%fm).", rtabmap_.getGoalReachedRadius()); rtabmap_.clearPath(1); if(goalReachedPub_.getNumSubscribers()) @@ -1689,7 +1358,7 @@ void CoreWrapper::goalCommonCallback( currentMetricGoal_ = rtabmap_.getPose(rtabmap_.getPathCurrentGoalId()); if(!currentMetricGoal_.isNull()) { - ROS_INFO("Planning: Path successfully created (size=%d)", (int)poses.size()); + NODELET_INFO("Planning: Path successfully created (size=%d)", (int)poses.size()); // Adjust the target pose relative to last node if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size()) @@ -1715,26 +1384,33 @@ void CoreWrapper::goalCommonCallback( } stream << iter->first; } - ROS_INFO("Global path: [%s]", stream.str().c_str()); + NODELET_INFO("Global path: [%s]", stream.str().c_str()); success=true; } else { - ROS_ERROR("Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathCurrentGoalId()); + NODELET_ERROR("Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathCurrentGoalId()); } } } else if(!label.empty()) { - ROS_ERROR("Planning: Node with label \"%s\" not found!", label.c_str()); + NODELET_ERROR("Planning: Node with label \"%s\" not found!", label.c_str()); } else if(pose.isNull()) { - ROS_ERROR("Planning: Node id should be > 0 !"); + if(id > 0) + { + NODELET_ERROR("Planning: Could not plan to node %d! The node is not in map's graph (look for warnings before this message for more details).", id); + } + else + { + NODELET_ERROR("Planning: Node id should be > 0 !"); + } } else { - ROS_ERROR("Planning: A node near the goal's pose not found! The pose may be to far from the graph (RGBD/LocalRadius=%f m)", rtabmap_.getLocalRadius()); + NODELET_ERROR("Planning: A node near the goal's pose not found! The pose may be to far from the graph (RGBD/LocalRadius=%f m)", rtabmap_.getLocalRadius()); } if(!success) @@ -1754,17 +1430,17 @@ void CoreWrapper::goalCallback(const geometry_msgs::PoseStampedConstPtr & msg) Transform targetPose = rtabmap_ros::transformFromPoseMsg(msg->pose); if(targetPose.isNull()) { - ROS_ERROR("Pose received is null!"); + NODELET_ERROR("Pose received is null!"); return; } // transform goal in /map frame if(mapFrameId_.compare(msg->header.frame_id) != 0) { - Transform t = this->getTransform(mapFrameId_, msg->header.frame_id, msg->header.stamp); + Transform t = rtabmap_ros::getTransform(mapFrameId_, msg->header.frame_id, msg->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0); if(t.isNull()) { - ROS_ERROR("Cannot transform goal pose from \"%s\" frame to \"%s\" frame!", + NODELET_ERROR("Cannot transform goal pose from \"%s\" frame to \"%s\" frame!", msg->header.frame_id.c_str(), mapFrameId_.c_str()); return; } @@ -1778,7 +1454,7 @@ void CoreWrapper::goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg) { if(msg->node_id <= 0 && msg->node_label.empty()) { - ROS_ERROR("Node id or label should be set!"); + NODELET_ERROR("Node id or label should be set!"); return; } goalCommonCallback(msg->node_id, msg->node_label, Transform(), msg->header.stamp); @@ -1795,38 +1471,39 @@ bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Emp double vDouble; if(nh.getParam(iter->first, vStr)) { - ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str()); + NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str()); iter->second = vStr; } else if(nh.getParam(iter->first, vBool)) { - ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str()); + NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str()); iter->second = uBool2Str(vBool); } else if(nh.getParam(iter->first, vInt)) { - ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str()); + NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str()); iter->second = uNumber2Str(vInt).c_str(); } else if(nh.getParam(iter->first, vDouble)) { - ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str()); + NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str()); iter->second = uNumber2Str(vDouble).c_str(); } } - ROS_INFO("rtabmap: Updating parameters"); + NODELET_INFO("rtabmap: Updating parameters"); if(parameters_.find(Parameters::kRtabmapDetectionRate()) != parameters_.end()) { rate_ = uStr2Float(parameters_.at(Parameters::kRtabmapDetectionRate())); - ROS_INFO("RTAB-Map rate detection = %f Hz", rate_); + NODELET_INFO("RTAB-Map rate detection = %f Hz", rate_); } rtabmap_.parseParameters(parameters_); + mapsManager_.setParameters(parameters_); return true; } bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { - ROS_INFO("rtabmap: Reset"); + NODELET_INFO("rtabmap: Reset"); rtabmap_.resetMemory(); rotVariance_ = 0; transVariance_ = 0; @@ -1843,12 +1520,12 @@ bool CoreWrapper::pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt { if(paused_) { - ROS_WARN("rtabmap: Already paused!"); + NODELET_WARN("rtabmap: Already paused!"); } else { paused_ = true; - ROS_INFO("rtabmap: paused!"); + NODELET_INFO("rtabmap: paused!"); ros::NodeHandle nh; nh.setParam("is_rtabmap_paused", true); } @@ -1859,12 +1536,12 @@ bool CoreWrapper::resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Emp { if(!paused_) { - ROS_WARN("rtabmap: Already running!"); + NODELET_WARN("rtabmap: Already running!"); } else { paused_ = false; - ROS_INFO("rtabmap: resumed!"); + NODELET_INFO("rtabmap: resumed!"); ros::NodeHandle nh; nh.setParam("is_rtabmap_paused", false); } @@ -1873,16 +1550,16 @@ bool CoreWrapper::resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Emp bool CoreWrapper::triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { - ROS_INFO("rtabmap: Trigger new map"); + NODELET_INFO("rtabmap: Trigger new map"); rtabmap_.triggerNewMap(); return true; } bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { - ROS_INFO("Backup: Saving memory..."); + NODELET_INFO("Backup: Saving memory..."); rtabmap_.close(); - ROS_INFO("Backup: Saving memory... done!"); + NODELET_INFO("Backup: Saving memory... done!"); rotVariance_ = 0; transVariance_ = 0; @@ -1890,69 +1567,73 @@ bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Em currentMetricGoal_.setNull(); latestNodeWasReached_ = false; - ROS_INFO("Backup: Saving \"%s\" to \"%s\"...", databasePath_.c_str(), (databasePath_+".back").c_str()); + NODELET_INFO("Backup: Saving \"%s\" to \"%s\"...", databasePath_.c_str(), (databasePath_+".back").c_str()); UFile::copy(databasePath_, databasePath_+".back"); - ROS_INFO("Backup: Saving \"%s\" to \"%s\"... done!", databasePath_.c_str(), (databasePath_+".back").c_str()); + NODELET_INFO("Backup: Saving \"%s\" to \"%s\"... done!", databasePath_.c_str(), (databasePath_+".back").c_str()); - ROS_INFO("Backup: Reloading memory..."); + NODELET_INFO("Backup: Reloading memory..."); rtabmap_.init(parameters_, databasePath_); - ROS_INFO("Backup: Reloading memory... done!"); + NODELET_INFO("Backup: Reloading memory... done!"); return true; } bool CoreWrapper::setModeLocalizationCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { - ROS_INFO("rtabmap: Set localization mode"); + NODELET_INFO("rtabmap: Set localization mode"); rtabmap::ParametersMap parameters; parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false")); + ros::NodeHandle & nh = getNodeHandle(); + nh.setParam(rtabmap::Parameters::kMemIncrementalMemory(), "false"); rtabmap_.parseParameters(parameters); return true; } bool CoreWrapper::setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { - ROS_INFO("rtabmap: Set mapping mode"); + NODELET_INFO("rtabmap: Set mapping mode"); rtabmap::ParametersMap parameters; parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "true")); + ros::NodeHandle & nh = getNodeHandle(); + nh.setParam(rtabmap::Parameters::kMemIncrementalMemory(), "true"); rtabmap_.parseParameters(parameters); return true; } bool CoreWrapper::setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { - ROS_INFO("rtabmap: Set log level to Debug"); + NODELET_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"); + NODELET_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"); + NODELET_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"); + NODELET_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) +bool CoreWrapper::getMapDataCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res) { - ROS_INFO("rtabmap: Getting map (global=%s optimized=%s graphOnly=%s)...", + NODELET_INFO("rtabmap: Getting map (global=%s optimized=%s graphOnly=%s)...", req.global?"true":"false", req.optimized?"true":"false", req.graphOnly?"true":"false"); std::map signatures; std::map poses; - std::multimap constraints; + std::multimap constraints; if(req.graphOnly) { @@ -1988,65 +1669,41 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros: bool CoreWrapper::getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res) { - std::map filteredPoses; - filteredPoses = mapsManager_.updateMapCaches( - rtabmap_.getLocalOptimizedPoses(), - rtabmap_.getMemory(), - false, - true, - false, - false, - false); - if(filteredPoses.size()) + if(parameters_.find(Parameters::kGridFromDepth()) != parameters_.end() && + !uStr2Bool(parameters_.at(Parameters::kGridFromDepth()))) { - // create the projection map - float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; - cv::Mat pixels = mapsManager_.generateProjMap(filteredPoses, xMin, yMin, gridCellSize); - - if(!pixels.empty()) - { - //init - res.map.info.resolution = gridCellSize; - res.map.info.origin.position.x = 0.0; - res.map.info.origin.position.y = 0.0; - res.map.info.origin.position.z = 0.0; - res.map.info.origin.orientation.x = 0.0; - res.map.info.origin.orientation.y = 0.0; - res.map.info.origin.orientation.z = 0.0; - res.map.info.origin.orientation.w = 1.0; - - res.map.info.width = pixels.cols; - res.map.info.height = pixels.rows; - res.map.info.origin.position.x = xMin; - res.map.info.origin.position.y = yMin; - res.map.data.resize(res.map.info.width * res.map.info.height); - - memcpy(res.map.data.data(), pixels.data, res.map.info.width * res.map.info.height); - - res.map.header.frame_id = mapFrameId_; - res.map.header.stamp = ros::Time::now(); - return true; - } + NODELET_WARN("/get_proj_map service is deprecated! Call /get_grid_map service " + "instead with . " + "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see " + "all occupancy grid parameters.", + Parameters::kGridFromDepth().c_str()); } - return false; + else + { + NODELET_WARN("/get_proj_map service is deprecated! Call /get_grid_map service instead."); + } + return getGridMapCallback(req, res); } bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res) +{ + NODELET_WARN("/get_grid_map service is deprecated! Call /get_map service instead."); + return getMapCallback(req, res); +} + +bool CoreWrapper::getMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res) { std::map filteredPoses; filteredPoses = mapsManager_.updateMapCaches( rtabmap_.getLocalOptimizedPoses(), rtabmap_.getMemory(), - false, - false, true, - false, false); if(filteredPoses.size()) { // create the grid map float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; - cv::Mat pixels = mapsManager_.generateProjMap(filteredPoses, xMin, yMin, gridCellSize); + cv::Mat pixels = mapsManager_.generateGridMap(filteredPoses, xMin, yMin, gridCellSize); if(!pixels.empty()) { @@ -2078,14 +1735,14 @@ bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs:: bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtabmap_ros::PublishMap::Response& res) { - ROS_INFO("rtabmap: Publishing map..."); + NODELET_INFO("rtabmap: Publishing map..."); if(mapDataPub_.getNumSubscribers() || (!req.graphOnly && mapsManager_.hasSubscribers()) || (req.graphOnly && labelsPub_.getNumSubscribers())) { std::map poses; - std::multimap constraints; + std::multimap constraints; std::map signatures; if(req.graphOnly) @@ -2109,7 +1766,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab if(poses.size() && poses.size() != signatures.size()) { - ROS_WARN("poses and signatures are not the same size!? %d vs %d", (int)poses.size(), (int)signatures.size()); + NODELET_WARN("poses and signatures are not the same size!? %d vs %d", (int)poses.size(), (int)signatures.size()); } ros::Time now = ros::Time::now(); @@ -2152,9 +1809,6 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab rtabmap_.getMemory(), false, false, - false, - false, - false, signatures); } else @@ -2270,7 +1924,7 @@ bool CoreWrapper::cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Em { if(rtabmap_.getPath().size()) { - ROS_WARN("Goal cancelled!"); + NODELET_WARN("Goal cancelled!"); rtabmap_.clearPath(0); currentMetricGoal_.setNull(); latestNodeWasReached_ = false; @@ -2295,22 +1949,22 @@ bool CoreWrapper::setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ { if(req.node_id > 0) { - ROS_INFO("Set label \"%s\" to node %d", req.node_label.c_str(), req.node_id); + NODELET_INFO("Set label \"%s\" to node %d", req.node_label.c_str(), req.node_id); } else { - ROS_INFO("Set label \"%s\" to last node", req.node_label.c_str()); + NODELET_INFO("Set label \"%s\" to last node", req.node_label.c_str()); } } else { if(req.node_id > 0) { - ROS_ERROR("Could not set label \"%s\" to node %d", req.node_label.c_str(), req.node_id); + NODELET_ERROR("Could not set label \"%s\" to node %d", req.node_label.c_str(), req.node_id); } else { - ROS_ERROR("Could not set label \"%s\" to last node", req.node_label.c_str()); + NODELET_ERROR("Could not set label \"%s\" to last node", req.node_label.c_str()); } } return true; @@ -2322,7 +1976,7 @@ bool CoreWrapper::listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtab { std::map labels = rtabmap_.getMemory()->getAllLabels(); res.labels = uValues(labels); - ROS_INFO("List labels service: %d labels found.", (int)res.labels.size()); + NODELET_INFO("List labels service: %d labels found.", (int)res.labels.size()); } return true; } @@ -2334,7 +1988,7 @@ void CoreWrapper::publishStats(const ros::Time & stamp) if(infoPub_.getNumSubscribers()) { - //ROS_INFO("Sending RtabmapInfo msg (last_id=%d)...", stat.refImageId()); + //NODELET_INFO("Sending RtabmapInfo msg (last_id=%d)...", stat.refImageId()); rtabmap_ros::InfoPtr msg(new rtabmap_ros::Info); msg->header.stamp = stamp; msg->header.frame_id = mapFrameId_; @@ -2457,7 +2111,7 @@ void CoreWrapper::publishCurrentGoal(const ros::Time & stamp) { if(!currentMetricGoal_.isNull()) { - ROS_INFO("Publishing next goal: %d -> %s", + NODELET_INFO("Publishing next goal: %d -> %s", rtabmap_.getPathCurrentGoalId(), currentMetricGoal_.prettyPrint().c_str()); geometry_msgs::PoseStamped poseMsg; @@ -2469,7 +2123,7 @@ void CoreWrapper::publishCurrentGoal(const ros::Time & stamp) { if(!mbClient_.isServerConnected()) { - ROS_INFO("Connecting to move_base action server..."); + NODELET_INFO("Connecting to move_base action server..."); mbClient_.waitForServer(ros::Duration(5.0)); } if(mbClient_.isServerConnected()) @@ -2484,7 +2138,7 @@ void CoreWrapper::publishCurrentGoal(const ros::Time & stamp) } else { - ROS_ERROR("Cannot connect to move_base action server!"); + NODELET_ERROR("Cannot connect to move_base action server!"); } } if(nextMetricGoalPub_.getNumSubscribers()) @@ -2507,19 +2161,19 @@ void CoreWrapper::goalDoneCb(const actionlib::SimpleClientGoalState& state, rtabmap_.getPathCurrentGoalId() != rtabmap_.getPath().back().first && (!uContains(rtabmap_.getLocalOptimizedPoses(), rtabmap_.getPath().back().first) || !latestNodeWasReached_)) { - ROS_WARN("Planning: move_base reached current goal but it is not " + NODELET_WARN("Planning: move_base reached current goal but it is not " "the last one planned by rtabmap. A new goal should be sent when " "rtabmap will be able to retrieve next locations on the path."); ignore = true; } else { - ROS_INFO("Planning: move_base success!"); + NODELET_INFO("Planning: move_base success!"); } } else { - ROS_ERROR("Planning: move_base failed for some reason. Aborting the plan..."); + NODELET_ERROR("Planning: move_base failed for some reason. Aborting the plan..."); } if(!ignore && goalReachedPub_.getNumSubscribers()) @@ -2541,14 +2195,14 @@ void CoreWrapper::goalDoneCb(const actionlib::SimpleClientGoalState& state, // Called once when the goal becomes active void CoreWrapper::goalActiveCb() { - //ROS_INFO("Planning: Goal just went active"); + //NODELET_INFO("Planning: Goal just went active"); } // Called every time feedback is received for the goal void CoreWrapper::goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback) { //Transform basePosition = rtabmap_ros::transformFromPoseMsg(feedback->base_position.pose); - //ROS_INFO("Planning: feedback base_position = %s", basePosition.prettyPrint().c_str()); + //NODELET_INFO("Planning: feedback base_position = %s", basePosition.prettyPrint().c_str()); } void CoreWrapper::publishLocalPath(const ros::Time & stamp) @@ -2615,12 +2269,12 @@ bool CoreWrapper::octomapBinaryCallback( octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res) { - ROS_INFO("Sending binary map data on service request"); + NODELET_INFO("Sending binary map data on service request"); res.map.header.frame_id = mapFrameId_; res.map.header.stamp = ros::Time::now(); std::map poses = rtabmap_.getLocalOptimizedPoses(); - poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, false, false, false, true); + poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, true); const rtabmap::OctoMap * octomap = mapsManager_.getOctomap(); bool success = octomap->octree()->size() && octomap_msgs::binaryMapToMsg(*octomap->octree(), res.map); @@ -2631,12 +2285,12 @@ bool CoreWrapper::octomapFullCallback( octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res) { - ROS_INFO("Sending full map data on service request"); + NODELET_INFO("Sending full map data on service request"); res.map.header.frame_id = mapFrameId_; res.map.header.stamp = ros::Time::now(); std::map poses = rtabmap_.getLocalOptimizedPoses(); - poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, false, false, false, true); + poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, true); const rtabmap::OctoMap * octomap = mapsManager_.getOctomap(); bool success = octomap->octree()->size() && octomap_msgs::fullMapToMsg(*octomap->octree(), res.map); @@ -2645,416 +2299,6 @@ bool CoreWrapper::octomapFullCallback( #endif #endif -/** - * exclusive callbacks: - * image - * image + depth - * image + scan - * image + depth + scan - * Which callback is called depends on - * the combination of these options: - * bool subscribe_laserScan - * bool subscribe_depth - */ -void CoreWrapper::setupCallbacks( - bool subscribeDepth, - bool subscribeScan2d, - bool subscribeScan3d, - bool subscribeStereo, - int queueSize, - bool approxSync, - int depthCameras) -{ - ros::NodeHandle nh; // public - ros::NodeHandle pnh("~"); // private +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::CoreWrapper, nodelet::Nodelet); - if(subscribeDepth) - { - UASSERT(depthCameras >= 1 && depthCameras <= 2); - UASSERT_MSG(depthCameras == 1 || !(subscribeScan2d || subscribeScan3d || !odomFrameId_.empty()), "Not yet supported!"); - - imageSubs_.resize(depthCameras); - imageDepthSubs_.resize(depthCameras); - cameraInfoSubs_.resize(depthCameras); - for(int i=0; i1) - { - rgbPrefix += uNumber2Str(i); - depthPrefix += uNumber2Str(i); - } - ros::NodeHandle rgb_nh(nh, rgbPrefix); - ros::NodeHandle depth_nh(nh, depthPrefix); - ros::NodeHandle rgb_pnh(pnh, rgbPrefix); - ros::NodeHandle depth_pnh(pnh, depthPrefix); - image_transport::ImageTransport rgb_it(rgb_nh); - image_transport::ImageTransport depth_it(depth_nh); - image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); - image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); - - imageSubs_[i] = new image_transport::SubscriberFilter; - imageDepthSubs_[i] = new image_transport::SubscriberFilter; - cameraInfoSubs_[i] = new message_filters::Subscriber; - imageSubs_[i]->subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); - imageDepthSubs_[i]->subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); - cameraInfoSubs_[i]->subscribe(rgb_nh, "camera_info", 1); - } - - if(odomFrameId_.empty()) - { - odomSub_.subscribe(nh, "odom", 1); - if(subscribeScan2d) - { - scanSub_.subscribe(nh, "scan", 1); - depthScanSync_ = new message_filters::Synchronizer( - MyDepthScanSyncPolicy(queueSize), - *imageSubs_[0], - odomSub_, - *imageDepthSubs_[0], - *cameraInfoSubs_[0], - scanSub_); - depthScanSync_->registerCallback(boost::bind(&CoreWrapper::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(subscribeScan3d) - { - 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) - { - depth2Sync_ = new message_filters::Synchronizer( - MyDepth2SyncPolicy(queueSize), - odomSub_, - *imageSubs_[0], - *imageDepthSubs_[0], - *cameraInfoSubs_[0], - *imageSubs_[1], - *imageDepthSubs_[1], - *cameraInfoSubs_[1]); - depth2Sync_->registerCallback(boost::bind(&CoreWrapper::depth2Callback, 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(), - imageSubs_[0]->getTopic().c_str(), - imageDepthSubs_[0]->getTopic().c_str(), - cameraInfoSubs_[0]->getTopic().c_str(), - imageSubs_[1]->getTopic().c_str(), - imageDepthSubs_[1]->getTopic().c_str(), - cameraInfoSubs_[1]->getTopic().c_str(), - odomSub_.getTopic().c_str()); - } - else - { - if(approxSync) - { - depthSync_ = new message_filters::Synchronizer( - MyDepthSyncPolicy(queueSize), - *imageSubs_[0], - odomSub_, - *imageDepthSubs_[0], - *cameraInfoSubs_[0]); - depthSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4)); - } - else - { - depthExactSync_ = new message_filters::Synchronizer( - MyDepthExactSyncPolicy(queueSize), - *imageSubs_[0], - odomSub_, - *imageDepthSubs_[0], - *cameraInfoSubs_[0]); - depthExactSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4)); - } - ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s", - ros::this_node::getName().c_str(), - approxSync?"approx":"exact", - imageSubs_[0]->getTopic().c_str(), - imageDepthSubs_[0]->getTopic().c_str(), - cameraInfoSubs_[0]->getTopic().c_str(), - odomSub_.getTopic().c_str()); - } - } - } - else - { - // use odom from TF, so subscribe to sensors only - if(subscribeScan2d) - { - scanSub_.subscribe(nh, "scan", 1); - depthScanTFSync_ = new message_filters::Synchronizer( - MyDepthScanTFSyncPolicy(queueSize), - *imageSubs_[0], - *imageDepthSubs_[0], - *cameraInfoSubs_[0], - scanSub_); - depthScanTFSync_->registerCallback(boost::bind(&CoreWrapper::depthScanTFCallback, 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(), - 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 - { - if(approxSync) - { - depthTFSync_ = new message_filters::Synchronizer( - MyDepthTFSyncPolicy(queueSize), - *imageSubs_[0], - *imageDepthSubs_[0], - *cameraInfoSubs_[0]); - depthTFSync_->registerCallback(boost::bind(&CoreWrapper::depthTFCallback, this, _1, _2, _3)); - } - else - { - depthTFExactSync_ = new message_filters::Synchronizer( - MyDepthTFExactSyncPolicy(queueSize), - *imageSubs_[0], - *imageDepthSubs_[0], - *cameraInfoSubs_[0]); - depthTFExactSync_->registerCallback(boost::bind(&CoreWrapper::depthTFCallback, this, _1, _2, _3)); - } - ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s", - ros::this_node::getName().c_str(), - approxSync?"approx":"exact", - imageSubs_[0]->getTopic().c_str(), - imageDepthSubs_[0]->getTopic().c_str(), - cameraInfoSubs_[0]->getTopic().c_str()); - } - } - } - else if(subscribeStereo) - { - ros::NodeHandle left_nh(nh, "left"); - ros::NodeHandle right_nh(nh, "right"); - ros::NodeHandle left_pnh(pnh, "left"); - ros::NodeHandle right_pnh(pnh, "right"); - image_transport::ImageTransport left_it(left_nh); - image_transport::ImageTransport right_it(right_nh); - image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh); - image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh); - - imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft); - imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight); - cameraInfoLeft_.subscribe(left_nh, "camera_info", 1); - cameraInfoRight_.subscribe(right_nh, "camera_info", 1); - - if(odomFrameId_.empty()) - { - odomSub_.subscribe(nh, "odom", 1); - if(subscribeScan2d) - { - scanSub_.subscribe(nh, "scan", 1); - stereoScanSync_ = new message_filters::Synchronizer( - MyStereoScanSyncPolicy(queueSize), - imageRectLeft_, - imageRectRight_, - cameraInfoLeft_, - cameraInfoRight_, - scanSub_, - odomSub_); - stereoScanSync_->registerCallback(boost::bind(&CoreWrapper::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(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(approxSync) - { - stereoApproxSync_ = new message_filters::Synchronizer( - MyStereoApproxSyncPolicy(queueSize), - imageRectLeft_, - imageRectRight_, - cameraInfoLeft_, - cameraInfoRight_, - odomSub_); - stereoApproxSync_->registerCallback(boost::bind(&CoreWrapper::stereoCallback, this, _1, _2, _3, _4, _5)); - } - else - { - stereoExactSync_ = new message_filters::Synchronizer( - MyStereoExactSyncPolicy(queueSize), - imageRectLeft_, - imageRectRight_, - cameraInfoLeft_, - cameraInfoRight_, - odomSub_); - stereoExactSync_->registerCallback(boost::bind(&CoreWrapper::stereoCallback, this, _1, _2, _3, _4, _5)); - } - - ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s,\n %s", - ros::this_node::getName().c_str(), - approxSync?"approx":"exact", - imageRectLeft_.getTopic().c_str(), - imageRectRight_.getTopic().c_str(), - cameraInfoLeft_.getTopic().c_str(), - cameraInfoRight_.getTopic().c_str(), - odomSub_.getTopic().c_str()); - } - } - else - { - // use odom from TF, so subscribe to sensors only - if(subscribeScan2d) - { - scanSub_.subscribe(nh, "scan", 1); - stereoScanTFSync_ = new message_filters::Synchronizer( - MyStereoScanTFSyncPolicy(queueSize), - imageRectLeft_, - imageRectRight_, - cameraInfoLeft_, - cameraInfoRight_, - scanSub_); - stereoScanTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoScanTFCallback, 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(), - scanSub_.getTopic().c_str()); - } - else if(subscribeScan3d) - { - 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(approxSync) - { - stereoApproxTFSync_ = new message_filters::Synchronizer( - MyStereoApproxTFSyncPolicy(queueSize), - imageRectLeft_, - imageRectRight_, - cameraInfoLeft_, - cameraInfoRight_); - stereoApproxTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoTFCallback, this, _1, _2, _3, _4)); - } - else - { - stereoExactTFSync_ = new message_filters::Synchronizer( - MyStereoExactTFSyncPolicy(queueSize), - imageRectLeft_, - imageRectRight_, - cameraInfoLeft_, - cameraInfoRight_); - stereoExactTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoTFCallback, this, _1, _2, _3, _4)); - } - - ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s", - ros::this_node::getName().c_str(), - approxSync?"approx":"exact", - imageRectLeft_.getTopic().c_str(), - imageRectRight_.getTopic().c_str(), - cameraInfoLeft_.getTopic().c_str(), - cameraInfoRight_.getTopic().c_str()); - } - } - } - else - { - ros::NodeHandle rgb_nh(nh, "rgb"); - ros::NodeHandle rgb_pnh(pnh, "rgb"); - image_transport::ImageTransport rgb_it(rgb_nh); - image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); - defaultSub_ = rgb_it.subscribe("image", 1, &CoreWrapper::defaultCallback, this); - - ROS_INFO("\n%s subscribed to:\n %s", ros::this_node::getName().c_str(), defaultSub_.getTopic().c_str()); - } } - - - diff --git a/src/CoreWrapper.h b/src/CoreWrapper.h deleted file mode 100644 index e9815ab5..00000000 --- a/src/CoreWrapper.h +++ /dev/null @@ -1,487 +0,0 @@ -/* -Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of the Universite de Sherbrooke nor the - names of its contributors may be used to endorse or promote products - derived from this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY -DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -#ifndef COREWRAPPER_H_ -#define COREWRAPPER_H_ - - -#include - -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "rtabmap_ros/GetMap.h" -#include "rtabmap_ros/ListLabels.h" -#include "rtabmap_ros/PublishMap.h" -#include "rtabmap_ros/SetGoal.h" -#include "rtabmap_ros/SetLabel.h" -#include "rtabmap_ros/Goal.h" - -#include "MapsManager.h" - -#include -#include -#include -#include - -#include -#include - -#ifdef WITH_OCTOMAP_ROS -#include -#endif - -#include -#include -#include -#include -#include -#include -typedef actionlib::SimpleActionClient MoveBaseClient; - -class CoreWrapper -{ -public: - CoreWrapper(bool deleteDbOnStart, const rtabmap::ParametersMap & parameters); - virtual ~CoreWrapper(); - -private: - void setupCallbacks( - bool subscribeDepth, - bool subscribeScan2d, - bool subscribeScan3d, - bool subscribeStereo, - int queueSize, - bool approxSync, - int depthCameras); - void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom - - bool commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg); - bool commonOdomTFUpdate(const ros::Time & stamp); // TF odom - rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const; - - void commonDepthCallback( - const std::string & odomFrameId, - const sensor_msgs::ImageConstPtr& imageMsg, - const sensor_msgs::ImageConstPtr& depthMsg, - const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, - 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::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::PointCloud2ConstPtr& scan3dMsg); - - // with odom msg - void depthCallback( - const sensor_msgs::ImageConstPtr& imageMsg, - const nav_msgs::OdometryConstPtr & odomMsg, - const sensor_msgs::ImageConstPtr& imageDepthMsg, - const sensor_msgs::CameraInfoConstPtr& camInfoMsg); - void depthScanCallback( - const sensor_msgs::ImageConstPtr& imageMsg, - const nav_msgs::OdometryConstPtr & odomMsg, - 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, - const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, - const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, - const nav_msgs::OdometryConstPtr & odomMsg); - void stereoScanCallback( - 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 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, - const sensor_msgs::ImageConstPtr& imageDepth1Msg, - const sensor_msgs::CameraInfoConstPtr& camInfo1Msg, - const sensor_msgs::ImageConstPtr& image2Msg, - const sensor_msgs::ImageConstPtr& imageDept2hMsg, - const sensor_msgs::CameraInfoConstPtr& camInfo2Msg); - - // without odom, when TF is used for odom - void depthTFCallback( - const sensor_msgs::ImageConstPtr& imageMsg, - const sensor_msgs::ImageConstPtr& imageDepthMsg, - const sensor_msgs::CameraInfoConstPtr& camInfoMsg); - void depthScanTFCallback( - const sensor_msgs::ImageConstPtr& imageMsg, - 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, - const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, - const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg); - void stereoScanTFCallback( - 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); - 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, double * planningTime = 0); - void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg); - void goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg); - void updateGoal(const ros::Time & stamp); - - void process( - const ros::Time & stamp, - const rtabmap::SensorData & data, - const rtabmap::Transform & odom = rtabmap::Transform(), - const std::string & odomFrameId = "", - 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&); - bool pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); - bool resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); - bool triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); - 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); - bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&); - bool setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res); - bool cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); - bool setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res); - bool listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtabmap_ros::ListLabels::Response& res); -#ifdef WITH_OCTOMAP_ROS - bool octomapBinaryCallback(octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res); - bool octomapFullCallback(octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res); -#endif - - rtabmap::ParametersMap loadParameters(const std::string & configFile); - void saveParameters(const std::string & configFile); - - void publishLoop(double tfDelay, double tfTolerance); - - void publishStats(const ros::Time & stamp); - void publishCurrentGoal(const ros::Time & stamp); - void goalDoneCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result); - void goalActiveCb(); - void goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback); - void publishLocalPath(const ros::Time & stamp); - void publishGlobalPath(const ros::Time & stamp); - -private: - rtabmap::Rtabmap rtabmap_; - bool paused_; - rtabmap::Transform lastPose_; - ros::Time lastPoseStamp_; - bool lastPoseIntermediate_; - float rotVariance_; - float transVariance_; - rtabmap::Transform currentMetricGoal_; - bool latestNodeWasReached_; - rtabmap::ParametersMap parameters_; - - std::string frameId_; - std::string mapFrameId_; - std::string odomFrameId_; - std::string groundTruthFrameId_; - std::string configPath_; - std::string databasePath_; - bool waitForTransform_; - double waitForTransformDuration_; - bool useActionForGoal_; - bool genScan_; - double genScanMaxDepth_; - double genScanMinDepth_; - int scanCloudMaxPoints_; - int scanCloudNormalK_; - bool flipScan_; - - rtabmap::Transform mapToOdom_; - boost::mutex mapToOdomMutex_; - - MapsManager mapsManager_; - - ros::Publisher infoPub_; - ros::Publisher mapDataPub_; - ros::Publisher mapGraphPub_; - ros::Publisher labelsPub_; - - //Planning stuff - ros::Subscriber goalSub_; - ros::Subscriber goalNodeSub_; - ros::Publisher nextMetricGoalPub_; - ros::Publisher goalReachedPub_; - ros::Publisher globalPathPub_; - ros::Publisher localPathPub_; - - // for loop closure detection only - image_transport::Subscriber defaultSub_; - - //for depth callback - std::vector imageSubs_; - std::vector imageDepthSubs_; - std::vector*> cameraInfoSubs_; - - //stereo callback - image_transport::SubscriberFilter imageRectLeft_; - image_transport::SubscriberFilter imageRectRight_; - message_filters::Subscriber cameraInfoLeft_; - message_filters::Subscriber cameraInfoRight_; - - message_filters::Subscriber odomSub_; - message_filters::Subscriber scanSub_; - message_filters::Subscriber scan3dSub_; - - typedef message_filters::sync_policies::ApproximateTime< - sensor_msgs::Image, - nav_msgs::Odometry, - sensor_msgs::Image, - sensor_msgs::CameraInfo, - 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, - sensor_msgs::Image, - sensor_msgs::CameraInfo> MyDepthSyncPolicy; - message_filters::Synchronizer * depthSync_; - typedef message_filters::sync_policies::ExactTime< - sensor_msgs::Image, - nav_msgs::Odometry, - sensor_msgs::Image, - sensor_msgs::CameraInfo> MyDepthExactSyncPolicy; - message_filters::Synchronizer * depthExactSync_; - - typedef message_filters::sync_policies::ApproximateTime< - sensor_msgs::Image, - sensor_msgs::Image, - sensor_msgs::CameraInfo, - sensor_msgs::CameraInfo, - sensor_msgs::LaserScan, - 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, - sensor_msgs::CameraInfo, - sensor_msgs::CameraInfo, - nav_msgs::Odometry> MyStereoApproxSyncPolicy; - message_filters::Synchronizer * stereoApproxSync_; - - typedef message_filters::sync_policies::ExactTime< - sensor_msgs::Image, - sensor_msgs::Image, - sensor_msgs::CameraInfo, - sensor_msgs::CameraInfo, - nav_msgs::Odometry> MyStereoExactSyncPolicy; - message_filters::Synchronizer * stereoExactSync_; - - typedef message_filters::sync_policies::ApproximateTime< - nav_msgs::Odometry, - sensor_msgs::Image, - sensor_msgs::Image, - sensor_msgs::CameraInfo, - sensor_msgs::Image, - sensor_msgs::Image, - sensor_msgs::CameraInfo> MyDepth2SyncPolicy; - message_filters::Synchronizer * depth2Sync_; - - // without odom, when TF is used for odom - typedef message_filters::sync_policies::ApproximateTime< - sensor_msgs::Image, - sensor_msgs::Image, - sensor_msgs::CameraInfo, - 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, - sensor_msgs::CameraInfo> MyDepthTFSyncPolicy; - message_filters::Synchronizer * depthTFSync_; - typedef message_filters::sync_policies::ExactTime< - sensor_msgs::Image, - sensor_msgs::Image, - sensor_msgs::CameraInfo> MyDepthTFExactSyncPolicy; - message_filters::Synchronizer * depthTFExactSync_; - - typedef message_filters::sync_policies::ApproximateTime< - sensor_msgs::Image, - sensor_msgs::Image, - sensor_msgs::CameraInfo, - sensor_msgs::CameraInfo, - 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, - sensor_msgs::CameraInfo, - sensor_msgs::CameraInfo> MyStereoApproxTFSyncPolicy; - message_filters::Synchronizer * stereoApproxTFSync_; - - typedef message_filters::sync_policies::ExactTime< - sensor_msgs::Image, - sensor_msgs::Image, - sensor_msgs::CameraInfo, - sensor_msgs::CameraInfo> MyStereoExactTFSyncPolicy; - message_filters::Synchronizer * stereoExactTFSync_; - - tf2_ros::TransformBroadcaster tfBroadcaster_; - tf::TransformListener tfListener_; - - ros::ServiceServer updateSrv_; - ros::ServiceServer resetSrv_; - ros::ServiceServer pauseSrv_; - ros::ServiceServer resumeSrv_; - ros::ServiceServer triggerNewMapSrv_; - 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_; - ros::ServiceServer publishMapDataSrv_; - ros::ServiceServer setGoalSrv_; - ros::ServiceServer cancelGoalSrv_; - ros::ServiceServer setLabelSrv_; - ros::ServiceServer listLabelsSrv_; -#ifdef WITH_OCTOMAP_ROS - ros::ServiceServer octomapBinarySrv_; - ros::ServiceServer octomapFullSrv_; -#endif - - MoveBaseClient mbClient_; - - boost::thread* transformThread_; - - float rate_; - bool createIntermediateNodes_; - ros::Time time_; - ros::Time previousStamp_; -}; - -#endif /* COREWRAPPER_H_ */ - diff --git a/src/GuiNode.cpp b/src/GuiNode.cpp index 4190ca3a..99a8c41b 100644 --- a/src/GuiNode.cpp +++ b/src/GuiNode.cpp @@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "GuiWrapper.h" +#include "rtabmap_ros/GuiWrapper.h" #include "rtabmap/utilite/ULogger.h" #include @@ -54,7 +54,7 @@ int main(int argc, char** argv) app = new QApplication(argc, argv); app->connect( app, SIGNAL( lastWindowClosed() ), app, SLOT( quit() ) ); - GuiWrapper * gui = new GuiWrapper(argc, argv); + rtabmap_ros::GuiWrapper * gui = new rtabmap_ros::GuiWrapper(argc, argv); // Catch ctrl-c to close the gui // (Place this after QApplication's constructor) diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp index 5682ae71..48af6efa 100644 --- a/src/GuiWrapper.cpp +++ b/src/GuiWrapper.cpp @@ -25,14 +25,12 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "GuiWrapper.h" +#include "rtabmap_ros/GuiWrapper.h" #include #include -#include #include #include -#include #include #include @@ -54,12 +52,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap_ros/GetMap.h" #include "rtabmap_ros/SetGoal.h" #include "rtabmap_ros/SetLabel.h" - -#include "PreferencesDialogROS.h" - -#include -#include -#include +#include "rtabmap_ros/PreferencesDialogROS.h" float max3( const float& a, const float& b, const float& c) { @@ -67,23 +60,21 @@ float max3( const float& a, const float& b, const float& c) return m>c?m:c; } +namespace rtabmap_ros { + GuiWrapper::GuiWrapper(int & argc, char** argv) : + CommonDataSubscriber(true), mainWindow_(0), frameId_("base_link"), + odomFrameId_(""), waitForTransform_(true), waitForTransformDuration_(0.2), // 200 ms + odomSensorSync_(false), cameraNodeName_(""), - lastOdomInfoUpdateTime_(0), - depthScanSync_(0), - depthSync_(0), - depthOdomInfoSync_(0), - stereoSync_(0), - stereoScanSync_(0), - stereoOdomInfoSync_(0), - depth2Sync_(0), - depthOdomInfo2Sync_(0) + lastOdomInfoUpdateTime_(0) { ros::NodeHandle nh; + ros::NodeHandle pnh("~"); QString configFile = QDir::homePath()+"/.ros/rtabmapGUI.ini"; for(int i=1; isetMonitoringState(paused); - ros::NodeHandle pnh("~"); - // To receive odometry events - bool subscribeLaserScan2d = false; - bool subscribeLaserScan3d = false; - bool subscribeDepth = false; - bool subscribeOdomInfo = false; - bool subscribeStereo = false; - int queueSize = 10; - int depthCameras = 1; std::string tfPrefix; std::string initCachePath; 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); - 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); - pnh.param("queue_size", queueSize, queueSize); pnh.param("tf_prefix", tfPrefix, tfPrefix); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_); + pnh.param("odom_sensor_sync", odomSensorSync_, odomSensorSync_); pnh.param("camera_node_name", cameraNodeName_, cameraNodeName_); // used to pause the rtabmap_ros/camera when pausing the process pnh.param("init_cache_path", initCachePath, initCachePath); if(initCachePath.size()) @@ -179,22 +151,13 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) : } } - this->setupCallbacks( - subscribeDepth, - subscribeLaserScan2d, - subscribeLaserScan3d, - subscribeOdomInfo, - subscribeStereo, - queueSize, - depthCameras); - UEventsManager::addHandler(this); UEventsManager::addHandler(mainWindow_); infoTopic_.subscribe(nh, "info", 1); mapDataTopic_.subscribe(nh, "mapData", 1); infoMapSync_ = new message_filters::Synchronizer( - MyInfoMapSyncPolicy(queueSize), + MyInfoMapSyncPolicy(this->getQueueSize()), infoTopic_, mapDataTopic_); infoMapSync_->registerCallback(boost::bind(&GuiWrapper::infoMapCallback, this, _1, _2)); @@ -202,48 +165,26 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) : goalTopic_.subscribe(nh, "goal_node", 1); pathTopic_.subscribe(nh, "global_path", 1); goalPathSync_ = new message_filters::Synchronizer( - MyGoalPathSyncPolicy(queueSize), + MyGoalPathSyncPolicy(this->getQueueSize()), goalTopic_, pathTopic_); goalPathSync_->registerCallback(boost::bind(&GuiWrapper::goalPathCallback, this, _1, _2)); goalReachedTopic_ = nh.subscribe("goal_reached", 1, &GuiWrapper::goalReachedCallback, this); + + setupCallbacks(nh, pnh, ros::this_node::getName()); // do it at the end + if(!this->isDataSubscribed()) + { + defaultSub_ = nh.subscribe("odom", queueSize_, &GuiWrapper::defaultCallback, this); + + ROS_INFO("\n%s subscribed to:\n %s", + ros::this_node::getName().c_str(), + defaultSub_.getTopic().c_str()); + } } GuiWrapper::~GuiWrapper() { UDEBUG(""); - if(depthSync_) - delete depthSync_; - if(depth2Sync_) - delete depth2Sync_; - if(depthScanSync_) - delete depthScanSync_; - if(depthOdomInfoSync_) - delete depthOdomInfoSync_; - if(depthOdomInfo2Sync_) - delete depthOdomInfo2Sync_; - if(stereoSync_) - delete stereoSync_; - if(stereoScanSync_) - delete stereoScanSync_; - if(stereoOdomInfoSync_) - delete stereoOdomInfoSync_; - - for(unsigned int i=0; i poses; std::map signatures; - std::multimap links; + std::multimap links; rtabmap_ros::mapDataFromROS(*mapMsg, poses, links, signatures, mapToOdom); @@ -408,9 +349,9 @@ void GuiWrapper::handleEvent(UEvent * anEvent) getMapSrv.request.global = cmdEvent->value1().toBool(); getMapSrv.request.optimized = cmdEvent->value2().toBool(); getMapSrv.request.graphOnly = cmdEvent->value3().toBool(); - if(!ros::service::call("get_map", getMapSrv)) + if(!ros::service::call("get_map_data", getMapSrv)) { - ROS_WARN("Can't call \"get_map\" service"); + ROS_WARN("Can't call \"get_map_data\" service"); this->post(new RtabmapEvent3DMap(1)); // service error } else @@ -474,64 +415,17 @@ void GuiWrapper::handleEvent(UEvent * anEvent) } } -Transform GuiWrapper::getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const -{ - // TF ready? - Transform transform; - try - { - if(waitForTransform_ && !stamp.isZero() && waitForTransformDuration_ > 0.0) - { - //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 (for stamp=%f)!", - fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_, stamp.toSec()); - return transform; - } - } - - tf::StampedTransform tmp; - tfListener_.lookupTransform(fromFrameId, toFrameId, stamp, tmp); - transform = rtabmap_ros::transformFromTF(tmp); - } - catch(tf::TransformException & ex) - { - ROS_WARN("%s",ex.what()); - } - return transform; -} - void GuiWrapper::commonDepthCallback( const nav_msgs::OdometryConstPtr & odomMsg, - const sensor_msgs::ImageConstPtr& imageMsg, - const sensor_msgs::ImageConstPtr& depthMsg, - const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const std::vector & imageMsgs, + const std::vector & depthMsgs, + const std::vector & cameraInfoMsgs, const sensor_msgs::LaserScanConstPtr& scan2dMsg, const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) { - std::vector imageMsgs; - std::vector depthMsgs; - std::vector cameraInfoMsgs; - imageMsgs.push_back(imageMsg); - depthMsgs.push_back(depthMsg); - cameraInfoMsgs.push_back(cameraInfoMsg); - commonDepthCallback(odomMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scan2dMsg, scan3dMsg, odomInfoMsg); -} - -void GuiWrapper::commonDepthCallback( - const nav_msgs::OdometryConstPtr & odomMsg, - const std::vector & imageMsgs, - const std::vector & depthMsgs, - const std::vector & cameraInfoMsgs, - const sensor_msgs::LaserScanConstPtr& scan2dMsg, - const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, - const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) -{ - UASSERT(imageMsgs.size()>0 && - imageMsgs.size() == depthMsgs.size() && - imageMsgs.size() == cameraInfoMsgs.size()); + UASSERT(imageMsgs.size() == 0 || (imageMsgs.size() == depthMsgs.size() && imageMsgs.size() == cameraInfoMsgs.size())); std_msgs::Header odomHeader; if(odomMsg.get()) @@ -548,9 +442,9 @@ void GuiWrapper::commonDepthCallback( { odomHeader = scan3dMsg->header; } - else if(cameraInfoMsgs.size() && cameraInfoMsgs[0].get()) + else if(cameraInfoMsgs.size()) { - odomHeader = cameraInfoMsgs[0]->header; + odomHeader = cameraInfoMsgs[0].header; } else if(depthMsgs.size() && depthMsgs[0].get()) { @@ -563,19 +457,19 @@ void GuiWrapper::commonDepthCallback( odomHeader.frame_id = odomFrameId_; } - Transform odomT = getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp); + Transform odomT = rtabmap_ros::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0); cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); if(odomMsg.get()) { - UASSERT(odomMsg->pose.covariance.size() == 36); - if(!(odomMsg->pose.covariance[0] == 0 && - odomMsg->pose.covariance[7] == 0 && - odomMsg->pose.covariance[14] == 0 && - odomMsg->pose.covariance[21] == 0 && - odomMsg->pose.covariance[28] == 0 && - odomMsg->pose.covariance[35] == 0)) + UASSERT(odomMsg->twist.covariance.size() == 36); + if(odomMsg->twist.covariance[0] != 0 && + odomMsg->twist.covariance[7] != 0 && + odomMsg->twist.covariance[14] != 0 && + odomMsg->twist.covariance[21] != 0 && + odomMsg->twist.covariance[28] != 0 && + odomMsg->twist.covariance[35] != 0) { - covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->pose.covariance.data()).clone(); + covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->twist.covariance.data()).clone(); } } if(odomHeader.frame_id.empty()) @@ -588,6 +482,7 @@ void GuiWrapper::commonDepthCallback( cv::Mat depth; std::vector cameraModels; cv::Mat scan; + Transform scanLocalTransform = Transform::getIdentity(); rtabmap::OdometryInfo info; bool ignoreData = false; @@ -597,136 +492,58 @@ void GuiWrapper::commonDepthCallback( { lastOdomInfoUpdateTime_ = UTimer::now(); - if(imageMsgs[0].get() && depthMsgs[0].get()) + if(imageMsgs.size() && imageMsgs[0].get() && depthMsgs[0].get()) { - int imageWidth = imageMsgs[0]->width; - int imageHeight = imageMsgs[0]->height; - int cameraCount = imageMsgs.size(); - pcl::PointCloud scanCloud; - for(unsigned int i=0; iencoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || - !(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 || - depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 || - depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)) - { - ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16"); - return; - } - UASSERT(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight); - UASSERT(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight); - - Transform localTransform = getTransform(frameId_, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp); - if(localTransform.isNull()) - { - return; - } - // sync with odometry stamp - if(odomHeader.stamp != depthMsgs[i]->header.stamp) - { - if(!odomT.isNull()) - { - Transform sensorT = getTransform(odomHeader.frame_id, frameId_, depthMsgs[i]->header.stamp); - if(sensorT.isNull()) - { - return; - } - localTransform = odomT.inverse() * sensorT * localTransform; - } - } - - cv_bridge::CvImageConstPtr ptrImage; - if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0) - { - ptrImage = cv_bridge::toCvShare(imageMsgs[i]); - } - else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8"); - } - else - { - ptrImage = cv_bridge::toCvShare(imageMsgs[i], "bgr8"); - } - cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]); - cv::Mat subDepth = ptrDepth->image; - - // initialize - if(rgb.empty()) - { - rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type()); - } - if(depth.empty()) - { - depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type()); - } - - if(ptrImage->image.type() == rgb.type()) - { - ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight))); - } - else - { - ROS_ERROR("Some RGB images are not the same type!"); - return; - } - - if(subDepth.type() == depth.type()) - { - subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight))); - } - else - { - ROS_ERROR("Some Depth images are not the same type!"); - return; - } - - cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*cameraInfoMsgs[i], localTransform)); + ROS_ERROR("Could not convert rgb/depth msgs! Aborting rtabmapviz update..."); + return; } } if(scan2dMsg.get() != 0) { - // make sure the frame of the laser is updated too - if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull()) + if(!rtabmap_ros::convertScanMsg( + scan2dMsg, + frameId_, + odomSensorSync_?odomHeader.frame_id:"", + odomHeader.stamp, + scan, + scanLocalTransform, + tfListener_, + waitForTransform_?waitForTransformDuration_:0)) { + ROS_ERROR("Could not convert laser scan msg! Aborting rtabmapviz update..."); return; } - - //transform in frameId_ frame - sensor_msgs::PointCloud2 scanOut; - laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_); - pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(scanOut, *pclScan); - - // sync with odometry stamp - if(odomHeader.stamp != scan2dMsg->header.stamp) - { - if(!odomT.isNull()) - { - Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scan2dMsg->header.stamp); - if(sensorT.isNull()) - { - return; - } - Transform t = odomT.inverse() * sensorT; - pclScan = util3d::transformPointCloud(pclScan, t); - - } - } - scan = util3d::laserScanFromPointCloud(*pclScan); } else if(scan3dMsg.get() != 0) { - pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(*scan3dMsg, *pclScan); - scan = util3d::laserScanFromPointCloud(*pclScan); + if(!rtabmap_ros::convertScan3dMsg( + scan3dMsg, + frameId_, + odomSensorSync_?odomHeader.frame_id:"", + odomHeader.stamp, + 0, + scan, + scanLocalTransform, + tfListener_, + waitForTransform_?waitForTransformDuration_:0)) + { + ROS_ERROR("Could not convert 3d laser scan msg! Aborting rtabmapviz update..."); + return; + } } if(odomInfoMsg.get()) @@ -749,8 +566,10 @@ void GuiWrapper::commonDepthCallback( rtabmap::OdometryEvent odomEvent( rtabmap::SensorData( scan, - scan2dMsg.get()?(int)scan2dMsg->ranges.size():0, - scan2dMsg.get()?(int)scan2dMsg->range_max:0, + LaserScanInfo( + scan2dMsg.get()?(int)scan2dMsg->ranges.size():0, + scan2dMsg.get()?(int)scan2dMsg->range_max:0, + scanLocalTransform), rgb, depth, cameraModels, @@ -773,22 +592,6 @@ void GuiWrapper::commonStereoCallback( const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) { - UASSERT(leftImageMsg.get() && rightImageMsg.get()); - UASSERT(leftCamInfoMsg.get() && rightCamInfoMsg.get()); - - if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 || - leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || - !(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 || - rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - rightImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)) - { - ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8"); - return; - } - std_msgs::Header odomHeader; if(odomMsg.get()) { @@ -811,19 +614,19 @@ void GuiWrapper::commonStereoCallback( odomHeader.frame_id = odomFrameId_; } - Transform odomT = getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp); + Transform odomT = rtabmap_ros::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0); cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); if(odomMsg.get()) { - UASSERT(odomMsg->pose.covariance.size() == 36); - if(!(odomMsg->pose.covariance[0] == 0 && - odomMsg->pose.covariance[7] == 0 && - odomMsg->pose.covariance[14] == 0 && - odomMsg->pose.covariance[21] == 0 && - odomMsg->pose.covariance[28] == 0 && - odomMsg->pose.covariance[35] == 0)) + UASSERT(odomMsg->twist.covariance.size() == 36); + if(odomMsg->twist.covariance[0] != 0 && + odomMsg->twist.covariance[7] != 0 && + odomMsg->twist.covariance[14] != 0 && + odomMsg->twist.covariance[21] != 0 && + odomMsg->twist.covariance[28] != 0 && + odomMsg->twist.covariance[35] != 0) { - covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->pose.covariance.data()).clone(); + covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->twist.covariance.data()).clone(); } } if(odomHeader.frame_id.empty()) @@ -832,28 +635,10 @@ void GuiWrapper::commonStereoCallback( return; } - Transform localTransform = getTransform(frameId_, leftCamInfoMsg->header.frame_id, leftCamInfoMsg->header.stamp); - if(localTransform.isNull()) - { - return; - } - // sync with odometry stamp - if(odomHeader.stamp != leftCamInfoMsg->header.stamp) - { - if(!odomT.isNull()) - { - Transform sensorT = getTransform(odomHeader.frame_id, frameId_, leftCamInfoMsg->header.stamp); - if(sensorT.isNull()) - { - return; - } - localTransform = odomT.inverse() * sensorT * localTransform; - } - } - cv::Mat left; cv::Mat right; cv::Mat scan; + Transform scanLocalTransform = Transform::getIdentity(); rtabmap::StereoCameraModel stereoModel; rtabmap::OdometryInfo info; bool ignoreData = false; @@ -865,73 +650,56 @@ void GuiWrapper::commonStereoCallback( { lastOdomInfoUpdateTime_ = UTimer::now(); - stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform); - - if(stereoModel.baseline() > 10.0) + if(!rtabmap_ros::convertStereoMsg( + leftImageMsg, + rightImageMsg, + leftCamInfoMsg, + rightCamInfoMsg, + frameId_, + odomSensorSync_?odomHeader.frame_id:"", + odomHeader.stamp, + left, + right, + stereoModel, + tfListener_, + waitForTransform_?waitForTransformDuration_:0.0)) { - static bool shown = false; - if(!shown) - { - ROS_WARN("Detected baseline (%f m) is quite large! Is your " - "right camera_info P(0,3) correctly set? Note that " - "baseline=-P(0,3)/P(0,0). This warning is printed only once.", - stereoModel.baseline()); - shown = true; - } + ROS_ERROR("Could not convert stereo msgs! Aborting rtabmapviz update..."); + return; } - // left - cv_bridge::CvImageConstPtr ptrImage; - if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - left = cv_bridge::toCvCopy(leftImageMsg, "mono8")->image; - } - else - { - left = cv_bridge::toCvCopy(leftImageMsg, "bgr8")->image; - } - - // right - right = cv_bridge::toCvCopy(rightImageMsg, "mono8")->image; - if(scan2dMsg.get() != 0) { - // make sure the frame of the laser is updated too - if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull()) + if(!rtabmap_ros::convertScanMsg( + scan2dMsg, + frameId_, + odomSensorSync_?odomHeader.frame_id:"", + odomHeader.stamp, + scan, + scanLocalTransform, + tfListener_, + waitForTransform_?waitForTransformDuration_:0)) { + ROS_ERROR("Could not convert laser scan msg! Aborting rtabmapviz update..."); return; } - - //transform in frameId_ frame - sensor_msgs::PointCloud2 scanOut; - laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_); - pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(scanOut, *pclScan); - - // sync with odometry stamp - if(odomHeader.stamp != scan2dMsg->header.stamp) - { - if(!odomT.isNull()) - { - Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scan2dMsg->header.stamp); - if(sensorT.isNull()) - { - return; - } - 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); + if(!rtabmap_ros::convertScan3dMsg( + scan3dMsg, + frameId_, + odomSensorSync_?odomHeader.frame_id:"", + odomHeader.stamp, + 0, + scan, + scanLocalTransform, + tfListener_, + waitForTransform_?waitForTransformDuration_:0)) + { + ROS_ERROR("Could not convert 3d laser scan msg! Aborting rtabmapviz update..."); + return; + } } if(odomInfoMsg.get()) @@ -954,8 +722,10 @@ void GuiWrapper::commonStereoCallback( rtabmap::OdometryEvent odomEvent( rtabmap::SensorData( scan, - scan2dMsg.get()?(int)scan2dMsg->ranges.size():0, - scan2dMsg.get()?(int)scan2dMsg->range_max:0, + LaserScanInfo( + scan2dMsg.get()?(int)scan2dMsg->ranges.size():0, + scan2dMsg.get()?(int)scan2dMsg->range_max:0, + scanLocalTransform), left, right, stereoModel, @@ -971,1001 +741,15 @@ void GuiWrapper::commonStereoCallback( // With odom msg void GuiWrapper::defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg) { - commonDepthCallback( + this->commonSingleDepthCallback( odomMsg, - sensor_msgs::ImageConstPtr(), - sensor_msgs::ImageConstPtr(), - sensor_msgs::CameraInfoConstPtr(), + rtabmap_ros::UserDataConstPtr(), + cv_bridge::CvImageConstPtr(), + cv_bridge::CvImageConstPtr(), + sensor_msgs::CameraInfo(), sensor_msgs::LaserScanConstPtr(), sensor_msgs::PointCloud2ConstPtr(), rtabmap_ros::OdomInfoConstPtr()); } -void GuiWrapper::depthCallback( - 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(), - sensor_msgs::PointCloud2ConstPtr(), - rtabmap_ros::OdomInfoConstPtr()); } - -void GuiWrapper::depth2Callback( - const nav_msgs::OdometryConstPtr & odomMsg, - const sensor_msgs::ImageConstPtr& image1Msg, - const sensor_msgs::ImageConstPtr& depth1Msg, - const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg, - const sensor_msgs::ImageConstPtr& image2Msg, - const sensor_msgs::ImageConstPtr& depth2Msg, - const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg) -{ - std::vector imageMsgs; - std::vector depthMsgs; - std::vector cameraInfoMsgs; - imageMsgs.push_back(image1Msg); - imageMsgs.push_back(image2Msg); - depthMsgs.push_back(depth1Msg); - depthMsgs.push_back(depth2Msg); - cameraInfoMsgs.push_back(cameraInfo1Msg); - cameraInfoMsgs.push_back(cameraInfo2Msg); - - commonDepthCallback( - odomMsg, - imageMsgs, - depthMsgs, - cameraInfoMsgs, - sensor_msgs::LaserScanConstPtr(), - sensor_msgs::PointCloud2ConstPtr(), - rtabmap_ros::OdomInfoConstPtr()); -} - -void GuiWrapper::depthOdomInfoCallback( - const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, - 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(), - sensor_msgs::PointCloud2ConstPtr(), - odomInfoMsg); -} - -void GuiWrapper::depthOdomInfo2Callback( - const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, - const nav_msgs::OdometryConstPtr & odomMsg, - const sensor_msgs::ImageConstPtr& image1Msg, - const sensor_msgs::ImageConstPtr& depth1Msg, - const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg, - const sensor_msgs::ImageConstPtr& image2Msg, - const sensor_msgs::ImageConstPtr& depth2Msg, - const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg) -{ - std::vector imageMsgs; - std::vector depthMsgs; - std::vector cameraInfoMsgs; - imageMsgs.push_back(image1Msg); - imageMsgs.push_back(image2Msg); - depthMsgs.push_back(depth1Msg); - depthMsgs.push_back(depth2Msg); - cameraInfoMsgs.push_back(cameraInfo1Msg); - cameraInfoMsgs.push_back(cameraInfo2Msg); - - commonDepthCallback( - odomMsg, - imageMsgs, - depthMsgs, - cameraInfoMsgs, - sensor_msgs::LaserScanConstPtr(), - sensor_msgs::PointCloud2ConstPtr(), - odomInfoMsg); -} - -void GuiWrapper::depthScanCallback( - 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(), - 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, - 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(), - 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, - 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(), - sensor_msgs::PointCloud2ConstPtr(), - odomInfoMsg); -} - -void GuiWrapper::stereoCallback( - 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(), - sensor_msgs::PointCloud2ConstPtr(), - rtabmap_ros::OdomInfoConstPtr()); -} - -// With odom TF -void GuiWrapper::depthTFCallback( - 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(), - sensor_msgs::PointCloud2ConstPtr(), - rtabmap_ros::OdomInfoConstPtr()); -} - -void GuiWrapper::depthOdomInfoTFCallback( - const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, - 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(), - sensor_msgs::PointCloud2ConstPtr(), - odomInfoMsg); -} - -void GuiWrapper::depthScanTFCallback( - const sensor_msgs::LaserScanConstPtr& scanMsg, - const sensor_msgs::ImageConstPtr& imageMsg, - const sensor_msgs::ImageConstPtr& depthMsg, - const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg) -{ - commonDepthCallback( - nav_msgs::OdometryConstPtr(), - imageMsg, - 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()); -} - -void GuiWrapper::stereoScanTFCallback( - const sensor_msgs::LaserScanConstPtr& 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, - 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()); -} - -void GuiWrapper::stereoOdomInfoTFCallback( - const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, - 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(), - sensor_msgs::PointCloud2ConstPtr(), - odomInfoMsg); -} - -void GuiWrapper::stereoTFCallback( - 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(), - sensor_msgs::PointCloud2ConstPtr(), - rtabmap_ros::OdomInfoConstPtr()); -} - -void GuiWrapper::setupCallbacks( - bool subscribeDepth, - bool subscribeLaserScan2d, - bool subscribeLaserScan3d, - bool subscribeOdomInfo, - bool subscribeStereo, - int queueSize, - int depthCameras) -{ - ros::NodeHandle nh; // public - ros::NodeHandle pnh("~"); // private - - if(subscribeDepth && subscribeStereo) - { - 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 && (subscribeLaserScan2d || subscribeLaserScan3d)) - { - ROS_WARN("Cannot subscribe to laser scan without depth or stereo subscription..."); - } - if(depthCameras <= 0) - { - depthCameras = 1; - } - if(depthCameras > 2) - { - ROS_WARN("Cannot subscribe to more than 2 cameras yet..."); - depthCameras = 2; - } - - if(subscribeDepth) - { - UASSERT(depthCameras >= 1 && depthCameras <= 2); - UASSERT_MSG(depthCameras == 1 || !(subscribeLaserScan2d || subscribeLaserScan3d || !odomFrameId_.empty()), "Not yet supported!"); - - imageSubs_.resize(depthCameras); - imageDepthSubs_.resize(depthCameras); - cameraInfoSubs_.resize(depthCameras); - for(int i=0; i1) - { - rgbPrefix += uNumber2Str(i); - depthPrefix += uNumber2Str(i); - } - ros::NodeHandle rgb_nh(nh, rgbPrefix); - ros::NodeHandle depth_nh(nh, depthPrefix); - ros::NodeHandle rgb_pnh(pnh, rgbPrefix); - ros::NodeHandle depth_pnh(pnh, depthPrefix); - image_transport::ImageTransport rgb_it(rgb_nh); - image_transport::ImageTransport depth_it(depth_nh); - image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); - image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); - - imageSubs_[i] = new image_transport::SubscriberFilter; - imageDepthSubs_[i] = new image_transport::SubscriberFilter; - cameraInfoSubs_[i] = new message_filters::Subscriber; - imageSubs_[i]->subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); - imageDepthSubs_[i]->subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); - cameraInfoSubs_[i]->subscribe(rgb_nh, "camera_info", 1); - } - - if(odomFrameId_.empty()) - { - odomSub_.subscribe(nh, "odom", 1); - if(subscribeLaserScan2d) - { - scanSub_.subscribe(nh, "scan", 1); - 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,\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) - { - odomInfoSub_.subscribe(nh, "odom_info", 1); - if(depthCameras > 1) - { - depthOdomInfo2Sync_ = new message_filters::Synchronizer( - MyDepthOdomInfo2SyncPolicy(queueSize), - odomInfoSub_, - odomSub_, - *imageSubs_[0], - *imageDepthSubs_[0], - *cameraInfoSubs_[0], - *imageSubs_[1], - *imageDepthSubs_[1], - *cameraInfoSubs_[1]); - depthOdomInfo2Sync_->registerCallback(boost::bind(&GuiWrapper::depthOdomInfo2Callback, this, _1, _2, _3, _4, _5, _6, _7, _8)); - - ROS_INFO("\n%s subscribed to:\n %s,\n %s,\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(), - imageSubs_[1]->getTopic().c_str(), - imageDepthSubs_[1]->getTopic().c_str(), - cameraInfoSubs_[1]->getTopic().c_str(), - odomSub_.getTopic().c_str(), - odomInfoSub_.getTopic().c_str()); - } - else - { - depthOdomInfoSync_ = new message_filters::Synchronizer( - MyDepthOdomInfoSyncPolicy(queueSize), - odomInfoSub_, - odomSub_, - *imageSubs_[0], - *imageDepthSubs_[0], - *cameraInfoSubs_[0]); - depthOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::depthOdomInfoCallback, 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(), - odomInfoSub_.getTopic().c_str()); - } - } - else - { - if(depthCameras > 1) - { - depth2Sync_ = new message_filters::Synchronizer( - MyDepth2SyncPolicy(queueSize), - odomSub_, - *imageSubs_[0], - *imageDepthSubs_[0], - *cameraInfoSubs_[0], - *imageSubs_[1], - *imageDepthSubs_[1], - *cameraInfoSubs_[1]); - depth2Sync_->registerCallback(boost::bind(&GuiWrapper::depth2Callback, 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(), - imageSubs_[0]->getTopic().c_str(), - imageDepthSubs_[0]->getTopic().c_str(), - cameraInfoSubs_[0]->getTopic().c_str(), - imageSubs_[1]->getTopic().c_str(), - imageDepthSubs_[1]->getTopic().c_str(), - cameraInfoSubs_[1]->getTopic().c_str(), - odomSub_.getTopic().c_str()); - } - else - { - depthSync_ = new message_filters::Synchronizer( - MyDepthSyncPolicy(queueSize), - odomSub_, - *imageSubs_[0], - *imageDepthSubs_[0], - *cameraInfoSubs_[0]); - depthSync_->registerCallback(boost::bind(&GuiWrapper::depthCallback, 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(), - odomSub_.getTopic().c_str()); - } - } - } - else - { - // use TF as odom - if(subscribeLaserScan2d) - { - scanSub_.subscribe(nh, "scan", 1); - depthScanTFSync_ = new message_filters::Synchronizer( - MyDepthScanTFSyncPolicy(queueSize), - scanSub_, - *imageSubs_[0], - *imageDepthSubs_[0], - *cameraInfoSubs_[0]); - depthScanTFSync_->registerCallback(boost::bind(&GuiWrapper::depthScanTFCallback, 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(), - 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); - depthOdomInfoTFSync_ = new message_filters::Synchronizer( - MyDepthOdomInfoTFSyncPolicy(queueSize), - odomInfoSub_, - *imageSubs_[0], - *imageDepthSubs_[0], - *cameraInfoSubs_[0]); - depthOdomInfoTFSync_->registerCallback(boost::bind(&GuiWrapper::depthOdomInfoTFCallback, 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(), - odomInfoSub_.getTopic().c_str()); - } - else - { - depthTFSync_ = new message_filters::Synchronizer( - MyDepthTFSyncPolicy(queueSize), - *imageSubs_[0], - *imageDepthSubs_[0], - *cameraInfoSubs_[0]); - depthTFSync_->registerCallback(boost::bind(&GuiWrapper::depthTFCallback, this, _1, _2, _3)); - - ROS_INFO("\n%s subscribed to:\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()); - } - } - } - else if(subscribeStereo) - { - ros::NodeHandle left_nh(nh, "left"); - ros::NodeHandle right_nh(nh, "right"); - ros::NodeHandle left_pnh(pnh, "left"); - ros::NodeHandle right_pnh(pnh, "right"); - image_transport::ImageTransport left_it(left_nh); - image_transport::ImageTransport right_it(right_nh); - image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh); - image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh); - - imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft); - imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight); - cameraInfoLeft_.subscribe(left_nh, "camera_info", 1); - cameraInfoRight_.subscribe(right_nh, "camera_info", 1); - - if(odomFrameId_.empty()) - { - odomSub_.subscribe(nh, "odom", 1); - if(subscribeLaserScan2d) - { - scanSub_.subscribe(nh, "scan", 1); - 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,\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) - { - odomInfoSub_.subscribe(nh, "odom_info", 1); - stereoOdomInfoSync_ = new message_filters::Synchronizer( - MyStereoOdomInfoSyncPolicy(queueSize), - odomInfoSub_, - odomSub_, - imageRectLeft_, - imageRectRight_, - cameraInfoLeft_, - cameraInfoRight_); - stereoOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::stereoOdomInfoCallback, 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(), - odomInfoSub_.getTopic().c_str()); - } - else - { - stereoSync_ = new message_filters::Synchronizer( - MyStereoSyncPolicy(queueSize), - odomSub_, - imageRectLeft_, - imageRectRight_, - cameraInfoLeft_, - cameraInfoRight_); - stereoSync_->registerCallback(boost::bind(&GuiWrapper::stereoCallback, 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(), - odomSub_.getTopic().c_str()); - } - } - else - { - //use odom TF - if(subscribeLaserScan2d) - { - scanSub_.subscribe(nh, "scan", 1); - stereoScanTFSync_ = new message_filters::Synchronizer( - MyStereoScanTFSyncPolicy(queueSize), - scanSub_, - imageRectLeft_, - imageRectRight_, - cameraInfoLeft_, - cameraInfoRight_); - stereoScanTFSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanTFCallback, 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(), - 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); - stereoOdomInfoTFSync_ = new message_filters::Synchronizer( - MyStereoOdomInfoTFSyncPolicy(queueSize), - odomInfoSub_, - imageRectLeft_, - imageRectRight_, - cameraInfoLeft_, - cameraInfoRight_); - stereoOdomInfoTFSync_->registerCallback(boost::bind(&GuiWrapper::stereoOdomInfoTFCallback, 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(), - odomInfoSub_.getTopic().c_str()); - } - else - { - stereoTFSync_ = new message_filters::Synchronizer( - MyStereoTFSyncPolicy(queueSize), - imageRectLeft_, - imageRectRight_, - cameraInfoLeft_, - cameraInfoRight_); - stereoTFSync_->registerCallback(boost::bind(&GuiWrapper::stereoTFCallback, 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(), - imageRectLeft_.getTopic().c_str(), - imageRectRight_.getTopic().c_str(), - cameraInfoLeft_.getTopic().c_str(), - cameraInfoRight_.getTopic().c_str()); - } - } - } - else // default odom only - { - defaultSub_ = nh.subscribe("odom", 1, &GuiWrapper::defaultCallback, this); - - ROS_INFO("\n%s subscribed to:\n %s", - ros::this_node::getName().c_str(), - defaultSub_.getTopic().c_str()); - } -} - diff --git a/src/GuiWrapper.h b/src/GuiWrapper.h deleted file mode 100644 index b9ba729f..00000000 --- a/src/GuiWrapper.h +++ /dev/null @@ -1,492 +0,0 @@ -/* -Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of the Universite de Sherbrooke nor the - names of its contributors may be used to endorse or promote products - derived from this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY -DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -#ifndef GUIWRAPPER_H_ -#define GUIWRAPPER_H_ - -#include -#include "rtabmap_ros/Info.h" -#include "rtabmap_ros/MapData.h" -#include "rtabmap_ros/OdomInfo.h" -#include "rtabmap_ros/Goal.h" -#include "rtabmap/utilite/UEventsHandler.h" -#include "rtabmap/core/Transform.h" - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -namespace rtabmap -{ - class MainWindow; -} - -class QApplication; - -class GuiWrapper : public UEventsHandler -{ -public: - GuiWrapper(int & argc, char** argv); - virtual ~GuiWrapper(); - -protected: - virtual void handleEvent(UEvent * anEvent); - -private: - void infoMapCallback(const rtabmap_ros::InfoConstPtr & infoMsg, const rtabmap_ros::MapDataConstPtr & mapMsg); - void goalPathCallback(const rtabmap_ros::GoalConstPtr & goalMsg, const nav_msgs::PathConstPtr & pathMsg); - void goalReachedCallback(const std_msgs::BoolConstPtr & value); - - void setupCallbacks( - bool subscribeDepth, - bool subscribeLaserScan2d, - bool subscribeLaserScan3d, - bool subscribeOdomInfo, - bool subscribeStereo, - int queueSize, - int depthCameras); - - void commonDepthCallback( - const nav_msgs::OdometryConstPtr & odomMsg, - const sensor_msgs::ImageConstPtr& imageMsg, - const sensor_msgs::ImageConstPtr& depthMsg, - const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, - 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& scan2dMsg, - const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, - const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg); - void commonStereoCallback( - const nav_msgs::OdometryConstPtr & odomMsg, - const sensor_msgs::ImageConstPtr& leftImageMsg, - const sensor_msgs::ImageConstPtr& rightImageMsg, - const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, - const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, - const sensor_msgs::LaserScanConstPtr& scan2dMsg, - const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, - const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg); - - void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg); - - // With odom msg - void depthCallback( - const nav_msgs::OdometryConstPtr & odomMsg, - const sensor_msgs::ImageConstPtr& imageMsg, - const sensor_msgs::ImageConstPtr& imageDepthMsg, - const sensor_msgs::CameraInfoConstPtr& camInfoMsg); - void depth2Callback( - const nav_msgs::OdometryConstPtr & odomMsg, - const sensor_msgs::ImageConstPtr& image1Msg, - const sensor_msgs::ImageConstPtr& depth1Msg, - const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg, - const sensor_msgs::ImageConstPtr& image2Msg, - const sensor_msgs::ImageConstPtr& depth2Msg, - const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg); - void depthOdomInfoCallback( - const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, - const nav_msgs::OdometryConstPtr & odomMsg, - const sensor_msgs::ImageConstPtr& imageMsg, - const sensor_msgs::ImageConstPtr& depthMsg, - const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg); - void depthOdomInfo2Callback( - const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, - const nav_msgs::OdometryConstPtr & odomMsg, - const sensor_msgs::ImageConstPtr& image1Msg, - const sensor_msgs::ImageConstPtr& depth1Msg, - const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg, - const sensor_msgs::ImageConstPtr& image2Msg, - const sensor_msgs::ImageConstPtr& depth2Msg, - const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg); - void depthScanCallback( - 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 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, - 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 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, - const sensor_msgs::ImageConstPtr& leftImageMsg, - const sensor_msgs::ImageConstPtr& rightImageMsg, - const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, - const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg); - void stereoCallback( - 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); - - // with TF - void depthTFCallback(const sensor_msgs::ImageConstPtr& imageMsg, - const sensor_msgs::ImageConstPtr& imageDepthMsg, - const sensor_msgs::CameraInfoConstPtr& camInfoMsg); - void depthOdomInfoTFCallback( - const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, - const sensor_msgs::ImageConstPtr& imageMsg, - const sensor_msgs::ImageConstPtr& depthMsg, - const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg); - void depthScanTFCallback( - const sensor_msgs::LaserScanConstPtr& scanMsg, - 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, - const sensor_msgs::ImageConstPtr& leftImageMsg, - 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, - const sensor_msgs::ImageConstPtr& rightImageMsg, - const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, - const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg); - void stereoTFCallback( - const sensor_msgs::ImageConstPtr& leftImageMsg, - const sensor_msgs::ImageConstPtr& rightImageMsg, - const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, - const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg); - - void processRequestedMap(const rtabmap_ros::MapData & map); - rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const; - -private: - rtabmap::MainWindow * mainWindow_; - std::string cameraNodeName_; - double lastOdomInfoUpdateTime_; - - // odometry subscription stuffs - std::string frameId_; - std::string odomFrameId_; - bool waitForTransform_; - double waitForTransformDuration_; - tf::TransformListener tfListener_; - - message_filters::Subscriber infoTopic_; - message_filters::Subscriber mapDataTopic_; - - message_filters::Subscriber goalTopic_; - message_filters::Subscriber pathTopic_; - ros::Subscriber goalReachedTopic_; - - ros::Subscriber defaultSub_; // odometry only - std::vector imageSubs_; - std::vector imageDepthSubs_; - std::vector* > cameraInfoSubs_; - message_filters::Subscriber odomSub_; - message_filters::Subscriber odomInfoSub_; - message_filters::Subscriber scanSub_; - message_filters::Subscriber scan3dSub_; - - image_transport::SubscriberFilter imageRectLeft_; - image_transport::SubscriberFilter imageRectRight_; - message_filters::Subscriber cameraInfoLeft_; - message_filters::Subscriber cameraInfoRight_; - - typedef message_filters::sync_policies::ExactTime< - rtabmap_ros::Info, - rtabmap_ros::MapData> MyInfoMapSyncPolicy; - message_filters::Synchronizer * infoMapSync_; - - typedef message_filters::sync_policies::ExactTime< - rtabmap_ros::Goal, - nav_msgs::Path> MyGoalPathSyncPolicy; - message_filters::Synchronizer * goalPathSync_; - - // with odom msg - typedef message_filters::sync_policies::ApproximateTime< - sensor_msgs::LaserScan, - nav_msgs::Odometry, - sensor_msgs::Image, - sensor_msgs::Image, - 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, - sensor_msgs::Image, - sensor_msgs::CameraInfo> MyDepthSyncPolicy; - message_filters::Synchronizer * depthSync_; - - typedef message_filters::sync_policies::ApproximateTime< - rtabmap_ros::OdomInfo, - nav_msgs::Odometry, - sensor_msgs::Image, - sensor_msgs::Image, - sensor_msgs::CameraInfo> MyDepthOdomInfoSyncPolicy; - message_filters::Synchronizer * depthOdomInfoSync_; - - typedef message_filters::sync_policies::ApproximateTime< - nav_msgs::Odometry, - sensor_msgs::Image, - sensor_msgs::Image, - sensor_msgs::CameraInfo, - sensor_msgs::CameraInfo> MyStereoSyncPolicy; - message_filters::Synchronizer * stereoSync_; - - typedef message_filters::sync_policies::ApproximateTime< - sensor_msgs::LaserScan, - nav_msgs::Odometry, - sensor_msgs::Image, - sensor_msgs::Image, - sensor_msgs::CameraInfo, - 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, - sensor_msgs::Image, - sensor_msgs::Image, - sensor_msgs::CameraInfo, - sensor_msgs::CameraInfo> MyStereoOdomInfoSyncPolicy; - message_filters::Synchronizer * stereoOdomInfoSync_; - - typedef message_filters::sync_policies::ApproximateTime< - nav_msgs::Odometry, - sensor_msgs::Image, - sensor_msgs::Image, - sensor_msgs::CameraInfo, - sensor_msgs::Image, - sensor_msgs::Image, - sensor_msgs::CameraInfo> MyDepth2SyncPolicy; - message_filters::Synchronizer * depth2Sync_; - - typedef message_filters::sync_policies::ApproximateTime< - rtabmap_ros::OdomInfo, - nav_msgs::Odometry, - sensor_msgs::Image, - sensor_msgs::Image, - sensor_msgs::CameraInfo, - sensor_msgs::Image, - sensor_msgs::Image, - sensor_msgs::CameraInfo> MyDepthOdomInfo2SyncPolicy; - message_filters::Synchronizer * depthOdomInfo2Sync_; - - // with odom TF - typedef message_filters::sync_policies::ApproximateTime< - sensor_msgs::LaserScan, - sensor_msgs::Image, - sensor_msgs::Image, - 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, - sensor_msgs::CameraInfo> MyDepthTFSyncPolicy; - message_filters::Synchronizer * depthTFSync_; - - typedef message_filters::sync_policies::ApproximateTime< - rtabmap_ros::OdomInfo, - sensor_msgs::Image, - sensor_msgs::Image, - sensor_msgs::CameraInfo> MyDepthOdomInfoTFSyncPolicy; - message_filters::Synchronizer * depthOdomInfoTFSync_; - - typedef message_filters::sync_policies::ApproximateTime< - sensor_msgs::Image, - sensor_msgs::Image, - sensor_msgs::CameraInfo, - sensor_msgs::CameraInfo> MyStereoTFSyncPolicy; - message_filters::Synchronizer * stereoTFSync_; - - typedef message_filters::sync_policies::ApproximateTime< - sensor_msgs::LaserScan, - sensor_msgs::Image, - sensor_msgs::Image, - sensor_msgs::CameraInfo, - 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, - sensor_msgs::Image, - sensor_msgs::CameraInfo, - sensor_msgs::CameraInfo> MyStereoOdomInfoTFSyncPolicy; - message_filters::Synchronizer * stereoOdomInfoTFSync_; -}; - -#endif /* GUIWRAPPER_H_ */ diff --git a/src/ICPOdometryNode.cpp b/src/ICPOdometryNode.cpp new file mode 100644 index 00000000..1cc106f3 --- /dev/null +++ b/src/ICPOdometryNode.cpp @@ -0,0 +1,78 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include "ros/ros.h" +#include "nodelet/loader.h" +#include +#include + +int main(int argc, char **argv) +{ + ULogger::setType(ULogger::kTypeConsole); + ULogger::setLevel(ULogger::kWarning); + ros::init(argc, argv, "icp_odometry"); + + // process "--params" argument + nodelet::V_string nargv; + for(int i=1;ifirst + " = \"" + iter->second + "\""; + std::cout << + str << + std::setw(60 - str.size()) << + " [" << + rtabmap::Parameters::getDescription(iter->first).c_str() << + "]" << + std::endl; + } + ROS_WARN("Node will now exit after showing default odometry parameters because " + "argument \"--params\" is detected!"); + exit(0); + } + else if(strcmp(argv[i], "--udebug") == 0) + { + ULogger::setLevel(ULogger::kDebug); + } + else if(strcmp(argv[i], "--uinfo") == 0) + { + ULogger::setLevel(ULogger::kInfo); + } + nargv.push_back(argv[i]); + } + + nodelet::Loader nodelet; + nodelet::M_string remap(ros::names::getRemappings()); + std::string nodelet_name = ros::this_node::getName(); + nodelet.load(nodelet_name, "rtabmap_ros/icp_odometry", remap, nargv); + ros::spin(); + return 0; +} diff --git a/src/MapAssemblerNode.cpp b/src/MapAssemblerNode.cpp index a8055d53..1b8729f3 100644 --- a/src/MapAssemblerNode.cpp +++ b/src/MapAssemblerNode.cpp @@ -28,7 +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 "rtabmap_ros/MapsManager.h" #include #include #include @@ -49,12 +49,13 @@ class MapAssembler { public: - MapAssembler() : - mapsManager_(false) + MapAssembler() { ros::NodeHandle pnh("~"); - ros::NodeHandle nh; + + mapsManager_.init(nh, pnh, ros::this_node::getName(), false); + mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this); // private service @@ -99,9 +100,6 @@ public: 0, false, false, - false, - false, - false, nodes_); mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id); diff --git a/src/MapOptimizerNode.cpp b/src/MapOptimizerNode.cpp index 64e5522a..7d188ec5 100644 --- a/src/MapOptimizerNode.cpp +++ b/src/MapOptimizerNode.cpp @@ -84,7 +84,7 @@ public: 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::kRegForce3DoF(), uBool2Str(slam2d))); parameters.insert(ParametersPair(Parameters::kOptimizerVarianceIgnored(), uBool2Str(ignoreVariance))); optimizer_ = Optimizer::create(parameters); @@ -254,10 +254,10 @@ public: { optimizedPoses = poses; } - else if(poses.size() || constraints.size()) + else if(poses.size() == 0 && constraints.size()) { - ROS_ERROR("map_optimizer: Poses=%d and edges=%d (poses must " - "not be null if there are edges, and edges must be null if poses <= 1)", + ROS_ERROR("map_optimizer: Poses=%d and edges=%d: poses must " + "not be null if there are edges.", (int)poses.size(), (int)constraints.size()); } diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp index c6f2012b..d32f8d59 100644 --- a/src/MapsManager.cpp +++ b/src/MapsManager.cpp @@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "MapsManager.h" +#include "rtabmap_ros/MapsManager.h" #include #include @@ -38,6 +38,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include + +#include #include #include @@ -47,101 +50,49 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifdef WITH_OCTOMAP_ROS #ifdef RTABMAP_OCTOMAP #include +#include #include #endif #endif using namespace rtabmap; -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), - projMaxObstaclesHeight_(2.0), // meters (<=0 disabled) - projMaxGroundHeight_(0.0), // meters (<=0 disabled, only works if proj_detect_flat_obstacles is true) - projDetectFlatObstacles_(false), - projMapFrame_(false), +MapsManager::MapsManager() : + cloudOutputVoxelized_(true), + cloudSubtractFiltering_(false), + cloudSubtractFilteringMinNeighbors_(2), gridCellSize_(0.05), // meters + gridIncremental_(false), gridSize_(0), // meters gridEroded_(false), - gridUnknownSpaceFilled_(false), - gridMaxUnknownSpaceFilledRange_(6.0), + footprintRadius_(0.0), mapFilterRadius_(0.0), mapFilterAngle_(30.0), // degrees mapCacheCleanup_(true), negativePosesIgnored_(false), + assembledObstacles_(new pcl::PointCloud), + assembledGround_(new pcl::PointCloud), + occupancyGrid_(new OccupancyGrid), octomap_(0), octomapTreeDepth_(16), - octomapGroundIsObstacle_(false) + octomapOccupancyThr_(0.5) { +} - ros::NodeHandle nh; - ros::NodeHandle pnh("~"); - - // 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_); - 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_); - pnh.param("proj_map_frame", projMapFrame_, projMapFrame_); - +void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::string & name, bool usePublicNamespace) +{ // 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_); } + occupancyGrid_->setCellSize(gridCellSize_); + + pnh.param("grid_incremental", gridIncremental_, gridIncremental_); // m 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_); + pnh.param("grid_footprint_radius", footprintRadius_, footprintRadius_); // common map stuff pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_); @@ -149,9 +100,37 @@ MapsManager::MapsManager(bool usePublicNamespace) : pnh.param("map_cleanup", mapCacheCleanup_, mapCacheCleanup_); pnh.param("map_negative_poses_ignored", negativePosesIgnored_, negativePosesIgnored_); + if(pnh.hasParam("scan_output_voxelized")) + { + ROS_WARN("Parameter \"scan_output_voxelized\" has been " + "removed. Use \"cloud_output_voxelized\" instead."); + if(!pnh.hasParam("cloud_output_voxelized")) + { + pnh.getParam("scan_output_voxelized", cloudOutputVoxelized_); + } + } + pnh.param("cloud_output_voxelized", cloudOutputVoxelized_, cloudOutputVoxelized_); + pnh.param("cloud_subtract_filtering", cloudSubtractFiltering_, cloudSubtractFiltering_); + pnh.param("cloud_subtract_filtering_min_neighbors", cloudSubtractFilteringMinNeighbors_, cloudSubtractFilteringMinNeighbors_); + + ROS_INFO("%s(maps): grid_cell_size = %f", name.c_str(), gridCellSize_); + ROS_INFO("%s(maps): grid_incremental = %s", name.c_str(), gridIncremental_?"true":"false"); + ROS_INFO("%s(maps): grid_size = %f", name.c_str(), gridSize_); + ROS_INFO("%s(maps): grid_eroded = %s", name.c_str(), gridEroded_?"true":"false"); + ROS_INFO("%s(maps): grid_footprint_radius = %f", name.c_str(), footprintRadius_); + ROS_INFO("%s(maps): map_filter_radius = %f", name.c_str(), mapFilterRadius_); + ROS_INFO("%s(maps): map_filter_angle = %f", name.c_str(), mapFilterAngle_); + ROS_INFO("%s(maps): map_cleanup = %s", name.c_str(), mapCacheCleanup_?"true":"false"); + ROS_INFO("%s(maps): map_negative_poses_ignored = %s", name.c_str(), negativePosesIgnored_?"true":"false"); + ROS_INFO("%s(maps): cloud_output_voxelized = %s", name.c_str(), cloudOutputVoxelized_?"true":"false"); + ROS_INFO("%s(maps): cloud_subtract_filtering = %s", name.c_str(), cloudSubtractFiltering_?"true":"false"); + ROS_INFO("%s(maps): cloud_subtract_filtering_min_neighbors = %d", name.c_str(), cloudSubtractFilteringMinNeighbors_); + #ifdef WITH_OCTOMAP_ROS #ifdef RTABMAP_OCTOMAP - octomap_ = new OctoMap(gridCellSize_); + pnh.param("octomap_occupancy_thr", octomapOccupancyThr_, octomapOccupancyThr_); + UASSERT(octomapOccupancyThr_>=0.0 && octomapOccupancyThr_<=1.0); + octomap_ = new OctoMap(gridCellSize_, octomapOccupancyThr_); pnh.param("octomap_tree_depth", octomapTreeDepth_, octomapTreeDepth_); if(octomapTreeDepth_ > 16) { @@ -163,7 +142,8 @@ MapsManager::MapsManager(bool usePublicNamespace) : ROS_WARN("octomap_tree_depth cannot be negative, set to 16 instead"); octomapTreeDepth_ = 16; } - pnh.param("octomap_ground_is_obstacle", octomapGroundIsObstacle_, octomapGroundIsObstacle_); + ROS_INFO("%s(maps): octomap_tree_depth = %d", name.c_str(), octomapTreeDepth_); + ROS_INFO("%s(maps): octomap_occupancy_thr = %f", name.c_str(), octomapOccupancyThr_); #endif #endif @@ -174,43 +154,40 @@ MapsManager::MapsManager(bool usePublicNamespace) : pnh.param("latch", latch, latch); // mapping topics + ros::NodeHandle * nht; 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); -#ifdef WITH_OCTOMAP_ROS -#ifdef RTABMAP_OCTOMAP - octoMapPubBin_ = nh.advertise("octomap_binary", 1, latch); - octoMapPubFull_ = nh.advertise("octomap_full", 1, latch); - octoMapCloud_ = nh.advertise("octomap_cloud", 1, latch); - octoMapEmptySpace_ = nh.advertise("octomap_empty_space", 1, latch); - octoMapProj_ = nh.advertise("octomap_proj", 1, latch); -#endif -#endif + nht = &nh; } 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); + nht = &pnh; + } + gridMapPub_ = nht->advertise("grid_map", 1, latch); + cloudMapPub_ = nht->advertise("cloud_map", 1, latch); + cloudObstaclesPub_ = nht->advertise("cloud_obstacles", 1, latch); + cloudGroundPub_ = nht->advertise("cloud_ground", 1, latch); + + // deprecated + projMapPub_ = nht->advertise("proj_map", 1, latch); + scanMapPub_ = nht->advertise("scan_map", 1, latch); + #ifdef WITH_OCTOMAP_ROS #ifdef RTABMAP_OCTOMAP - octoMapPubBin_ = pnh.advertise("octomap_binary", 1, latch); - octoMapPubFull_ = pnh.advertise("octomap_full", 1, latch); - octoMapCloud_ = pnh.advertise("octomap_cloud", 1, latch); - octoMapEmptySpace_ = pnh.advertise("octomap_cloud_ground", 1, latch); - octoMapProj_ = pnh.advertise("octomap_proj", 1, latch); + octoMapPubBin_ = nht->advertise("octomap_binary", 1, latch); + octoMapPubFull_ = nht->advertise("octomap_full", 1, latch); + octoMapCloud_ = nht->advertise("octomap_occupied_space", 1, latch); + octoMapEmptySpace_ = nht->advertise("octomap_empty_space", 1, latch); + octoMapProj_ = nht->advertise("octomap_grid", 1, latch); #endif #endif - } } MapsManager::~MapsManager() { clear(); + delete occupancyGrid_; + #ifdef WITH_OCTOMAP_ROS #ifdef RTABMAP_OCTOMAP if(octomap_) @@ -222,12 +199,112 @@ MapsManager::~MapsManager() { #endif } +void parameterMoved( + ros::NodeHandle & nh, + const std::string & rosName, + const std::string & parameterName, + ParametersMap & parameters) +{ + if(nh.hasParam(rosName)) + { + ParametersMap::const_iterator iter = Parameters::getDefaultParameters().find(parameterName); + if(iter != Parameters::getDefaultParameters().end()) + { + ROS_WARN("Parameter \"%s\" has moved from " + "rtabmap_ros to rtabmap library. Use " + "parameter \"%s\" instead. The value is still " + "copied to new parameter name.", + rosName.c_str(), + parameterName.c_str()); + std::string type = Parameters::getType(parameterName); + if(type.compare("float") || type.compare("double")) + { + double v = uStr2Double(iter->second); + nh.getParam(rosName, v); + parameters.insert(ParametersPair(parameterName, uNumber2Str(v))); + } + else if(type.compare("int") || type.compare("unsigned int")) + { + int v = uStr2Int(iter->second); + nh.getParam(rosName, v); + parameters.insert(ParametersPair(parameterName, uNumber2Str(v))); + } + else + { + ROS_ERROR("Not handled type \"%s\" for parameter \"%s\"", type.c_str(), parameterName.c_str()); + } + } + else + { + ROS_ERROR("Parameter \"%s\" not found in default parameters.", parameterName.c_str()); + } + } +} + +void MapsManager::backwardCompatibilityParameters(ros::NodeHandle & pnh, ParametersMap & parameters) const +{ + // removed + if(pnh.hasParam("cloud_frustum_culling")) + { + ROS_WARN("Parameter \"cloud_frustum_culling\" has been removed. OctoMap topics " + "already do it. You can remove it from your launch file."); + } + + // moved + parameterMoved(pnh, "cloud_decimation", Parameters::kGridDepthDecimation(), parameters); + parameterMoved(pnh, "cloud_max_depth", Parameters::kGridDepthMax(), parameters); + parameterMoved(pnh, "cloud_min_depth", Parameters::kGridDepthMin(), parameters); + parameterMoved(pnh, "cloud_voxel_size", Parameters::kGridCellSize(), parameters); + parameterMoved(pnh, "cloud_floor_culling_height", Parameters::kGridMaxGroundHeight(), parameters); + parameterMoved(pnh, "cloud_ceiling_culling_height", Parameters::kGridMaxObstacleHeight(), parameters); + parameterMoved(pnh, "cloud_noise_filtering_radius", Parameters::kGridNoiseFilteringRadius(), parameters); + parameterMoved(pnh, "cloud_noise_filtering_min_neighbors", Parameters::kGridNoiseFilteringMinNeighbors(), parameters); + parameterMoved(pnh, "scan_decimation", Parameters::kGridScanDecimation(), parameters); + parameterMoved(pnh, "scan_voxel_size", Parameters::kGridCellSize(), parameters); + parameterMoved(pnh, "proj_max_ground_angle", Parameters::kGridMaxGroundAngle(), parameters); + parameterMoved(pnh, "proj_min_cluster_size", Parameters::kGridMinClusterSize(), parameters); + parameterMoved(pnh, "proj_max_height", Parameters::kGridMaxObstacleHeight(), parameters); + parameterMoved(pnh, "proj_max_obstacles_height", Parameters::kGridMaxObstacleHeight(), parameters); + parameterMoved(pnh, "proj_max_ground_height", Parameters::kGridMaxGroundHeight(), parameters); + + parameterMoved(pnh, "proj_detect_flat_obstacles", Parameters::kGridFlatObstacleDetected(), parameters); + parameterMoved(pnh, "proj_map_frame", Parameters::kGridMapFrameProjection(), parameters); + parameterMoved(pnh, "grid_unknown_space_filled", Parameters::kGridScan2dUnknownSpaceFilled(), parameters); + parameterMoved(pnh, "grid_unknown_space_filled_max_range", Parameters::kGridScan2dMaxFilledRange(), parameters); + +#ifdef WITH_OCTOMAP_ROS +#ifdef RTABMAP_OCTOMAP + parameterMoved(pnh, "octomap_ground_is_obstacle", Parameters::kGrid3DGroundIsObstacle(), parameters); +#endif +#endif +} + +void MapsManager::setParameters(const rtabmap::ParametersMap & parameters) +{ + parameters_ = parameters; + + // don't use grid cell size from parameters as we use grid_cell_size ros param + uInsert(parameters_, ParametersPair(Parameters::kGridCellSize(), uNumber2Str(gridCellSize_))); + + // For negative laser scans, always fill empty space + uInsert(parameters_, ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), "true")); + + occupancyGrid_->parseParameters(parameters_); +} + void MapsManager::clear() { - clouds_.clear(); - cameraModels_.clear(); - projMaps_.clear(); gridMaps_.clear(); + gridMapsViewpoints_.clear(); + assembledGround_->clear(); + assembledObstacles_->clear(); + assembledGroundPoses_.clear(); + assembledObstaclePoses_.clear(); + assembledGroundIndex_.release(); + assembledObstacleIndex_.release(); + groundClouds_.clear(); + obstacleClouds_.clear(); + occupancyGrid_->clear(); #ifdef WITH_OCTOMAP_ROS #ifdef RTABMAP_OCTOMAP octomap_->clear(); @@ -238,6 +315,8 @@ void MapsManager::clear() bool MapsManager::hasSubscribers() const { return cloudMapPub_.getNumSubscribers() != 0 || + cloudObstaclesPub_.getNumSubscribers() != 0 || + cloudGroundPub_.getNumSubscribers() != 0 || projMapPub_.getNumSubscribers() != 0 || gridMapPub_.getNumSubscribers() != 0 || scanMapPub_.getNumSubscribers() != 0 || @@ -262,20 +341,14 @@ std::map MapsManager::getFilteredPoses(const std::map MapsManager::updateMapCaches( const std::map & poses, const rtabmap::Memory * memory, - bool updateCloud, - bool updateProj, bool updateGrid, - bool updateScan, bool updateOctomap, const std::map & signatures) { - if(!updateCloud && !updateProj && !updateGrid && !updateScan && !updateOctomap) + if(!updateGrid && !updateOctomap) { // 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; + updateGrid = this->hasSubscribers(); updateOctomap = octoMapPubBin_.getNumSubscribers() != 0 || octoMapPubFull_.getNumSubscribers() != 0 || @@ -303,7 +376,7 @@ std::map MapsManager::updateMapCaches( std::map filteredPoses; // update cache - if(updateCloud || updateProj || updateGrid || updateScan || updateOctomap) + if(updateGrid || updateOctomap) { // filter nodes if(mapFilterRadius_ > 0.0) @@ -347,29 +420,14 @@ std::map MapsManager::updateMapCaches( bool longUpdate = false; if(filteredPoses.size() > 20) { - if(updateCloud && clouds_.size() < 5) + if(updateGrid && gridMaps_.size() < 5) { - ROS_WARN("Many clouds should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-clouds_.size())); - longUpdate = true; - } - else if(updateProj && projMaps_.size() < 5) - { - ROS_WARN("Many occupancy grid map from projections should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-projMaps_.size())); - longUpdate = true; - } - else if(updateGrid && gridMaps_.size() < 5) - { - ROS_WARN("Many occupancy grid map from laser scans should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-gridMaps_.size())); - longUpdate = true; - } - else if(updateScan && scans_.size() < 5) - { - ROS_WARN("Many scans should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-scans_.size())); + ROS_WARN("Many occupancy grids should be loaded (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-gridMaps_.size())); longUpdate = true; } #ifdef WITH_OCTOMAP_ROS #ifdef RTABMAP_OCTOMAP - else if(updateOctomap && octomap_->addedNodes().size() < 5) + if(updateOctomap && octomap_->addedNodes().size() < 5) { ROS_WARN("Many clouds should be added to octomap (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-octomap_->addedNodes().size())); longUpdate = true; @@ -378,32 +436,14 @@ std::map MapsManager::updateMapCaches( #endif } + bool occupancySavedInDB = memory && uStrNumCmp(memory->getDatabaseVersion(), "0.11.10")>=0?true:false; + for(std::map::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter) { if(!iter->second.isNull()) { rtabmap::SensorData data; - bool rgbDepthRequired = updateCloud && (iter->first < 0 || !uContains(clouds_, iter->first)); - bool depthRequired = updateProj && (iter->first < 0 || !uContains(projMaps_, iter->first)); - bool gridRequired = updateGrid && (iter->first < 0 || !uContains(gridMaps_, iter->first)); - bool scanRequired = updateScan && (iter->first < 0 || !uContains(scans_, iter->first)); - -#ifdef WITH_OCTOMAP_ROS -#ifdef RTABMAP_OCTOMAP - if(!rgbDepthRequired) - { - rgbDepthRequired = updateOctomap && - (iter->first < 0 || - octomap_->addedNodes().empty() || - iter->first > octomap_->addedNodes().rbegin()->first); - } -#endif -#endif - - if(rgbDepthRequired || - depthRequired || - scanRequired || - gridRequired) + if((updateGrid || updateOctomap) && (iter->first < 0 || !uContains(gridMaps_, iter->first))) { UDEBUG("Data required for %d", iter->first); std::map::const_iterator findIter = signatures.find(iter->first); @@ -413,277 +453,110 @@ std::map MapsManager::updateMapCaches( } else if(memory) { - data = memory->getSignatureDataConst(iter->first); + data = memory->getSignatureDataConst(iter->first, occupancyGrid_->isGridFromDepth() && !occupancySavedInDB, !occupancyGrid_->isGridFromDepth() && !occupancySavedInDB, false, true); } - } - if(data.id() != 0) - { - if(!(data.imageCompressed().empty() && data.imageRaw().empty()) && - !(data.depthOrRightCompressed().empty() && data.depthOrRightRaw().empty()) && - (data.cameraModels().size() || data.stereoCameraModel().isValidForProjection())) + if(data.id() != 0) { - // Which data should we decompress? - cv::Mat image, depth, scan; - data.uncompressData( - (rgbDepthRequired||data.stereoCameraModel().isValidForProjection()) ? &image:0, - (rgbDepthRequired||depthRequired) ? &depth:0, - scanRequired||gridRequired?&scan:0); - - pcl::PointCloud::Ptr cloudRGB; - pcl::PointCloud::Ptr cloudXYZ; - if(rgbDepthRequired) + UDEBUG("Adding grid map %d to cache...", iter->first); + cv::Point3f viewPoint; + cv::Mat ground, obstacles; + if(iter->first > 0) { - UDEBUG("rgbDepthRequired"); - if(!image.empty() && !depth.empty()) + cv::Mat rgb, depth, scan; + bool generateGrid = data.gridCellSize() == 0.0f; + static bool warningShown = false; + if(occupancySavedInDB && generateGrid && !warningShown) { - pcl::IndicesPtr validIndices(new std::vector); - cloudRGB = util3d::cloudRGBFromSensorData( - data, - cloudDecimation_, - cloudMaxDepth_, - 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; - } + warningShown = true; + UWARN("Occupancy grid for location %d should be added to global map (e..g, a ROS node is subscribed to " + "any occupancy grid output) but it cannot be found " + "in memory. For convenience, the occupancy " + "grid is regenerated. Make sure parameter \"%s\" is true to " + "avoid this warning for the next locations added to map. For older " + "locations already in database without an occupancy grid map, you can use the " + "\"rtabmap-databaseViewer\" to regenerate the missing occupancy grid maps and " + "save them back in the database for next sessions. This warning is only shown once.", + data.id(), Parameters::kRGBDCreateOccupancyGrid().c_str()); } - else + data.uncompressData( + occupancyGrid_->isGridFromDepth() && generateGrid?&rgb:0, + occupancyGrid_->isGridFromDepth() && generateGrid?&depth:0, + !occupancyGrid_->isGridFromDepth() && generateGrid?&scan:0, + 0, + generateGrid?0:&ground, + generateGrid?0:&obstacles); + + if(generateGrid) { - ROS_ERROR("RGB or Depth image not found (node=%d)!", iter->first); - } - } - else if(depthRequired) - { - UDEBUG("depthRequired"); - if( !depth.empty()) - { - pcl::IndicesPtr validIndices(new std::vector); - cloudXYZ = util3d::cloudFromSensorData( - data, - cloudDecimation_, - cloudMaxDepth_, - 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 - { - ROS_ERROR("RGB or Depth image not found (node=%d)!", iter->first); - } - } - - if(cloudRGB.get()) - { - uInsert(clouds_, std::make_pair(iter->first, cloudRGB)); - - // 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().isValidForProjection()) - { - //insert only the left camera model - rtabmap::CameraModel model = data.stereoCameraModel().left(); - model.setImageSize(cv::Size(data.imageRaw().cols, data.imageRaw().rows)); - models.push_back(model); - } - else if(data.cameraModels().size()) - { - UASSERT_MSG(data.imageRaw().cols % data.cameraModels().size() == 0, - uFormat("data.imageRaw().cols=%d data.cameraModels().size()=%d", - data.imageRaw().cols, (int)data.cameraModels().size()).c_str()); - - models.resize(data.cameraModels().size()); - for(unsigned int i=0; ifirst, models)); - } - - if(depthRequired || updateOctomap) - { - UDEBUG("Creating proj map / octomap for %d...", iter->first); - cv::Mat ground, obstacles; - if(cloudRGB.get()) - { - pcl::PointCloud::Ptr cloudClipped = cloudRGB; - if(cloudClipped->size() && projMaxObstaclesHeight_ > 0) - { - cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits::min(), projMaxObstaclesHeight_); - } - if(cloudClipped->size() && gridCellSize_ > cloudVoxelSize_) - { - cloudClipped = util3d::voxelize(cloudClipped, gridCellSize_); - } - if(cloudClipped->size()) - { - // add pose rotation without yaw - float roll, pitch, yaw; - iter->second.getEulerAngles(roll, pitch, yaw); - cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,projMapFrame_?iter->second.z():0, roll, pitch, 0)); - - pcl::IndicesPtr groundIndices, obstaclesIndices; - util3d::segmentObstaclesFromGround( - cloudClipped, - groundIndices, - obstaclesIndices, - 20, - projMaxGroundAngle_*M_PI/180.0, - gridCellSize_*2.0f, - projMinClusterSize_, - projDetectFlatObstacles_, - projMaxGroundHeight_); - - pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); - pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); - - if(groundIndices->size()) - { - pcl::copyPointCloud(*cloudClipped, *groundIndices, *groundCloud); - } - - if(obstaclesIndices->size()) - { - pcl::copyPointCloud(*cloudClipped, *obstaclesIndices, *obstaclesCloud); - } - - if(updateProj) - { - util3d::occupancy2DFromGroundObstacles( - groundCloud, - obstaclesCloud, - ground, - obstacles, - gridCellSize_); - uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); - } - -#ifdef WITH_OCTOMAP_ROS -#ifdef RTABMAP_OCTOMAP - if(updateOctomap) - { - Transform tinv = Transform(0,0,projMapFrame_?iter->second.z():0, roll, pitch, 0).inverse(); - groundCloud = util3d::transformPointCloud(groundCloud, tinv); - obstaclesCloud = util3d::transformPointCloud(obstaclesCloud, tinv); - if(octomapGroundIsObstacle_) - { - *obstaclesCloud += *groundCloud; - groundCloud->clear(); - } - octomap_->addToCache(iter->first, groundCloud, obstaclesCloud); - } -#endif -#endif - } - } - else if(updateProj && cloudXYZ.get()) - { - pcl::PointCloud::Ptr cloudClipped = cloudXYZ; - if(cloudClipped->size() && projMaxObstaclesHeight_ > 0) - { - cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits::min(), projMaxObstaclesHeight_); - } - if(cloudClipped->size()) - { - // add pose rotation without yaw - float roll, pitch, yaw; - iter->second.getEulerAngles(roll, pitch, yaw); - cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,projMapFrame_?iter->second.z():0, roll, pitch, 0)); - - pcl::IndicesPtr groundIndices, obstaclesIndices; - util3d::segmentObstaclesFromGround( - cloudClipped, - groundIndices, - obstaclesIndices, - 20, - projMaxGroundAngle_*M_PI/180.0, - gridCellSize_*2.0f, - projMinClusterSize_, - projDetectFlatObstacles_, - projMaxGroundHeight_); - - util3d::occupancy2DFromGroundObstacles( - cloudClipped, - groundIndices, - obstaclesIndices, - ground, - obstacles, - gridCellSize_); - uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); - } - } - } - - if(scanRequired || gridRequired) - { - if(scan.cols && (scanRequired || scanVoxelSize_ > 0.0 || scanDecimation_ > 1)) - { - 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()); + Signature tmp(data); + tmp.setPose(iter->second); + occupancyGrid_->createLocalMap(tmp, ground, obstacles, viewPoint); uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); + uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint)); } + else + { + viewPoint = data.gridViewPoint(); + gridMaps_.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles))); + gridMapsViewpoints_.insert(std::make_pair(iter->first, viewPoint)); + } + } + else + { + // generate tmp occupancy grid for negative ids (assuming data is already uncompressed) + // we need the signature + std::map::const_iterator findIter = signatures.find(iter->first); + if(findIter != signatures.end()) + { + // normally data should be already uncompressed for negative ids + occupancyGrid_->createLocalMap(findIter->second, ground, obstacles, viewPoint); + uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); + uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint)); + } + else + { + ROS_WARN("%d signature not found in cache?!?!?", iter->first); + } + } + if(ground.cols || obstacles.cols) + { + occupancyGrid_->addToCache(iter->first, ground, obstacles); } } else { - ROS_ERROR("Some data missing for node %d to update the maps (image=%d, depth=%d, camera=%d)", - iter->first, - !(data.imageCompressed().empty() && data.imageRaw().empty())?1:0, - !(data.depthOrRightCompressed().empty() && data.depthOrRightRaw().empty())?1:0, - (data.cameraModels().size() || data.stereoCameraModel().isValidForProjection())?1:0); + ROS_ERROR("Data missing for node %d to update the maps", iter->first); } } + +#ifdef WITH_OCTOMAP_ROS +#ifdef RTABMAP_OCTOMAP + if(updateOctomap && + (iter->first < 0 || + octomap_->addedNodes().empty() || + iter->first > octomap_->addedNodes().rbegin()->first)) + { + std::map >::iterator mter = gridMaps_.find(iter->first); + std::map::iterator pter = gridMapsViewpoints_.find(iter->first); + if(mter != gridMaps_.end() && pter!=gridMapsViewpoints_.end()) + { + if((mter->second.first.empty() || mter->second.first.channels() > 2) && + (mter->second.second.empty() || mter->second.second.channels() > 2)) + { + octomap_->addToCache(iter->first, mter->second.first, mter->second.second, pter->second); + } + else if(!mter->second.first.empty() && !mter->second.second.empty()) + { + ROS_WARN("Node %d: Cannot update octomap with 2D occupancy grids. " + "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see " + "all occupancy grid parameters.", + iter->first); + } + } + } +#endif +#endif } else { @@ -701,50 +574,12 @@ std::map MapsManager::updateMapCaches( } #endif #endif - - // cleanup not used nodes - UDEBUG("Cleanup not used nodes"); - for(std::map::Ptr >::iterator iter=clouds_.begin(); - iter!=clouds_.end();) - { - if(!uContains(poses, iter->first)) - { - clouds_.erase(iter++); - } - else - { - ++iter; - } - } - for(std::map::Ptr >::iterator iter=scans_.begin(); - iter!=scans_.end();) - { - if(!uContains(poses, iter->first)) - { - scans_.erase(iter++); - } - else - { - ++iter; - } - } - for(std::map >::iterator iter=projMaps_.begin(); - iter!=projMaps_.end();) - { - if(!uContains(poses, iter->first)) - { - projMaps_.erase(iter++); - } - else - { - ++iter; - } - } for(std::map >::iterator iter=gridMaps_.begin(); iter!=gridMaps_.end();) { if(!uContains(poses, iter->first)) { + UASSERT(gridMapsViewpoints_.erase(iter->first) != 0); gridMaps_.erase(iter++); } else @@ -752,12 +587,26 @@ std::map MapsManager::updateMapCaches( ++iter; } } - for(std::map >::iterator iter=cameraModels_.begin(); - iter!=cameraModels_.end();) + + for(std::map::Ptr >::iterator iter=groundClouds_.begin(); + iter!=groundClouds_.end();) { if(!uContains(poses, iter->first)) { - cameraModels_.erase(iter++); + groundClouds_.erase(iter++); + } + else + { + ++iter; + } + } + + for(std::map::Ptr >::iterator iter=obstacleClouds_.begin(); + iter!=obstacleClouds_.end();) + { + if(!uContains(poses, iter->first)) + { + obstacleClouds_.erase(iter++); } else { @@ -774,6 +623,33 @@ std::map MapsManager::updateMapCaches( return filteredPoses; } +pcl::PointCloud::Ptr subtractFiltering( + const pcl::PointCloud::Ptr & cloud, + const rtabmap::FlannIndex & substractCloudIndex, + float radiusSearch, + int minNeighborsInRadius) +{ + UASSERT(minNeighborsInRadius > 0); + UASSERT(substractCloudIndex.indexedFeatures()); + + pcl::PointCloud::Ptr output(new pcl::PointCloud); + output->resize(cloud->size()); + int oi = 0; // output iterator + for(unsigned int i=0; isize(); ++i) + { + std::vector > kIndices; + std::vector > kDistances; + cv::Mat pt = (cv::Mat_(1, 3) << cloud->at(i).x, cloud->at(i).y, cloud->at(i).z); + substractCloudIndex.radiusSearch(pt, kIndices, kDistances, radiusSearch, minNeighborsInRadius, 32, 0, false); + if(kIndices.size() == 1 && kIndices[0].size() < minNeighborsInRadius) + { + output->at(oi++) = cloud->at(i); + } + } + output->resize(oi); + return output; +} + void MapsManager::publishMaps( const std::map & poses, const ros::Time & stamp, @@ -782,108 +658,322 @@ void MapsManager::publishMaps( UDEBUG("Publishing maps..."); // publish maps - if(cloudMapPub_.getNumSubscribers()) + if(cloudMapPub_.getNumSubscribers() || + scanMapPub_.getNumSubscribers() || + cloudObstaclesPub_.getNumSubscribers() || + cloudGroundPub_.getNumSubscribers()) { // generate the assembled cloud! UTimer time; - pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); - int count = 0; - std::list > negativePoses; + + if(scanMapPub_.getNumSubscribers()) + { + if(parameters_.find(Parameters::kGridFromDepth()) != parameters_.end() && + uStr2Bool(parameters_.at(Parameters::kGridFromDepth()))) + { + ROS_WARN("/scan_map topic is deprecated! Subscribe to /cloud_map topic " + "instead with . " + "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see " + "all occupancy grid parameters.", + Parameters::kGridFromDepth().c_str()); + } + else + { + ROS_WARN("/scan_map topic is deprecated! Subscribe to /cloud_map topic instead."); + } + } + + // detect if the graph has changed, if so, recreate the clouds + bool graphGroundChanged = false; + bool graphObstacleChanged = false; + bool updateGround = cloudMapPub_.getNumSubscribers() || + scanMapPub_.getNumSubscribers() || + cloudGroundPub_.getNumSubscribers(); + bool updateObstacles = cloudMapPub_.getNumSubscribers() || + scanMapPub_.getNumSubscribers() || + cloudObstaclesPub_.getNumSubscribers(); + for(std::map::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + std::map::const_iterator jter; + if(updateGround) + { + jter = assembledGroundPoses_.find(iter->first); + if(jter != assembledGroundPoses_.end()) + { + UASSERT(!iter->second.isNull() && !jter->second.isNull()); + if(iter->second.getDistanceSquared(jter->second) > 0.0001) + { + graphGroundChanged = true; + } + } + } + if(updateObstacles) + { + jter = assembledObstaclePoses_.find(iter->first); + if(jter != assembledObstaclePoses_.end()) + { + UASSERT(!iter->second.isNull() && !jter->second.isNull()); + if(iter->second.getDistanceSquared(jter->second) > 0.0001) + { + graphObstacleChanged = true; + } + } + } + } + int countObstacles = 0; + int countGrounds = 0; + int previousIndexedGroundSize = assembledGroundIndex_.indexedFeatures(); + int previousIndexedObstacleSize = assembledObstacleIndex_.indexedFeatures(); + if(graphGroundChanged) + { + int previousSize = assembledGround_->size(); + assembledGround_->clear(); + assembledGround_->reserve(previousSize); + assembledGroundPoses_.clear(); + assembledGroundIndex_.release(); + } + if(graphObstacleChanged) + { + int previousSize = assembledObstacles_->size(); + assembledObstacles_->clear(); + assembledObstacles_->reserve(previousSize); + assembledObstaclePoses_.clear(); + assembledObstacleIndex_.release(); + } + + if(graphGroundChanged || graphObstacleChanged) + { + UTimer t; + cv::Mat tmpGroundPts; + cv::Mat tmpObstaclePts; + for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) + { + if(iter->first > 0) + { + if(updateGround && + (graphGroundChanged || assembledGroundPoses_.find(iter->first) == assembledGroundPoses_.end())) + { + assembledGroundPoses_.insert(*iter); + std::map::Ptr >::iterator kter=groundClouds_.find(iter->first); + if(kter != groundClouds_.end() && kter->second->size()) + { + pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(kter->second, iter->second); + *assembledGround_+=*transformed; + if(cloudSubtractFiltering_) + { + for(unsigned int i=0; isize(); ++i) + { + if(tmpGroundPts.empty()) + { + tmpGroundPts = (cv::Mat_(1, 3) << transformed->at(i).x, transformed->at(i).y, transformed->at(i).z); + tmpGroundPts.reserve(previousIndexedGroundSize>0?previousIndexedGroundSize:100); + } + else + { + cv::Mat pt = (cv::Mat_(1, 3) << transformed->at(i).x, transformed->at(i).y, transformed->at(i).z); + tmpGroundPts.push_back(pt); + } + } + } + ++countGrounds; + } + } + if(updateObstacles && + (graphObstacleChanged || assembledObstaclePoses_.find(iter->first) == assembledObstaclePoses_.end())) + { + assembledObstaclePoses_.insert(*iter); + std::map::Ptr >::iterator kter=obstacleClouds_.find(iter->first); + if(kter != obstacleClouds_.end() && kter->second->size()) + { + pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(kter->second, iter->second); + *assembledObstacles_+=*transformed; + if(cloudSubtractFiltering_) + { + for(unsigned int i=0; isize(); ++i) + { + if(tmpObstaclePts.empty()) + { + tmpObstaclePts = (cv::Mat_(1, 3) << transformed->at(i).x, transformed->at(i).y, transformed->at(i).z); + tmpObstaclePts.reserve(previousIndexedObstacleSize>0?previousIndexedObstacleSize:100); + } + else + { + cv::Mat pt = (cv::Mat_(1, 3) << transformed->at(i).x, transformed->at(i).y, transformed->at(i).z); + tmpObstaclePts.push_back(pt); + } + } + } + ++countObstacles; + } + } + } + } + double addingPointsTime = t.ticks(); + + if(graphGroundChanged && !tmpGroundPts.empty()) + { + assembledGroundIndex_.buildKDTreeSingleIndex(tmpGroundPts, 15); + } + if(graphObstacleChanged && !tmpObstaclePts.empty()) + { + assembledObstacleIndex_.buildKDTreeSingleIndex(tmpObstaclePts, 15); + } + double indexingTime = t.ticks(); + UINFO("Graph changed! Time recreating clouds (%d ground, %d obstacles) = %f s (indexing %fs)", countGrounds, countObstacles, addingPointsTime+indexingTime, indexingTime); + } + for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) { if(iter->first > 0) { - std::map::Ptr >::iterator jter = clouds_.find(iter->first); - if(jter != clouds_.end()) + std::map >::iterator jter = gridMaps_.find(iter->first); + if(updateGround && assembledGroundPoses_.find(iter->first) == assembledGroundPoses_.end()) { - pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); - *assembledCloud+=*transformed; - ++count; - } - } - else - { - negativePoses.push_back(*iter); - } - } - - for(std::list >::reverse_iterator iter=negativePoses.rbegin(); iter!=negativePoses.rend(); ++iter) - { - std::map::Ptr >::iterator jter = clouds_.find(iter->first); - - if(jter != clouds_.end() && jter->second->size()) - { - std::map >::iterator kter = cameraModels_.find(iter->first); - if(cloudFrustumCulling_ && kter != cameraModels_.end() && assembledCloud->size()) - { - for(unsigned int i=0; isecond.size(); ++i) + assembledGroundPoses_.insert(*iter); + if(jter!=gridMaps_.end() && jter->second.first.cols) { - if(kter->second[i].isValidForProjection()) + pcl::PointCloud::Ptr transformed = util3d::laserScanToPointCloudRGB(jter->second.first, iter->second, 0, 255, 0); + pcl::PointCloud::Ptr subtractedCloud = transformed; + if(cloudSubtractFiltering_) { - assembledCloud = util3d::frustumFiltering( - assembledCloud, - 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; - ++count; + if(assembledGroundIndex_.indexedFeatures()) + { + subtractedCloud = subtractFiltering(transformed, assembledGroundIndex_, gridCellSize_, cloudSubtractFilteringMinNeighbors_); + } + if(subtractedCloud->size()) + { + UDEBUG("Adding ground %d pts=%d/%d (index=%d)", iter->first, subtractedCloud->size(), transformed->size(), assembledGroundIndex_.indexedFeatures()); + cv::Mat pts(subtractedCloud->size(), 3, CV_32FC1); + for(unsigned int i=0; isize(); ++i) + { + pts.at(i, 0) = subtractedCloud->at(i).x; + pts.at(i, 1) = subtractedCloud->at(i).y; + pts.at(i, 2) = subtractedCloud->at(i).z; + } + if(!assembledGroundIndex_.isBuilt()) + { + assembledGroundIndex_.buildKDTreeSingleIndex(pts, 15); + } + else + { + assembledGroundIndex_.addPoints(pts); + } + } } + groundClouds_.insert(std::make_pair(iter->first, util3d::transformPointCloud(subtractedCloud, iter->second.inverse()))); + if(subtractedCloud->size()) + { + *assembledGround_+=*subtractedCloud; + } + ++countGrounds; } } - else + if(updateObstacles && assembledObstaclePoses_.find(iter->first) == assembledObstaclePoses_.end()) { - pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); - if(assembledCloud->size()) + assembledObstaclePoses_.insert(*iter); + if(jter!=gridMaps_.end() && jter->second.second.cols) { - *assembledCloud+=*transformed; + pcl::PointCloud::Ptr transformed = util3d::laserScanToPointCloudRGB(jter->second.second, iter->second, 255, 0, 0); + pcl::PointCloud::Ptr subtractedCloud = transformed; + if(cloudSubtractFiltering_) + { + if(assembledObstacleIndex_.indexedFeatures()) + { + subtractedCloud = subtractFiltering(transformed, assembledObstacleIndex_, gridCellSize_, cloudSubtractFilteringMinNeighbors_); + } + if(subtractedCloud->size()) + { + UDEBUG("Adding obstacle %d pts=%d/%d (index=%d)", iter->first, subtractedCloud->size(), transformed->size(), assembledObstacleIndex_.indexedFeatures()); + cv::Mat pts(subtractedCloud->size(), 3, CV_32FC1); + for(unsigned int i=0; isize(); ++i) + { + pts.at(i, 0) = subtractedCloud->at(i).x; + pts.at(i, 1) = subtractedCloud->at(i).y; + pts.at(i, 2) = subtractedCloud->at(i).z; + } + if(!assembledObstacleIndex_.isBuilt()) + { + assembledObstacleIndex_.buildKDTreeSingleIndex(pts, 15); + } + else + { + assembledObstacleIndex_.addPoints(pts); + } + } + } + obstacleClouds_.insert(std::make_pair(iter->first, util3d::transformPointCloud(subtractedCloud, iter->second.inverse()))); + if(subtractedCloud->size()) + { + *assembledObstacles_+=*subtractedCloud; + } + ++countObstacles; } - else - { - assembledCloud = transformed; - } - ++count; } } } - if(assembledCloud->size() && (cloudFloorCullingHeight_ > 0.0 || cloudCeilingCullingHeight_ > 0.0)) + if(cloudOutputVoxelized_) { - assembledCloud = util3d::passThrough(assembledCloud, "z", - cloudFloorCullingHeight_>0.0?cloudFloorCullingHeight_:-999.0, - cloudCeilingCullingHeight_>0.0 && (cloudFloorCullingHeight_<=0.0 || cloudCeilingCullingHeight_>cloudFloorCullingHeight_)?cloudCeilingCullingHeight_:999.0); + UASSERT(gridCellSize_ > 0.0); + if(countGrounds && assembledGround_->size()) + { + assembledGround_ = util3d::voxelize(assembledGround_, gridCellSize_); + } + if(countObstacles && assembledObstacles_->size()) + { + assembledObstacles_ = util3d::voxelize(assembledObstacles_, gridCellSize_); + } } - if(assembledCloud->size() && cloudVoxelSize_ > 0 && cloudOutputVoxelized_) - { - assembledCloud = util3d::voxelize(assembledCloud, cloudVoxelSize_); - } + ROS_INFO("Assembled %d obstacle and %d ground clouds (%d points, %fs)", + countObstacles, countGrounds, (int)(assembledGround_->size() + assembledObstacles_->size()), time.ticks()); - ROS_INFO("Assembled %d clouds (%fs)", count, time.ticks()); - - if(assembledCloud->size()) + if(cloudGroundPub_.getNumSubscribers()) { sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); - pcl::toROSMsg(*assembledCloud, *cloudMsg); + pcl::toROSMsg(*assembledGround_, *cloudMsg); cloudMsg->header.stamp = stamp; cloudMsg->header.frame_id = mapFrameId; - cloudMapPub_.publish(cloudMsg); + cloudGroundPub_.publish(cloudMsg); } - else if(poses.size() - negativePoses.size()) + if(cloudObstaclesPub_.getNumSubscribers()) { - ROS_WARN("Cloud map is empty! (poses=%d clouds=%d)", (int)poses.size(), (int)clouds_.size()); + sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); + pcl::toROSMsg(*assembledObstacles_, *cloudMsg); + cloudMsg->header.stamp = stamp; + cloudMsg->header.frame_id = mapFrameId; + cloudObstaclesPub_.publish(cloudMsg); + } + if(cloudMapPub_.getNumSubscribers() || scanMapPub_.getNumSubscribers()) + { + pcl::PointCloud cloud = *assembledObstacles_ + *assembledGround_; + sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); + pcl::toROSMsg(cloud, *cloudMsg); + cloudMsg->header.stamp = stamp; + cloudMsg->header.frame_id = mapFrameId; + + if(cloudMapPub_.getNumSubscribers()) + { + cloudMapPub_.publish(cloudMsg); + } + if(scanMapPub_.getNumSubscribers()) + { + scanMapPub_.publish(cloudMsg); + } } } else if(mapCacheCleanup_) { - clouds_.clear(); - cameraModels_.clear(); + assembledGround_->clear(); + assembledObstacles_->clear(); + assembledGroundPoses_.clear(); + assembledObstaclePoses_.clear(); + assembledGroundIndex_.release(); + assembledObstacleIndex_.release(); + groundClouds_.clear(); + obstacleClouds_.clear(); } + #ifdef WITH_OCTOMAP_ROS #ifdef RTABMAP_OCTOMAP if(octoMapPubBin_.getNumSubscribers() || @@ -911,14 +1001,14 @@ void MapsManager::publishMaps( if(octoMapCloud_.getNumSubscribers() || octoMapEmptySpace_.getNumSubscribers()) { sensor_msgs::PointCloud2 msg; - pcl::IndicesPtr obstacles(new std::vector); - pcl::IndicesPtr ground(new std::vector); - pcl::PointCloud::Ptr cloud = octomap_->createCloud(octomapTreeDepth_, obstacles.get(), ground.get()); + pcl::IndicesPtr obstacleIndices(new std::vector); + pcl::IndicesPtr emptyIndices(new std::vector); + pcl::PointCloud::Ptr cloud = octomap_->createCloud(octomapTreeDepth_, obstacleIndices.get(), emptyIndices.get()); if(octoMapCloud_.getNumSubscribers()) { pcl::PointCloud cloudObstacles; - pcl::copyPointCloud(*cloud, *obstacles, cloudObstacles); + pcl::copyPointCloud(*cloud, *obstacleIndices, cloudObstacles); pcl::toROSMsg(cloudObstacles, msg); msg.header.frame_id = mapFrameId; msg.header.stamp = stamp; @@ -926,9 +1016,9 @@ void MapsManager::publishMaps( } if(octoMapEmptySpace_.getNumSubscribers()) { - pcl::PointCloud cloudGround; - pcl::copyPointCloud(*cloud, *ground, cloudGround); - pcl::toROSMsg(cloudGround, msg); + pcl::PointCloud cloudEmptySpace; + pcl::copyPointCloud(*cloud, *emptyIndices, cloudEmptySpace); + pcl::toROSMsg(cloudEmptySpace, msg); msg.header.frame_id = mapFrameId; msg.header.stamp = stamp; octoMapEmptySpace_.publish(msg); @@ -968,109 +1058,40 @@ void MapsManager::publishMaps( } else if(poses.size()) { - ROS_WARN("Projection map is empty! (proj maps=%d)", (int)projMaps_.size()); + ROS_WARN("Octomap projection map is empty! (poses=%d octomap nodes=%d). " + "Make sure you activated \"%s\" and \"%s\" to true. " + "See \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" for more info.", + (int)poses.size(), (int)octomap_->octree()->size(), + Parameters::kGrid3D().c_str(), Parameters::kGridFromDepth().c_str()); } } } - else + else if(mapCacheCleanup_) { octomap_->clear(); } #endif #endif - - if(scanMapPub_.getNumSubscribers()) + if(gridMapPub_.getNumSubscribers() || projMapPub_.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(projMapPub_.getNumSubscribers()) { - if(iter->first > 0) + if(parameters_.find(Parameters::kGridFromDepth()) != parameters_.end() && + !uStr2Bool(parameters_.at(Parameters::kGridFromDepth()))) { - 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; - } + ROS_WARN("/proj_map topic is deprecated! Subscribe to /grid_map topic " + "instead with . " + "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see " + "all occupancy grid parameters.", + Parameters::kGridFromDepth().c_str()); } - // negative poses are not used - } - - if(assembledCloud->size()) - { - if(assembledCloud->size() && scanVoxelSize_ > 0 && scanOutputVoxelized_) + else { - assembledCloud = util3d::voxelize(assembledCloud, scanVoxelSize_); + ROS_WARN("/proj_map topic is deprecated! Subscribe to /grid_map topic instead."); } - - 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 - float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; - cv::Mat pixels = this->generateProjMap(poses, xMin, yMin, gridCellSize); - - if(!pixels.empty()) - { - //init - nav_msgs::OccupancyGrid map; - map.info.resolution = gridCellSize; - map.info.origin.position.x = 0.0; - map.info.origin.position.y = 0.0; - map.info.origin.position.z = 0.0; - map.info.origin.orientation.x = 0.0; - map.info.origin.orientation.y = 0.0; - map.info.origin.orientation.z = 0.0; - map.info.origin.orientation.w = 1.0; - - map.info.width = pixels.cols; - map.info.height = pixels.rows; - map.info.origin.position.x = xMin; - map.info.origin.position.y = yMin; - map.data.resize(map.info.width * map.info.height); - - memcpy(map.data.data(), pixels.data, map.info.width * map.info.height); - - map.header.frame_id = mapFrameId; - map.header.stamp = stamp; - - projMapPub_.publish(map); - } - else if(poses.size()) - { - ROS_WARN("Projection map is empty! (proj maps=%d)", (int)projMaps_.size()); - } - } - else if(mapCacheCleanup_) - { - projMaps_.clear(); - } - - if(gridMapPub_.getNumSubscribers()) - { // create the grid map float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; cv::Mat pixels = this->generateGridMap(poses, xMin, yMin, gridCellSize); @@ -1099,35 +1120,28 @@ void MapsManager::publishMaps( map.header.frame_id = mapFrameId; map.header.stamp = stamp; - gridMapPub_.publish(map); + if(gridMapPub_.getNumSubscribers()) + { + gridMapPub_.publish(map); + } + if(projMapPub_.getNumSubscribers()) + { + projMapPub_.publish(map); + } } else if(poses.size()) { ROS_WARN("Grid map is empty! (local maps=%d)", (int)gridMaps_.size()); } } - else if(mapCacheCleanup_) + + if(!this->hasSubscribers() && mapCacheCleanup_) { gridMaps_.clear(); + gridMapsViewpoints_.clear(); } } -cv::Mat MapsManager::generateProjMap( - const std::map & poses, - float & xMin, - float & yMin, - float & gridCellSize) -{ - gridCellSize = gridCellSize_; - return util3d::create2DMapFromOccupancyLocalMaps( - poses, - projMaps_, - gridCellSize_, - xMin, yMin, - gridSize_, - gridEroded_); -} - cv::Mat MapsManager::generateGridMap( const std::map & poses, float & xMin, @@ -1135,13 +1149,23 @@ cv::Mat MapsManager::generateGridMap( float & gridCellSize) { gridCellSize = gridCellSize_; - cv::Mat map = util3d::create2DMapFromOccupancyLocalMaps( - poses, - gridMaps_, - gridCellSize_, - xMin, yMin, - gridSize_, - gridEroded_); + cv::Mat map; + if(gridIncremental_) + { + occupancyGrid_->update(poses, gridSize_, footprintRadius_); + map = occupancyGrid_->getMap(xMin, yMin); + } + else + { + map = util3d::create2DMapFromOccupancyLocalMaps( + poses, + gridMaps_, + gridCellSize_, + xMin, yMin, + gridSize_, + gridEroded_, + footprintRadius_); + } return map; } diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp index 6d1ff691..8160a099 100644 --- a/src/MsgConversion.cpp +++ b/src/MsgConversion.cpp @@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -38,6 +39,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include +#include +#include namespace rtabmap_ros { @@ -114,6 +118,78 @@ rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg) return rtabmap::Transform::fromEigen3d(tfPose); } +void toCvCopy(const rtabmap_ros::RGBDImage & image, cv_bridge::CvImagePtr & rgb, cv_bridge::CvImagePtr & depth) +{ + if(!image.rgb.data.empty()) + { + rgb = cv_bridge::toCvCopy(image.rgb); + } + else if(!image.rgbCompressed.data.empty()) + { + rgb = cv_bridge::toCvCopy(image.rgbCompressed); + } + else + { + // empty + rgb = boost::make_shared(); + } + + if(!image.depth.data.empty()) + { + depth = cv_bridge::toCvCopy(image.depth); + } + else if(!image.depthCompressed.data.empty()) + { + cv_bridge::CvImagePtr ptr = boost::make_shared(); + ptr->header = image.depthCompressed.header; + ptr->image = rtabmap::uncompressImage(image.depthCompressed.data); + ROS_ASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1); + ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1; + depth = ptr; + } + else + { + // empty + depth = boost::make_shared(); + } +} + +void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth) +{ + if(!image->rgb.data.empty()) + { + rgb = cv_bridge::toCvShare(image->rgb, image); + } + else if(!image->rgbCompressed.data.empty()) + { + rgb = cv_bridge::toCvCopy(image->rgbCompressed); + } + else + { + // empty + rgb = boost::make_shared(); + } + + if(!image->depth.data.empty()) + { + depth = cv_bridge::toCvShare(image->depth, image); + } + else if(!image->depthCompressed.data.empty()) + { + cv_bridge::CvImagePtr ptr = boost::make_shared(); + ptr->header = image->depthCompressed.header; + ptr->image = rtabmap::uncompressImage(image->depthCompressed.data); + ROS_ASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1); + ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1; + depth = ptr; + } + else + { + // empty + depth = boost::make_shared(); + } +} + void compressedMatToBytes(const cv::Mat & compressed, std::vector & bytes) { UASSERT(compressed.empty() || compressed.type() == CV_8UC1); @@ -331,16 +407,42 @@ rtabmap::CameraModel cameraModelFromROS( const sensor_msgs::CameraInfo & camInfo, const rtabmap::Transform & localTransform) { - image_geometry::PinholeCameraModel model; - model.fromCameraInfo(camInfo); + cv::Mat D; + if(camInfo.D.size()) + { + D = cv::Mat(1, camInfo.D.size(), CV_64FC1); + memcpy(D.data, camInfo.D.data(), D.cols*sizeof(double)); + } + + cv:: Mat K; + UASSERT(camInfo.K.empty() || camInfo.K.size() == 9); + if(!camInfo.K.empty()) + { + K = cv::Mat(3, 3, CV_64FC1); + memcpy(K.data, camInfo.K.elems, 9*sizeof(double)); + } + + cv:: Mat R; + UASSERT(camInfo.R.empty() || camInfo.R.size() == 9); + if(!camInfo.R.empty()) + { + R = cv::Mat(3, 3, CV_64FC1); + memcpy(R.data, camInfo.R.elems, 9*sizeof(double)); + } + + cv:: Mat P; + UASSERT(camInfo.P.empty() || camInfo.P.size() == 12); + if(!camInfo.P.empty()) + { + P = cv::Mat(3, 4, CV_64FC1); + memcpy(P.data, camInfo.P.elems, 12*sizeof(double)); + } + return rtabmap::CameraModel( - model.fx(), - model.fy(), - model.cx(), - model.cy(), - localTransform, - 0.0, - cv::Size(model.fullResolution().width, model.fullResolution().height)); + "ros", + cv::Size(camInfo.width, camInfo.height), + K, D, R, P, + localTransform); } void cameraModelToROS( const rtabmap::CameraModel & model, @@ -400,16 +502,11 @@ rtabmap::StereoCameraModel stereoCameraModelFromROS( 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)); + "ros", + cameraModelFromROS(leftCamInfo, localTransform), + cameraModelFromROS(rightCamInfo, localTransform), + rtabmap::Transform()); } void mapDataFromROS( @@ -504,11 +601,14 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg) //Features stuff... std::multimap words; std::multimap words3D; + std::multimap wordsDescriptors; pcl::PointCloud cloud; + cv::Mat descriptors; if(msg.wordPts.data.size() && msg.wordPts.height*msg.wordPts.width == msg.wordIds.size()) { pcl::fromROSMsg(msg.wordPts, cloud); + descriptors = rtabmap::uncompressData(msg.descriptors); } for(unsigned int i=0; isecond.cols, + signature.getWordsDescriptors().begin()->second.type()); + index = 0; + bool valid = true; + for(std::multimap::const_iterator jter=signature.getWordsDescriptors().begin(); + jter!=signature.getWordsDescriptors().end() && valid; + ++jter) + { + if(jter->second.cols == descriptors.cols && + jter->second.type() == descriptors.type()) + { + jter->second.copyTo(descriptors.row(index++)); + } + else + { + valid = false; + ROS_ERROR("Some descriptors have different type/size! Cannot copy them..."); + } + } + + if(valid) + { + msg.descriptors = rtabmap::compressData(descriptors); + } + } + else if(signature.getWordsDescriptors().size()) + { + ROS_ERROR("Words and descriptors must have the same size (%d vs %d)!", + (int)signature.getWords().size(), + (int)signature.getWordsDescriptors().size()); + } } rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData & msg) @@ -711,8 +877,10 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg) info.features = msg.features; info.inliers = msg.inliers; info.localMapSize = msg.localMapSize; + info.localScanMapSize = msg.localScanMapSize; info.timeEstimation = msg.timeEstimation; - info.variance = msg.variance; + info.varianceLin = msg.varianceLin; + info.varianceAng = msg.varianceAng; info.timeParticleFiltering = msg.timeParticleFiltering; info.stamp = msg.stamp; info.interval = msg.interval; @@ -742,6 +910,8 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg) info.localMap.insert(std::make_pair(msg.localMapKeys[i], point3fFromROS(msg.localMapValues[i]))); } + info.localScanMap = rtabmap::uncompressData(msg.localScanMap); + return info; } @@ -752,8 +922,10 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m msg.features = info.features; msg.inliers = info.inliers; msg.localMapSize = info.localMapSize; + msg.localScanMapSize = info.localScanMapSize; msg.timeEstimation = info.timeEstimation; - msg.variance = info.variance; + msg.varianceLin = info.varianceLin; + msg.varianceAng = info.varianceAng; msg.timeParticleFiltering = info.timeParticleFiltering; msg.stamp = info.stamp; msg.interval = info.interval; @@ -777,6 +949,492 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m msg.localMapKeys = uKeys(info.localMap); points3fToROS(uValues(info.localMap), msg.localMapValues); + msg.localScanMap = rtabmap::compressData(info.localScanMap); +} + +cv::Mat userDataFromROS(const rtabmap_ros::UserData & dataMsg) +{ + cv::Mat data; + if(!dataMsg.data.empty()) + { + if(dataMsg.cols > 0 && dataMsg.rows > 0 && dataMsg.type >= 0) + { + data = cv::Mat(dataMsg.rows, dataMsg.cols, dataMsg.type, (void*)dataMsg.data.data()).clone(); + } + else + { + if(dataMsg.cols != (int)dataMsg.data.size() || dataMsg.rows != 1 || dataMsg.type != CV_8UC1) + { + ROS_ERROR("cols, rows and type fields of the UserData msg " + "are not correctly set (cols=%d, rows=%d, type=%d)! We assume that the data " + "is compressed (cols=%d, rows=1, type=%d(CV_8UC1)).", + dataMsg.cols, dataMsg.rows, dataMsg.type, (int)dataMsg.data.size(), CV_8UC1); + + } + data = cv::Mat(1, dataMsg.data.size(), CV_8UC1, (void*)dataMsg.data.data()).clone(); + } + } + return data; +} +void userDataToROS(const cv::Mat & data, rtabmap_ros::UserData & dataMsg, bool compress) +{ + if(!data.empty()) + { + if(compress) + { + dataMsg.data = rtabmap::compressData(data); + dataMsg.rows = 1; + dataMsg.cols = dataMsg.data.size(); + dataMsg.type = CV_8UC1; + } + else + { + dataMsg.data.resize(data.step[0] * data.rows); // use step for non-contiguous matrices + memcpy(dataMsg.data.data(), data.data, dataMsg.data.size()); + dataMsg.rows = data.rows; + dataMsg.cols = data.cols; + dataMsg.type = data.type(); + } + } +} + +rtabmap::Transform getTransform( + const std::string & fromFrameId, + const std::string & toFrameId, + const ros::Time & stamp, + tf::TransformListener & listener, + double waitForTransform) +{ + // TF ready? + rtabmap::Transform transform; + try + { + if(waitForTransform > 0.0 && !stamp.isZero()) + { + //if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1))) + std::string errorMsg; + if(!listener.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransform), ros::Duration(0.01), &errorMsg)) + { + ROS_WARN("Could not get transform from %s to %s after %f seconds (for stamp=%f)! Error=\"%s\".", + fromFrameId.c_str(), toFrameId.c_str(), waitForTransform, stamp.toSec(), errorMsg.c_str()); + return transform; + } + } + + tf::StampedTransform tmp; + listener.lookupTransform(fromFrameId, toFrameId, stamp, tmp); + transform = rtabmap_ros::transformFromTF(tmp); + } + catch(tf::TransformException & ex) + { + ROS_WARN("%s",ex.what()); + } + return transform; +} + +// get moving transform accordingly to a fixed frame. For example get +// transform between moving /base_link between two stamps accordingly to /odom frame. +rtabmap::Transform getTransform( + const std::string & sourceTargetFrame, + const std::string & fixedFrame, + const ros::Time & stampSource, + const ros::Time & stampTarget, + tf::TransformListener & listener, + double waitForTransform) +{ + // TF ready? + rtabmap::Transform transform; + try + { + ros::Time stamp = stampSource>stampTarget?stampSource:stampTarget; + if(waitForTransform > 0.0 && !stamp.isZero()) + { + std::string errorMsg; + if(!listener.waitForTransform(sourceTargetFrame, fixedFrame, stamp, ros::Duration(waitForTransform), ros::Duration(0.01), &errorMsg)) + { + ROS_WARN("Could not get transform from %s to %s accordingly to %s after %f seconds (for stamps=%f -> %f)! Error=\"%s\".", + sourceTargetFrame.c_str(), sourceTargetFrame.c_str(), fixedFrame.c_str(), waitForTransform, stampSource.toSec(), stampTarget.toSec(), errorMsg.c_str()); + return transform; + } + } + + tf::StampedTransform tmp; + listener.lookupTransform(sourceTargetFrame, stampTarget, sourceTargetFrame, stampSource, fixedFrame, tmp); + transform = rtabmap_ros::transformFromTF(tmp); + } + catch(tf::TransformException & ex) + { + ROS_WARN("%s",ex.what()); + } + return transform; +} + +bool convertRGBDMsgs( + const std::vector & imageMsgs, + const std::vector & depthMsgs, + const std::vector & cameraInfoMsgs, + const std::string & frameId, + const std::string & odomFrameId, + const ros::Time & odomStamp, + cv::Mat & rgb, + cv::Mat & depth, + std::vector & cameraModels, + tf::TransformListener & listener, + double waitForTransform) +{ + UASSERT(imageMsgs.size()>0 && + imageMsgs.size() == depthMsgs.size() && + imageMsgs.size() == cameraInfoMsgs.size()); + + int imageWidth = imageMsgs[0]->image.cols; + int imageHeight = imageMsgs[0]->image.rows; + int cameraCount = imageMsgs.size(); + for(unsigned int i=0; iencoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || + imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || + imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || + imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || + imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || + !(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 || + depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 || + depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)) + { + ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16"); + return false; + } + + UASSERT_MSG(imageMsgs[i]->image.cols == imageWidth && imageMsgs[i]->image.rows == imageHeight, + uFormat("imageWidth=%d vs %d imageHeight=%d vs %d", + imageWidth, + imageMsgs[i]->image.cols, + imageHeight, + imageMsgs[i]->image.rows).c_str()); + UASSERT_MSG(depthMsgs[i]->image.cols == imageWidth && depthMsgs[i]->image.rows == imageHeight, + uFormat("imageWidth=%d vs %d imageHeight=%d vs %d", + imageWidth, + depthMsgs[i]->image.cols, + imageHeight, + depthMsgs[i]->image.rows).c_str()); + + rtabmap::Transform localTransform = rtabmap_ros::getTransform(frameId, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp, listener, waitForTransform); + if(localTransform.isNull()) + { + ROS_ERROR("TF of received depth image %d at time %fs is not set!", i, depthMsgs[i]->header.stamp.toSec()); + return false; + } + // sync with odometry stamp + if(!odomFrameId.empty() && odomStamp != depthMsgs[i]->header.stamp) + { + rtabmap::Transform sensorT = getTransform( + frameId, + odomFrameId, + odomStamp, + depthMsgs[i]->header.stamp, + listener, + waitForTransform); + if(sensorT.isNull()) + { + ROS_WARN("Could not get odometry value for depth image stamp (%fs). Latest odometry " + "stamp is %fs. The depth image pose will not be synchronized with odometry.", depthMsgs[i]->header.stamp.toSec(), odomStamp.toSec()); + } + else + { + //ROS_WARN("RGBD correction = %s (time diff=%fs)", sensorT.prettyPrint().c_str(), fabs(depthMsgs[i]->header.stamp.toSec()-odomStamp.toSec())); + localTransform = sensorT * localTransform; + } + } + + cv_bridge::CvImageConstPtr ptrImage = imageMsgs[i]; + if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 || + imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0) + { + // do nothing + } + else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + ptrImage = cv_bridge::cvtColor(imageMsgs[i], "mono8"); + } + else + { + ptrImage = cv_bridge::cvtColor(imageMsgs[i], "bgr8"); + } + cv_bridge::CvImageConstPtr ptrDepth = depthMsgs[i]; + cv::Mat subDepth = ptrDepth->image; + + // initialize + if(rgb.empty()) + { + rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type()); + } + if(depth.empty()) + { + depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type()); + } + + if(ptrImage->image.type() == rgb.type()) + { + ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight))); + } + else + { + ROS_ERROR("Some RGB images are not the same type!"); + return false; + } + + if(subDepth.type() == depth.type()) + { + subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight))); + } + else + { + ROS_ERROR("Some Depth images are not the same type!"); + return false; + } + + cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfoMsgs[i], localTransform)); + } + return true; +} + +bool convertStereoMsg( + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, + const std::string & frameId, + const std::string & odomFrameId, + const ros::Time & odomStamp, + cv::Mat & left, + cv::Mat & right, + rtabmap::StereoCameraModel & stereoModel, + tf::TransformListener & listener, + double waitForTransform) +{ + UASSERT(leftImageMsg.get() && rightImageMsg.get()); + UASSERT(leftCamInfoMsg.get() && rightCamInfoMsg.get()); + + if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 || + leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || + leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || + !(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 || + rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || + rightImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)) + { + ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8"); + return false; + } + + if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + left = cv_bridge::toCvCopy(leftImageMsg, "mono8")->image; + } + else + { + left = cv_bridge::toCvCopy(leftImageMsg, "bgr8")->image; + } + right = cv_bridge::toCvCopy(rightImageMsg, "mono8")->image; + + rtabmap::Transform localTransform = getTransform(frameId, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, listener, waitForTransform); + if(localTransform.isNull()) + { + return false; + } + // sync with odometry stamp + if(!odomFrameId.empty() && odomStamp != leftImageMsg->header.stamp) + { + rtabmap::Transform sensorT = getTransform( + frameId, + odomFrameId, + odomStamp, + leftImageMsg->header.stamp, + listener, + waitForTransform); + if(sensorT.isNull()) + { + ROS_WARN("Could not get odometry value for stereo msg stamp (%fs). Latest odometry " + "stamp is %fs. The stereo image pose will not be synchronized with odometry.", leftImageMsg->header.stamp.toSec(), odomStamp.toSec()); + } + else + { + localTransform = sensorT * localTransform; + } + } + + stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform); + + if(stereoModel.baseline() > 10.0) + { + static bool shown = false; + if(!shown) + { + ROS_WARN("Detected baseline (%f m) is quite large! Is your " + "right camera_info P(0,3) correctly set? Note that " + "baseline=-P(0,3)/P(0,0). You may need to calibrate your camera. " + "This warning is printed only once.", + stereoModel.baseline()); + shown = true; + } + } + return true; +} + +bool convertScanMsg( + const sensor_msgs::LaserScanConstPtr& scan2dMsg, + const std::string & frameId, + const std::string & odomFrameId, + const ros::Time & odomStamp, + cv::Mat & scan, + rtabmap::Transform & scanLocalTransform, + tf::TransformListener & listener, + double waitForTransform) +{ + // make sure the frame of the laser is updated too + rtabmap::Transform tmpT = getTransform( + odomFrameId.empty()?frameId:odomFrameId, + scan2dMsg->header.frame_id, + scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment), + listener, + waitForTransform); + if(tmpT.isNull()) + { + return false; + } + + scanLocalTransform = getTransform( + frameId, + scan2dMsg->header.frame_id, + scan2dMsg->header.stamp, + listener, + waitForTransform); + if(scanLocalTransform.isNull()) + { + return false; + } + + //transform in frameId_ frame + sensor_msgs::PointCloud2 scanOut; + laser_geometry::LaserProjection projection; + projection.transformLaserScanToPointCloud(odomFrameId.empty()?frameId:odomFrameId, *scan2dMsg, scanOut, listener); + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(scanOut, *pclScan); + + //transform back in laser frame + rtabmap::Transform laserToOdom = getTransform( + scan2dMsg->header.frame_id, + odomFrameId.empty()?frameId:odomFrameId, + scan2dMsg->header.stamp, + listener, + waitForTransform); + if(laserToOdom.isNull()) + { + return false; + } + + // sync with odometry stamp + if(!odomFrameId.empty() && odomStamp != scan2dMsg->header.stamp) + { + rtabmap::Transform sensorT = getTransform( + frameId, + odomFrameId, + odomStamp, + scan2dMsg->header.stamp, + listener, + waitForTransform); + if(sensorT.isNull()) + { + 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(), odomStamp.toSec()); + } + else + { + //ROS_WARN("scan correction = %s (time diff=%fs)", sensorT.prettyPrint().c_str(), fabs(scan2dMsg->header.stamp.toSec()-odomStamp.toSec())); + scanLocalTransform = sensorT * scanLocalTransform; + } + } + scan = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom); // put back in laser frame + return true; +} + +bool convertScan3dMsg( + const sensor_msgs::PointCloud2ConstPtr & scan3dMsg, + const std::string & frameId, + const std::string & odomFrameId, + const ros::Time & odomStamp, + int scanCloudNormalK, + cv::Mat & scan, + rtabmap::Transform & scanLocalTransform, + tf::TransformListener & listener, + double waitForTransform) +{ + bool containNormals = false; + for(unsigned int i=0; ifields.size(); ++i) + { + if(scan3dMsg->fields[i].name.compare("normal_x") == 0) + { + containNormals = true; + break; + } + } + + scanLocalTransform = getTransform(frameId, scan3dMsg->header.frame_id, scan3dMsg->header.stamp, listener, waitForTransform); + if(scanLocalTransform.isNull()) + { + ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", scan3dMsg->header.stamp.toSec()); + return false; + } + + // sync with odometry stamp + if(!odomFrameId.empty() && odomStamp != scan3dMsg->header.stamp) + { + rtabmap::Transform sensorT = getTransform( + frameId, + odomFrameId, + odomStamp, + scan3dMsg->header.stamp, + listener, + waitForTransform); + if(sensorT.isNull()) + { + ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry " + "stamp is %fs. The 3d laser scan pose will not be synchronized with odometry.", scan3dMsg->header.stamp.toSec(), odomStamp.toSec()); + } + else + { + scanLocalTransform = sensorT * scanLocalTransform; + } + } + + if(containNormals) + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*scan3dMsg, *pclScan); + + scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan); + } + else + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*scan3dMsg, *pclScan); + + if(scanCloudNormalK > 0) + { + //compute normals + pcl::PointCloud::Ptr normals = rtabmap::util3d::computeNormals(pclScan, scanCloudNormalK); + pcl::PointCloud::Ptr pclScanNormal(new pcl::PointCloud); + pcl::concatenateFields(*pclScan, *normals, *pclScanNormal); + scan = rtabmap::util3d::laserScanFromPointCloud(*pclScanNormal); + } + else + { + scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan); + } + } + return true; } } diff --git a/src/nodelets/OdometryROS.cpp b/src/OdometryROS.cpp similarity index 79% rename from src/nodelets/OdometryROS.cpp rename to src/OdometryROS.cpp index 317265db..169dc991 100644 --- a/src/nodelets/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "OdometryROS.h" +#include "rtabmap_ros/OdometryROS.h" #include #include @@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -55,11 +56,14 @@ using namespace rtabmap; namespace rtabmap_ros { -OdometryROS::OdometryROS(bool stereo) : +OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) : odometry_(0), + warningThread_(0), + callbackCalled_(false), frameId_("base_link"), odomFrameId_("odom"), groundTruthFrameId_(""), + guessFrameId_(""), publishTf_(true), waitForTransform_(true), waitForTransformDuration_(0.1), // 100 ms @@ -68,13 +72,21 @@ OdometryROS::OdometryROS(bool stereo) : paused_(false), resetCountdown_(0), resetCurrentCount_(0), - stereo_(stereo) + stereoParams_(stereoParams), + visParams_(visParams), + icpParams_(icpParams) { } OdometryROS::~OdometryROS() { + if(warningThread_) + { + callbackCalled(); + warningThread_->join(); + delete warningThread_; + } ros::NodeHandle & pnh = getPrivateNodeHandle(); if(pnh.ok()) { @@ -95,6 +107,7 @@ void OdometryROS::onInit() odomPub_ = nh.advertise("odom", 1); odomInfoPub_ = nh.advertise("odom_info", 1); odomLocalMap_ = nh.advertise("odom_local_map", 1); + odomLocalScanMap_ = nh.advertise("odom_local_scan_map", 1); odomLastFrame_ = nh.advertise("odom_last_frame", 1); Transform initialPose = Transform::getIdentity(); @@ -112,10 +125,13 @@ void OdometryROS::onInit() pnh.param("config_path", configPath, configPath); pnh.param("publish_null_when_lost", publishNullWhenLost_, publishNullWhenLost_); pnh.param("guess_from_tf", guessFromTf_, guessFromTf_); + pnh.param("guess_frame_id", guessFrameId_, frameId_); - if(publishTf_ && guessFromTf_) + if(publishTf_ && guessFromTf_ && guessFrameId_.compare(frameId_) == 0) { - NODELET_WARN( "\"publish_tf\" and \"guess_from_tf\" cannot be used at the same time. \"guess_from_tf\" is disabled."); + NODELET_WARN( "\"publish_tf\" and \"guess_from_tf\" cannot be used " + "at the same time if \"guess_frame_id\" and \"frame_id\" " + "are the same frame (value=\"%s\"). \"guess_from_tf\" is disabled.", frameId_.c_str()); guessFromTf_ = false; } @@ -159,7 +175,7 @@ void OdometryROS::onInit() //parameters - parameters_ = Parameters::getDefaultOdometryParameters(stereo_); + parameters_ = Parameters::getDefaultOdometryParameters(stereoParams_, visParams_, icpParams_); if(!configPath.empty()) { if(UFile::exists(configPath.c_str())) @@ -267,6 +283,9 @@ void OdometryROS::onInit() Parameters::parse(parameters_, Parameters::kOdomResetCountdown(), resetCountdown_); parameters_.at(Parameters::kOdomResetCountdown()) = "0"; // use modified reset countdown here + + this->updateParameters(parameters_); + odometry_ = Odometry::create(parameters_); if(!initialPose.isIdentity()) { @@ -286,6 +305,31 @@ void OdometryROS::onInit() onOdomInit(); } +void OdometryROS::startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync) +{ + warningThread_ = new boost::thread(boost::bind(&OdometryROS::warningLoop, this, subscribedTopicsMsg, approxSync)); + NODELET_INFO("%s", subscribedTopicsMsg.c_str()); +} + +void OdometryROS::warningLoop(const std::string & subscribedTopicsMsg, bool approxSync) +{ + ros::Duration r(5.0); + while(!callbackCalled_) + { + r.sleep(); + if(!callbackCalled_) + { + ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are " + "published (\"$ rostopic hz my_topic\") and the timestamps in their " + "header are set. %s%s", + getName().c_str(), + approxSync?"":"Parameter \"approx_sync\" is false, which means that input " + "topics should have all the exact timestamp for the callback to be called.", + subscribedTopicsMsg.c_str()); + } + } +} + Transform OdometryROS::getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const { // TF ready? @@ -295,10 +339,11 @@ Transform OdometryROS::getTransform(const std::string & fromFrameId, const std:: if(waitForTransform_ && !stamp.isZero() && waitForTransformDuration_ > 0.0) { //if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1))) - if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_))) + std::string errorMsg; + if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_), ros::Duration(0.01), &errorMsg)) { - NODELET_WARN( "odometry: Could not get transform from %s to %s (stamp=%f) after %f seconds (\"wait_for_transform_duration\"=%f)!", - fromFrameId.c_str(), toFrameId.c_str(), stamp.toSec(), waitForTransformDuration_, waitForTransformDuration_); + NODELET_WARN( "odometry: Could not get transform from %s to %s (stamp=%f) after %f seconds (\"wait_for_transform_duration\"=%f)! Error=\"%s\"", + fromFrameId.c_str(), toFrameId.c_str(), stamp.toSec(), waitForTransformDuration_, waitForTransformDuration_, errorMsg.c_str()); return transform; } } @@ -336,8 +381,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) Transform guess; if(guessFromTf_) { - Transform previousPose = this->getTransform(odomFrameId_, frameId_, ros::Time(odometry_->previousStamp())); - Transform pose = this->getTransform(odomFrameId_, frameId_, stamp); + Transform previousPose = this->getTransform(odomFrameId_, guessFrameId_, ros::Time(odometry_->previousStamp())); + Transform pose = this->getTransform(odomFrameId_, guessFrameId_, stamp); if(!previousPose.isNull() && !pose.isNull()) { guess = previousPose.inverse() * pose; @@ -352,6 +397,11 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) } NODELET_WARN( "TF Guess %s", guess.prettyPrint().c_str());*/ } + else + { + ROS_ERROR("\"guess_from_tf\" is true, but guess cannot be computed between frames \"%s\" -> \"%s\". Aborting odometry update...", odomFrameId_.c_str(), guessFrameId_.c_str()); + return; + } } // process data @@ -393,12 +443,12 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) //set covariance // 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 + odom.pose.covariance.at(0) = info.varianceLin*2; // xx + odom.pose.covariance.at(7) = info.varianceLin*2; // yy + odom.pose.covariance.at(14) = info.varianceLin*2; // zz + odom.pose.covariance.at(21) = info.varianceAng*2; // rr + odom.pose.covariance.at(28) = info.varianceAng*2; // pp + odom.pose.covariance.at(35) = info.varianceAng*2; // yawyaw //set velocity bool setTwist = !odometry_->previousVelocityTransform().isNull(); @@ -414,19 +464,19 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) 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 + odom.twist.covariance.at(0) = setTwist?info.varianceLin:BAD_COVARIANCE; // xx + odom.twist.covariance.at(7) = setTwist?info.varianceLin:BAD_COVARIANCE; // yy + odom.twist.covariance.at(14) = setTwist?info.varianceLin:BAD_COVARIANCE; // zz + odom.twist.covariance.at(21) = setTwist?info.varianceAng:BAD_COVARIANCE; // rr + odom.twist.covariance.at(28) = setTwist?info.varianceAng:BAD_COVARIANCE; // pp + odom.twist.covariance.at(35) = setTwist?info.varianceAng:BAD_COVARIANCE; // yawyaw //publish the message odomPub_.publish(odom); } // local map / reference frame - if(odomLocalMap_.getNumSubscribers() && dynamic_cast(odometry_)) + if(odomLocalMap_.getNumSubscribers() && odometry_->getType() == Odometry::kTypeF2M) { pcl::PointCloud cloud; const std::multimap & map = ((OdometryF2M*)odometry_)->getMap().getWords3(); @@ -443,7 +493,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) if(odomLastFrame_.getNumSubscribers()) { - if(dynamic_cast(odometry_)) + // check which type of Odometry is using + if(odometry_->getType() == Odometry::kTypeF2M) // If it's Frame to Map Odometry { const std::multimap & words3 = ((OdometryF2M*)odometry_)->getLastFrame().getWords3(); if(words3.size()) @@ -463,10 +514,10 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) odomLastFrame_.publish(cloudMsg); } } - else + else if(odometry_->getType() == Odometry::kTypeF2F) // if Using Frame to Frame Odometry { - //Frame to Frame const Signature & refFrame = ((OdometryF2F*)odometry_)->getRefFrame(); + if(refFrame.getWords3().size()) { pcl::PointCloud cloud; @@ -482,8 +533,29 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) cloudMsg.header.frame_id = odomFrameId_; odomLastFrame_.publish(cloudMsg); } + }else{ + NODELET_ERROR("ERROR, Wrong Type of Odometry, Shouldn't happen"); } } + + if(odomLocalScanMap_.getNumSubscribers() && !info.localScanMap.empty()) + { + sensor_msgs::PointCloud2 cloudMsg; + if(info.localScanMap.channels() == 6) + { + pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloudNormal(info.localScanMap); + pcl::toROSMsg(*cloud, cloudMsg); + } + else + { + pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloud(info.localScanMap); + pcl::toROSMsg(*cloud, cloudMsg); + } + + cloudMsg.header.stamp = stamp; // use corresponding time stamp to image + cloudMsg.header.frame_id = odomFrameId_; + odomLocalScanMap_.publish(cloudMsg); + } } else if(publishNullWhenLost_) { @@ -544,12 +616,22 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) odomInfoPub_.publish(infoMsg); } - NODELET_INFO( "Odom: quality=%d, std dev=%fm, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.variance), (ros::WallTime::now()-time).toSec()); -} + if(visParams_) + { + if(icpParams_) + { + NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.inliers, info.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.varianceLin), pose.isNull()?0.0f:std::sqrt(info.varianceAng), (ros::WallTime::now()-time).toSec()); + } + else + { + NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.varianceLin), pose.isNull()?0.0f:std::sqrt(info.varianceAng), (ros::WallTime::now()-time).toSec()); + } + } + else + { + NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.varianceLin), pose.isNull()?0.0f:std::sqrt(info.varianceAng), (ros::WallTime::now()-time).toSec()); + } -bool OdometryROS::isOdometryF2M() const -{ - return dynamic_cast(odometry_) != 0; } bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) diff --git a/src/PreferencesDialogROS.cpp b/src/PreferencesDialogROS.cpp index dbd7957f..d93e7152 100644 --- a/src/PreferencesDialogROS.cpp +++ b/src/PreferencesDialogROS.cpp @@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "PreferencesDialogROS.h" +#include "rtabmap_ros/PreferencesDialogROS.h" #include #include #include diff --git a/src/RGBDICPOdometryNode.cpp b/src/RGBDICPOdometryNode.cpp new file mode 100644 index 00000000..e5fae1de --- /dev/null +++ b/src/RGBDICPOdometryNode.cpp @@ -0,0 +1,78 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include "ros/ros.h" +#include "nodelet/loader.h" +#include +#include + +int main(int argc, char **argv) +{ + ULogger::setType(ULogger::kTypeConsole); + ULogger::setLevel(ULogger::kWarning); + ros::init(argc, argv, "rgbdicp_odometry"); + + // process "--params" argument + nodelet::V_string nargv; + for(int i=1;ifirst + " = \"" + iter->second + "\""; + std::cout << + str << + std::setw(60 - str.size()) << + " [" << + rtabmap::Parameters::getDescription(iter->first).c_str() << + "]" << + std::endl; + } + ROS_WARN("Node will now exit after showing default odometry parameters because " + "argument \"--params\" is detected!"); + exit(0); + } + else if(strcmp(argv[i], "--udebug") == 0) + { + ULogger::setLevel(ULogger::kDebug); + } + else if(strcmp(argv[i], "--uinfo") == 0) + { + ULogger::setLevel(ULogger::kInfo); + } + nargv.push_back(argv[i]); + } + + nodelet::Loader nodelet; + nodelet::M_string remap(ros::names::getRemappings()); + std::string nodelet_name = ros::this_node::getName(); + nodelet.load(nodelet_name, "rtabmap_ros/rgbdicp_odometry", remap, nargv); + ros::spin(); + return 0; +} diff --git a/src/impl/CommonDataSubscriberDepth.cpp b/src/impl/CommonDataSubscriberDepth.cpp new file mode 100644 index 00000000..9b0473a1 --- /dev/null +++ b/src/impl/CommonDataSubscriberDepth.cpp @@ -0,0 +1,369 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include + +namespace rtabmap_ros { + +// RGB + Depth +void CommonDataSubscriber::depthCallback( + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg) +{ + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::depthScan2dCallback( + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, + const sensor_msgs::LaserScanConstPtr& scanMsg) +{ + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::depthScan3dCallback( + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg) +{ + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::LaserScanConstPtr scan2dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg); +} +void CommonDataSubscriber::depthInfoCallback( + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::LaserScanConstPtr scan2dMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg); +} + +// RGB + Depth + Odom +void CommonDataSubscriber::depthOdomCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg) +{ + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::depthOdomScan2dCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, + const sensor_msgs::LaserScanConstPtr& scanMsg) +{ + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::depthOdomScan3dCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg) +{ + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + sensor_msgs::LaserScanConstPtr scan2dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg); +} +void CommonDataSubscriber::depthOdomInfoCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + sensor_msgs::LaserScanConstPtr scan2dMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg); +} + +// RGB + Depth + User Data +void CommonDataSubscriber::depthDataCallback( + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg) +{ + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::depthDataScan2dCallback( + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, + const sensor_msgs::LaserScanConstPtr& scanMsg) +{ + nav_msgs::OdometryConstPtr odomMsg; // null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::depthDataScan3dCallback( + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg) +{ + nav_msgs::OdometryConstPtr odomMsg; // null + sensor_msgs::LaserScanConstPtr scan2dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg); +} +void CommonDataSubscriber::depthDataInfoCallback( + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + nav_msgs::OdometryConstPtr odomMsg; // null + sensor_msgs::LaserScanConstPtr scan2dMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg); +} + +// RGB + Depth + Odom + User Data +void CommonDataSubscriber::depthOdomDataCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg) +{ + sensor_msgs::LaserScanConstPtr scanMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::depthOdomDataScan2dCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, + const sensor_msgs::LaserScanConstPtr& scanMsg) +{ + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::depthOdomDataScan3dCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg) +{ + sensor_msgs::LaserScanConstPtr scan2dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg); +} +void CommonDataSubscriber::depthOdomDataInfoCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + sensor_msgs::LaserScanConstPtr scan2dMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg); +} + +void CommonDataSubscriber::setupDepthCallbacks( + ros::NodeHandle & nh, + ros::NodeHandle & pnh, + bool subscribeOdom, + bool subscribeUserData, + bool subscribeScan2d, + bool subscribeScan3d, + bool subscribeOdomInfo, + int queueSize, + bool approxSync) +{ + ROS_INFO("Setup depth callback"); + + std::string rgbPrefix = "rgb"; + std::string depthPrefix = "depth"; + ros::NodeHandle rgb_nh(nh, rgbPrefix); + ros::NodeHandle depth_nh(nh, depthPrefix); + ros::NodeHandle rgb_pnh(pnh, rgbPrefix); + ros::NodeHandle depth_pnh(pnh, depthPrefix); + image_transport::ImageTransport rgb_it(rgb_nh); + image_transport::ImageTransport depth_it(depth_nh); + image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); + image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); + + imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); + imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); + cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1); + + if(subscribeOdom && subscribeUserData) + { + odomSub_.subscribe(nh, "odom", 1); + userDataSub_.subscribe(nh, "user_data", 1); + + if(subscribeScan2d) + { + subscribedToScan2d_ = true; + scanSub_.subscribe(nh, "scan", 1); + SYNC_DECL6(depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_); + } + else if(subscribeScan3d) + { + subscribedToScan3d_ = true; + scan3dSub_.subscribe(nh, "scan_cloud", 1); + SYNC_DECL6(depthOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_); + } + else if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL6(depthOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); + } + else + { + SYNC_DECL5(depthOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_); + } + } + else if(subscribeOdom) + { + odomSub_.subscribe(nh, "odom", 1); + + if(subscribeScan2d) + { + subscribedToScan2d_ = true; + scanSub_.subscribe(nh, "scan", 1); + SYNC_DECL5(depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_); + } + else if(subscribeScan3d) + { + subscribedToScan3d_ = true; + scan3dSub_.subscribe(nh, "scan_cloud", 1); + SYNC_DECL5(depthOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_); + } + else if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL5(depthOdomInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); + } + else + { + SYNC_DECL4(depthOdom, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_); + } + } + else if(subscribeUserData) + { + userDataSub_.subscribe(nh, "user_data", 1); + + if(subscribeScan2d) + { + subscribedToScan2d_ = true; + scanSub_.subscribe(nh, "scan", 1); + SYNC_DECL5(depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_); + } + else if(subscribeScan3d) + { + subscribedToScan3d_ = true; + scan3dSub_.subscribe(nh, "scan_cloud", 1); + SYNC_DECL5(depthDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_); + } + else if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL5(depthDataInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); + } + else + { + SYNC_DECL4(depthData, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_); + } + } + else + { + if(subscribeScan2d) + { + subscribedToScan2d_ = true; + scanSub_.subscribe(nh, "scan", 1); + SYNC_DECL4(depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_); + } + else if(subscribeScan3d) + { + subscribedToScan3d_ = true; + scan3dSub_.subscribe(nh, "scan_cloud", 1); + SYNC_DECL4(depthScan3d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_); + } + else if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL4(depthInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); + } + else + { + SYNC_DECL3(depth, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_); + } + } +} + +} /* namespace rtabmap_ros */ diff --git a/src/impl/CommonDataSubscriberRGBD.cpp b/src/impl/CommonDataSubscriberRGBD.cpp new file mode 100644 index 00000000..a179b4fe --- /dev/null +++ b/src/impl/CommonDataSubscriberRGBD.cpp @@ -0,0 +1,387 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include +#include +#include +#include + +namespace rtabmap_ros { + +// 1 RGBD camera +void CommonDataSubscriber::rgbdCallback( + const rtabmap_ros::RGBDImageConstPtr& image1Msg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbdScan2dCallback( + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const sensor_msgs::LaserScanConstPtr& scanMsg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbdScan3dCallback( + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbdInfoCallback( + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} + +// 1 RGBD camera + Odom +void CommonDataSubscriber::rgbdOdomCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbdOdomScan2dCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const sensor_msgs::LaserScanConstPtr& scanMsg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbdOdomScan3dCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbdOdomInfoCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} + +// 1 RGBD camera + User Data +void CommonDataSubscriber::rgbdDataCallback( + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbdDataScan2dCallback( + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const sensor_msgs::LaserScanConstPtr& scanMsg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbdDataScan3dCallback( + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbdDataInfoCallback( + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} + +// 1 RGBD camera + Odom + User Data +void CommonDataSubscriber::rgbdOdomDataCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + sensor_msgs::LaserScanConstPtr scanMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbdOdomDataScan2dCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const sensor_msgs::LaserScanConstPtr& scanMsg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbdOdomDataScan3dCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + sensor_msgs::LaserScanConstPtr scanMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbdOdomDataInfoCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::UserDataConstPtr & userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + cv_bridge::CvImageConstPtr rgb, depth; + rtabmap_ros::toCvShare(image1Msg, rgb, depth); + + sensor_msgs::LaserScanConstPtr scanMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg); +} + +void CommonDataSubscriber::setupRGBDCallbacks( + ros::NodeHandle & nh, + ros::NodeHandle & pnh, + bool subscribeOdom, + bool subscribeUserData, + bool subscribeScan2d, + bool subscribeScan3d, + bool subscribeOdomInfo, + int queueSize, + bool approxSync) +{ + ROS_INFO("Setup rgbd callback"); + + if(subscribeOdom || subscribeUserData || subscribeScan2d || subscribeScan3d || subscribeOdomInfo) + { + rgbdSubs_.resize(1); + rgbdSubs_[0] = new message_filters::Subscriber; + rgbdSubs_[0]->subscribe(nh, "rgbd_image", 1); + + + if(subscribeOdom && subscribeUserData) + { + odomSub_.subscribe(nh, "odom", 1); + userDataSub_.subscribe(nh, "user_data", 1); + if(subscribeScan2d) + { + subscribedToScan2d_ = true; + scanSub_.subscribe(nh, "scan", 1); + SYNC_DECL4(rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_); + } + else if(subscribeScan3d) + { + subscribedToScan3d_ = true; + scan3dSub_.subscribe(nh, "scan_cloud", 1); + SYNC_DECL4(rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_); + } + else if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL4(rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_); + } + else + { + SYNC_DECL3(rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0])); + } + } + else if(subscribeOdom) + { + odomSub_.subscribe(nh, "odom", 1); + if(subscribeScan2d) + { + subscribedToScan2d_ = true; + scanSub_.subscribe(nh, "scan", 1); + SYNC_DECL3(rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_); + } + else if(subscribeScan3d) + { + subscribedToScan3d_ = true; + scan3dSub_.subscribe(nh, "scan_cloud", 1); + SYNC_DECL3(rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_); + } + else if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL3(rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_); + } + else + { + SYNC_DECL2(rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0])); + } + } + else if(subscribeUserData) + { + userDataSub_.subscribe(nh, "user_data", 1); + if(subscribeScan2d) + { + subscribedToScan2d_ = true; + scanSub_.subscribe(nh, "scan", 1); + SYNC_DECL3(rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_); + } + else if(subscribeScan3d) + { + subscribedToScan3d_ = true; + scan3dSub_.subscribe(nh, "scan_cloud", 1); + SYNC_DECL3(rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_); + } + else if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL3(rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_); + } + else + { + SYNC_DECL2(rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0])); + } + } + else + { + if(subscribeScan2d) + { + subscribedToScan2d_ = true; + scanSub_.subscribe(nh, "scan", 1); + SYNC_DECL2(rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_); + } + else if(subscribeScan3d) + { + subscribedToScan3d_ = true; + scan3dSub_.subscribe(nh, "scan_cloud", 1); + SYNC_DECL2(rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_); + } + else if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL2(rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_); + } + else + { + ROS_FATAL("Not supposed to be here!"); + } + } + } + else + { + rgbdSub_ = nh.subscribe("rgbd_image", 1, &CommonDataSubscriber::rgbdCallback, this); + + subscribedTopicsMsg_ = + uFormat("\n%s subscribed to:\n %s", + ros::this_node::getName().c_str(), + rgbdSub_.getTopic().c_str()); + } +} + +} /* namespace rtabmap_ros */ diff --git a/src/impl/CommonDataSubscriberRGBD2.cpp b/src/impl/CommonDataSubscriberRGBD2.cpp new file mode 100644 index 00000000..ac9dde64 --- /dev/null +++ b/src/impl/CommonDataSubscriberRGBD2.cpp @@ -0,0 +1,386 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include +#include +#include +#include + +namespace rtabmap_ros { + +#define IMAGE_CONVERSION() \ + callbackCalled(); \ + std::vector imageMsgs(2); \ + std::vector depthMsgs(2); \ + rtabmap_ros::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \ + rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \ + std::vector cameraInfoMsgs; \ + cameraInfoMsgs.push_back(image1Msg->cameraInfo); \ + cameraInfoMsgs.push_back(image2Msg->cameraInfo); + +// 2 RGBD +void CommonDataSubscriber::rgbd2Callback( + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg) +{ + IMAGE_CONVERSION(); + + nav_msgs::OdometryConstPtr odomMsg; // Null + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbd2Scan2dCallback( + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg, + const sensor_msgs::LaserScanConstPtr& scanMsg) +{ + IMAGE_CONVERSION(); + + nav_msgs::OdometryConstPtr odomMsg; // Null + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbd2Scan3dCallback( + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg) +{ + IMAGE_CONVERSION(); + + nav_msgs::OdometryConstPtr odomMsg; // Null + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbd2InfoCallback( + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + IMAGE_CONVERSION(); + + nav_msgs::OdometryConstPtr odomMsg; // Null + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} + +// 2 RGBD + Odom +void CommonDataSubscriber::rgbd2OdomCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg) +{ + IMAGE_CONVERSION(); + + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbd2OdomScan2dCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg, + const sensor_msgs::LaserScanConstPtr& scanMsg) +{ + IMAGE_CONVERSION(); + + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbd2OdomScan3dCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg) +{ + IMAGE_CONVERSION(); + + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbd2OdomInfoCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + IMAGE_CONVERSION(); + + rtabmap_ros::UserDataConstPtr userDataMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} + +// 2 RGBD + User Data +void CommonDataSubscriber::rgbd2DataCallback( + const rtabmap_ros::UserDataConstPtr& userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg) +{ + IMAGE_CONVERSION(); + + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbd2DataScan2dCallback( + const rtabmap_ros::UserDataConstPtr& userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg, + const sensor_msgs::LaserScanConstPtr& scanMsg) +{ + IMAGE_CONVERSION(); + + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbd2DataScan3dCallback( + const rtabmap_ros::UserDataConstPtr& userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg) +{ + IMAGE_CONVERSION(); + + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbd2DataInfoCallback( + const rtabmap_ros::UserDataConstPtr& userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + IMAGE_CONVERSION(); + + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} + +// 2 RGBD + Odom + User Data +void CommonDataSubscriber::rgbd2OdomDataCallback( + const nav_msgs::OdometryConstPtr& odomMsg, + const rtabmap_ros::UserDataConstPtr& userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg) +{ + IMAGE_CONVERSION(); + + sensor_msgs::LaserScanConstPtr scanMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbd2OdomDataScan2dCallback( + const nav_msgs::OdometryConstPtr& odomMsg, + const rtabmap_ros::UserDataConstPtr& userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg, + const sensor_msgs::LaserScanConstPtr& scanMsg) +{ + IMAGE_CONVERSION(); + + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbd2OdomDataScan3dCallback( + const nav_msgs::OdometryConstPtr& odomMsg, + const rtabmap_ros::UserDataConstPtr& userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg) +{ + IMAGE_CONVERSION(); + + sensor_msgs::LaserScanConstPtr scanMsg; // Null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::rgbd2OdomDataInfoCallback( + const nav_msgs::OdometryConstPtr& odomMsg, + const rtabmap_ros::UserDataConstPtr& userDataMsg, + const rtabmap_ros::RGBDImageConstPtr& image1Msg, + const rtabmap_ros::RGBDImageConstPtr& image2Msg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + IMAGE_CONVERSION(); + + sensor_msgs::LaserScanConstPtr scanMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); +} + +void CommonDataSubscriber::setupRGBD2Callbacks( + ros::NodeHandle & nh, + ros::NodeHandle & pnh, + bool subscribeOdom, + bool subscribeUserData, + bool subscribeScan2d, + bool subscribeScan3d, + bool subscribeOdomInfo, + int queueSize, + bool approxSync) +{ + ROS_INFO("Setup rgbd2 callback"); + + rgbdSubs_.resize(2); + for(int i=0; i<2; ++i) + { + rgbdSubs_[i] = new message_filters::Subscriber; + rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), 1); + } + if(subscribeOdom && subscribeUserData) + { + odomSub_.subscribe(nh, "odom", 1); + userDataSub_.subscribe(nh, "user_data", 1); + if(subscribeScan2d) + { + subscribedToScan2d_ = true; + scanSub_.subscribe(nh, "scan", 1); + SYNC_DECL5(rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); + } + else if(subscribeScan3d) + { + subscribedToScan3d_ = true; + scan3dSub_.subscribe(nh, "scan_cloud", 1); + SYNC_DECL5(rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); + } + else if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL5(rgbd2OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); + } + else + { + SYNC_DECL4(rgbd2OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); + } + } + else if(subscribeOdom) + { + odomSub_.subscribe(nh, "odom", 1); + if(subscribeScan2d) + { + subscribedToScan2d_ = true; + scanSub_.subscribe(nh, "scan", 1); + SYNC_DECL4(rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); + } + else if(subscribeScan3d) + { + subscribedToScan3d_ = true; + scan3dSub_.subscribe(nh, "scan_cloud", 1); + SYNC_DECL4(rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); + } + else if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL4(rgbd2OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); + } + else + { + SYNC_DECL3(rgbd2Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); + } + } + else if(subscribeUserData) + { + userDataSub_.subscribe(nh, "user_data", 1); + if(subscribeScan2d) + { + subscribedToScan2d_ = true; + scanSub_.subscribe(nh, "scan", 1); + SYNC_DECL4(rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); + } + else if(subscribeScan3d) + { + subscribedToScan3d_ = true; + scan3dSub_.subscribe(nh, "scan_cloud", 1); + SYNC_DECL4(rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); + } + else if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL4(rgbd2DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); + } + else + { + SYNC_DECL3(rgbd2Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); + } + } + else + { + if(subscribeScan2d) + { + subscribedToScan2d_ = true; + scanSub_.subscribe(nh, "scan", 1); + SYNC_DECL3(rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); + } + else if(subscribeScan3d) + { + subscribedToScan3d_ = true; + scan3dSub_.subscribe(nh, "scan_cloud", 1); + SYNC_DECL3(rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); + } + else if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL3(rgbd2Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); + } + else + { + SYNC_DECL2(rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1])); + } + } +} + +} /* namespace rtabmap_ros */ diff --git a/src/impl/CommonDataSubscriberStereo.cpp b/src/impl/CommonDataSubscriberStereo.cpp new file mode 100644 index 00000000..bf27bdf2 --- /dev/null +++ b/src/impl/CommonDataSubscriberStereo.cpp @@ -0,0 +1,142 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include + +namespace rtabmap_ros { + +// Stereo +void CommonDataSubscriber::stereoCallback( + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg) +{ + callbackCalled(); + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::LaserScanConstPtr scanMsg; // null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonStereoCallback(odomMsg, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::stereoInfoCallback( + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, + const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) +{ + callbackCalled(); + nav_msgs::OdometryConstPtr odomMsg; // Null + sensor_msgs::LaserScanConstPtr scan2dMsg; // null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null + commonStereoCallback(odomMsg, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg); +} + +// Stereo + Odom +void CommonDataSubscriber::stereoOdomCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg) +{ + callbackCalled(); + sensor_msgs::LaserScanConstPtr scanMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null + rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null + commonStereoCallback(odomMsg, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg); +} +void CommonDataSubscriber::stereoOdomInfoCallback( + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, + const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg) +{ + callbackCalled(); + sensor_msgs::LaserScanConstPtr scan2dMsg; // Null + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonStereoCallback(odomMsg, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg); +} + +void CommonDataSubscriber::setupStereoCallbacks( + ros::NodeHandle & nh, + ros::NodeHandle & pnh, + bool subscribeOdom, + bool subscribeOdomInfo, + int queueSize, + bool approxSync) +{ + ROS_INFO("Setup stereo callback"); + + ros::NodeHandle left_nh(nh, "left"); + ros::NodeHandle right_nh(nh, "right"); + ros::NodeHandle left_pnh(pnh, "left"); + ros::NodeHandle right_pnh(pnh, "right"); + image_transport::ImageTransport left_it(left_nh); + image_transport::ImageTransport right_it(right_nh); + image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh); + image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh); + + imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft); + imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight); + cameraInfoLeft_.subscribe(left_nh, "camera_info", 1); + cameraInfoRight_.subscribe(right_nh, "camera_info", 1); + + if(subscribeOdom) + { + odomSub_.subscribe(nh, "odom", 1); + + if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL6(stereoOdomInfo, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_); + } + else + { + SYNC_DECL5(stereoOdom, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + } + } + else + { + if(subscribeOdomInfo) + { + subscribedToOdomInfo_ = true; + odomInfoSub_.subscribe(nh, "odom_info", 1); + SYNC_DECL5(stereoInfo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_); + } + else + { + SYNC_DECL4(stereo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + } + } +} + +} /* namespace rtabmap_ros */ diff --git a/src/nodelets/icp_odometry.cpp b/src/nodelets/icp_odometry.cpp new file mode 100644 index 00000000..6c96fcb2 --- /dev/null +++ b/src/nodelets/icp_odometry.cpp @@ -0,0 +1,198 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include + +#include +#include + +#include + +#include +#include +#include + +#include "rtabmap_ros/MsgConversion.h" + +#include +#include +#include +#include +#include +#include +#include + +using namespace rtabmap; + +namespace rtabmap_ros +{ + +class ICPOdometry : public rtabmap_ros::OdometryROS +{ +public: + ICPOdometry() : + OdometryROS(false, false, true), + scanCloudMaxPoints_(0), + scanCloudNormalK_(0) + { + } + + virtual ~ICPOdometry() + { + } + +private: + + virtual void onOdomInit() + { + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_); + pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_); + + scan_sub_ = nh.subscribe("scan", 1, &ICPOdometry::callbackScan, this); + cloud_sub_ = nh.subscribe("scan_cloud", 1, &ICPOdometry::callbackCloud, this); + } + + virtual void updateParameters(ParametersMap & parameters) + { + //make sure we are using Reg/Strategy=0 + ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy()); + if(iter != parameters.end() && iter->second.compare("0") != 0) + { + ROS_WARN("ICP odometry works only with \"Reg/Strategy\"=1. Ignoring value %s.", iter->second.c_str()); + } + uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "1")); + } + + void callbackScan(const sensor_msgs::LaserScanConstPtr& scanMsg) + { + // make sure the frame of the laser is updated too + Transform localScanTransform = getTransform(this->frameId(), + scanMsg->header.frame_id, + scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)); + if(localScanTransform.isNull()) + { + ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec()); + return; + } + + //transform in frameId_ frame + sensor_msgs::PointCloud2 scanOut; + laser_geometry::LaserProjection projection; + projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener()); + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(scanOut, *pclScan); + + cv::Mat scan = util3d::laserScan2dFromPointCloud(*pclScan); + + rtabmap::SensorData data( + scan, + LaserScanInfo((int)scanMsg->ranges.size(), scanMsg->range_max, localScanTransform), + cv::Mat(), + cv::Mat(), + CameraModel(), + 0, + rtabmap_ros::timestampFromROS(scanMsg->header.stamp)); + + this->processData(data, scanMsg->header.stamp); + } + + void callbackCloud(const sensor_msgs::PointCloud2ConstPtr& cloudMsg) + { + cv::Mat scan; + bool containNormals = false; + for(unsigned int i=0; ifields.size(); ++i) + { + if(cloudMsg->fields[i].name.compare("normal_x") == 0) + { + containNormals = true; + break; + } + } + + Transform localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp); + if(localScanTransform.isNull()) + { + ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg->header.stamp.toSec()); + return; + } + + if(containNormals) + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*cloudMsg, *pclScan); + scan = util3d::laserScanFromPointCloud(*pclScan); + } + else + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*cloudMsg, *pclScan); + + if(scanCloudNormalK_ > 0) + { + //compute normals + pcl::PointCloud::Ptr normals = util3d::computeNormals(pclScan, scanCloudNormalK_); + pcl::PointCloud::Ptr pclScanNormal(new pcl::PointCloud); + pcl::concatenateFields(*pclScan, *normals, *pclScanNormal); + scan = util3d::laserScanFromPointCloud(*pclScanNormal); + } + else + { + scan = util3d::laserScanFromPointCloud(*pclScan); + } + } + + rtabmap::SensorData data( + scan, + LaserScanInfo(scanCloudMaxPoints_, 0, localScanTransform), + cv::Mat(), + cv::Mat(), + CameraModel(), + 0, + rtabmap_ros::timestampFromROS(cloudMsg->header.stamp)); + + this->processData(data, cloudMsg->header.stamp); + } + +protected: + virtual void flushCallbacks() + { + // flush callbacks + } + +private: + ros::Subscriber scan_sub_; + ros::Subscriber cloud_sub_; + int scanCloudMaxPoints_; + int scanCloudNormalK_; +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ICPOdometry, nodelet::Nodelet); + +} diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 0e7dc91e..7a834733 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -36,28 +36,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include -#include -#include -#include - -#include -#include - -#include - -#include -#include - -#include -#include #include -#include "rtabmap/core/util3d.h" -#include "rtabmap/core/util3d_filtering.h" -#include "rtabmap/core/util3d_mapping.h" -#include "rtabmap/core/util3d_transforms.h" +#include "rtabmap/core/OccupancyGrid.h" +#include "rtabmap/utilite/UStl.h" namespace rtabmap_ros { @@ -67,51 +50,156 @@ class ObstaclesDetection : public nodelet::Nodelet public: ObstaclesDetection() : frameId_("base_link"), - 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), - projVoxelSize_(0.01) + waitForTransform_(false) {} virtual ~ObstaclesDetection() {} private: + + void parameterMoved( + ros::NodeHandle & nh, + const std::string & rosName, + const std::string & parameterName, + rtabmap::ParametersMap & parameters) + { + if(nh.hasParam(rosName)) + { + rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid"); + rtabmap::ParametersMap::const_iterator iter =gridParameters.find(parameterName); + if(iter != gridParameters.end()) + { + NODELET_ERROR("obstacles_detection: Parameter \"%s\" has moved from " + "rtabmap_ros to rtabmap library. Use " + "parameter \"%s\" instead. The value is still " + "copied to new parameter name.", + rosName.c_str(), + parameterName.c_str()); + std::string type = rtabmap::Parameters::getType(parameterName); + if(type.compare("float") || type.compare("double")) + { + double v = uStr2Double(iter->second); + nh.getParam(rosName, v); + parameters.insert(rtabmap::ParametersPair(parameterName, uNumber2Str(v))); + } + else if(type.compare("int") || type.compare("unsigned int")) + { + int v = uStr2Int(iter->second); + nh.getParam(rosName, v); + parameters.insert(rtabmap::ParametersPair(parameterName, uNumber2Str(v))); + } + else + { + NODELET_ERROR("Not handled type \"%s\" for parameter \"%s\"", type.c_str(), parameterName.c_str()); + } + } + else + { + NODELET_ERROR("Parameter \"%s\" not found in default parameters.", parameterName.c_str()); + } + } + } + virtual void onInit() { + ROS_DEBUG("_"); // not sure why, but all NODELET_*** log are not shown if a normal ROS_*** is not called!? ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); int queueSize = 10; pnh.param("queue_size", queueSize, queueSize); pnh.param("frame_id", frameId_, frameId_); - 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("map_frame_id", mapFrameId_, mapFrameId_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); - pnh.param("optimize_for_close_objects", optimizeForCloseObjects_, optimizeForCloseObjects_); - pnh.param("proj_voxel_size", projVoxelSize_, projVoxelSize_); + + if(pnh.hasParam("optimize_for_close_objects")) + { + NODELET_ERROR("\"optimize_for_close_objects\" parameter doesn't exist " + "anymore. Use rtabmap_ros/obstacles_detection_old nodelet to use " + "the old interface."); + } + + rtabmap::ParametersMap parameters; + + // Backward compatibility + for(std::map >::const_iterator iter=rtabmap::Parameters::getRemovedParameters().begin(); + iter!=rtabmap::Parameters::getRemovedParameters().end(); + ++iter) + { + std::string vStr; + if(pnh.getParam(iter->first, vStr)) + { + if(iter->second.first) + { + // can be migrated + uInsert(parameters, rtabmap::ParametersPair(iter->second.second, vStr)); + NODELET_ERROR("obstacles_detection: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.", + iter->first.c_str(), iter->second.second.c_str(), vStr.c_str()); + } + else + { + if(iter->second.second.empty()) + { + NODELET_ERROR("obstacles_detection: Parameter \"%s\" doesn't exist anymore!", + iter->first.c_str()); + } + else + { + NODELET_ERROR("obstacles_detection: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", + iter->first.c_str(), iter->second.second.c_str()); + } + } + } + } + + rtabmap::ParametersMap gridParameters2 = rtabmap::Parameters::getDefaultParameters(); + rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid"); + for(rtabmap::ParametersMap::iterator iter=gridParameters.begin(); iter!=gridParameters.end(); ++iter) + { + std::string vStr; + bool vBool; + int vInt; + double vDouble; + if(pnh.getParam(iter->first, vStr)) + { + NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str()); + iter->second = vStr; + } + else if(pnh.getParam(iter->first, vBool)) + { + NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str()); + iter->second = uBool2Str(vBool); + } + else if(pnh.getParam(iter->first, vDouble)) + { + NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str()); + iter->second = uNumber2Str(vDouble); + } + else if(pnh.getParam(iter->first, vInt)) + { + NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str()); + iter->second = uNumber2Str(vInt); + } + } + uInsert(parameters, gridParameters); + parameterMoved(pnh, "proj_voxel_size", rtabmap::Parameters::kGridCellSize(), parameters); + parameterMoved(pnh, "ground_normal_angle", rtabmap::Parameters::kGridMaxGroundAngle(), parameters); + parameterMoved(pnh, "min_cluster_size", rtabmap::Parameters::kGridMinClusterSize(), parameters); + parameterMoved(pnh, "normal_estimation_radius", rtabmap::Parameters::kGridClusterRadius(), parameters); + parameterMoved(pnh, "cluster_radius", rtabmap::Parameters::kGridClusterRadius(), parameters); + parameterMoved(pnh, "max_obstacles_height", rtabmap::Parameters::kGridMaxObstacleHeight(), parameters); + parameterMoved(pnh, "max_ground_height", rtabmap::Parameters::kGridMaxGroundHeight(), parameters); + parameterMoved(pnh, "detect_flat_obstacles", rtabmap::Parameters::kGridFlatObstacleDetected(), parameters); + parameterMoved(pnh, "normal_k", rtabmap::Parameters::kGridNormalK(), parameters); + + UASSERT(uContains(parameters, rtabmap::Parameters::kGridMapFrameProjection())); + if(uStr2Bool(parameters.at(rtabmap::Parameters::kGridMapFrameProjection())) && mapFrameId_.empty()) + { + NODELET_ERROR("obstacles_detection: Parameter \"%s\" is true but map_frame_id is not set!", rtabmap::Parameters::kGridMapFrameProjection().c_str()); + } + + grid_.parseParameters(parameters); cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this); @@ -153,8 +241,32 @@ private: return; } - pcl::PointCloud::Ptr originalCloud(new pcl::PointCloud); - pcl::fromROSMsg(*cloudMsg, *originalCloud); + rtabmap::Transform pose = rtabmap::Transform::getIdentity(); + if(!mapFrameId_.empty()) + { + try + { + if(waitForTransform_) + { + if(!tfListener_.waitForTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, ros::Duration(1))) + { + NODELET_ERROR("Could not get transform from %s to %s after 1 second!", mapFrameId_.c_str(), frameId_.c_str()); + return; + } + } + tf::StampedTransform tmp; + tfListener_.lookupTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, tmp); + pose = rtabmap_ros::transformFromTF(tmp); + } + catch(tf::TransformException & ex) + { + NODELET_ERROR("%s",ex.what()); + return; + } + } + + pcl::PointCloud::Ptr inputCloud(new pcl::PointCloud); + pcl::fromROSMsg(*cloudMsg, *inputCloud); //Common variables for all strategies pcl::IndicesPtr ground, obstacles; @@ -162,135 +274,69 @@ private: pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); pcl::PointCloud::Ptr obstaclesCloudWithoutFlatSurfaces(new pcl::PointCloud); - if(originalCloud->size()) + if(inputCloud->size()) { - originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); - if(maxObstaclesHeight_ > 0) - { - // std::numeric_limits::lowest() exists only for c++11 - originalCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxObstaclesHeight_); - } + inputCloud = rtabmap::util3d::transformPointCloud(inputCloud, localTransform); - if(originalCloud->size()) + pcl::IndicesPtr flatObstacles(new std::vector); + pcl::PointCloud::Ptr cloud = grid_.segmentCloud( + inputCloud, + pcl::IndicesPtr(new std::vector), + pose, + cv::Point3f(localTransform.x(), localTransform.y(), localTransform.z()), + ground, + obstacles, + &flatObstacles); + + if(cloud->size() && (ground->size() || obstacles->size())) { - if(!optimizeForCloseObjects_) + if(groundPub_.getNumSubscribers() && + ground.get() && ground->size()) { - // This is the default strategy - pcl::IndicesPtr flatObstacles(new std::vector); - rtabmap::util3d::segmentObstaclesFromGround( - originalCloud, - ground, - obstacles, - normalKSearch_, - groundNormalAngle_, - clusterRadius_, - minClusterSize_, - segmentFlatObstacles_, - maxGroundHeight_, - &flatObstacles); - - if(groundPub_.getNumSubscribers() && - ground.get() && ground->size()) - { - pcl::copyPointCloud(*originalCloud, *ground, *groundCloud); - } - - if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && - obstacles.get() && obstacles->size()) - { - // remove flat obstacles from obstacles - std::set flatObstaclesSet; - if(projObstaclesPub_.getNumSubscribers()) - { - flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end()); - } - - obstaclesCloud->resize(obstacles->size()); - obstaclesCloudWithoutFlatSurfaces->resize(obstacles->size()); - - int oi=0; - for(unsigned int i=0; isize(); ++i) - { - obstaclesCloud->points[i] = originalCloud->at(obstacles->at(i)); - if(flatObstaclesSet.size() == 0 || - flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end()) - { - obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i]; - obstaclesCloudWithoutFlatSurfaces->points[oi].z = 0; - ++oi; - } - - } - - obstaclesCloudWithoutFlatSurfaces->resize(oi); - if(obstaclesCloudWithoutFlatSurfaces->size() && projVoxelSize_ > 0.0) - { - obstaclesCloudWithoutFlatSurfaces = rtabmap::util3d::voxelize(obstaclesCloudWithoutFlatSurfaces, projVoxelSize_); - } - } + pcl::copyPointCloud(*cloud, *ground, *groundCloud); } - else + + if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && + obstacles.get() && obstacles->size()) { - // in this case optimizeForCloseObject_ is true: - // we divide the floor point cloud into two subsections, one for all potential floor points up to 1m - // one for potential floor points further away than 1m. - // For the points at closer range, we use a smaller normal estimation radius and ground normal angle, - // which allows to detect smaller objects, without increasing the number of false positive. - // For all other points, we use a bigger normal estimation radius (* 3.) and tolerance for the - // grond normal angle (* 2.). - - pcl::PointCloud::Ptr originalCloud_near = rtabmap::util3d::passThrough(originalCloud, "x", std::numeric_limits::min(), 1.); - pcl::PointCloud::Ptr originalCloud_far = rtabmap::util3d::passThrough(originalCloud, "x", 1., std::numeric_limits::max()); - - // Part 1: segment floor and obstacles near the robot - rtabmap::util3d::segmentObstaclesFromGround( - originalCloud_near, - ground, - obstacles, - normalKSearch_, - groundNormalAngle_, - clusterRadius_, - minClusterSize_, - segmentFlatObstacles_, - maxGroundHeight_); - - if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) + // remove flat obstacles from obstacles + std::set flatObstaclesSet; + if(projObstaclesPub_.getNumSubscribers()) { - pcl::copyPointCloud(*originalCloud_near, *ground, *groundCloud); - ground->clear(); + flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end()); } - if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && obstacles.get() && obstacles->size()) + obstaclesCloud->resize(obstacles->size()); + obstaclesCloudWithoutFlatSurfaces->resize(obstacles->size()); + + int oi=0; + for(unsigned int i=0; isize(); ++i) { - pcl::copyPointCloud(*originalCloud_near, *obstacles, *obstaclesCloud); - obstacles->clear(); + obstaclesCloud->points[i] = cloud->at(obstacles->at(i)); + if(flatObstaclesSet.size() == 0 || + flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end()) + { + obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i]; + obstaclesCloudWithoutFlatSurfaces->points[oi].z = 0; + ++oi; + } + } - // Part 2: segment floor and obstacles far from the robot - rtabmap::util3d::segmentObstaclesFromGround( - originalCloud_far, - ground, - obstacles, - normalKSearch_, - 2.*groundNormalAngle_, - 3.*clusterRadius_, - minClusterSize_, - segmentFlatObstacles_, - maxGroundHeight_); + obstaclesCloudWithoutFlatSurfaces->resize(oi); + } - if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) + if(!localTransform.isIdentity()) + { + //transform back in topic frame + rtabmap::Transform localTransformInv = localTransform.inverse(); + if(groundCloud->size()) { - pcl::PointCloud::Ptr groundCloud2 (new pcl::PointCloud); - pcl::copyPointCloud(*originalCloud_far, *ground, *groundCloud2); - *groundCloud += *groundCloud2; + groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, localTransformInv); } - - - if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && obstacles.get() && obstacles->size()) + if(obstaclesCloud->size()) { - pcl::PointCloud::Ptr obstacles2(new pcl::PointCloud); - pcl::copyPointCloud(*originalCloud_far, *obstacles, *obstacles2); - *obstaclesCloud += *obstacles2; + obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, localTransformInv); } } } @@ -300,8 +346,7 @@ private: { sensor_msgs::PointCloud2 rosCloud; pcl::toROSMsg(*groundCloud, rosCloud); - rosCloud.header.stamp = cloudMsg->header.stamp; - rosCloud.header.frame_id = frameId_; + rosCloud.header = cloudMsg->header; //publish the message groundPub_.publish(rosCloud); @@ -311,8 +356,7 @@ private: { sensor_msgs::PointCloud2 rosCloud; pcl::toROSMsg(*obstaclesCloud, rosCloud); - rosCloud.header.stamp = cloudMsg->header.stamp; - rosCloud.header.frame_id = frameId_; + rosCloud.header = cloudMsg->header; //publish the message obstaclesPub_.publish(rosCloud); @@ -334,16 +378,10 @@ private: private: std::string frameId_; - int normalKSearch_; - double groundNormalAngle_; - double clusterRadius_; - int minClusterSize_; - double maxObstaclesHeight_; - double maxGroundHeight_; - bool segmentFlatObstacles_; + std::string mapFrameId_; bool waitForTransform_; - bool optimizeForCloseObjects_; - double projVoxelSize_; + + rtabmap::OccupancyGrid grid_; tf::TransformListener tfListener_; @@ -357,3 +395,4 @@ private: PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ObstaclesDetection, nodelet::Nodelet); } + diff --git a/src/nodelets/obstacles_detection_old.cpp b/src/nodelets/obstacles_detection_old.cpp new file mode 100644 index 00000000..40bcd474 --- /dev/null +++ b/src/nodelets/obstacles_detection_old.cpp @@ -0,0 +1,357 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include +#include +#include + +#include +#include +#include + +#include + +#include + +#include + +#include "rtabmap/core/util3d.h" +#include "rtabmap/core/util3d_filtering.h" +#include "rtabmap/core/util3d_mapping.h" +#include "rtabmap/core/util3d_transforms.h" + +namespace rtabmap_ros +{ + +class ObstaclesDetectionOld : public nodelet::Nodelet +{ +public: + ObstaclesDetectionOld() : + frameId_("base_link"), + 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), + projVoxelSize_(0.01) + {} + + virtual ~ObstaclesDetectionOld() + {} + +private: + virtual void onInit() + { + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + int queueSize = 10; + pnh.param("queue_size", queueSize, queueSize); + pnh.param("frame_id", frameId_, frameId_); + 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_); + pnh.param("proj_voxel_size", projVoxelSize_, projVoxelSize_); + + cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetectionOld::callback, this); + + groundPub_ = nh.advertise("ground", 1); + obstaclesPub_ = nh.advertise("obstacles", 1); + projObstaclesPub_ = nh.advertise("proj_obstacles", 1); + } + + + + void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) + { + ros::WallTime time = ros::WallTime::now(); + + if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0 && projObstaclesPub_.getNumSubscribers() == 0) + { + // no one wants the results + return; + } + + rtabmap::Transform localTransform; + try + { + if(waitForTransform_) + { + if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1))) + { + NODELET_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str()); + return; + } + } + tf::StampedTransform tmp; + tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp); + localTransform = rtabmap_ros::transformFromTF(tmp); + } + catch(tf::TransformException & ex) + { + NODELET_ERROR("%s",ex.what()); + return; + } + + pcl::PointCloud::Ptr originalCloud(new pcl::PointCloud); + pcl::fromROSMsg(*cloudMsg, *originalCloud); + + //Common variables for all strategies + pcl::IndicesPtr ground, obstacles; + pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); + pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); + pcl::PointCloud::Ptr obstaclesCloudWithoutFlatSurfaces(new pcl::PointCloud); + + if(originalCloud->size()) + { + originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); + if(maxObstaclesHeight_ > 0) + { + // std::numeric_limits::lowest() exists only for c++11 + originalCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxObstaclesHeight_); + } + + if(originalCloud->size()) + { + if(!optimizeForCloseObjects_) + { + // This is the default strategy + pcl::IndicesPtr flatObstacles(new std::vector); + rtabmap::util3d::segmentObstaclesFromGround( + originalCloud, + ground, + obstacles, + normalKSearch_, + groundNormalAngle_, + clusterRadius_, + minClusterSize_, + segmentFlatObstacles_, + maxGroundHeight_, + &flatObstacles); + + if(groundPub_.getNumSubscribers() && + ground.get() && ground->size()) + { + pcl::copyPointCloud(*originalCloud, *ground, *groundCloud); + } + + if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && + obstacles.get() && obstacles->size()) + { + // remove flat obstacles from obstacles + std::set flatObstaclesSet; + if(projObstaclesPub_.getNumSubscribers()) + { + flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end()); + } + + obstaclesCloud->resize(obstacles->size()); + obstaclesCloudWithoutFlatSurfaces->resize(obstacles->size()); + + int oi=0; + for(unsigned int i=0; isize(); ++i) + { + obstaclesCloud->points[i] = originalCloud->at(obstacles->at(i)); + if(flatObstaclesSet.size() == 0 || + flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end()) + { + obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i]; + obstaclesCloudWithoutFlatSurfaces->points[oi].z = 0; + ++oi; + } + + } + + obstaclesCloudWithoutFlatSurfaces->resize(oi); + if(obstaclesCloudWithoutFlatSurfaces->size() && projVoxelSize_ > 0.0) + { + obstaclesCloudWithoutFlatSurfaces = rtabmap::util3d::voxelize(obstaclesCloudWithoutFlatSurfaces, projVoxelSize_); + } + } + } + else + { + // in this case optimizeForCloseObject_ is true: + // we divide the floor point cloud into two subsections, one for all potential floor points up to 1m + // one for potential floor points further away than 1m. + // For the points at closer range, we use a smaller normal estimation radius and ground normal angle, + // which allows to detect smaller objects, without increasing the number of false positive. + // For all other points, we use a bigger normal estimation radius (* 3.) and tolerance for the + // grond normal angle (* 2.). + + pcl::PointCloud::Ptr originalCloud_near = rtabmap::util3d::passThrough(originalCloud, "x", std::numeric_limits::min(), 1.); + pcl::PointCloud::Ptr originalCloud_far = rtabmap::util3d::passThrough(originalCloud, "x", 1., std::numeric_limits::max()); + + // Part 1: segment floor and obstacles near the robot + rtabmap::util3d::segmentObstaclesFromGround( + originalCloud_near, + ground, + obstacles, + normalKSearch_, + groundNormalAngle_, + clusterRadius_, + minClusterSize_, + segmentFlatObstacles_, + maxGroundHeight_); + + if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) + { + pcl::copyPointCloud(*originalCloud_near, *ground, *groundCloud); + ground->clear(); + } + + if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && obstacles.get() && obstacles->size()) + { + pcl::copyPointCloud(*originalCloud_near, *obstacles, *obstaclesCloud); + obstacles->clear(); + } + + // Part 2: segment floor and obstacles far from the robot + rtabmap::util3d::segmentObstaclesFromGround( + originalCloud_far, + ground, + obstacles, + normalKSearch_, + 2.*groundNormalAngle_, + 3.*clusterRadius_, + minClusterSize_, + segmentFlatObstacles_, + maxGroundHeight_); + + if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) + { + pcl::PointCloud::Ptr groundCloud2 (new pcl::PointCloud); + pcl::copyPointCloud(*originalCloud_far, *ground, *groundCloud2); + *groundCloud += *groundCloud2; + } + + + if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && obstacles.get() && obstacles->size()) + { + pcl::PointCloud::Ptr obstacles2(new pcl::PointCloud); + pcl::copyPointCloud(*originalCloud_far, *obstacles, *obstacles2); + *obstaclesCloud += *obstacles2; + } + } + + if(!localTransform.isIdentity()) + { + //transform back in topic frame + rtabmap::Transform localTransformInv = localTransform.inverse(); + if(groundCloud->size()) + { + groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, localTransformInv); + } + if(obstaclesCloud->size()) + { + obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, localTransformInv); + } + } + } + } + + if(groundPub_.getNumSubscribers()) + { + sensor_msgs::PointCloud2 rosCloud; + pcl::toROSMsg(*groundCloud, rosCloud); + rosCloud.header = cloudMsg->header; + + //publish the message + groundPub_.publish(rosCloud); + } + + if(obstaclesPub_.getNumSubscribers()) + { + sensor_msgs::PointCloud2 rosCloud; + pcl::toROSMsg(*obstaclesCloud, rosCloud); + rosCloud.header = cloudMsg->header; + + //publish the message + obstaclesPub_.publish(rosCloud); + } + + if(projObstaclesPub_.getNumSubscribers()) + { + sensor_msgs::PointCloud2 rosCloud; + pcl::toROSMsg(*obstaclesCloudWithoutFlatSurfaces, rosCloud); + rosCloud.header.stamp = cloudMsg->header.stamp; + rosCloud.header.frame_id = frameId_; + + //publish the message + projObstaclesPub_.publish(rosCloud); + } + + NODELET_DEBUG("Obstacles segmentation time = %f s", (ros::WallTime::now() - time).toSec()); + } + +private: + std::string frameId_; + int normalKSearch_; + double groundNormalAngle_; + double clusterRadius_; + int minClusterSize_; + double maxObstaclesHeight_; + double maxGroundHeight_; + bool segmentFlatObstacles_; + bool waitForTransform_; + bool optimizeForCloseObjects_; + double projVoxelSize_; + + tf::TransformListener tfListener_; + + ros::Publisher groundPub_; + ros::Publisher obstaclesPub_; + ros::Publisher projObstaclesPub_; + + ros::Subscriber cloudSub_; +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ObstaclesDetectionOld, nodelet::Nodelet); +} + + diff --git a/src/nodelets/rgbd_odometry.cpp b/src/nodelets/rgbd_odometry.cpp index b0f480cf..d00a35cb 100644 --- a/src/nodelets/rgbd_odometry.cpp +++ b/src/nodelets/rgbd_odometry.cpp @@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "OdometryROS.h" +#include #include #include @@ -49,6 +49,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include using namespace rtabmap; @@ -59,16 +60,18 @@ class RGBDOdometry : public rtabmap_ros::OdometryROS { public: RGBDOdometry() : - OdometryROS(false), + OdometryROS(false, true, false), approxSync_(0), exactSync_(0), - sync2_(0), + approxSync2_(0), + exactSync2_(0), queueSize_(5) { } virtual ~RGBDOdometry() { + rgbdSub_.shutdown(); if(approxSync_) { delete approxSync_; @@ -77,9 +80,13 @@ public: { delete exactSync_; } - if(sync2_) + if(approxSync2_) { - delete sync2_; + delete approxSync2_; + } + if(exactSync2_) + { + delete exactSync2_; } } @@ -90,66 +97,65 @@ private: ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int depthCameras = 1; + int rgbdCameras = 1; bool approxSync = true; + bool subscribeRGBD = false; pnh.param("approx_sync", approxSync, approxSync); pnh.param("queue_size", queueSize_, queueSize_); - pnh.param("depth_cameras", depthCameras, depthCameras); - if(depthCameras <= 0) + pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD); + if(pnh.hasParam("depth_cameras")) { - depthCameras = 1; + ROS_ERROR("\"depth_cameras\" parameter doesn't exist anymore. It is replaced by \"rgbd_cameras\" with the \"rgbd_image\" input topics. \"subscribe_rgbd\" should be also set to true."); } - if(depthCameras > 2) + pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras); + if(rgbdCameras <= 0) + { + rgbdCameras = 1; + } + if(rgbdCameras > 2) { NODELET_FATAL("Only 2 cameras maximum supported yet."); } - if(depthCameras == 2) + std::string subscribedTopicsMsg; + if(subscribeRGBD) { - ros::NodeHandle rgb0_nh(nh, "rgb0"); - ros::NodeHandle depth0_nh(nh, "depth0"); - ros::NodeHandle rgb0_pnh(pnh, "rgb0"); - ros::NodeHandle depth0_pnh(pnh, "depth0"); - image_transport::ImageTransport rgb0_it(rgb0_nh); - image_transport::ImageTransport depth0_it(depth0_nh); - image_transport::TransportHints hintsRgb0("raw", ros::TransportHints(), rgb0_pnh); - image_transport::TransportHints hintsDepth0("raw", ros::TransportHints(), depth0_pnh); + if(rgbdCameras == 2) + { + rgbd_image1_sub_.subscribe(nh, "rgbd_image0", 1); + rgbd_image2_sub_.subscribe(nh, "rgbd_image1", 1); - image_mono_sub_.subscribe(rgb0_it, rgb0_nh.resolveName("image"), 1, hintsRgb0); - image_depth_sub_.subscribe(depth0_it, depth0_nh.resolveName("image"), 1, hintsDepth0); - info_sub_.subscribe(rgb0_nh, "camera_info", 1); + if(approxSync) + { + approxSync2_ = new message_filters::Synchronizer( + MyApproxSync2Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_); + approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2)); + } + else + { + exactSync2_ = new message_filters::Synchronizer( + MyExactSync2Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_); + exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2)); + } + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s", + getName().c_str(), + approxSync?"approx":"exact", + rgbd_image1_sub_.getTopic().c_str(), + rgbd_image2_sub_.getTopic().c_str()); + } + else + { + rgbdSub_ = nh.subscribe("rgbd_image", queueSize_, &RGBDOdometry::callbackRGBD, this); - ros::NodeHandle rgb1_nh(nh, "rgb1"); - ros::NodeHandle depth1_nh(nh, "depth1"); - ros::NodeHandle rgb1_pnh(pnh, "rgb1"); - ros::NodeHandle depth1_pnh(pnh, "depth1"); - image_transport::ImageTransport rgb1_it(rgb1_nh); - image_transport::ImageTransport depth1_it(depth1_nh); - image_transport::TransportHints hintsRgb1("raw", ros::TransportHints(), rgb1_pnh); - image_transport::TransportHints hintsDepth1("raw", ros::TransportHints(), depth1_pnh); - - image_mono2_sub_.subscribe(rgb1_it, rgb1_nh.resolveName("image"), 1, hintsRgb1); - image_depth2_sub_.subscribe(depth1_it, depth1_nh.resolveName("image"), 1, hintsDepth1); - info2_sub_.subscribe(rgb1_nh, "camera_info", 1); - - NODELET_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s", - ros::this_node::getName().c_str(), - image_mono_sub_.getTopic().c_str(), - image_depth_sub_.getTopic().c_str(), - info_sub_.getTopic().c_str(), - image_mono2_sub_.getTopic().c_str(), - image_depth2_sub_.getTopic().c_str(), - info2_sub_.getTopic().c_str()); - - sync2_ = new message_filters::Synchronizer( - MySync2Policy(queueSize_), - image_mono_sub_, - image_depth_sub_, - info_sub_, - image_mono2_sub_, - image_depth2_sub_, - info2_sub_); - sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6)); + subscribedTopicsMsg = + uFormat("\n%s subscribed to:\n %s", + getName().c_str(), + rgbdSub_.getTopic().c_str()); + } } else { @@ -177,13 +183,136 @@ private: exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3)); } - NODELET_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s", - ros::this_node::getName().c_str(), + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s", + getName().c_str(), approxSync?"approx":"exact", image_mono_sub_.getTopic().c_str(), image_depth_sub_.getTopic().c_str(), info_sub_.getTopic().c_str()); } + this->startWarningThread(subscribedTopicsMsg, approxSync); + } + + virtual void updateParameters(ParametersMap & parameters) + { + //make sure we are using Reg/Strategy=0 + ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy()); + if(iter != parameters.end() && iter->second.compare("0") != 0) + { + ROS_WARN("RGBD odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str()); + } + uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0")); + } + + void commonCallback( + const std::vector & rgbImages, + const std::vector & depthImages, + const std::vector& cameraInfos) + { + ROS_ASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size()); + ros::Time higherStamp; + int imageWidth = rgbImages[0]->image.cols; + int imageHeight = rgbImages[0]->image.rows; + int cameraCount = rgbImages.size(); + cv::Mat rgb; + cv::Mat depth; + pcl::PointCloud scanCloud; + std::vector cameraModels; + for(unsigned int i=0; iencoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || + rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || + rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || + rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || + rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || + !(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 || + depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 || + depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)) + { + NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16"); + return; + } + UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight, + uFormat("imageWidth=%d vs %d imageHeight=%d vs %d", + imageWidth, + rgbImages[i]->image.cols, + imageHeight, + rgbImages[i]->image.rows).c_str()); + UASSERT_MSG(depthImages[i]->image.cols == imageWidth && depthImages[i]->image.rows == imageHeight, + uFormat("imageWidth=%d vs %d imageHeight=%d vs %d", + imageWidth, + depthImages[i]->image.cols, + imageHeight, + depthImages[i]->image.rows).c_str()); + + ros::Time stamp = rgbImages[i]->header.stamp>depthImages[i]->header.stamp?rgbImages[i]->header.stamp:depthImages[i]->header.stamp; + + if(i == 0) + { + higherStamp = stamp; + } + else if(stamp > higherStamp) + { + higherStamp = stamp; + } + + Transform localTransform = getTransform(this->frameId(), rgbImages[i]->header.frame_id, stamp); + if(localTransform.isNull()) + { + return; + } + + cv_bridge::CvImageConstPtr ptrImage = rgbImages[i]; + if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 && + rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0) + { + ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8"); + } + + cv_bridge::CvImageConstPtr ptrDepth = depthImages[i]; + cv::Mat subDepth = ptrDepth->image; + + // initialize + if(rgb.empty()) + { + rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type()); + } + if(depth.empty()) + { + depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type()); + } + + if(ptrImage->image.type() == rgb.type()) + { + ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight))); + } + else + { + NODELET_ERROR("Some RGB images are not the same type!"); + return; + } + + if(subDepth.type() == depth.type()) + { + subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight))); + } + else + { + NODELET_ERROR("Some Depth images are not the same type!"); + return; + } + + cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfos[i], localTransform)); + } + + rtabmap::SensorData data( + rgb, + depth, + cameraModels, + 0, + rtabmap_ros::timestampFromROS(higherStamp)); + + this->processData(data, higherStamp); } void callback( @@ -191,179 +320,52 @@ private: const sensor_msgs::ImageConstPtr& depth, const sensor_msgs::CameraInfoConstPtr& cameraInfo) { + callbackCalled(); if(!this->isPaused()) { - if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || - image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || - image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || - image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || - !(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 || - depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 || - depth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0)) - { - NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 " - "recommended) and image_depth=16UC1,32FC1,mono16. Types detected: %s %s", - image->encoding.c_str(), depth->encoding.c_str()); - return; - } + std::vector imageMsgs(1); + std::vector depthMsgs(1); + std::vector infoMsgs; + imageMsgs[0] = cv_bridge::toCvShare(image); + depthMsgs[0] = cv_bridge::toCvShare(depth); + infoMsgs.push_back(*cameraInfo); - ros::Time stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp; - - Transform localTransform = getTransform(this->frameId(), image->header.frame_id, stamp); - if(localTransform.isNull()) - { - return; - } - - if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0) - { - rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform); - cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8"); - cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth); - - rtabmap::SensorData data( - ptrImage->image, - ptrDepth->image, - rtabmapModel, - 0, - rtabmap_ros::timestampFromROS(stamp)); - - this->processData(data, stamp); - } + this->commonCallback(imageMsgs, depthMsgs, infoMsgs); } } - void callback2( - const sensor_msgs::ImageConstPtr& image, - const sensor_msgs::ImageConstPtr& depth, - const sensor_msgs::CameraInfoConstPtr& cameraInfo, - const sensor_msgs::ImageConstPtr& image2, - const sensor_msgs::ImageConstPtr& depth2, - const sensor_msgs::CameraInfoConstPtr& cameraInfo2) + void callbackRGBD( + const rtabmap_ros::RGBDImageConstPtr& image) { + callbackCalled(); if(!this->isPaused()) { - std::vector imageMsgs; - std::vector depthMsgs; - std::vector infoMsgs; - imageMsgs.push_back(image); - imageMsgs.push_back(image2); - depthMsgs.push_back(depth); - depthMsgs.push_back(depth2); - infoMsgs.push_back(cameraInfo); - infoMsgs.push_back(cameraInfo2); + std::vector imageMsgs(1); + std::vector depthMsgs(1); + std::vector infoMsgs; + rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]); + infoMsgs.push_back(image->cameraInfo); - ros::Time higherStamp; - int imageWidth = imageMsgs[0]->width; - int imageHeight = imageMsgs[0]->height; - int cameraCount = imageMsgs.size(); - cv::Mat rgb; - cv::Mat depth; - pcl::PointCloud scanCloud; - std::vector cameraModels; - for(unsigned int i=0; iencoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || - !(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 || - depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 || - depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)) - { - NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16"); - return; - } - UASSERT_MSG(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight, - uFormat("imageWidth=%d vs %d imageHeight=%d vs %d", - imageWidth, - imageMsgs[i]->width, - imageHeight, - imageMsgs[i]->height).c_str()); - UASSERT_MSG(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight, - uFormat("imageWidth=%d vs %d imageHeight=%d vs %d", - imageWidth, - depthMsgs[i]->width, - imageHeight, - depthMsgs[i]->height).c_str()); + this->commonCallback(imageMsgs, depthMsgs, infoMsgs); + } + } - ros::Time stamp = imageMsgs[i]->header.stamp>depthMsgs[i]->header.stamp?imageMsgs[i]->header.stamp:depthMsgs[i]->header.stamp; + void callbackRGBD2( + const rtabmap_ros::RGBDImageConstPtr& image, + const rtabmap_ros::RGBDImageConstPtr& image2) + { + callbackCalled(); + if(!this->isPaused()) + { + std::vector imageMsgs(2); + std::vector depthMsgs(2); + std::vector infoMsgs; + rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]); + rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]); + infoMsgs.push_back(image->cameraInfo); + infoMsgs.push_back(image2->cameraInfo); - if(i == 0) - { - higherStamp = stamp; - } - else if(stamp > higherStamp) - { - higherStamp = stamp; - } - - Transform localTransform = getTransform(this->frameId(), imageMsgs[i]->header.frame_id, stamp); - if(localTransform.isNull()) - { - return; - } - - cv_bridge::CvImageConstPtr ptrImage; - if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0) - { - ptrImage = cv_bridge::toCvShare(imageMsgs[i]); - } - else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8"); - } - else - { - ptrImage = cv_bridge::toCvShare(imageMsgs[i], "bgr8"); - } - cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]); - cv::Mat subDepth = ptrDepth->image; - - // initialize - if(rgb.empty()) - { - rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type()); - } - if(depth.empty()) - { - depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type()); - } - - if(ptrImage->image.type() == rgb.type()) - { - ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight))); - } - else - { - NODELET_ERROR("Some RGB images are not the same type!"); - return; - } - - if(subDepth.type() == depth.type()) - { - subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight))); - } - else - { - NODELET_ERROR("Some Depth images are not the same type!"); - return; - } - - cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*infoMsgs[i], localTransform)); - } - - rtabmap::SensorData data( - rgb, - depth, - cameraModels, - 0, - rtabmap_ros::timestampFromROS(higherStamp)); - - this->processData(data, higherStamp); + this->commonCallback(imageMsgs, depthMsgs, infoMsgs); } } @@ -383,18 +385,23 @@ protected: exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3)); } - if(sync2_) + if(approxSync2_) { - delete sync2_; - sync2_ = new message_filters::Synchronizer( - MySync2Policy(queueSize_), - image_mono_sub_, - image_depth_sub_, - info_sub_, - image_mono2_sub_, - image_depth2_sub_, - info2_sub_); - sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6)); + delete approxSync2_; + approxSync2_ = new message_filters::Synchronizer( + MyApproxSync2Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_); + approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2)); + } + if(exactSync2_) + { + delete exactSync2_; + exactSync2_ = new message_filters::Synchronizer( + MyExactSync2Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_); + exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2)); } } @@ -402,15 +409,19 @@ private: image_transport::SubscriberFilter image_mono_sub_; image_transport::SubscriberFilter image_depth_sub_; message_filters::Subscriber info_sub_; - image_transport::SubscriberFilter image_mono2_sub_; - image_transport::SubscriberFilter image_depth2_sub_; - message_filters::Subscriber info2_sub_; + + ros::Subscriber rgbdSub_; + message_filters::Subscriber rgbd_image1_sub_; + message_filters::Subscriber rgbd_image2_sub_; + typedef message_filters::sync_policies::ApproximateTime MyApproxSyncPolicy; message_filters::Synchronizer * approxSync_; - typedef message_filters::sync_policies::ApproximateTime MyExactSyncPolicy; + typedef message_filters::sync_policies::ExactTime MyExactSyncPolicy; message_filters::Synchronizer * exactSync_; - typedef message_filters::sync_policies::ApproximateTime MySync2Policy; - message_filters::Synchronizer * sync2_; + typedef message_filters::sync_policies::ApproximateTime MyApproxSync2Policy; + message_filters::Synchronizer * approxSync2_; + typedef message_filters::sync_policies::ExactTime MyExactSync2Policy; + message_filters::Synchronizer * exactSync2_; int queueSize_; }; diff --git a/src/nodelets/rgbd_sync.cpp b/src/nodelets/rgbd_sync.cpp new file mode 100644 index 00000000..37c7851e --- /dev/null +++ b/src/nodelets/rgbd_sync.cpp @@ -0,0 +1,165 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include "rtabmap_ros/RGBDImage.h" + +#include "rtabmap/core/Compression.h" + +namespace rtabmap_ros +{ + +class RGBDSync : public nodelet::Nodelet +{ +public: + RGBDSync() : + approxSyncDepth_(0), + exactSyncDepth_(0) + {} + + virtual ~RGBDSync() + { + if(approxSyncDepth_) + delete approxSyncDepth_; + if(exactSyncDepth_) + delete exactSyncDepth_; + } + +private: + virtual void onInit() + { + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + int queueSize = 10; + bool approxSync = true; + pnh.param("approx_sync", approxSync, approxSync); + pnh.param("queue_size", queueSize, queueSize); + + NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false"); + + rgbdImagePub_ = nh.advertise("rgbd_image", 1); + rgbdImageCompressedPub_ = nh.advertise("rgbd_image/compressed", 1); + + if(approxSync) + { + approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_); + approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3)); + } + else + { + exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_); + exactSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3)); + } + + ros::NodeHandle rgb_nh(nh, "rgb"); + ros::NodeHandle depth_nh(nh, "depth"); + ros::NodeHandle rgb_pnh(pnh, "rgb"); + ros::NodeHandle depth_pnh(pnh, "depth"); + image_transport::ImageTransport rgb_it(rgb_nh); + image_transport::ImageTransport depth_it(depth_nh); + image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); + image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); + + imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); + imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); + cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1); + } + + void callback( + const sensor_msgs::ImageConstPtr& image, + const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& cameraInfo) + { + if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers()) + { + rtabmap_ros::RGBDImage msg; + msg.header.frame_id = cameraInfo->header.frame_id; + msg.header.stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp; + msg.cameraInfo = *cameraInfo; + + if(rgbdImageCompressedPub_.getNumSubscribers()) + { + rtabmap_ros::RGBDImage msgCompressed = msg; + + cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image); + imagePtr->toCompressedImageMsg(msgCompressed.rgbCompressed, cv_bridge::JPG); + + cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth); + ROS_ASSERT(imageDepthPtr->image.type() == CV_32FC1 || imageDepthPtr->image.type() == CV_16UC1); + msgCompressed.depthCompressed.header = imageDepthPtr->header; + msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png"); + msgCompressed.depthCompressed.format = "png"; + + rgbdImageCompressedPub_.publish(msgCompressed); + } + + if(rgbdImagePub_.getNumSubscribers()) + { + msg.rgb = *image; + msg.depth = *depth; + rgbdImagePub_.publish(msg); + } + } + } + +private: + ros::Publisher rgbdImagePub_; + ros::Publisher rgbdImageCompressedPub_; + + image_transport::SubscriberFilter imageSub_; + image_transport::SubscriberFilter imageDepthSub_; + message_filters::Subscriber cameraInfoSub_; + + typedef message_filters::sync_policies::ApproximateTime MyApproxSyncDepthPolicy; + message_filters::Synchronizer * approxSyncDepth_; + + typedef message_filters::sync_policies::ExactTime MyExactSyncDepthPolicy; + message_filters::Synchronizer * exactSyncDepth_; +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDSync, nodelet::Nodelet); +} + diff --git a/src/nodelets/rgbdicp_odometry.cpp b/src/nodelets/rgbdicp_odometry.cpp new file mode 100644 index 00000000..01aee2c4 --- /dev/null +++ b/src/nodelets/rgbdicp_odometry.cpp @@ -0,0 +1,406 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "rtabmap_ros/MsgConversion.h" + +#include +#include +#include +#include +#include +#include +#include + +using namespace rtabmap; + +namespace rtabmap_ros +{ + +class RGBDICPOdometry : public rtabmap_ros::OdometryROS +{ +public: + RGBDICPOdometry() : + OdometryROS(false, true, true), + approxScanSync_(0), + exactScanSync_(0), + approxCloudSync_(0), + exactCloudSync_(0), + queueSize_(5), + scanCloudMaxPoints_(0), + scanCloudNormalK_(0) + { + } + + virtual ~RGBDICPOdometry() + { + if(approxScanSync_) + { + delete approxScanSync_; + } + if(exactScanSync_) + { + delete exactScanSync_; + } + if(approxCloudSync_) + { + delete approxCloudSync_; + } + if(exactCloudSync_) + { + delete exactCloudSync_; + } + } + +private: + + virtual void onOdomInit() + { + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + bool approxSync = true; + bool subscribeScanCloud = false; + pnh.param("approx_sync", approxSync, approxSync); + pnh.param("queue_size", queueSize_, queueSize_); + pnh.param("subscribe_scan_cloud", subscribeScanCloud, subscribeScanCloud); + pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_); + pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_); + + ros::NodeHandle rgb_nh(nh, "rgb"); + ros::NodeHandle depth_nh(nh, "depth"); + ros::NodeHandle rgb_pnh(pnh, "rgb"); + ros::NodeHandle depth_pnh(pnh, "depth"); + image_transport::ImageTransport rgb_it(rgb_nh); + image_transport::ImageTransport depth_it(depth_nh); + image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); + image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); + + image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); + image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); + info_sub_.subscribe(rgb_nh, "camera_info", 1); + + std::string subscribedTopicsMsg; + if(subscribeScanCloud) + { + cloud_sub_.subscribe(nh, "scan_cloud", 1); + if(approxSync) + { + approxCloudSync_ = new message_filters::Synchronizer(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); + approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4)); + } + else + { + exactCloudSync_ = new message_filters::Synchronizer(MyExactCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); + exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4)); + } + + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s, \n %s", + getName().c_str(), + approxSync?"approx":"exact", + image_mono_sub_.getTopic().c_str(), + image_depth_sub_.getTopic().c_str(), + info_sub_.getTopic().c_str(), + cloud_sub_.getTopic().c_str()); + } + else + { + scan_sub_.subscribe(nh, "scan", 1); + if(approxSync) + { + approxScanSync_ = new message_filters::Synchronizer(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); + approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4)); + } + else + { + exactScanSync_ = new message_filters::Synchronizer(MyExactScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); + exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4)); + } + + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s, \n %s", + getName().c_str(), + approxSync?"approx":"exact", + image_mono_sub_.getTopic().c_str(), + image_depth_sub_.getTopic().c_str(), + info_sub_.getTopic().c_str(), + scan_sub_.getTopic().c_str()); + } + this->startWarningThread(subscribedTopicsMsg, approxSync); + } + + virtual void updateParameters(ParametersMap & parameters) + { + //make sure we are using Reg/Strategy=0 + ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy()); + if(iter != parameters.end() && iter->second.compare("0") != 0) + { + ROS_WARN("RGBDICP odometry works only with \"Reg/Strategy\"=2. Ignoring value %s.", iter->second.c_str()); + } + uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2")); + } + + void callback( + const sensor_msgs::ImageConstPtr& image, + const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& cameraInfo) + { + sensor_msgs::LaserScanConstPtr scanMsg; + sensor_msgs::PointCloud2ConstPtr cloudMsg; + callbackCommon(image, depth, cameraInfo, scanMsg, cloudMsg); + } + + void callbackScan( + const sensor_msgs::ImageConstPtr& image, + const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& cameraInfo, + const sensor_msgs::LaserScanConstPtr& scanMsg) + { + sensor_msgs::PointCloud2ConstPtr cloudMsg; + callbackCommon(image, depth, cameraInfo, scanMsg, cloudMsg); + } + + void callbackCloud( + const sensor_msgs::ImageConstPtr& image, + const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& cameraInfo, + const sensor_msgs::PointCloud2ConstPtr& cloudMsg) + { + sensor_msgs::LaserScanConstPtr scanMsg; + callbackCommon(image, depth, cameraInfo, scanMsg, cloudMsg); + } + + void callbackCommon( + const sensor_msgs::ImageConstPtr& image, + const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& cameraInfo, + const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::PointCloud2ConstPtr& cloudMsg) + { + callbackCalled(); + if(!this->isPaused()) + { + if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || + image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || + image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || + image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || + image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || + !(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 || + depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 || + depth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0)) + { + NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 " + "recommended) and image_depth=16UC1,32FC1,mono16. Types detected: %s %s", + image->encoding.c_str(), depth->encoding.c_str()); + return; + } + + // use the highest stamp to make sure that there will be no future interpolation required when synchronized with another node + ros::Time stamp = image->header.stamp > depth->header.stamp? image->header.stamp : depth->header.stamp; + if(scanMsg.get() != 0) + { + if(stamp < scanMsg->header.stamp) + { + stamp = scanMsg->header.stamp; + } + } + else if(cloudMsg.get() != 0) + { + if(stamp < cloudMsg->header.stamp) + { + stamp = cloudMsg->header.stamp; + } + } + + Transform localTransform = getTransform(this->frameId(), image->header.frame_id, stamp); + if(localTransform.isNull()) + { + return; + } + + if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0) + { + rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform); + cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8"); + cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth); + + cv::Mat scan; + Transform localScanTransform = Transform::getIdentity(); + if(scanMsg.get() != 0) + { + // make sure the frame of the laser is updated too + localScanTransform = getTransform(this->frameId(), + scanMsg->header.frame_id, + scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)); + if(localScanTransform.isNull()) + { + ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec()); + return; + } + + //transform in frameId_ frame + sensor_msgs::PointCloud2 scanOut; + laser_geometry::LaserProjection projection; + projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener()); + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(scanOut, *pclScan); + + scan = util3d::laserScan2dFromPointCloud(*pclScan); + } + else if(cloudMsg.get() != 0) + { + bool containNormals = false; + for(unsigned int i=0; ifields.size(); ++i) + { + if(cloudMsg->fields[i].name.compare("normal_x") == 0) + { + containNormals = true; + break; + } + } + localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp); + if(localScanTransform.isNull()) + { + ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg->header.stamp.toSec()); + return; + } + + if(containNormals) + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*cloudMsg, *pclScan); + scan = util3d::laserScanFromPointCloud(*pclScan); + } + else + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*cloudMsg, *pclScan); + + if(scanCloudNormalK_ > 0) + { + //compute normals + pcl::PointCloud::Ptr normals = util3d::computeNormals(pclScan, scanCloudNormalK_); + pcl::PointCloud::Ptr pclScanNormal(new pcl::PointCloud); + pcl::concatenateFields(*pclScan, *normals, *pclScanNormal); + scan = util3d::laserScanFromPointCloud(*pclScanNormal); + } + else + { + scan = util3d::laserScanFromPointCloud(*pclScan); + } + } + } + + rtabmap::SensorData data( + scan, + LaserScanInfo( + scanMsg.get() != 0?(int)scanMsg->ranges.size():cloudMsg.get() != 0?scanCloudMaxPoints_:0, + scanMsg.get() != 0?scanMsg->range_max:0, + localScanTransform), + ptrImage->image, + ptrDepth->image, + rtabmapModel, + 0, + rtabmap_ros::timestampFromROS(stamp)); + + this->processData(data, stamp); + } + } + } + +protected: + virtual void flushCallbacks() + { + // flush callbacks + if(approxScanSync_) + { + delete approxScanSync_; + approxScanSync_ = new message_filters::Synchronizer(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); + approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4)); + } + if(exactScanSync_) + { + delete exactScanSync_; + exactScanSync_ = new message_filters::Synchronizer(MyExactScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); + exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4)); + } + if(approxCloudSync_) + { + delete approxCloudSync_; + approxCloudSync_ = new message_filters::Synchronizer(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); + approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4)); + } + if(exactCloudSync_) + { + delete exactCloudSync_; + exactCloudSync_ = new message_filters::Synchronizer(MyExactCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); + exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4)); + } + } + +private: + image_transport::SubscriberFilter image_mono_sub_; + image_transport::SubscriberFilter image_depth_sub_; + message_filters::Subscriber info_sub_; + message_filters::Subscriber scan_sub_; + message_filters::Subscriber cloud_sub_; + typedef message_filters::sync_policies::ApproximateTime MyApproxScanSyncPolicy; + message_filters::Synchronizer * approxScanSync_; + typedef message_filters::sync_policies::ApproximateTime MyExactScanSyncPolicy; + message_filters::Synchronizer * exactScanSync_; + typedef message_filters::sync_policies::ApproximateTime MyApproxCloudSyncPolicy; + message_filters::Synchronizer * approxCloudSync_; + typedef message_filters::sync_policies::ApproximateTime MyExactCloudSyncPolicy; + message_filters::Synchronizer * exactCloudSync_; + int queueSize_; + int scanCloudMaxPoints_; + int scanCloudNormalK_; +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDICPOdometry, nodelet::Nodelet); + +} diff --git a/src/nodelets/stereo_odometry.cpp b/src/nodelets/stereo_odometry.cpp index de1a1657..2b40a107 100644 --- a/src/nodelets/stereo_odometry.cpp +++ b/src/nodelets/stereo_odometry.cpp @@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "OdometryROS.h" +#include "rtabmap_ros/OdometryROS.h" #include "pluginlib/class_list_macros.h" #include "nodelet/nodelet.h" @@ -47,6 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include using namespace rtabmap; @@ -57,7 +58,7 @@ class StereoOdometry : public rtabmap_ros::OdometryROS { public: StereoOdometry() : - rtabmap_ros::OdometryROS(true), + rtabmap_ros::OdometryROS(true, true, false), approxSync_(0), exactSync_(0), queueSize_(5) @@ -85,7 +86,6 @@ private: bool approxSync = false; pnh.param("approx_sync", approxSync, approxSync); pnh.param("queue_size", queueSize_, queueSize_); - NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false"); ros::NodeHandle left_nh(nh, "left"); ros::NodeHandle right_nh(nh, "right"); @@ -113,13 +113,25 @@ private: } - NODELET_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s", - ros::this_node::getName().c_str(), + std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s", + getName().c_str(), approxSync?"approx":"exact", imageRectLeft_.getTopic().c_str(), imageRectRight_.getTopic().c_str(), cameraInfoLeft_.getTopic().c_str(), cameraInfoRight_.getTopic().c_str()); + this->startWarningThread(subscribedTopicsMsg, approxSync); + } + + virtual void updateParameters(ParametersMap & parameters) + { + //make sure we are using Reg/Strategy=0 + ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy()); + if(iter != parameters.end() && iter->second.compare("0") != 0) + { + ROS_WARN("Stereo odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str()); + } + uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0")); } void callback( @@ -128,6 +140,7 @@ private: const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft, const sensor_msgs::CameraInfoConstPtr& cameraInfoRight) { + callbackCalled(); if(!this->isPaused()) { if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || diff --git a/src/nodelets/undistort_depth.cpp b/src/nodelets/undistort_depth.cpp new file mode 100644 index 00000000..c64c7e1a --- /dev/null +++ b/src/nodelets/undistort_depth.cpp @@ -0,0 +1,123 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include "rtabmap/core/clams/discrete_depth_distortion_model.h" +#include "rtabmap/utilite/UConversion.h" + +namespace rtabmap_ros +{ + +class UndistortDepth : public nodelet::Nodelet +{ +public: + UndistortDepth() + {} + + virtual ~UndistortDepth() + { + } + +private: + virtual void onInit() + { + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + int queueSize = 10; + std::string modelPath; + pnh.param("queue_size", queueSize, queueSize); + pnh.param("model", modelPath, modelPath); + + if(modelPath.empty()) + { + NODELET_ERROR("undistort_depth: \"model\" parameter should be set!"); + } + + model_.load(modelPath); + if(!model_.isValid()) + { + NODELET_ERROR("Loaded distortion model from \"%s\" is not valid!", modelPath.c_str()); + } + else + { + image_transport::ImageTransport it(nh); + sub_ = it.subscribe("depth", queueSize, &UndistortDepth::callback, this); + pub_ = it.advertise(uFormat("%s_undistorted", nh.resolveName("depth").c_str()), 1); + } + } + + void callback(const sensor_msgs::ImageConstPtr& depth) + { + if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 && + depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 && + depth->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0) + { + NODELET_ERROR("Input type depth=32FC1,16UC1,MONO16"); + return; + } + + if(pub_.getNumSubscribers()) + { + if(depth->width == model_.getWidth() && depth->width == model_.getWidth()) + { + cv_bridge::CvImagePtr imageDepthPtr = cv_bridge::toCvCopy(depth); + model_.undistort(imageDepthPtr->image); + pub_.publish(imageDepthPtr->toImageMsg()); + } + else + { + NODELET_ERROR("Input depth image size (%dx%d) and distortion model " + "size (%dx%d) don't match! Cannot undistort image.", + depth->width, depth->height, + model_.getWidth(), model_.getHeight()); + } + } + } + +private: + clams::DiscreteDepthDistortionModel model_; + image_transport::Publisher pub_; + image_transport::Subscriber sub_; +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::UndistortDepth, nodelet::Nodelet); +} + diff --git a/src/rviz/MapCloudDisplay.cpp b/src/rviz/MapCloudDisplay.cpp index b178f6bb..8110ce9c 100644 --- a/src/rviz/MapCloudDisplay.cpp +++ b/src/rviz/MapCloudDisplay.cpp @@ -272,58 +272,60 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map) 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_min_depth_->getFloat(), + validIndices.get()); - - if(!s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty()) + if(cloud_voxel_size_->getFloat()) { - pcl::PointCloud::Ptr cloud; - pcl::IndicesPtr validIndices(new std::vector); - cloud = rtabmap::util3d::cloudRGBFromSensorData( - s.sensorData(), - cloud_decimation_->getInt(), - cloud_max_depth_->getFloat(), - cloud_min_depth_->getFloat(), - validIndices.get()); + cloud = rtabmap::util3d::voxelize(cloud, validIndices, cloud_voxel_size_->getFloat()); + } - if(cloud_voxel_size_->getFloat()) + if(cloud->size()) + { + if(cloud_filter_floor_height_->getFloat() > 0.0f || cloud_filter_ceiling_height_->getFloat() > 0.0f) { - cloud = rtabmap::util3d::voxelize(cloud, validIndices, cloud_voxel_size_->getFloat()); + // convert in /odom frame + cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose()); + 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); + // convert back in /base_link frame + cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose().inverse()); } - if(cloud->size()) + sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); + pcl::toROSMsg(*cloud, *cloudMsg); + cloudMsg->header = map.header; + + CloudInfoPtr info(new CloudInfo); + info->message_ = cloudMsg; + info->pose_ = rtabmap::Transform::getIdentity(); + info->id_ = id; + + if (transformCloud(info, true)) { - 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()>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); - pcl::toROSMsg(*cloud, *cloudMsg); - cloudMsg->header = map.header; - - CloudInfoPtr info(new CloudInfo); - info->message_ = cloudMsg; - info->pose_ = rtabmap::Transform::getIdentity(); - info->id_ = id; - - if (transformCloud(info, true)) - { - boost::mutex::scoped_lock lock(new_clouds_mutex_); - new_cloud_infos_.insert(std::make_pair(id, info)); - } + boost::mutex::scoped_lock lock(new_clouds_mutex_); + new_cloud_infos_.erase(id); + new_cloud_infos_.insert(std::make_pair(id, info)); } } } @@ -485,7 +487,7 @@ void MapCloudDisplay::downloadMap() ros::NodeHandle nh; QMessageBox * messageBox = new QMessageBox( QMessageBox::NoIcon, - tr("Calling \"%1\" service...").arg(nh.resolveName("rtabmap/get_map").c_str()), + tr("Calling \"%1\" service...").arg(nh.resolveName("rtabmap/get_map_data").c_str()), tr("Downloading the map... please wait (rviz could become gray!)"), QMessageBox::NoButton); messageBox->setAttribute(Qt::WA_DeleteOnClose, true); @@ -493,18 +495,18 @@ void MapCloudDisplay::downloadMap() QApplication::processEvents(); uSleep(100); // hack make sure the text in the QMessageBox is shown... QApplication::processEvents(); - if(!ros::service::call("rtabmap/get_map", getMapSrv)) + if(!ros::service::call("rtabmap/get_map_data", getMapSrv)) { ROS_ERROR("MapCloudDisplay: Can't call \"%s\" service. " "Tip: if rtabmap node is not in rtabmap namespace, you can remap the service " "to \"get_map\" in the launch " - "file like: .", - nh.resolveName("rtabmap/get_map").c_str()); + "file like: .", + nh.resolveName("rtabmap/get_map_data").c_str()); messageBox->setText(tr("MapCloudDisplay: Can't call \"%1\" service. " "Tip: if rtabmap node is not in rtabmap namespace, you can remap the service " "to \"get_map\" in the launch " - "file like: ."). - arg(nh.resolveName("rtabmap/get_map").c_str())); + "file like: ."). + arg(nh.resolveName("rtabmap/get_map_data").c_str())); } else { @@ -633,6 +635,7 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt ) cloud_info->scene_node_->attachObject( cloud_info->cloud_.get() ); cloud_info->scene_node_->setVisible(false); + cloud_infos_.erase(it->first); cloud_infos_.insert(*it); }