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

odom: Added min_update_rate parameter

Browse Source
This commit is contained in:
matlabbe
2022-12-05 20:56:29 -08:00
parent f327b3f289
commit e34ea3e0b3
2 changed files with 27 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/OdometryROS.cpp
+26 -8
View File
@@ -81,6 +81,7 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
previousStamp_(0.0),
expectedUpdateRate_(0.0),
maxUpdateRate_(0.0),
minUpdateRate_(0.0),
odomStrategy_(Parameters::defaultOdomStrategy()),
waitIMUToinit_(false),
imuProcessed_(false)
@@ -148,6 +149,7 @@ void OdometryROS::onInit()
pnh.param("expected_update_rate", expectedUpdateRate_, expectedUpdateRate_); // expected sensor rate
pnh.param("max_update_rate", maxUpdateRate_, maxUpdateRate_);
pnh.param("min_update_rate", minUpdateRate_, minUpdateRate_);
pnh.param("wait_imu_to_init", waitIMUToinit_, waitIMUToinit_);
@@ -180,6 +182,7 @@ void OdometryROS::onInit()
NODELET_INFO("Odometry: guess_min_time = %f", guessMinTime_);
NODELET_INFO("Odometry: expected_update_rate = %f Hz", expectedUpdateRate_);
NODELET_INFO("Odometry: max_update_rate = %f Hz", maxUpdateRate_);
NODELET_INFO("Odometry: min_update_rate = %f Hz", minUpdateRate_);
NODELET_INFO("Odometry: wait_imu_to_init = %s", waitIMUToinit_?"true":"false");
configPath = uReplaceChar(configPath, '~', UDirectory::homeDir());
@@ -260,8 +263,8 @@ void OdometryROS::onInit()
}
else if(pnh.getParam(iter->first, vDouble))
{
NODELET_INFO( "Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
iter->second = uNumber2Str(vDouble);
NODELET_INFO( "Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble, 6).c_str());
iter->second = uNumber2Str(vDouble, 6);
}
else if(pnh.getParam(iter->first, vInt))
{
@@ -638,6 +641,8 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
}
}
bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && (header.stamp.toSec()-previousStamp_) > 1.0/minUpdateRate_;
// process data
ros::WallTime time = ros::WallTime::now();
rtabmap::OdometryInfo info;
@@ -645,7 +650,11 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
{
data.setGroundTruth(groundTruth);
}
rtabmap::Transform pose = odometry_->process(data, guess_, &info);
rtabmap::Transform pose;
if(!tooOldPreviousData)
{
pose = odometry_->process(data, guess_, &info);
}
if(!pose.isNull())
{
guess_.setNull();
@@ -871,12 +880,21 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
}
}
if(pose.isNull() && resetCurrentCount_ > 0)
if(pose.isNull() && (resetCurrentCount_ > 0 || tooOldPreviousData))
{
NODELET_WARN( "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_);
if(tooOldPreviousData)
{
NODELET_WARN( "Odometry lost! Odometry will be reset because last update "
"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
{
NODELET_WARN( "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_);
--resetCurrentCount_;
}
--resetCurrentCount_;
if(resetCurrentCount_ == 0)
if(resetCurrentCount_ == 0 || tooOldPreviousData)
{
if(!guess_.isNull())
{
@@ -898,7 +916,7 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
{
NODELET_WARN( "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!",
odomFrameId_.c_str(), frameId_.c_str());
odometry_->reset(odometry_->getPose());
odometry_->reset(tfPose);
}
}
}