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

Odometry: added max_update_rate option.

Browse Source
This commit is contained in:
matlabbe
2020-05-20 15:10:17 -04:00
parent 0c4779c19e
commit cc79e19b1f
3 changed files with 25 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/OdometryROS.cpp
+15 -4
View File
@@ -80,6 +80,7 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
icpParams_(icpParams),
previousStamp_(0.0),
expectedUpdateRate_(0.0),
maxUpdateRate_(0.0),
odomStrategy_(Parameters::defaultOdomStrategy()),
waitIMUToinit_(false),
imuProcessed_(false),
@@ -152,7 +153,8 @@ void OdometryROS::onInit()
pnh.param("guess_min_rotation", guessMinRotation_, guessMinRotation_);
pnh.param("guess_min_time", guessMinTime_, guessMinTime_);
pnh.param("expected_update_rate", expectedUpdateRate_, expectedUpdateRate_);
pnh.param("expected_update_rate", expectedUpdateRate_, expectedUpdateRate_); // expected sensor rate
pnh.param("max_update_rate", maxUpdateRate_, maxUpdateRate_);
pnh.param("wait_imu_to_init", waitIMUToinit_, waitIMUToinit_);
@@ -178,6 +180,7 @@ void OdometryROS::onInit()
NODELET_INFO("Odometry: guess_min_rotation = %f", guessMinRotation_);
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: wait_imu_to_init = %s", waitIMUToinit_?"true":"false");
configPath = uReplaceChar(configPath, '~', UDirectory::homeDir());
@@ -536,9 +539,17 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
previousStamp_, stamp.toSec());
return;
}
else if(expectedUpdateRate_ > 0 &&
previousStamp_ > 0 &&
(stamp.toSec()-previousStamp_) < 1.0/expectedUpdateRate_)
else if(maxUpdateRate_ > 0 &&
previousStamp_ > 0 &&
(stamp.toSec()-previousStamp_+(expectedUpdateRate_ > 0?1.0/expectedUpdateRate_:0)) < 1.0/maxUpdateRate_)
{
// throttling
return;
}
else if(maxUpdateRate_ == 0 &&
expectedUpdateRate_ > 0 &&
previousStamp_ > 0 &&
(stamp.toSec()-previousStamp_) < 1.0/expectedUpdateRate_)
{
NODELET_WARN("Odometry: Aborting odometry update, higher frame rate detected (%f Hz) than the expected one (%f Hz). (stamps: previous=%fs new=%fs)",
1.0/(stamp.toSec()-previousStamp_), expectedUpdateRate_, previousStamp_, stamp.toSec());