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

sync with upstream 0.18.2. Fixed icp_odometry pause not working, added expected_update_rate parameter to odometry to filter messages with bad stamps (gazebo issue), filter consecutive messages with same stamp

Browse Source
This commit is contained in:
matlabbe
2018-11-21 12:01:09 -05:00
parent 801bac5221
commit 7f8a221f7e
7 changed files with 83 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+4
View File
@@ -1122,6 +1122,8 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
info.transform = transformFromGeometryMsg(msg.transform);
info.transformFiltered = transformFromGeometryMsg(msg.transformFiltered);
info.transformGroundTruth = transformFromGeometryMsg(msg.transformGroundTruth);
info.guessVelocity = transformFromGeometryMsg(msg.guessVelocity);
UASSERT(msg.localMapKeys.size() == msg.localMapValues.size());
for(unsigned int i=0; i<msg.localMapKeys.size(); ++i)
@@ -1176,6 +1178,8 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
transformToGeometryMsg(info.transform, msg.transform);
transformToGeometryMsg(info.transformFiltered, msg.transformFiltered);
transformToGeometryMsg(info.transformGroundTruth, msg.transformGroundTruth);
transformToGeometryMsg(info.guessVelocity, msg.guessVelocity);
msg.localMapKeys = uKeys(info.localMap);
points3fToROS(uValues(info.localMap), msg.localMapValues);
src/OdometryROS.cpp
+62 -23
View File
@@ -78,7 +78,9 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
stereoParams_(stereoParams),
visParams_(visParams),
icpParams_(icpParams),
guessStamp_(0.0)
guessStamp_(0.0),
previousStamp_(0.0),
expectedUpdateRate_(0.0)
{
}
@@ -146,6 +148,8 @@ void OdometryROS::onInit()
pnh.param("guess_min_translation", guessMinTranslation_, guessMinTranslation_);
pnh.param("guess_min_rotation", guessMinRotation_, guessMinRotation_);
pnh.param("expected_update_rate", expectedUpdateRate_, expectedUpdateRate_);
if(publishTf_ && !guessFrameId_.empty() && guessFrameId_.compare(odomFrameId_) == 0)
{
NODELET_WARN( "\"publish_tf\" and \"guess_frame_id\" cannot be used "
@@ -166,6 +170,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: expected_update_rate = %f Hz", expectedUpdateRate_);
configPath = uReplaceChar(configPath, '~', UDirectory::homeDir());
if(configPath.size() && configPath.at(0) != '/')
@@ -389,26 +394,53 @@ Transform OdometryROS::getTransform(const std::string & fromFrameId, const std::
void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
{
if(!data.imageRaw().empty())
if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
{
if(odometry_->getPose().isIdentity() &&
!groundTruthFrameId_.empty())
if(previousStamp_>0.0 && previousStamp_ >= stamp.toSec())
{
// sync with the first value of the ground truth
Transform initialPose = getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, stamp);
if(initialPose.isNull())
static bool warned = false;
if(!warned)
{
NODELET_WARN("Ground truth frames \"%s\" -> \"%s\" are set but failed to "
"get them, odometry won't be synchronized with ground truth.",
groundTruthFrameId_.c_str(), groundTruthBaseFrameId_.c_str());
NODELET_WARN("Odometry: Detected not valid consecutive stamps (previous=%fs new=%fs). New stamp should be always greater than previous stamp. This new data is ignored. This message will appear only once.",
previousStamp_, stamp.toSec());
warned = true;
}
else
return;
}
else if(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());
return;
}
}
Transform groundTruth;
if(!groundTruthFrameId_.empty())
{
groundTruth = getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, stamp);
if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
{
if(odometry_->getPose().isIdentity())
{
NODELET_INFO( "Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
initialPose.prettyPrint().c_str(),
groundTruthFrameId_.c_str(),
groundTruthBaseFrameId_.c_str());
odometry_->reset(initialPose);
// sync with the first value of the ground truth
if(groundTruth.isNull())
{
NODELET_WARN("Ground truth frames \"%s\" -> \"%s\" are set but failed to "
"get them, odometry won't be initialized with ground truth.",
groundTruthFrameId_.c_str(), groundTruthBaseFrameId_.c_str());
}
else
{
NODELET_INFO( "Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
groundTruth.prettyPrint().c_str(),
groundTruthFrameId_.c_str(),
groundTruthBaseFrameId_.c_str());
odometry_->reset(groundTruth);
}
}
}
}
@@ -463,6 +495,10 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
ros::WallTime time = ros::WallTime::now();
rtabmap::OdometryInfo info;
SensorData dataCpy = data;
if(!groundTruth.isNull())
{
dataCpy.setGroundTruth(groundTruth);
}
rtabmap::Transform pose = odometry_->process(dataCpy, guess_, &info);
guess_.setNull();
if(!pose.isNull())
@@ -632,7 +668,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
odomLocalScanMap_.publish(cloudMsg);
}
}
else if(data.imageRaw().empty() && !data.imu().empty())
else if(data.imageRaw().empty() && data.laserScanRaw().isEmpty() && !data.imu().empty())
{
return;
}
@@ -695,7 +731,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
odomInfoPub_.publish(infoMsg);
}
if(!data.imageRaw().empty())
if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
{
if(visParams_)
{
@@ -708,11 +744,12 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.reg.inliers, 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)), (ros::WallTime::now()-time).toSec());
}
}
else
else // if(icpParams_)
{
NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, 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)), (ros::WallTime::now()-time).toSec());
}
}
previousStamp_ = stamp.toSec();
}
bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
@@ -721,6 +758,7 @@ bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
odometry_->reset();
guess_.setNull();
guessStamp_ = 0.0;
previousStamp_ = 0.0;
resetCurrentCount_ = resetCountdown_;
this->flushCallbacks();
return true;
@@ -733,6 +771,7 @@ bool OdometryROS::resetToPose(rtabmap_ros::ResetPose::Request& req, rtabmap_ros:
odometry_->reset(pose);
guess_.setNull();
guessStamp_ = 0.0;
previousStamp_ = 0.0;
resetCurrentCount_ = resetCountdown_;
this->flushCallbacks();
return true;
@@ -742,12 +781,12 @@ bool OdometryROS::pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
if(paused_)
{
NODELET_WARN( "visual_odometry: Already paused!");
NODELET_WARN( "Odometry: Already paused!");
}
else
{
paused_ = true;
NODELET_INFO( "visual_odometry: paused!");
NODELET_INFO( "Odometry: paused!");
}
return true;
}
@@ -756,12 +795,12 @@ bool OdometryROS::resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
if(!paused_)
{
NODELET_WARN( "visual_odometry: Already running!");
NODELET_WARN( "Odometry: Already running!");
}
else
{
paused_ = false;
NODELET_INFO( "visual_odometry: resumed!");
NODELET_INFO( "Odometry: resumed!");
}
return true;
}
src/nodelets/icp_odometry.cpp
+11 -2
View File
@@ -90,9 +90,9 @@ private:
NODELET_INFO("IcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
NODELET_INFO("IcpOdometry: scan_downsampling_step = %d", scanDownsamplingStep_);
NODELET_INFO("IcpOdometry: scan_voxel_size = %f", scanVoxelSize_);
NODELET_INFO("IcpOdometry: scan_voxel_size = %f m", scanVoxelSize_);
NODELET_INFO("IcpOdometry: scan_normal_k = %d", scanNormalK_);
NODELET_INFO("IcpOdometry: scan_normal_radius = %f", scanNormalRadius_);
NODELET_INFO("IcpOdometry: scan_normal_radius = %f m", scanNormalRadius_);
scan_sub_ = nh.subscribe("scan", 1, &ICPOdometry::callbackScan, this);
cloud_sub_ = nh.subscribe("scan_cloud", 1, &ICPOdometry::callbackCloud, this);
@@ -175,6 +175,11 @@ private:
void callbackScan(const sensor_msgs::LaserScanConstPtr& scanMsg)
{
if(this->isPaused())
{
return;
}
// make sure the frame of the laser is updated too
Transform localScanTransform = getTransform(this->frameId(),
scanMsg->header.frame_id,
@@ -244,6 +249,10 @@ private:
void callbackCloud(const sensor_msgs::PointCloud2ConstPtr& cloudMsg)
{
if(this->isPaused())
{
return;
}
cv::Mat scan;
bool containNormals = false;
if(scanVoxelSize_ == 0.0f)