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

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
include/rtabmap_ros/OdometryROS.h
+1
View File
@@ -147,6 +147,7 @@ private:
double previousStamp_; double previousStamp_;
double expectedUpdateRate_; double expectedUpdateRate_;
double maxUpdateRate_; double maxUpdateRate_;
double minUpdateRate_;
int odomStrategy_; int odomStrategy_;
bool waitIMUToinit_; bool waitIMUToinit_;
bool imuProcessed_; bool imuProcessed_;
src/OdometryROS.cpp
+26 -8
View File
@@ -81,6 +81,7 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
previousStamp_(0.0), previousStamp_(0.0),
expectedUpdateRate_(0.0), expectedUpdateRate_(0.0),
maxUpdateRate_(0.0), maxUpdateRate_(0.0),
minUpdateRate_(0.0),
odomStrategy_(Parameters::defaultOdomStrategy()), odomStrategy_(Parameters::defaultOdomStrategy()),
waitIMUToinit_(false), waitIMUToinit_(false),
imuProcessed_(false) imuProcessed_(false)
@@ -148,6 +149,7 @@ void OdometryROS::onInit()
pnh.param("expected_update_rate", expectedUpdateRate_, expectedUpdateRate_); // expected sensor rate pnh.param("expected_update_rate", expectedUpdateRate_, expectedUpdateRate_); // expected sensor rate
pnh.param("max_update_rate", maxUpdateRate_, maxUpdateRate_); pnh.param("max_update_rate", maxUpdateRate_, maxUpdateRate_);
pnh.param("min_update_rate", minUpdateRate_, minUpdateRate_);
pnh.param("wait_imu_to_init", waitIMUToinit_, waitIMUToinit_); 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: guess_min_time = %f", guessMinTime_);
NODELET_INFO("Odometry: expected_update_rate = %f Hz", expectedUpdateRate_); NODELET_INFO("Odometry: expected_update_rate = %f Hz", expectedUpdateRate_);
NODELET_INFO("Odometry: max_update_rate = %f Hz", maxUpdateRate_); 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"); NODELET_INFO("Odometry: wait_imu_to_init = %s", waitIMUToinit_?"true":"false");
configPath = uReplaceChar(configPath, '~', UDirectory::homeDir()); configPath = uReplaceChar(configPath, '~', UDirectory::homeDir());
@@ -260,8 +263,8 @@ void OdometryROS::onInit()
} }
else if(pnh.getParam(iter->first, vDouble)) else if(pnh.getParam(iter->first, vDouble))
{ {
NODELET_INFO( "Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str()); NODELET_INFO( "Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble, 6).c_str());
iter->second = uNumber2Str(vDouble); iter->second = uNumber2Str(vDouble, 6);
} }
else if(pnh.getParam(iter->first, vInt)) 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 // process data
ros::WallTime time = ros::WallTime::now(); ros::WallTime time = ros::WallTime::now();
rtabmap::OdometryInfo info; rtabmap::OdometryInfo info;
@@ -645,7 +650,11 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
{ {
data.setGroundTruth(groundTruth); 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()) if(!pose.isNull())
{ {
guess_.setNull(); 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 || tooOldPreviousData)
if(resetCurrentCount_ == 0)
{ {
if(!guess_.isNull()) 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)!", NODELET_WARN( "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!",
odomFrameId_.c_str(), frameId_.c_str()); odomFrameId_.c_str(), frameId_.c_str());
odometry_->reset(odometry_->getPose()); odometry_->reset(tfPose);
} }
} }
} }