00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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("_");
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
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
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
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
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
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
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
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
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
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