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

point_cloud_aggregator: added fixed_frame_id to adjust clouds when the robot is moving (accordingly to odometry). odom: added guess_min_time parameter (used to force odom to publish odom msg at this rate in case the guess is not changing).

Browse Source
This commit is contained in:
matlabbe
2019-01-15 21:37:43 -05:00
parent ba5738354d
commit d4de48847e
3 changed files with 46 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/OdometryROS.cpp
+5 -1
View File
@@ -67,6 +67,7 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
guessFrameId_(""),
guessMinTranslation_(0.0),
guessMinRotation_(0.0),
guessMinTime_(0.0),
publishTf_(true),
waitForTransform_(true),
waitForTransformDuration_(0.1), // 100 ms
@@ -147,6 +148,7 @@ void OdometryROS::onInit()
pnh.param("guess_frame_id", guessFrameId_, guessFrameId_); // odometry guess frame
pnh.param("guess_min_translation", guessMinTranslation_, guessMinTranslation_);
pnh.param("guess_min_rotation", guessMinRotation_, guessMinRotation_);
pnh.param("guess_min_time", guessMinTime_, guessMinTime_);
pnh.param("expected_update_rate", expectedUpdateRate_, expectedUpdateRate_);
@@ -172,6 +174,7 @@ void OdometryROS::onInit()
NODELET_INFO("Odometry: guess_frame_id = %s", guessFrameId_.c_str());
NODELET_INFO("Odometry: guess_min_translation = %f", guessMinTranslation_);
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: wait_imu_to_init = %s", waitIMUToinit_?"true":"false");
@@ -556,7 +559,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
float x,y,z,roll,pitch,yaw;
guess_.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
if((guessMinTranslation_ <= 0.0 || uMax3(fabs(x), fabs(y), fabs(z)) < guessMinTranslation_) &&
(guessMinRotation_ <= 0.0 || uMax3(fabs(roll), fabs(pitch), fabs(yaw)) < guessMinRotation_))
(guessMinRotation_ <= 0.0 || uMax3(fabs(roll), fabs(pitch), fabs(yaw)) < guessMinRotation_) &&
(guessMinTime_ <= 0.0 || (previousStamp_>0.0 && stamp.toSec()-previousStamp_ < guessMinTime_)))
{
// Ignore odometry update, we didn't move enough
if(publishTf_)