obstacles_detection.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include <ros/ros.h>
00029 #include <pluginlib/class_list_macros.h>
00030 #include <nodelet/nodelet.h>
00031 
00032 #include <pcl/point_cloud.h>
00033 #include <pcl/point_types.h>
00034 #include <pcl_conversions/pcl_conversions.h>
00035 #include <pcl/filters/filter.h>
00036 
00037 #include <tf/transform_listener.h>
00038 
00039 #include <sensor_msgs/PointCloud2.h>
00040 
00041 #include <rtabmap_ros/MsgConversion.h>
00042 
00043 #include "rtabmap/core/OccupancyGrid.h"
00044 #include "rtabmap/utilite/UStl.h"
00045 
00046 namespace rtabmap_ros
00047 {
00048 
00049 class ObstaclesDetection : public nodelet::Nodelet
00050 {
00051 public:
00052         ObstaclesDetection() :
00053                 frameId_("base_link"),
00054                 waitForTransform_(false),
00055                 mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection())
00056         {}
00057 
00058         virtual ~ObstaclesDetection()
00059         {}
00060 
00061 private:
00062 
00063         void parameterMoved(
00064                         ros::NodeHandle & nh,
00065                         const std::string & rosName,
00066                         const std::string & parameterName,
00067                         rtabmap::ParametersMap & parameters)
00068         {
00069                 if(nh.hasParam(rosName))
00070                 {
00071                         rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid");
00072                         rtabmap::ParametersMap::const_iterator iter =gridParameters.find(parameterName);
00073                         if(iter != gridParameters.end())
00074                         {
00075                                 NODELET_ERROR("obstacles_detection: Parameter \"%s\" has moved from "
00076                                                  "rtabmap_ros to rtabmap library. Use "
00077                                                  "parameter \"%s\" instead. The value is still "
00078                                                  "copied to new parameter name.",
00079                                                  rosName.c_str(),
00080                                                  parameterName.c_str());
00081                                 std::string type = rtabmap::Parameters::getType(parameterName);
00082                                 if(type.compare("float") || type.compare("double"))
00083                                 {
00084                                         double v = uStr2Double(iter->second);
00085                                         nh.getParam(rosName, v);
00086                                         parameters.insert(rtabmap::ParametersPair(parameterName, uNumber2Str(v)));
00087                                 }
00088                                 else if(type.compare("int") || type.compare("unsigned int"))
00089                                 {
00090                                         int v = uStr2Int(iter->second);
00091                                         nh.getParam(rosName, v);
00092                                         parameters.insert(rtabmap::ParametersPair(parameterName, uNumber2Str(v)));
00093                                 }
00094                                 else
00095                                 {
00096                                         NODELET_ERROR("Not handled type \"%s\" for parameter \"%s\"", type.c_str(), parameterName.c_str());
00097                                 }
00098                         }
00099                         else
00100                         {
00101                                 NODELET_ERROR("Parameter \"%s\" not found in default parameters.", parameterName.c_str());
00102                         }
00103                 }
00104         }
00105 
00106         virtual void onInit()
00107         {
00108                 ROS_DEBUG("_"); // not sure why, but all NODELET_*** log are not shown if a normal ROS_*** is not called!?
00109                 ros::NodeHandle & nh = getNodeHandle();
00110                 ros::NodeHandle & pnh = getPrivateNodeHandle();
00111 
00112                 int queueSize = 10;
00113                 pnh.param("queue_size", queueSize, queueSize);
00114                 pnh.param("frame_id", frameId_, frameId_);
00115                 pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
00116                 pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
00117 
00118                 if(pnh.hasParam("optimize_for_close_objects"))
00119                 {
00120                         NODELET_ERROR("\"optimize_for_close_objects\" parameter doesn't exist "
00121                                         "anymore. Use rtabmap_ros/obstacles_detection_old nodelet to use "
00122                                         "the old interface.");
00123                 }
00124 
00125                 rtabmap::ParametersMap parameters;
00126 
00127                 // Backward compatibility
00128                 for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=rtabmap::Parameters::getRemovedParameters().begin();
00129                         iter!=rtabmap::Parameters::getRemovedParameters().end();
00130                         ++iter)
00131                 {
00132                         std::string vStr;
00133                         bool vBool;
00134                         int vInt;
00135                         double vDouble;
00136                         std::string paramValue;
00137                         if(pnh.getParam(iter->first, vStr))
00138                         {
00139                                 paramValue = vStr;
00140                         }
00141                         else if(pnh.getParam(iter->first, vBool))
00142                         {
00143                                 paramValue = uBool2Str(vBool);
00144                         }
00145                         else if(pnh.getParam(iter->first, vDouble))
00146                         {
00147                                 paramValue = uNumber2Str(vDouble);
00148                         }
00149                         else if(pnh.getParam(iter->first, vInt))
00150                         {
00151                                 paramValue = uNumber2Str(vInt);
00152                         }
00153                         if(!paramValue.empty())
00154                         {
00155                                 if(iter->second.first)
00156                                 {
00157                                         // can be migrated
00158                                         uInsert(parameters, rtabmap::ParametersPair(iter->second.second, paramValue));
00159                                         NODELET_ERROR("obstacles_detection: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
00160                                                         iter->first.c_str(), iter->second.second.c_str(), paramValue.c_str());
00161                                 }
00162                                 else
00163                                 {
00164                                         if(iter->second.second.empty())
00165                                         {
00166                                                 NODELET_ERROR("obstacles_detection: Parameter \"%s\" doesn't exist anymore!",
00167                                                                 iter->first.c_str());
00168                                         }
00169                                         else
00170                                         {
00171                                                 NODELET_ERROR("obstacles_detection: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
00172                                                                 iter->first.c_str(), iter->second.second.c_str());
00173                                         }
00174                                 }
00175                         }
00176                 }
00177 
00178                 rtabmap::ParametersMap gridParameters2 = rtabmap::Parameters::getDefaultParameters();
00179                 rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid");
00180                 for(rtabmap::ParametersMap::iterator iter=gridParameters.begin(); iter!=gridParameters.end(); ++iter)
00181                 {
00182                         std::string vStr;
00183                         bool vBool;
00184                         int vInt;
00185                         double vDouble;
00186                         if(pnh.getParam(iter->first, vStr))
00187                         {
00188                                 NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
00189                                 iter->second = vStr;
00190                         }
00191                         else if(pnh.getParam(iter->first, vBool))
00192                         {
00193                                 NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
00194                                 iter->second = uBool2Str(vBool);
00195                         }
00196                         else if(pnh.getParam(iter->first, vDouble))
00197                         {
00198                                 NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
00199                                 iter->second = uNumber2Str(vDouble);
00200                         }
00201                         else if(pnh.getParam(iter->first, vInt))
00202                         {
00203                                 NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
00204                                 iter->second = uNumber2Str(vInt);
00205                         }
00206                 }
00207                 uInsert(parameters, gridParameters);
00208                 parameterMoved(pnh, "proj_voxel_size", rtabmap::Parameters::kGridCellSize(), parameters);
00209                 parameterMoved(pnh, "ground_normal_angle", rtabmap::Parameters::kGridMaxGroundAngle(), parameters);
00210                 parameterMoved(pnh, "min_cluster_size", rtabmap::Parameters::kGridMinClusterSize(), parameters);
00211                 parameterMoved(pnh, "normal_estimation_radius", rtabmap::Parameters::kGridClusterRadius(), parameters);
00212                 parameterMoved(pnh, "cluster_radius", rtabmap::Parameters::kGridClusterRadius(), parameters);
00213                 parameterMoved(pnh, "max_obstacles_height", rtabmap::Parameters::kGridMaxObstacleHeight(), parameters);
00214                 parameterMoved(pnh, "max_ground_height", rtabmap::Parameters::kGridMaxGroundHeight(), parameters);
00215                 parameterMoved(pnh, "detect_flat_obstacles", rtabmap::Parameters::kGridFlatObstacleDetected(), parameters);
00216                 parameterMoved(pnh, "normal_k", rtabmap::Parameters::kGridNormalK(), parameters);
00217 
00218                 UASSERT(uContains(parameters, rtabmap::Parameters::kGridMapFrameProjection()));
00219                 mapFrameProjection_ = uStr2Bool(parameters.at(rtabmap::Parameters::kGridMapFrameProjection()));
00220                 if(mapFrameProjection_ && mapFrameId_.empty())
00221                 {
00222                         NODELET_ERROR("obstacles_detection: Parameter \"%s\" is true but map_frame_id is not set!", rtabmap::Parameters::kGridMapFrameProjection().c_str());
00223                 }
00224 
00225                 grid_.parseParameters(parameters);
00226 
00227                 cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this);
00228 
00229                 groundPub_ = nh.advertise<sensor_msgs::PointCloud2>("ground", 1);
00230                 obstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("obstacles", 1);
00231                 projObstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("proj_obstacles", 1);
00232         }
00233 
00234 
00235 
00236         void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
00237         {
00238                 ros::WallTime time = ros::WallTime::now();
00239 
00240                 if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0 && projObstaclesPub_.getNumSubscribers() == 0)
00241                 {
00242                         // no one wants the results
00243                         return;
00244                 }
00245 
00246                 rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
00247                 try
00248                 {
00249                         if(waitForTransform_)
00250                         {
00251                                 if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
00252                                 {
00253                                         NODELET_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
00254                                         return;
00255                                 }
00256                         }
00257                         tf::StampedTransform tmp;
00258                         tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp);
00259                         localTransform = rtabmap_ros::transformFromTF(tmp);
00260                 }
00261                 catch(tf::TransformException & ex)
00262                 {
00263                         NODELET_ERROR("%s",ex.what());
00264                         return;
00265                 }
00266 
00267                 rtabmap::Transform pose = rtabmap::Transform::getIdentity();
00268                 if(!mapFrameId_.empty())
00269                 {
00270                         try
00271                         {
00272                                 if(waitForTransform_)
00273                                 {
00274                                         if(!tfListener_.waitForTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, ros::Duration(1)))
00275                                         {
00276                                                 NODELET_ERROR("Could not get transform from %s to %s after 1 second!", mapFrameId_.c_str(), frameId_.c_str());
00277                                                 return;
00278                                         }
00279                                 }
00280                                 tf::StampedTransform tmp;
00281                                 tfListener_.lookupTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, tmp);
00282                                 pose = rtabmap_ros::transformFromTF(tmp);
00283                         }
00284                         catch(tf::TransformException & ex)
00285                         {
00286                                 NODELET_ERROR("%s",ex.what());
00287                                 return;
00288                         }
00289                 }
00290 
00291                 pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud(new pcl::PointCloud<pcl::PointXYZ>);
00292                 pcl::fromROSMsg(*cloudMsg, *inputCloud);
00293                 if(inputCloud->isOrganized())
00294                 {
00295                         std::vector<int> indices;
00296                         pcl::removeNaNFromPointCloud(*inputCloud, *inputCloud, indices);
00297                 }
00298 
00299                 //Common variables for all strategies
00300                 pcl::IndicesPtr ground, obstacles;
00301                 pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
00302                 pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
00303                 pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloudWithoutFlatSurfaces(new pcl::PointCloud<pcl::PointXYZ>);
00304 
00305                 if(inputCloud->size())
00306                 {
00307                         inputCloud = rtabmap::util3d::transformPointCloud(inputCloud, localTransform);
00308 
00309                         pcl::IndicesPtr flatObstacles(new std::vector<int>);
00310                         pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = grid_.segmentCloud<pcl::PointXYZ>(
00311                                         inputCloud,
00312                                         pcl::IndicesPtr(new std::vector<int>),
00313                                         pose,
00314                                         cv::Point3f(localTransform.x(), localTransform.y(), localTransform.z()),
00315                                         ground,
00316                                         obstacles,
00317                                         &flatObstacles);
00318 
00319                         if(cloud->size() && (ground->size() || obstacles->size()))
00320                         {
00321                                 if(groundPub_.getNumSubscribers() &&
00322                                                 ground.get() && ground->size())
00323                                 {
00324                                         pcl::copyPointCloud(*cloud, *ground, *groundCloud);
00325                                 }
00326 
00327                                 if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) &&
00328                                                 obstacles.get() && obstacles->size())
00329                                 {
00330                                         // remove flat obstacles from obstacles
00331                                         std::set<int> flatObstaclesSet;
00332                                         if(projObstaclesPub_.getNumSubscribers())
00333                                         {
00334                                                 flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end());
00335                                         }
00336 
00337                                         obstaclesCloud->resize(obstacles->size());
00338                                         obstaclesCloudWithoutFlatSurfaces->resize(obstacles->size());
00339 
00340                                         int oi=0;
00341                                         for(unsigned int i=0; i<obstacles->size(); ++i)
00342                                         {
00343                                                 obstaclesCloud->points[i] = cloud->at(obstacles->at(i));
00344                                                 if(flatObstaclesSet.size() == 0 ||
00345                                                    flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end())
00346                                                 {
00347                                                         obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i];
00348                                                         obstaclesCloudWithoutFlatSurfaces->points[oi].z = 0;
00349                                                         ++oi;
00350                                                 }
00351 
00352                                         }
00353 
00354                                         obstaclesCloudWithoutFlatSurfaces->resize(oi);
00355                                 }
00356 
00357                                 if(!localTransform.isIdentity() || !pose.isIdentity())
00358                                 {
00359                                         //transform back in topic frame for 3d clouds and base frame for 2d clouds
00360 
00361                                         float roll, pitch, yaw;
00362                                         pose.getEulerAngles(roll, pitch, yaw);
00363                                         rtabmap::Transform t = rtabmap::Transform(0,0, mapFrameProjection_?pose.z():0, roll, pitch, 0);
00364 
00365                                         if(obstaclesCloudWithoutFlatSurfaces->size() && !pose.isIdentity())
00366                                         {
00367                                                 obstaclesCloudWithoutFlatSurfaces = rtabmap::util3d::transformPointCloud(obstaclesCloudWithoutFlatSurfaces, t.inverse());
00368                                         }
00369 
00370                                         t = (t*localTransform).inverse();
00371                                         if(groundCloud->size())
00372                                         {
00373                                                 groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, t);
00374                                         }
00375                                         if(obstaclesCloud->size())
00376                                         {
00377                                                 obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, t);
00378                                         }
00379                                 }
00380                         }
00381                 }
00382                 else
00383                 {
00384                         ROS_WARN("obstacles_detection: Input cloud is empty! (%d x %d, is_dense=%d)", cloudMsg->width, cloudMsg->height, cloudMsg->is_dense?1:0);
00385                 }
00386 
00387                 if(groundPub_.getNumSubscribers())
00388                 {
00389                         sensor_msgs::PointCloud2 rosCloud;
00390                         pcl::toROSMsg(*groundCloud, rosCloud);
00391                         rosCloud.header = cloudMsg->header;
00392 
00393                         //publish the message
00394                         groundPub_.publish(rosCloud);
00395                 }
00396 
00397                 if(obstaclesPub_.getNumSubscribers())
00398                 {
00399                         sensor_msgs::PointCloud2 rosCloud;
00400                         pcl::toROSMsg(*obstaclesCloud, rosCloud);
00401                         rosCloud.header = cloudMsg->header;
00402 
00403                         //publish the message
00404                         obstaclesPub_.publish(rosCloud);
00405                 }
00406 
00407                 if(projObstaclesPub_.getNumSubscribers())
00408                 {
00409                         sensor_msgs::PointCloud2 rosCloud;
00410                         pcl::toROSMsg(*obstaclesCloudWithoutFlatSurfaces, rosCloud);
00411                         rosCloud.header.stamp = cloudMsg->header.stamp;
00412                         rosCloud.header.frame_id = frameId_;
00413 
00414                         //publish the message
00415                         projObstaclesPub_.publish(rosCloud);
00416                 }
00417 
00418                 NODELET_DEBUG("Obstacles segmentation time = %f s", (ros::WallTime::now() - time).toSec());
00419         }
00420 
00421 private:
00422         std::string frameId_;
00423         std::string mapFrameId_;
00424         bool waitForTransform_;
00425 
00426         rtabmap::OccupancyGrid grid_;
00427         bool mapFrameProjection_;
00428 
00429         tf::TransformListener tfListener_;
00430 
00431         ros::Publisher groundPub_;
00432         ros::Publisher obstaclesPub_;
00433         ros::Publisher projObstaclesPub_;
00434 
00435         ros::Subscriber cloudSub_;
00436 };
00437 
00438 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ObstaclesDetection, nodelet::Nodelet);
00439 }
00440 
00441 


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:30:49