diff --git a/rtabmap_conversions/package.xml b/rtabmap_conversions/package.xml index 1c6718d1..e0454f93 100644 --- a/rtabmap_conversions/package.xml +++ b/rtabmap_conversions/package.xml @@ -1,7 +1,7 @@ rtabmap_conversions - 0.21.9 + 0.21.10 RTAB-Map's conversions package. This package can be used to convert rtabmap_msgs's msgs into RTAB-Map's library objects. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 5f59758e..b41c7ae5 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -3058,6 +3058,25 @@ bool deskew_impl( } } + if(secFirst > 1.e18) + { + // convert nanoseconds to seconds + secFirst /= 1.e9; + secLast /= 1.e9; + } + else if(secFirst > 1.e15) + { + // convert microseconds to seconds + secFirst /= 1.e6; + secLast /= 1.e6; + } + else if(secFirst > 1.e12) + { + // convert milliseconds to seconds + secFirst /= 1.e3; + secLast /= 1.e3; + } + firstStamp = ros::Time(secFirst); lastStamp = ros::Time(secLast); } @@ -3198,6 +3217,21 @@ bool deskew_impl( else if(timeDatatype == 8) //float64 { double sec = *((const double*)(&output.data[u*output.point_step]+offsetTime)); + if(sec > 1.e18) + { + // convert nanoseconds to seconds + sec /= 1.e9; + } + else if(sec > 1.e15) + { + // convert microseconds to seconds + sec /= 1.e6; + } + else if(sec > 1.e12) + { + // sec milliseconds to seconds + sec /= 1.e3; + } stamp = ros::Time(sec); } @@ -3277,6 +3311,21 @@ bool deskew_impl( else if(timeDatatype == 8) { double sec = *((const double*)(&output.data[v*output.row_step]+offsetTime)); + if(sec > 1.e18) + { + // convert nanoseconds to seconds + sec /= 1.e9; + } + else if(sec > 1.e15) + { + // convert microseconds to seconds + sec /= 1.e6; + } + else if(sec > 1.e12) + { + // sec milliseconds to seconds + sec /= 1.e3; + } stamp = ros::Time(sec); } diff --git a/rtabmap_costmap_plugins/package.xml b/rtabmap_costmap_plugins/package.xml index e81526de..70f746dd 100644 --- a/rtabmap_costmap_plugins/package.xml +++ b/rtabmap_costmap_plugins/package.xml @@ -1,7 +1,7 @@ rtabmap_costmap_plugins - 0.21.9 + 0.21.10 RTAB-Map's costmap_2d plugins Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_demos/package.xml b/rtabmap_demos/package.xml index 2ba3f4bb..560d8ad7 100644 --- a/rtabmap_demos/package.xml +++ b/rtabmap_demos/package.xml @@ -1,7 +1,7 @@ rtabmap_demos - 0.21.9 + 0.21.10 RTAB-Map's demo launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_examples/package.xml b/rtabmap_examples/package.xml index 1e178c53..ddb8cc10 100644 --- a/rtabmap_examples/package.xml +++ b/rtabmap_examples/package.xml @@ -1,7 +1,7 @@ rtabmap_examples - 0.21.9 + 0.21.10 RTAB-Map's example launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_launch/package.xml b/rtabmap_launch/package.xml index 3a0fce7f..4f9914a1 100644 --- a/rtabmap_launch/package.xml +++ b/rtabmap_launch/package.xml @@ -1,7 +1,7 @@ rtabmap_launch - 0.21.9 + 0.21.10 RTAB-Map's main launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_legacy/package.xml b/rtabmap_legacy/package.xml index 7273799d..883e4bb2 100644 --- a/rtabmap_legacy/package.xml +++ b/rtabmap_legacy/package.xml @@ -1,7 +1,7 @@ rtabmap_legacy - 0.21.9 + 0.21.10 RTAB-Map's legacy launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_msgs/package.xml b/rtabmap_msgs/package.xml index 61dbdcae..3371b25a 100644 --- a/rtabmap_msgs/package.xml +++ b/rtabmap_msgs/package.xml @@ -1,7 +1,7 @@ rtabmap_msgs - 0.21.9 + 0.21.10 RTAB-Map's msgs package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index fe632fd4..c5672d36 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -148,6 +148,7 @@ private: USemaphore dataReady_; rtabmap::SensorData dataToProcess_; std_msgs::Header dataHeaderToProcess_; + bool bufferedDataToProcess_; bool paused_; int resetCountdown_; diff --git a/rtabmap_odom/package.xml b/rtabmap_odom/package.xml index 837afdc9..6b828b33 100644 --- a/rtabmap_odom/package.xml +++ b/rtabmap_odom/package.xml @@ -1,7 +1,7 @@ rtabmap_odom - 0.21.9 + 0.21.10 RTAB-Map's odometry package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index a46d4fb2..f79fca44 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -436,13 +436,24 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg) cv::Mat(3,3,CV_64FC1,(void*)msg->linear_acceleration_covariance.data()).clone(), localTransform); - UScopeMutex m(imuMutex_); - - imus_.insert(std::make_pair(stamp, imu)); - if(imus_.size() > 1000) { - NODELET_WARN("Dropping imu data!"); - imus_.erase(imus_.begin()); + UScopeMutex m(imuMutex_); + + imus_.insert(std::make_pair(stamp, imu)); + if(imus_.size() > 1000) + { + NODELET_WARN("Dropping imu data!"); + imus_.erase(imus_.begin()); + } + } + if(dataMutex_.lockTry() == 0) + { + if(bufferedDataToProcess_ && dataHeaderToProcess_.stamp.toSec() <= stamp) + { + bufferedDataToProcess_ = false; + dataReady_.release(); + } + dataMutex_.unlock(); } } } @@ -452,8 +463,13 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header //NODELET_WARN("Received image: %f delay=%f", data.stamp(), (ros::Time::now() - header.stamp).toSec()); if(dataMutex_.lockTry() == 0) { + if(bufferedDataToProcess_) { + NODELET_ERROR("We didn't receive IMU newer than previous image (%f) and we just received a new image (%f). The previous image is dropped!", + dataHeaderToProcess_.stamp.toSec(), header.stamp.toSec()); + } dataToProcess_ = data; dataHeaderToProcess_ = header; + bufferedDataToProcess_ = false; dataReady_.release(); dataMutex_.unlock(); } @@ -497,8 +513,9 @@ void OdometryROS::mainLoop() if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < header.stamp.toSec())) { - NODELET_ERROR("Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f)", + NODELET_WARN("Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f). Buffering the image until an imu with same or greater stamp is received.", data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + bufferedDataToProcess_ = true; return; } // process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp) @@ -888,10 +905,9 @@ void OdometryROS::mainLoop() "is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.", header.stamp.toSec() - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, header.stamp.toSec()); } - else + else if(--resetCurrentCount_>0) { NODELET_WARN( "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_); - --resetCurrentCount_; } if(resetCurrentCount_ == 0 || tooOldPreviousData) @@ -919,6 +935,11 @@ void OdometryROS::mainLoop() odometry_->reset(tfPose); } } + // Keep resetting if the odometry cannot initialize in next updates (e.g., lack of features). + // This will make sure we keep updating to latest guess pose. + if(resetCurrentCount_ == 0) { + ++resetCurrentCount_; + } } } @@ -1109,6 +1130,7 @@ void OdometryROS::reset(const Transform & pose) imuProcessed_ = false; dataToProcess_ = SensorData(); dataHeaderToProcess_ = std_msgs::Header(); + bufferedDataToProcess_ = false; imuMutex_.lock(); imus_.clear(); imuMutex_.unlock(); diff --git a/rtabmap_python/package.xml b/rtabmap_python/package.xml index 8a0d0b23..07cad0da 100644 --- a/rtabmap_python/package.xml +++ b/rtabmap_python/package.xml @@ -1,7 +1,7 @@ rtabmap_python - 0.21.9 + 0.21.10 RTAB-Map's python package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_ros/package.xml b/rtabmap_ros/package.xml index caebced3..0d0a21d3 100644 --- a/rtabmap_ros/package.xml +++ b/rtabmap_ros/package.xml @@ -1,7 +1,7 @@ rtabmap_ros - 0.21.9 + 0.21.10 RTAB-Map Stack diff --git a/rtabmap_rviz_plugins/package.xml b/rtabmap_rviz_plugins/package.xml index 48bab9cb..7ff1e174 100644 --- a/rtabmap_rviz_plugins/package.xml +++ b/rtabmap_rviz_plugins/package.xml @@ -1,7 +1,7 @@ rtabmap_rviz_plugins - 0.21.9 + 0.21.10 RTAB-Map's rviz plugins. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_slam/package.xml b/rtabmap_slam/package.xml index a9030076..4427bd1c 100644 --- a/rtabmap_slam/package.xml +++ b/rtabmap_slam/package.xml @@ -1,7 +1,7 @@ rtabmap_slam - 0.21.9 + 0.21.10 RTAB-Map's SLAM package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_sync/package.xml b/rtabmap_sync/package.xml index 2299ac09..6bdc9e28 100644 --- a/rtabmap_sync/package.xml +++ b/rtabmap_sync/package.xml @@ -1,7 +1,7 @@ rtabmap_sync - 0.21.9 + 0.21.10 RTAB-Map's synchronization package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml index 0f7c2625..2286a923 100644 --- a/rtabmap_util/package.xml +++ b/rtabmap_util/package.xml @@ -1,7 +1,7 @@ rtabmap_util - 0.21.9 + 0.21.10 RTAB-Map's various useful nodes and nodelets. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_viz/package.xml b/rtabmap_viz/package.xml index 33a58e81..4bcba5fd 100644 --- a/rtabmap_viz/package.xml +++ b/rtabmap_viz/package.xml @@ -1,7 +1,7 @@ rtabmap_viz - 0.21.9 + 0.21.10 RTAB-Map's visualization package. Mathieu Labbe Mathieu Labbe