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

merged master->ros2

Browse Source
This commit is contained in:
matlabbe
2025-09-25 01:38:26 -07:00
parent 6e84c77d90 4b0067d875
commit 1a7403dbf6
5 changed files with 132 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_odom/src/OdometryROS.cpp
+78 -41
View File
@@ -76,6 +76,8 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
guessMinTranslation_(0.0),
guessMinRotation_(0.0),
guessMinTime_(0.0),
guessLinearVariance_(0.001),
guessAngularVariance_(0.001),
publishTf_(true),
waitForTransform_(0.1), // 100 ms
publishNullWhenLost_(true),
@@ -143,6 +145,8 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
guessMinTranslation_ = this->declare_parameter("guess_min_translation", guessMinTranslation_);
guessMinRotation_ = this->declare_parameter("guess_min_rotation", guessMinRotation_);
guessMinTime_ = this->declare_parameter("guess_min_time", guessMinTime_);
guessLinearVariance_ = this->declare_parameter("guess_linear_variance", guessLinearVariance_);
guessAngularVariance_ = this->declare_parameter("guess_angular_variance", guessAngularVariance_);
expectedUpdateRate_ = this->declare_parameter("expected_update_rate", expectedUpdateRate_);
maxUpdateRate_ = this->declare_parameter("max_update_rate", maxUpdateRate_);
@@ -205,6 +209,8 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
RCLCPP_INFO(this->get_logger(), "Odometry: guess_min_translation = %f", guessMinTranslation_);
RCLCPP_INFO(this->get_logger(), "Odometry: guess_min_rotation = %f", guessMinRotation_);
RCLCPP_INFO(this->get_logger(), "Odometry: guess_min_time = %f", guessMinTime_);
RCLCPP_INFO(this->get_logger(), "Odometry: guess_linear_variance = %f", guessLinearVariance_);
RCLCPP_INFO(this->get_logger(), "Odometry: guess_angular_variance = %f", guessAngularVariance_);
RCLCPP_INFO(this->get_logger(), "Odometry: expected_update_rate = %f Hz", expectedUpdateRate_);
RCLCPP_INFO(this->get_logger(), "Odometry: max_update_rate = %f Hz", maxUpdateRate_);
RCLCPP_INFO(this->get_logger(), "Odometry: min_update_rate = %f Hz", minUpdateRate_);
@@ -710,6 +716,44 @@ void OdometryROS::processData()
}
}
bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_ > 1.0/minUpdateRate_;
if(tooOldPreviousData)
{
RCLCPP_WARN(this->get_logger(), "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.",
rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, rtabmap_conversions::timestampFromROS(header.stamp));
if(!guess_.isNull())
{
RCLCPP_WARN(this->get_logger(), "Odometry automatically reset based on latest guess available from TF (%s->%s, moved %s since got lost)!",
guessFrameId_.c_str(), frameId_.c_str(), guess_.prettyPrint().c_str());
odometry_->reset(odometry_->getPose() * guess_);
guess_.setNull();
guessPreviousPose_.setNull();
}
else
{
// Check TF to see if sensor fusion is used (e.g., the output of robot_localization)
Transform tfPose = rtabmap_conversions::getTransform(odomFrameId_, frameId_, header.stamp, *tfBuffer_, waitForTransform_);
if(tfPose.isNull())
{
RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest computed pose!");
odometry_->reset(odometry_->getPose());
}
else
{
RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!",
odomFrameId_.c_str(), frameId_.c_str());
odometry_->reset(tfPose);
}
}
}
bool skipOdometryUpdate = false;
rtabmap::Transform pose;
rtabmap::OdometryInfo info;
rtabmap::Transform guessVelocity;
Transform guessCurrentPose;
if(!guessFrameId_.empty())
@@ -746,28 +790,22 @@ void OdometryROS::processData()
(guessMinTime_ <= 0.0 || (previousStamp_>0.0 && rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_ < guessMinTime_)))
{
// Ignore odometry update, we didn't move enough
if(publishTf_)
{
geometry_msgs::msg::TransformStamped correctionMsg;
correctionMsg.child_frame_id = guessFrameId_;
correctionMsg.header.frame_id = odomFrameId_;
correctionMsg.header.stamp = header.stamp;
Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse();
rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform);
pose = odometry_->getPose() * guess_;
info.reg.covariance = cv::Mat::zeros(6,6,CV_64FC1);
info.reg.covariance.at<double>(0,0) = guessLinearVariance_; // xx
info.reg.covariance.at<double>(1,1) = guessLinearVariance_; // yy
info.reg.covariance.at<double>(2,2) = guessLinearVariance_; // zz
info.reg.covariance.at<double>(3,3) = guessAngularVariance_; // rr
info.reg.covariance.at<double>(4,4) = guessAngularVariance_; // pp
info.reg.covariance.at<double>(5,5) = guessAngularVariance_; // yawyaw
double time_now = now().seconds();
if(time_now >= previousClockTime_) {
tfBroadcaster_->sendTransform(correctionMsg);
}
else {
RCLCPP_WARN(this->get_logger(), "TF %s->%s is not published because we detected a time jump in the past of %f sec.",
correctionMsg.header.frame_id.c_str(),
correctionMsg.child_frame_id.c_str(),
previousClockTime_ - time_now);
}
}
guessPreviousPose_ = guessCurrentPose;
return;
//set velocity
double dt = rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_;
UASSERT(dt>0.0);
// use part of guess matching dt
(previousPose.inverse() * guessCurrentPose).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
guessVelocity = rtabmap::Transform(x/dt, y/dt, z/dt, roll/dt, pitch/dt, yaw/dt);
skipOdometryUpdate = true;
}
}
guessPreviousPose_ = guessCurrentPose;
@@ -779,23 +817,21 @@ void OdometryROS::processData()
}
}
bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && (rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_) > 1.0/minUpdateRate_;
// process data
rclcpp::Time timeStart = rclcpp::Clock().now();
rtabmap::OdometryInfo info;
if(!groundTruth.isNull())
{
data.setGroundTruth(groundTruth);
}
rtabmap::Transform pose;
if(!tooOldPreviousData)
if(!skipOdometryUpdate)
{
pose = odometry_->process(data, guess_, &info);
}
if(!pose.isNull())
{
guess_.setNull();
if(!skipOdometryUpdate) {
guess_.setNull();
}
resetCurrentCount_ = resetCountdown_;
//*********************
@@ -869,11 +905,16 @@ void OdometryROS::processData()
odom.pose.covariance.at(35) = info.reg.covariance.at<double>(5,5)*2; // yawyaw
//set velocity
bool setTwist = !odometry_->getVelocityGuess().isNull();
bool setTwist = !guessVelocity.isNull() || !odometry_->getVelocityGuess().isNull();
if(setTwist)
{
float x,y,z,roll,pitch,yaw;
odometry_->getVelocityGuess().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
if(skipOdometryUpdate) {
UASSERT(!guessVelocity.isNull());
guessVelocity.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
} else {
odometry_->getVelocityGuess().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
}
odom.twist.twist.linear.x = x;
odom.twist.twist.linear.y = y;
odom.twist.twist.linear.z = z;
@@ -918,7 +959,7 @@ void OdometryROS::processData()
odomLocalMap_->publish(cloudMsg);
}
if(odomLastFrame_->get_subscription_count())
if(!skipOdometryUpdate && odomLastFrame_->get_subscription_count())
{
// check which type of Odometry is using
if(odometry_->getType() == Odometry::kTypeF2M) // If it's Frame to Map Odometry
@@ -1049,20 +1090,14 @@ void OdometryROS::processData()
}
if(pose.isNull() && (resetCurrentCount_ > 0 || tooOldPreviousData))
if(pose.isNull() && resetCurrentCount_ > 0)
{
if(tooOldPreviousData)
{
RCLCPP_WARN(this->get_logger(), "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.",
rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, rtabmap_conversions::timestampFromROS(header.stamp));
}
else if(--resetCurrentCount_>0)
if(--resetCurrentCount_>0)
{
RCLCPP_WARN(this->get_logger(), "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_);
}
if(resetCurrentCount_ == 0 || tooOldPreviousData)
if(resetCurrentCount_ == 0)
{
if(!guess_.isNull())
{
@@ -1225,9 +1260,11 @@ void OdometryROS::processData()
msg.header.stamp = header.stamp; // use corresponding time stamp to image
odomSensorDataCompressedPub_->publish(msg);
}
double delay = (now()-header.stamp).seconds();
if(visParams_)
if(skipOdometryUpdate) {
RCLCPP_INFO(this->get_logger(), "Odom: <skipped: guess not moving enough>, std dev=%fm|%frad, update time=%fs, delay=%fs", pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (rclcpp::Clock().now()-timeStart).seconds(), delay);
}
else if(visParams_)
{
if(icpParams_)
{