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

data_player: added --clock argument (like rosbag). Rtabmap: Detection rate update is done using only topic stamps (added warning if stamp is null). (#276)

Browse Source
This commit is contained in:
matlabbe
2018-09-30 15:21:05 -04:00
parent 803be533bc
commit 1a9dfaf084
6 changed files with 64 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+21 -9
View File
@@ -114,7 +114,6 @@ CoreWrapper::CoreWrapper() :
rate_(Parameters::defaultRtabmapDetectionRate()),
createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()),
maxMappingNodes_(Parameters::defaultGridGlobalMaxNodes()),
time_(ros::Time::now()),
previousStamp_(0),
mbClient_("move_base", true)
{
@@ -741,14 +740,21 @@ void CoreWrapper::defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg)
{
if(!paused_)
{
ros::Time stamp = imageMsg->header.stamp;
if(stamp.toSec() == 0.0)
{
ROS_WARN("A null stamp has been detected in the input topic. Make sure the stamp is set.");
return;
}
if(rate_>0.0f)
{
if(ros::Time::now() - time_ < ros::Duration(1.0f/rate_))
if(previousStamp_.toSec() > 0.0 && stamp.toSec() > previousStamp_.toSec() && stamp - previousStamp_ < ros::Duration(1.0f/rate_))
{
return;
}
}
time_ = ros::Time::now();
previousStamp_ = stamp;
if(!(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
@@ -894,10 +900,14 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg, ros::Ti
// Throttle
bool ignoreFrame = false;
if(stamp.toSec() == 0.0)
{
ROS_WARN("A null stamp has been detected in the input topics. Make sure the stamp in all input topics is set.");
ignoreFrame = true;
}
if(rate_>0.0f)
{
if((previousStamp_.toSec() > 0.0 && stamp.toSec() > previousStamp_.toSec() && stamp - previousStamp_ < ros::Duration(1.0f/rate_)) ||
((previousStamp_.toSec() <= 0.0 || stamp.toSec() <= previousStamp_.toSec()) && ros::Time::now() - time_ < ros::Duration(1.0f/rate_)))
if(previousStamp_.toSec() > 0.0 && stamp.toSec() > previousStamp_.toSec() && stamp - previousStamp_ < ros::Duration(1.0f/rate_))
{
ignoreFrame = true;
}
@@ -915,7 +925,6 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg, ros::Ti
}
else if(!ignoreFrame)
{
time_ = ros::Time::now();
previousStamp_ = stamp;
}
@@ -947,10 +956,14 @@ bool CoreWrapper::odomTFUpdate(const ros::Time & stamp)
lastPoseStamp_ = stamp;
bool ignoreFrame = false;
if(stamp.toSec() == 0.0)
{
ROS_WARN("A null stamp has been detected in the input topics. Make sure the stamp in all input topics is set.");
ignoreFrame = true;
}
if(rate_>0.0f)
{
if((previousStamp_.toSec() > 0.0 && stamp.toSec() > previousStamp_.toSec() && stamp - previousStamp_ < ros::Duration(1.0f/rate_)) ||
((previousStamp_.toSec() <= 0.0 || stamp.toSec() <= previousStamp_.toSec()) && ros::Time::now() - time_ < ros::Duration(1.0f/rate_)))
if(previousStamp_.toSec() > 0.0 && stamp.toSec() > previousStamp_.toSec() && stamp - previousStamp_ < ros::Duration(1.0f/rate_))
{
ignoreFrame = true;
}
@@ -968,7 +981,6 @@ bool CoreWrapper::odomTFUpdate(const ros::Time & stamp)
}
else if(!ignoreFrame)
{
time_ = ros::Time::now();
previousStamp_ = stamp;
}