i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-11 12:09:51 +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
include/rtabmap_ros/OdometryROS.h
+1
View File
@@ -137,6 +137,7 @@ private:
rtabmap::Transform guessPreviousPose_; rtabmap::Transform guessPreviousPose_;
double previousStamp_; double previousStamp_;
double expectedUpdateRate_; double expectedUpdateRate_;
double maxUpdateRate_;
int odomStrategy_; int odomStrategy_;
bool waitIMUToinit_; bool waitIMUToinit_;
bool imuProcessed_; bool imuProcessed_;
launch/rtabmap.launch
+9 -1
View File
@@ -100,7 +100,9 @@
<arg name="odom_sensor_sync" default="false"/> <arg name="odom_sensor_sync" default="false"/>
<arg name="odom_guess_frame_id" default=""/> <arg name="odom_guess_frame_id" default=""/>
<arg name="odom_guess_min_translation" default="0"/> <arg name="odom_guess_min_translation" default="0"/>
<arg name="odom_guess_min_rotation" default="0"/> <arg name="odom_guess_min_rotation" default="0"/>
<arg name="odom_max_rate" default="0"/>
<arg name="odom_expected_rate" default="0"/>
<arg name="imu_topic" default="/imu/data"/> <!-- only used with VIO approaches --> <arg name="imu_topic" default="/imu/data"/> <!-- only used with VIO approaches -->
<arg name="wait_imu_to_init" default="false"/> <arg name="wait_imu_to_init" default="false"/>
@@ -213,6 +215,8 @@
<param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/> <param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/>
<param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/> <param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/>
<param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/> <param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/>
<param name="expected_update_rate" type="double" value="$(arg odom_expected_rate)"/>
<param name="max_update_rate" type="double" value="$(arg odom_max_rate)"/>
</node> </node>
<!-- Stereo Odometry --> <!-- Stereo Odometry -->
@@ -239,6 +243,8 @@
<param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/> <param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/>
<param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/> <param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/>
<param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/> <param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/>
<param name="expected_update_rate" type="double" value="$(arg odom_expected_rate)"/>
<param name="max_update_rate" type="double" value="$(arg odom_max_rate)"/>
</node> </node>
</group> </group>
</group> </group>
@@ -263,6 +269,8 @@
<param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/> <param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/>
<param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/> <param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/>
<param name="scan_cloud_max_points" type="int" value="$(arg scan_cloud_max_points)"/> <param name="scan_cloud_max_points" type="int" value="$(arg scan_cloud_max_points)"/>
<param name="expected_update_rate" type="double" value="$(arg odom_expected_rate)"/>
<param name="max_update_rate" type="double" value="$(arg odom_max_rate)"/>
</node> </node>
<node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen"> <node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen">
src/OdometryROS.cpp
+15 -4
View File
@@ -80,6 +80,7 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
icpParams_(icpParams), icpParams_(icpParams),
previousStamp_(0.0), previousStamp_(0.0),
expectedUpdateRate_(0.0), expectedUpdateRate_(0.0),
maxUpdateRate_(0.0),
odomStrategy_(Parameters::defaultOdomStrategy()), odomStrategy_(Parameters::defaultOdomStrategy()),
waitIMUToinit_(false), waitIMUToinit_(false),
imuProcessed_(false), imuProcessed_(false),
@@ -152,7 +153,8 @@ void OdometryROS::onInit()
pnh.param("guess_min_rotation", guessMinRotation_, guessMinRotation_); pnh.param("guess_min_rotation", guessMinRotation_, guessMinRotation_);
pnh.param("guess_min_time", guessMinTime_, guessMinTime_); 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_); 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_rotation = %f", guessMinRotation_);
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: 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());
@@ -536,9 +539,17 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
previousStamp_, stamp.toSec()); previousStamp_, stamp.toSec());
return; return;
} }
else if(expectedUpdateRate_ > 0 && else if(maxUpdateRate_ > 0 &&
previousStamp_ > 0 && previousStamp_ > 0 &&
(stamp.toSec()-previousStamp_) < 1.0/expectedUpdateRate_) (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)", 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()); 1.0/(stamp.toSec()-previousStamp_), expectedUpdateRate_, previousStamp_, stamp.toSec());