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

Added RtabmapROS/TimeMsgConversion/ms stat

Browse Source
This commit is contained in:
matlabbe
2020-12-13 13:30:50 -05:00
parent b7a66e41f1
commit 2b0e313a6d
2 changed files with 20 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/rtabmap_ros/CoreWrapper.h
+2 -1
View File
@@ -182,7 +182,8 @@ private:
const rtabmap::Transform & odom = rtabmap::Transform(), const rtabmap::Transform & odom = rtabmap::Transform(),
const std::string & odomFrameId = "", const std::string & odomFrameId = "",
const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1), const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1),
const rtabmap::OdometryInfo & odomInfo = rtabmap::OdometryInfo()); const rtabmap::OdometryInfo & odomInfo = rtabmap::OdometryInfo(),
double timeMsgConversion = 0.0);
bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
src/CoreWrapper.cpp
+18 -7
View File
@@ -1149,6 +1149,7 @@ void CoreWrapper::commonDepthCallbackImpl(
const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3dMsgs, const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3dMsgs,
const std::vector<cv::Mat> & localDescriptorsMsgs) const std::vector<cv::Mat> & localDescriptorsMsgs)
{ {
UTimer timerConversion;
cv::Mat rgb; cv::Mat rgb;
cv::Mat depth; cv::Mat depth;
std::vector<rtabmap::CameraModel> cameraModels; std::vector<rtabmap::CameraModel> cameraModels;
@@ -1331,7 +1332,8 @@ void CoreWrapper::commonDepthCallbackImpl(
lastPose_, lastPose_,
odomFrameId, odomFrameId,
covariance_, covariance_,
odomInfo); odomInfo,
timerConversion.ticks());
covariance_ = cv::Mat(); covariance_ = cv::Mat();
} }
@@ -1350,6 +1352,7 @@ void CoreWrapper::commonStereoCallback(
const std::vector<rtabmap_ros::Point3f> & localPoints3dMsg, const std::vector<rtabmap_ros::Point3f> & localPoints3dMsg,
const cv::Mat & localDescriptorsMsg) const cv::Mat & localDescriptorsMsg)
{ {
UTimer timerConversion;
std::string odomFrameId = odomFrameId_; std::string odomFrameId = odomFrameId_;
if(odomMsg.get()) if(odomMsg.get())
{ {
@@ -1577,7 +1580,8 @@ void CoreWrapper::commonStereoCallback(
lastPose_, lastPose_,
odomFrameId, odomFrameId,
covariance_, covariance_,
odomInfo); odomInfo,
timerConversion.ticks());
covariance_ = cv::Mat(); covariance_ = cv::Mat();
} }
@@ -1590,6 +1594,7 @@ void CoreWrapper::commonLaserScanCallback(
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg, const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
const rtabmap_ros::GlobalDescriptor & globalDescriptor) const rtabmap_ros::GlobalDescriptor & globalDescriptor)
{ {
UTimer timerConversion;
std::string odomFrameId = odomFrameId_; std::string odomFrameId = odomFrameId_;
if(odomMsg.get()) if(odomMsg.get())
{ {
@@ -1721,7 +1726,8 @@ void CoreWrapper::commonLaserScanCallback(
lastPose_, lastPose_,
odomFrameId, odomFrameId,
covariance_, covariance_,
odomInfo); odomInfo,
timerConversion.ticks());
covariance_ = cv::Mat(); covariance_ = cv::Mat();
} }
@@ -1731,6 +1737,7 @@ void CoreWrapper::commonOdomCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg, const rtabmap_ros::UserDataConstPtr & userDataMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{ {
UTimer timerConversion;
UASSERT(odomMsg.get()); UASSERT(odomMsg.get());
std::string odomFrameId = odomFrameId_; std::string odomFrameId = odomFrameId_;
@@ -1788,7 +1795,8 @@ void CoreWrapper::commonOdomCallback(
lastPose_, lastPose_,
odomFrameId, odomFrameId,
covariance_, covariance_,
odomInfo); odomInfo,
timerConversion.ticks());
covariance_ = cv::Mat(); covariance_ = cv::Mat();
} }
@@ -1799,7 +1807,8 @@ void CoreWrapper::process(
const Transform & odom, const Transform & odom,
const std::string & odomFrameId, const std::string & odomFrameId,
const cv::Mat & odomCovariance, const cv::Mat & odomCovariance,
const OdometryInfo & odomInfo) const OdometryInfo & odomInfo,
double timeMsgConversion)
{ {
UTimer timer; UTimer timer;
if(rtabmap_.isIDsGenerated() || data.id() > 0) if(rtabmap_.isIDsGenerated() || data.id() > 0)
@@ -2231,20 +2240,22 @@ void CoreWrapper::process(
{ {
timeRtabmap = timer.ticks(); timeRtabmap = timer.ticks();
} }
NODELET_INFO("rtabmap (%d): Rate=%.2fs, Limit=%.3fs, RTAB-Map=%.4fs, Maps update=%.4fs pub=%.4fs (local map=%d, WM=%d)", NODELET_INFO("rtabmap (%d): Rate=%.2fs, Limit=%.3fs, Conversion=%.4fs, RTAB-Map=%.4fs, Maps update=%.4fs pub=%.4fs (local map=%d, WM=%d)",
rtabmap_.getLastLocationId(), rtabmap_.getLastLocationId(),
rate_>0?1.0f/rate_:0, rate_>0?1.0f/rate_:0,
rtabmap_.getTimeThreshold()/1000.0f, rtabmap_.getTimeThreshold()/1000.0f,
timeMsgConversion,
timeRtabmap, timeRtabmap,
timeUpdateMaps, timeUpdateMaps,
timePublishMaps, timePublishMaps,
(int)rtabmap_.getLocalOptimizedPoses().size(), (int)rtabmap_.getLocalOptimizedPoses().size(),
rtabmap_.getWMSize()+rtabmap_.getSTMSize()); rtabmap_.getWMSize()+rtabmap_.getSTMSize());
rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/HasSubscribers/"), mapsManager_.hasSubscribers()?1:0)); rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/HasSubscribers/"), mapsManager_.hasSubscribers()?1:0));
rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeMsgConversion/ms"), timeMsgConversion*1000.0f));
rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeRtabmap/ms"), timeRtabmap*1000.0f)); rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeRtabmap/ms"), timeRtabmap*1000.0f));
rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeUpdatingMaps/ms"), timeUpdateMaps*1000.0f)); rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeUpdatingMaps/ms"), timeUpdateMaps*1000.0f));
rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimePublishing/ms"), timePublishMaps*1000.0f)); rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimePublishing/ms"), timePublishMaps*1000.0f));
rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeTotal/ms"), (timeRtabmap+timeUpdateMaps+timePublishMaps)*1000.0f)); rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeTotal/ms"), (timeMsgConversion+timeRtabmap+timeUpdateMaps+timePublishMaps)*1000.0f));
} }
else if(!rtabmap_.isIDsGenerated()) else if(!rtabmap_.isIDsGenerated())
{ {