i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-07 18:27:46 +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;
}
src/DbPlayerNode.cpp
+36 -3
View File
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/CameraInfo.h>
#include <rosgraph_msgs/Clock.h>
#include <pcl_conversions/pcl_conversions.h>
#include <nav_msgs/Odometry.h>
#include <cv_bridge/cv_bridge.h>
@@ -86,6 +87,14 @@ int main(int argc, char** argv)
//ULogger::setLevel(ULogger::kDebug);
//ULogger::setEventLevel(ULogger::kWarning);
bool publishClock = false;
for(int i=1;i<argc;++i)
{
if(strcmp(argv[i], "--clock") == 0)
{
publishClock = true;
}
}
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
@@ -98,6 +107,7 @@ int main(int argc, char** argv)
std::string databasePath = "";
bool publishTf = true;
int startId = 0;
bool useDbStamps = true;
pnh.param("frame_id", frameId, frameId);
pnh.param("odom_frame_id", odomFrameId, odomFrameId);
@@ -108,7 +118,7 @@ int main(int argc, char** argv)
pnh.param("publish_tf", publishTf, publishTf);
pnh.param("start_id", startId, startId);
// based on URG-04LX
// A general 360 lidar with 0.5 deg increment
double scanAngleMin, scanAngleMax, scanAngleIncrement, scanTime, scanRangeMin, scanRangeMax;
pnh.param<double>("scan_angle_min", scanAngleMin, -M_PI);
pnh.param<double>("scan_angle_max", scanAngleMax, M_PI);
@@ -124,6 +134,7 @@ int main(int argc, char** argv)
ROS_INFO("rate = %f", rate);
ROS_INFO("publish_tf = %s", publishTf?"true":"false");
ROS_INFO("start_id = %d", startId);
ROS_INFO("Publish clock (--clock): %s", publishClock?"true":"false");
if(databasePath.empty())
{
@@ -159,8 +170,14 @@ int main(int argc, char** argv)
ros::Publisher rightCamInfoPub;
ros::Publisher odometryPub;
ros::Publisher scanPub;
ros::Publisher clockPub;
tf2_ros::TransformBroadcaster tfBroadcaster;
if(publishClock)
{
clockPub = nh.advertise<rosgraph_msgs::Clock>("/clock", 1);
}
UTimer timer;
rtabmap::CameraInfo cameraInfo;
rtabmap::SensorData data = reader.takeImage(&cameraInfo);
@@ -172,7 +189,14 @@ int main(int argc, char** argv)
{
ROS_INFO("Reading sensor data %d...", odom.data().id());
ros::Time time = ros::Time::now();
ros::Time time(odom.data().stamp());
if(publishClock)
{
rosgraph_msgs::Clock msg;
msg.clock = time;
clockPub.publish(msg);
}
sensor_msgs::CameraInfo camInfoA; //rgb or left
sensor_msgs::CameraInfo camInfoB; //depth or right
@@ -263,7 +287,16 @@ int main(int argc, char** argv)
if(!odom.data().laserScanRaw().isEmpty())
{
if(scanPub.getTopic().empty()) scanPub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
if(scanPub.getTopic().empty())
{
scanPub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
ROS_INFO("Scan will be published with those parameters:");
ROS_INFO(" scan_angle_min=%f", scanAngleMin);
ROS_INFO(" scan_angle_max=%f", scanAngleMax);
ROS_INFO(" scan_angle_increment=%f", scanAngleIncrement);
ROS_INFO(" scan_range_min=%f", scanRangeMin);
ROS_INFO(" scan_range_max=%f", scanRangeMax);
}
}
// publish transforms first