i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-10 19:49:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:introlab/rtabmap_ros into noetic-devel

Browse Source
This commit is contained in:
matlabbe
2025-02-12 19:55:24 -08:00
parent fa702f6b4b 59390d8199
commit 42718aed29
18 changed files with 96 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_conversions/package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>rtabmap_conversions</name>
<version>0.21.9</version>
<version>0.21.10</version>
<description>RTAB-Map's conversions package. This package can be used to convert rtabmap_msgs's msgs into RTAB-Map's library objects.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
rtabmap_conversions/src/MsgConversion.cpp
+49
View File
@@ -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);
}
rtabmap_costmap_plugins/package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>rtabmap_costmap_plugins</name>
<version>0.21.9</version>
<version>0.21.10</version>
<description>RTAB-Map's costmap_2d plugins</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
rtabmap_demos/package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>rtabmap_demos</name>
<version>0.21.9</version>
<version>0.21.10</version>
<description>RTAB-Map's demo launch files.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
rtabmap_examples/package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>rtabmap_examples</name>
<version>0.21.9</version>
<version>0.21.10</version>
<description>RTAB-Map's example launch files.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
rtabmap_launch/package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>rtabmap_launch</name>
<version>0.21.9</version>
<version>0.21.10</version>
<description>RTAB-Map's main launch files.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
rtabmap_legacy/package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>rtabmap_legacy</name>
<version>0.21.9</version>
<version>0.21.10</version>
<description>RTAB-Map's legacy launch files.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
rtabmap_msgs/package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>rtabmap_msgs</name>
<version>0.21.9</version>
<version>0.21.10</version>
<description>RTAB-Map's msgs package.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
rtabmap_odom/include/rtabmap_odom/OdometryROS.h
+1
View File
@@ -148,6 +148,7 @@ private:
USemaphore dataReady_;
rtabmap::SensorData dataToProcess_;
std_msgs::Header dataHeaderToProcess_;
bool bufferedDataToProcess_;
bool paused_;
int resetCountdown_;
rtabmap_odom/package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>rtabmap_odom</name>
<version>0.21.9</version>
<version>0.21.10</version>
<description>RTAB-Map's odometry package.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
rtabmap_odom/src/OdometryROS.cpp
+31 -9
View File
@@ -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();
rtabmap_python/package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>rtabmap_python</name>
<version>0.21.9</version>
<version>0.21.10</version>
<description>RTAB-Map's python package.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
rtabmap_ros/package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>rtabmap_ros</name>
<version>0.21.9</version>
<version>0.21.10</version>
<description>
RTAB-Map Stack
</description>
rtabmap_rviz_plugins/package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>rtabmap_rviz_plugins</name>
<version>0.21.9</version>
<version>0.21.10</version>
<description>RTAB-Map's rviz plugins.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
rtabmap_slam/package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>rtabmap_slam</name>
<version>0.21.9</version>
<version>0.21.10</version>
<description>RTAB-Map's SLAM package.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
rtabmap_sync/package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>rtabmap_sync</name>
<version>0.21.9</version>
<version>0.21.10</version>
<description>RTAB-Map's synchronization package.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
rtabmap_util/package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>rtabmap_util</name>
<version>0.21.9</version>
<version>0.21.10</version>
<description>RTAB-Map's various useful nodes and nodelets.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
rtabmap_viz/package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>rtabmap_viz</name>
<version>0.21.9</version>
<version>0.21.10</version>
<description>RTAB-Map's visualization package.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>