diff --git a/CMakeLists.txt b/CMakeLists.txt
index 19f75b7b..06ee0373 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -31,7 +31,7 @@ find_package(find_object_2d)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.20.9 REQUIRED)
+find_package(RTABMap 0.20.10 REQUIRED)
find_package(OpenCV REQUIRED)
diff --git a/docker/melodic/latest/Dockerfile b/docker/melodic/latest/Dockerfile
index 02d8d6c8..dc8cd93f 100644
--- a/docker/melodic/latest/Dockerfile
+++ b/docker/melodic/latest/Dockerfile
@@ -8,6 +8,6 @@ RUN source /ros_entrypoint.sh && \
catkin_init_workspace && \
git clone https://github.com/introlab/rtabmap_ros.git && \
cd .. && \
- catkin_make -j1 -DCMAKE_INSTALL_PREFIX=/opt/ros/melodic install && \
+ catkin_make -j1 -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_INSTALL_PREFIX=/opt/ros/melodic install && \
cd && \
rm -rf catkin_ws
diff --git a/docker/noetic/latest/Dockerfile b/docker/noetic/latest/Dockerfile
index 44a95a74..2e2d7e95 100644
--- a/docker/noetic/latest/Dockerfile
+++ b/docker/noetic/latest/Dockerfile
@@ -8,6 +8,6 @@ RUN source /ros_entrypoint.sh && \
catkin_init_workspace && \
git clone https://github.com/introlab/rtabmap_ros.git && \
cd .. && \
- catkin_make -j1 -DCMAKE_INSTALL_PREFIX=/opt/ros/noetic install && \
+ catkin_make -j1 -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_INSTALL_PREFIX=/opt/ros/noetic install && \
cd && \
rm -rf catkin_ws
diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch
index 2a1e39e0..6d3da43d 100644
--- a/launch/rtabmap.launch
+++ b/launch/rtabmap.launch
@@ -37,6 +37,7 @@
+
@@ -310,6 +311,7 @@
+
diff --git a/launch/tests/test_ouster_gen2.launch b/launch/tests/test_ouster_gen2.launch
index 8219e7f6..e5650408 100644
--- a/launch/tests/test_ouster_gen2.launch
+++ b/launch/tests/test_ouster_gen2.launch
@@ -4,23 +4,48 @@
-
-
-
+
-
+
+
+
+
+
-
-
+
+
+
+
+
@@ -30,6 +55,8 @@
+
+
@@ -41,7 +68,7 @@
-
+
@@ -51,12 +78,12 @@
+
-
@@ -72,7 +99,7 @@
-
+
@@ -88,11 +115,13 @@
-
+
+
-
+
+
@@ -127,6 +156,13 @@
+
+
+
+
+
+
+
diff --git a/launch/tests/test_velodyne.launch b/launch/tests/test_velodyne.launch
index 14734fa9..d2c25d2d 100644
--- a/launch/tests/test_velodyne.launch
+++ b/launch/tests/test_velodyne.launch
@@ -14,6 +14,7 @@
+
@@ -22,6 +23,7 @@
+
diff --git a/package.xml b/package.xml
index f3c6e81a..62d6fc01 100644
--- a/package.xml
+++ b/package.xml
@@ -1,7 +1,7 @@
rtabmap_ros
- 0.20.9
+ 0.20.10
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/CoreWrapper.cpp b/src/CoreWrapper.cpp
index 5b49f110..91262176 100644
--- a/src/CoreWrapper.cpp
+++ b/src/CoreWrapper.cpp
@@ -142,12 +142,14 @@ void CoreWrapper::onInit()
bool publishTf = true;
double tfDelay = 0.05; // 20 Hz
double tfTolerance = 0.1; // 100 ms
+ std::string odomFrameIdInit;
pnh.param("config_path", configPath_, configPath_);
pnh.param("database_path", databasePath_, databasePath_);
pnh.param("frame_id", frameId_, frameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF
+ pnh.param("odom_frame_id_init", odomFrameIdInit, odomFrameIdInit); // set to publish map->odom TF before receiving odom topic
pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
pnh.param("ground_truth_base_frame_id", groundTruthBaseFrameId_, frameId_);
@@ -157,6 +159,18 @@ void CoreWrapper::onInit()
"anymore! It is replaced by \"rgbd_cameras\" parameter "
"used when \"subscribe_rgbd\" is true");
}
+ if(!odomFrameIdInit.empty())
+ {
+ if(odomFrameId_.empty())
+ {
+ ROS_INFO("rtabmap: odom_frame_id_init = %s", odomFrameIdInit.c_str());
+ odomFrameId_ = odomFrameIdInit;
+ }
+ else
+ {
+ ROS_WARN("odom_frame_id_init (%s) is ignored if odom_frame_id (%s) is set.", odomFrameIdInit.c_str(), odomFrameId_.c_str());
+ }
+ }
pnh.param("publish_tf", publishTf, publishTf);
pnh.param("tf_delay", tfDelay, tfDelay);
@@ -1859,7 +1873,12 @@ void CoreWrapper::process(
{
if(iter->first.header.stamp < lastPoseStamp_)
{
- Transform interOdom = rtabmap_ros::transformFromPoseMsg(iter->first.pose.pose);
+ Transform interOdom;
+ if(!rtabmap_.getLocalOptimizedPoses().empty())
+ {
+ // add intermediate poses only if the current local graph is not empty
+ interOdom = rtabmap_ros::transformFromPoseMsg(iter->first.pose.pose);
+ }
if(!interOdom.isNull())
{
cv::Mat covariance;
@@ -2105,6 +2124,15 @@ void CoreWrapper::process(
timeRtabmap = timer.ticks();
mapToOdomMutex_.lock();
mapToOdom_ = rtabmap_.getMapCorrection();
+
+ if(!odomFrameId.empty() && !odomFrameId_.empty() && odomFrameId_.compare(odomFrameId)!=0)
+ {
+ ROS_ERROR("Odometry received doesn't have same frame_id "
+ "than the one previously set (old=%s, new=%s). "
+ "Are there multiple nodes publishing on same odometry topic name? "
+ "The new frame_id is now used.", odomFrameId_.c_str(), odomFrameId.c_str());
+ }
+
odomFrameId_ = odomFrameId;
mapToOdomMutex_.unlock();
diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp
index e3efb597..d8163bc5 100644
--- a/src/MsgConversion.cpp
+++ b/src/MsgConversion.cpp
@@ -369,7 +369,6 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_ros::RGBDImageConstPtr & imag
int depthHeight = depthMsg->image.rows;
UASSERT_MSG(
- imageWidth % depthWidth == 0 && imageHeight % depthHeight == 0 &&
imageWidth/depthWidth == imageHeight/depthHeight,
uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
@@ -1731,7 +1730,6 @@ bool convertRGBDMsgs(
if(depthMsgs.size())
{
UASSERT_MSG(
- imageWidth % depthWidth == 0 && imageHeight % depthHeight == 0 &&
imageWidth/depthWidth == imageHeight/depthHeight,
uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
}
diff --git a/src/nodelets/rgbd_odometry.cpp b/src/nodelets/rgbd_odometry.cpp
index 805b082b..aa6761f9 100644
--- a/src/nodelets/rgbd_odometry.cpp
+++ b/src/nodelets/rgbd_odometry.cpp
@@ -331,7 +331,6 @@ private:
int depthHeight = depthImages[0]->image.rows;
UASSERT_MSG(
- imageWidth % depthWidth == 0 && imageHeight % depthHeight == 0 &&
imageWidth/depthWidth == imageHeight/depthHeight,
uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());