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

Merge pull request #2 from introlab/master

Browse Source 
Merge master to fork
This commit is contained in:
PrescilliaA
2019-11-27 10:29:40 -05:00
committed by GitHub
parent 3e0c501c32 954836c58b
commit 1952f8496e
8 changed files with 242 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.travis.yml
+96 -36
View File
@@ -1,47 +1,107 @@
sudo: true sudo: true
dist: trusty
language: cpp language: cpp
compiler: compiler:
- gcc - gcc
addons: matrix:
apt: include:
packages:
- cmake
- libopencv-dev
- libqt4-dev
- libsqlite3-dev
install: - dist: trusty
- sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list' install:
- wget http://packages.ros.org/ros.key -O - | sudo apt-key add - - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
- sudo apt-get update - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
- sudo apt-get install dpkg - sudo apt-get update
- sudo apt-get -y install ros-indigo-ros-base libpcl-1.7-all libfreenect-dev ros-indigo-libg2o ros-indigo-octomap libopenni2-dev ros-indigo-costmap-2d ros-indigo-octomap-msgs ros-indigo-rviz ros-indigo-cv-bridge ros-indigo-move-base-msgs ros-indigo-costmap-2d ros-indigo-image-geometry ros-indigo-message-filters ros-indigo-image-transport ros-indigo-eigen-conversions ros-indigo-stereo-msgs ros-indigo-nav-msgs ros-indigo-sensor-msgs ros-indigo-tf-conversions ros-indigo-laser-geometry ros-indigo-pcl-conversions ros-indigo-pcl-ros ros-indigo-dynamic-reconfigure ros-indigo-nodelet - sudo apt-get install dpkg
- sudo apt-get -y install ros-indigo-rtabmap-ros
- sudo apt-get -y remove ros-indigo-rtabmap
script: script:
- source /opt/ros/indigo/setup.bash - source /opt/ros/indigo/setup.bash
- export PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages - export PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages
- cd .. - cd ..
- mkdir -p catkin_ws/src - mkdir -p catkin_ws/src
- cd catkin_ws/src - cd catkin_ws/src
- catkin_init_workspace - catkin_init_workspace
- cd .. - cd ..
- catkin_make - catkin_make
- cd .. - cd ..
- mv rtabmap_ros catkin_ws/src/. - mv rtabmap_ros catkin_ws/src/.
- git clone https://github.com/introlab/rtabmap.git - git clone https://github.com/introlab/rtabmap.git
- cd rtabmap - cd rtabmap
- if [ "$TRAVIS_BRANCH" = "devel" ]; then git checkout devel; fi - if [ "$TRAVIS_BRANCH" = "devel" ]; then git checkout devel; fi
- mkdir -p build && cd build - mkdir -p build && cd build
- cmake -DCMAKE_INSTALL_PREFIX=~/build/introlab/catkin_ws/devel .. - cmake -DCMAKE_INSTALL_PREFIX=~/build/introlab/catkin_ws/devel ..
- make - make
- make install - make install
- cd ../../catkin_ws - cd ../../catkin_ws
- source devel/setup.bash - source devel/setup.bash
- catkin_make - catkin_make
- catkin_make install - catkin_make install
- dist: xenial
install:
- sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu xenial main" > /etc/apt/sources.list.d/ros-latest.list'
- wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
- sudo apt-get update
- sudo apt-get install dpkg
- sudo apt-get -y install ros-kinetic-rtabmap-ros
- sudo apt-get -y remove ros-kinetic-rtabmap
script:
- source /opt/ros/kinetic/setup.bash
- export PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages
- cd ..
- mkdir -p catkin_ws/src
- cd catkin_ws/src
- catkin_init_workspace
- cd ..
- catkin_make
- cd ..
- mv rtabmap_ros catkin_ws/src/.
- git clone https://github.com/introlab/rtabmap.git
- cd rtabmap
- if [ "$TRAVIS_BRANCH" = "devel" ]; then git checkout devel; fi
- mkdir -p build && cd build
- cmake -DCMAKE_INSTALL_PREFIX=~/build/introlab/catkin_ws/devel ..
- make
- make install
- cd ../../catkin_ws
- source devel/setup.bash
- catkin_make
- catkin_make install
- dist: bionic
install:
- sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu bionic main" > /etc/apt/sources.list.d/ros-latest.list'
- wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
- sudo apt-get update
- sudo apt-get install dpkg
- sudo apt-get -y install ros-melodic-rtabmap-ros
- sudo apt-get -y remove ros-melodic-rtabmap
script:
- source /opt/ros/melodic/setup.bash
- export PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages
- cd ..
- mkdir -p catkin_ws/src
- cd catkin_ws/src
- catkin_init_workspace
- cd ..
- catkin_make
- cd ..
- mv rtabmap_ros catkin_ws/src/.
- git clone https://github.com/introlab/rtabmap.git
- cd rtabmap
- if [ "$TRAVIS_BRANCH" = "devel" ]; then git checkout devel; fi
- mkdir -p build && cd build
- cmake -DCMAKE_INSTALL_PREFIX=~/build/introlab/catkin_ws/devel ..
- make
- make install
- cd ../../catkin_ws
- source devel/setup.bash
- catkin_make
- catkin_make install
notifications: notifications:
email: email:
include/rtabmap_ros/CoreWrapper.h
+8 -1
View File
@@ -57,6 +57,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap_ros/Goal.h" #include "rtabmap_ros/Goal.h"
#include "rtabmap_ros/GetPlan.h" #include "rtabmap_ros/GetPlan.h"
#include "rtabmap_ros/CommonDataSubscriber.h" #include "rtabmap_ros/CommonDataSubscriber.h"
#include "rtabmap_ros/OdomInfo.h"
#include "MapsManager.h" #include "MapsManager.h"
@@ -144,6 +145,7 @@ private:
#endif #endif
void imuAsyncCallback(const sensor_msgs::ImuConstPtr & tagDetections); void imuAsyncCallback(const sensor_msgs::ImuConstPtr & tagDetections);
void interOdomCallback(const nav_msgs::OdometryConstPtr & msg); void interOdomCallback(const nav_msgs::OdometryConstPtr & msg);
void interOdomInfoCallback(const nav_msgs::OdometryConstPtr & msg1, const rtabmap_ros::OdomInfoConstPtr & msg2);
void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg); void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg);
@@ -320,8 +322,13 @@ private:
std::map<int, geometry_msgs::PoseWithCovarianceStamped> tags_; std::map<int, geometry_msgs::PoseWithCovarianceStamped> tags_;
ros::Subscriber imuSub_; ros::Subscriber imuSub_;
std::map<double, rtabmap::Transform> imus_; std::map<double, rtabmap::Transform> imus_;
ros::Subscriber interOdomSub_; ros::Subscriber interOdomSub_;
std::list<nav_msgs::Odometry> interOdoms_; std::list<std::pair<nav_msgs::Odometry, rtabmap_ros::OdomInfo> > interOdoms_;
message_filters::Subscriber<nav_msgs::Odometry> interOdomSyncSub_;
message_filters::Subscriber<rtabmap_ros::OdomInfo> interOdomInfoSyncSub_;
typedef message_filters::sync_policies::ExactTime<nav_msgs::Odometry, rtabmap_ros::OdomInfo> MyExactInterOdomSyncPolicy;
message_filters::Synchronizer<MyExactInterOdomSyncPolicy> * interOdomSync_;
bool stereoToDepth_; bool stereoToDepth_;
bool odomSensorSync_; bool odomSensorSync_;
include/rtabmap_ros/MsgConversion.h
+1
View File
@@ -152,6 +152,7 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData & msg); rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData & msg);
void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg); void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg);
std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo & info);
rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg); rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg);
void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg); void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg);
src/CoreWrapper.cpp
+60 -30
View File
@@ -112,6 +112,7 @@ CoreWrapper::CoreWrapper() :
transformThread_(0), transformThread_(0),
tfThreadRunning_(false), tfThreadRunning_(false),
stereoToDepth_(false), stereoToDepth_(false),
interOdomSync_(0),
odomSensorSync_(false), odomSensorSync_(false),
rate_(Parameters::defaultRtabmapDetectionRate()), rate_(Parameters::defaultRtabmapDetectionRate()),
createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()), createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()),
@@ -518,8 +519,22 @@ void CoreWrapper::onInit()
NODELET_INFO("Create intermediate nodes"); NODELET_INFO("Create intermediate nodes");
if(rate_ == 0.0f) if(rate_ == 0.0f)
{ {
NODELET_INFO("Subscribe to inter odom messges"); bool interOdomInfo = false;
interOdomSub_ = nh.subscribe("inter_odom", 1, &CoreWrapper::interOdomCallback, this); pnh.getParam("subscribe_inter_odom_info", interOdomInfo);
if(interOdomInfo)
{
NODELET_INFO("Subscribe to inter odom + info messages");
interOdomSync_ = new message_filters::Synchronizer<MyExactInterOdomSyncPolicy>(MyExactInterOdomSyncPolicy(queueSize_), interOdomSyncSub_, interOdomInfoSyncSub_);
interOdomSync_->registerCallback(boost::bind(&CoreWrapper::interOdomInfoCallback, this, _1, _2));
interOdomSyncSub_.subscribe(nh, "inter_odom", 1);
interOdomInfoSyncSub_.subscribe(nh, "inter_odom_info", 1);
}
else
{
NODELET_INFO("Subscribe to inter odom messages");
interOdomSub_ = nh.subscribe("inter_odom", 1, &CoreWrapper::interOdomCallback, this);
}
} }
} }
} }
@@ -742,6 +757,7 @@ CoreWrapper::~CoreWrapper()
rtabmap_.close(); rtabmap_.close();
printf("rtabmap: Saving database/long-term memory...done! (located at %s, %ld MB)\n", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024)); printf("rtabmap: Saving database/long-term memory...done! (located at %s, %ld MB)\n", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024));
delete interOdomSync_;
delete mbClient_; delete mbClient_;
} }
@@ -1638,24 +1654,24 @@ void CoreWrapper::process(
if(rtabmap_.isIDsGenerated() || data.id() > 0) if(rtabmap_.isIDsGenerated() || data.id() > 0)
{ {
// Add intermediate nodes? // Add intermediate nodes?
for(std::list<nav_msgs::Odometry>::iterator iter=interOdoms_.begin(); iter!=interOdoms_.end();) for(std::list<std::pair<nav_msgs::Odometry, rtabmap_ros::OdomInfo> >::iterator iter=interOdoms_.begin(); iter!=interOdoms_.end();)
{ {
if(iter->header.stamp < lastPoseStamp_) if(iter->first.header.stamp < lastPoseStamp_)
{ {
Transform interOdom = rtabmap_ros::transformFromPoseMsg(iter->pose.pose); Transform interOdom = rtabmap_ros::transformFromPoseMsg(iter->first.pose.pose);
if(!interOdom.isNull()) if(!interOdom.isNull())
{ {
cv::Mat covariance; cv::Mat covariance;
double variance = iter->twist.covariance[0]; double variance = iter->first.twist.covariance[0];
if(variance == BAD_COVARIANCE || variance <= 0.0f) if(variance == BAD_COVARIANCE || variance <= 0.0f)
{ {
//use the one of the pose //use the one of the pose
covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->pose.covariance.data()).clone(); covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->first.pose.covariance.data()).clone();
covariance /= 2.0; covariance /= 2.0;
} }
else else
{ {
covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->twist.covariance.data()).clone(); covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->first.twist.covariance.data()).clone();
} }
if(!uIsFinite(covariance.at<double>(0,0)) || covariance.at<double>(0,0)<=0.0f) if(!uIsFinite(covariance.at<double>(0,0)) || covariance.at<double>(0,0)<=0.0f)
{ {
@@ -1684,18 +1700,40 @@ void CoreWrapper::process(
Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0), Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
0, 0,
cv::Size(1,2)); cv::Size(1,2));
SensorData interData(rgb, depth, model, -1, rtabmap_ros::timestampFromROS(iter->header.stamp)); SensorData interData(rgb, depth, model, -1, rtabmap_ros::timestampFromROS(iter->first.header.stamp));
Transform gt; Transform gt;
if(!groundTruthFrameId_.empty()) if(!groundTruthFrameId_.empty())
{ {
gt = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, iter->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0); gt = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, iter->first.header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
} }
interData.setGroundTruth(gt); interData.setGroundTruth(gt);
rtabmap_.process(interData, interOdom, covariance);
std::map<std::string, float> externalStats;
std::vector<float> odomVelocity;
if(iter->second.timeEstimation != 0.0f)
{
OdometryInfo info = odomInfoFromROS(iter->second);
externalStats = rtabmap_ros::odomInfoToStatistics(info);
if(info.interval>0.0)
{
odomVelocity.resize(6);
float x,y,z,roll,pitch,yaw;
info.transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
odomVelocity[0] = x/info.interval;
odomVelocity[1] = y/info.interval;
odomVelocity[2] = z/info.interval;
odomVelocity[3] = roll/info.interval;
odomVelocity[4] = pitch/info.interval;
odomVelocity[5] = yaw/info.interval;
}
}
rtabmap_.process(interData, interOdom, covariance, odomVelocity, externalStats);
} }
interOdoms_.erase(iter++); interOdoms_.erase(iter++);
} }
else if(iter->header.stamp == lastPoseStamp_) else if(iter->first.header.stamp == lastPoseStamp_)
{ {
interOdoms_.erase(iter++); interOdoms_.erase(iter++);
break; break;
@@ -1822,23 +1860,7 @@ void CoreWrapper::process(
std::vector<float> odomVelocity; std::vector<float> odomVelocity;
if(odomInfo.timeEstimation != 0.0f) if(odomInfo.timeEstimation != 0.0f)
{ {
externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f)); externalStats = rtabmap_ros::odomInfoToStatistics(odomInfo);
externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
float speed = 0.0f;
if(odomInfo.interval>0.0)
speed = odomInfo.transform.x()/odomInfo.interval*3.6;
externalStats.insert(std::make_pair("Odometry/Speed/kph", speed));
externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
externalStats.insert(std::make_pair("Odometry/RAM_usage/MB", odomInfo.memoryUsage));
if(odomInfo.interval>0.0) if(odomInfo.interval>0.0)
{ {
@@ -2181,7 +2203,15 @@ void CoreWrapper::interOdomCallback(const nav_msgs::OdometryConstPtr & msg)
{ {
if(!paused_) if(!paused_)
{ {
interOdoms_.push_back(*msg); interOdoms_.push_back(std::make_pair(*msg, rtabmap_ros::OdomInfo()));
}
}
void CoreWrapper::interOdomInfoCallback(const nav_msgs::OdometryConstPtr & msg1, const rtabmap_ros::OdomInfoConstPtr & msg2)
{
if(!paused_)
{
interOdoms_.push_back(std::make_pair(*msg1, *msg2));
} }
} }
src/MsgConversion.cpp
+42
View File
@@ -1110,6 +1110,48 @@ void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
transformToPoseMsg(signature.getGroundTruthPose(), msg.groundTruthPose); transformToPoseMsg(signature.getGroundTruthPose(), msg.groundTruthPose);
} }
std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo & info)
{
std::map<std::string, float> stats;
stats.insert(std::make_pair("Odometry/TimeRegistration/ms", info.reg.totalTime*1000.0f));
stats.insert(std::make_pair("Odometry/RAM_usage/MB", info.memoryUsage));
// Based on rtabmap/MainWindow.cpp
stats.insert(std::make_pair("Odometry/Features/", info.features));
stats.insert(std::make_pair("Odometry/Matches/", info.reg.matches));
stats.insert(std::make_pair("Odometry/MatchesRatio/", info.features<=0?0.0f:float(info.reg.inliers)/float(info.features)));
stats.insert(std::make_pair("Odometry/Inliers/", info.reg.inliers));
stats.insert(std::make_pair("Odometry/InliersMeanDistance/m", info.reg.inliersMeanDistance));
stats.insert(std::make_pair("Odometry/InliersDistribution/", info.reg.inliersDistribution));
stats.insert(std::make_pair("Odometry/InliersRatio/", info.reg.inliers));
stats.insert(std::make_pair("Odometry/ICPInliersRatio/", info.reg.icpInliersRatio));
stats.insert(std::make_pair("Odometry/ICPRotation/rad", info.reg.icpRotation));
stats.insert(std::make_pair("Odometry/ICPTranslation/m", info.reg.icpTranslation));
stats.insert(std::make_pair("Odometry/ICPStructuralComplexity/", info.reg.icpStructuralComplexity));
stats.insert(std::make_pair("Odometry/StdDevLin/", sqrt((float)info.reg.covariance.at<double>(0,0))));
stats.insert(std::make_pair("Odometry/StdDevAng/", sqrt((float)info.reg.covariance.at<double>(5,5))));
stats.insert(std::make_pair("Odometry/VarianceLin/", (float)info.reg.covariance.at<double>(0,0)));
stats.insert(std::make_pair("Odometry/VarianceAng/", (float)info.reg.covariance.at<double>(5,5)));
stats.insert(std::make_pair("Odometry/TimeEstimation/ms", info.timeEstimation*1000.0f));
stats.insert(std::make_pair("Odometry/TimeFiltering/ms", info.timeParticleFiltering*1000.0f));
stats.insert(std::make_pair("Odometry/LocalMapSize/", info.localMapSize));
stats.insert(std::make_pair("Odometry/LocalScanMapSize/", info.localScanMapSize));
stats.insert(std::make_pair("Odometry/LocalKeyFrames/", info.localKeyFrames));
stats.insert(std::make_pair("Odometry/LocalBundleOutliers/", info.localBundleOutliers));
stats.insert(std::make_pair("Odometry/LocalBundleConstraints/", info.localBundleConstraints));
stats.insert(std::make_pair("Odometry/LocalBundleTime/ms", info.localBundleTime*1000.0f));
stats.insert(std::make_pair("Odometry/KeyFrameAdded/", info.keyFrameAdded?1.0f:0.0f));
stats.insert(std::make_pair("Odometry/Interval/ms", (float)info.interval));
float speed = 0.0f;
if(info.interval>0.0)
speed = info.transform.x()/info.interval*3.6;
stats.insert(std::make_pair("Odometry/Speed/kph", speed));
stats.insert(std::make_pair("Odometry/Distance/m", info.distanceTravelled));
return stats;
}
rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg) rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
{ {
rtabmap::OdometryInfo info; rtabmap::OdometryInfo info;
src/nodelets/obstacles_detection.cpp
+17 -1
View File
@@ -52,7 +52,8 @@ public:
ObstaclesDetection() : ObstaclesDetection() :
frameId_("base_link"), frameId_("base_link"),
waitForTransform_(false), waitForTransform_(false),
mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection()) mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection()),
warned_(false)
{} {}
virtual ~ObstaclesDetection() virtual ~ObstaclesDetection()
@@ -109,6 +110,9 @@ private:
ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle();
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
int queueSize = 10; int queueSize = 10;
pnh.param("queue_size", queueSize, queueSize); pnh.param("queue_size", queueSize, queueSize);
pnh.param("frame_id", frameId_, frameId_); pnh.param("frame_id", frameId_, frameId_);
@@ -295,6 +299,17 @@ private:
std::vector<int> indices; std::vector<int> indices;
pcl::removeNaNFromPointCloud(*inputCloud, *inputCloud, indices); pcl::removeNaNFromPointCloud(*inputCloud, *inputCloud, indices);
} }
else if(!inputCloud->is_dense && inputCloud->height == 1)
{
if(!warned_)
{
NODELET_WARN("Detected possible wrong format of point cloud \"%s\", it is "
"indicated that it is not dense, but there is only one row. "
"Assuming it is dense... This message will only appear once.", cloudSub_.getTopic().c_str());
warned_ = true;
}
inputCloud->is_dense = true;
}
//Common variables for all strategies //Common variables for all strategies
pcl::IndicesPtr ground, obstacles; pcl::IndicesPtr ground, obstacles;
@@ -425,6 +440,7 @@ private:
rtabmap::OccupancyGrid grid_; rtabmap::OccupancyGrid grid_;
bool mapFrameProjection_; bool mapFrameProjection_;
bool warned_;
tf::TransformListener tfListener_; tf::TransformListener tfListener_;
src/nodelets/point_cloud_assembler.cpp
+3 -3
View File
@@ -107,7 +107,7 @@ private:
pnh.param("range_min", rangeMin_, rangeMin_); pnh.param("range_min", rangeMin_, rangeMin_);
pnh.param("range_max", rangeMax_, rangeMax_); pnh.param("range_max", rangeMax_, rangeMax_);
pnh.param("voxel_size", voxelSize_, voxelSize_); pnh.param("voxel_size", voxelSize_, voxelSize_);
ROS_ASSERT(maxClouds_>=0 && assemblingTime_ >=0); ROS_ASSERT(maxClouds_>0 || assemblingTime_ >0.0);
cloudsSkipped_ = skipClouds_; cloudsSkipped_ = skipClouds_;
@@ -168,9 +168,9 @@ private:
*cpy = *cloudMsg; *cpy = *cloudMsg;
clouds_.push_back(cpy); clouds_.push_back(cpy);
if( (int)clouds_.size() >= maxClouds_ && maxClouds_ != 0 if( (int)clouds_.size() >= maxClouds_ && maxClouds_ > 0
|| ||
(double)(*cpy).header.stamp.toSec() >= (double)clouds_[0]->header.stamp.toSec() + assemblingTime_ && assemblingTime_ != 0.0 ) (double)(*cpy).header.stamp.toSec() >= (double)clouds_[0]->header.stamp.toSec() + assemblingTime_ && assemblingTime_ > 0.0 )
{ {
pcl::PCLPointCloud2Ptr assembled(new pcl::PCLPointCloud2); pcl::PCLPointCloud2Ptr assembled(new pcl::PCLPointCloud2);
pcl_conversions::toPCL(*clouds_.back(), *assembled); pcl_conversions::toPCL(*clouds_.back(), *assembled);
src/nodelets/pointcloud_to_depthimage.cpp
+13 -4
View File
@@ -200,13 +200,22 @@ private:
pcl_conversions::toPCL(*pointCloud2Msg, *cloud); pcl_conversions::toPCL(*pointCloud2Msg, *cloud);
cv_bridge::CvImage depthImage; cv_bridge::CvImage depthImage;
depthImage.image = rtabmap::util3d::projectCloudToCamera(model.imageSize(), model.K(), cloud, model.localTransform());
if(fillHolesSize_ > 0 && fillIterations_ > 0) if(cloud->data.empty())
{ {
for(int i=0; i<fillIterations_;++i) ROS_WARN("Received an empty cloud on topic \"%s\"! A depth image with all zeros is returned.", pointCloudSub_.getTopic().c_str());
depthImage.image = cv::Mat::zeros(model.imageSize(), CV_32FC1);
}
else
{
depthImage.image = rtabmap::util3d::projectCloudToCamera(model.imageSize(), model.K(), cloud, model.localTransform());
if(fillHolesSize_ > 0 && fillIterations_ > 0)
{ {
depthImage.image = rtabmap::util2d::fillDepthHoles(depthImage.image, fillHolesSize_, fillHolesError_); for(int i=0; i<fillIterations_;++i)
{
depthImage.image = rtabmap::util2d::fillDepthHoles(depthImage.image, fillHolesSize_, fillHolesError_);
}
} }
} }