diff --git a/CMakeLists.txt b/CMakeLists.txt index 8c4d23cf..cb39e4a6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -135,6 +135,7 @@ SET(rtabmap_ros_lib_src src/nodelets/point_cloud_xyz.cpp src/nodelets/disparity_to_depth.cpp src/nodelets/obstacles_detection.cpp + src/nodelets/point_cloud_aggregator.cpp src/MsgConversion.cpp src/OdometryROS.cpp src/rviz/MapCloudDisplay.cpp diff --git a/README.md b/README.md index 80ad6168..ced87365 100644 --- a/README.md +++ b/README.md @@ -11,6 +11,10 @@ For the RTAB-Map libraries and standalone application, visit the [RTAB-Map's hom ### ROS distribution RTAB-Map is released as binaries in the ROS distribution. + * Jade + ``` +$ sudo apt-get install ros-jade-rtabmap-ros +``` * Indigo ``` $ sudo apt-get install ros-indigo-rtabmap-ros @@ -21,13 +25,13 @@ $ sudo apt-get install ros-hydro-rtabmap-ros ``` ### Build from source -This section shows how to install RTAB-Map ros-pkg on **ROS Hydro/Indigo** (Catkin build). RTAB-Map works only with the PCL 1.7, which is the default version installed with ROS Hydro/Indigo (**Fuerte and Groovy are not supported**). - * **Note for ROS Indigo**: If you want SURF/SIFT, you have to build OpenCV from source to have access to *nonfree* module. Install it in `/usr/local` (default) and the rtabmap library should link with it instead of the one installed in ROS. +This section shows how to install RTAB-Map ros-pkg on **ROS Hydro/Indigo/Jade** (Catkin build). RTAB-Map works only with the PCL 1.7, which is the default version installed with ROS Hydro/Indigo/Jade (**Fuerte and Groovy are not supported**). + * **Note for ROS Indigo/Jade**: If you want SURF/SIFT, you have to build OpenCV from source to have access to *nonfree* module. Install it in `/usr/local` (default) and the rtabmap library should link with it instead of the one installed in ROS. * The next instructions assume that you have setup your ROS workspace using this [tutorial](http://wiki.ros.org/catkin/Tutorials/create_a_workspace). The workspace path is `~/catkin_ws` and your `~/.bashrc` contains: ```bash -source /opt/ros/hydro/setup.bash +source /opt/ros/[hydro|indigo|jade]/setup.bash source ~/catkin_ws/devel/setup.bash ``` diff --git a/launch/tests/test_obstacles_detection.launch b/launch/tests/test_obstacles_detection.launch new file mode 100644 index 00000000..acaf83bc --- /dev/null +++ b/launch/tests/test_obstacles_detection.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml index 5d25ad92..d2aadf3a 100644 --- a/nodelet_plugins.xml +++ b/nodelet_plugins.xml @@ -54,4 +54,13 @@ This is my nodelet. + + + + This is my nodelet. + + + diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 1f47dae4..95d7196f 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -70,8 +70,9 @@ public: normalEstimationRadius_(0.05), groundNormalAngle_(M_PI_4), minClusterSize_(20), - maxObstaclesHeight_(0), - waitForTransform_(false) + maxObstaclesHeight_(0.0), // if<=0.0 -> disabled + waitForTransform_(false), + optimizeForCloseObjects_(false) {} virtual ~ObstaclesDetection() @@ -91,6 +92,7 @@ private: pnh.param("min_cluster_size", minClusterSize_, minClusterSize_); pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); + pnh.param("optimize_for_close_objects", optimizeForCloseObjects_, optimizeForCloseObjects_); cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this); @@ -102,80 +104,158 @@ private: void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) { - if(groundPub_.getNumSubscribers() || obstaclesPub_.getNumSubscribers()) + ros::Time time = ros::Time::now(); + + if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0) { - rtabmap::Transform localTransform; - try + // no one wants the results + return; + } + + rtabmap::Transform localTransform; + try + { + if(waitForTransform_) { - if(waitForTransform_) + if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1))) { - if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1))) + ROS_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str()); + return; + } + } + tf::StampedTransform tmp; + tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp); + localTransform = rtabmap_ros::transformFromTF(tmp); + } + catch(tf::TransformException & ex) + { + ROS_ERROR("%s",ex.what()); + return; + } + + pcl::PointCloud::Ptr originalCloud(new pcl::PointCloud); + pcl::fromROSMsg(*cloudMsg, *originalCloud); + + //Common variables for all strategies + pcl::IndicesPtr ground, obstacles; + pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); + pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); + + if(originalCloud->size()) + { + originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); + if(maxObstaclesHeight_ > 0) + { + originalCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxObstaclesHeight_); + } + + if(originalCloud->size()) + { + if(!optimizeForCloseObjects_) + { + // This is the default strategy + rtabmap::util3d::segmentObstaclesFromGround( + originalCloud, + ground, + obstacles, + normalEstimationRadius_, + groundNormalAngle_, + minClusterSize_); + + if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) { - ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str()); - return; + pcl::copyPointCloud(*originalCloud, *ground, *groundCloud); + } + + if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size()) + { + pcl::copyPointCloud(*originalCloud, *obstacles, *obstaclesCloud); } } - tf::StampedTransform tmp; - tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp); - localTransform = rtabmap_ros::transformFromTF(tmp); - } - catch(tf::TransformException & ex) - { - ROS_WARN("%s",ex.what()); - return; - } - - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - pcl::fromROSMsg(*cloudMsg, *cloud); - pcl::IndicesPtr ground, obstacles; - if(cloud->size()) - { - cloud = rtabmap::util3d::transformPointCloud(cloud, localTransform); - - if(maxObstaclesHeight_ > 0) + else { - cloud = rtabmap::util3d::passThrough(cloud, "z", std::numeric_limits::min(), maxObstaclesHeight_); + // in this case optimizeForCloseObject_ is true: + // we divide the floor point cloud into two subsections, one for all potential floor points up to 1m + // one for potential floor points further away than 1m. + // For the points at closer range, we use a smaller normal estimation radius and ground normal angle, + // which allows to detect smaller objects, without increasing the number of false positive. + // For all other points, we use a bigger normal estimation radius (* 3.) and tolerance for the + // grond normal angle (* 2.). + + pcl::PointCloud::Ptr originalCloud_near = rtabmap::util3d::passThrough(originalCloud, "x", std::numeric_limits::min(), 1.); + pcl::PointCloud::Ptr originalCloud_far = rtabmap::util3d::passThrough(originalCloud, "x", 1., std::numeric_limits::max()); + + // Part 1: segment floor and obstacles near the robot + rtabmap::util3d::segmentObstaclesFromGround( + originalCloud_near, + ground, + obstacles, + normalEstimationRadius_, + groundNormalAngle_, + minClusterSize_); + + if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) + { + pcl::copyPointCloud(*originalCloud_near, *ground, *groundCloud); + ground->clear(); + } + + if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size()) + { + pcl::copyPointCloud(*originalCloud_near, *obstacles, *obstaclesCloud); + obstacles->clear(); + } + + // Part 2: segment floor and obstacles far from the robot + rtabmap::util3d::segmentObstaclesFromGround( + originalCloud_far, + ground, + obstacles, + 3.*normalEstimationRadius_, + 2.*groundNormalAngle_, + minClusterSize_); + + if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) + { + pcl::PointCloud::Ptr groundCloud2 (new pcl::PointCloud); + pcl::copyPointCloud(*originalCloud_far, *ground, *groundCloud2); + *groundCloud += *groundCloud2; + } + + + if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size()) + { + pcl::PointCloud::Ptr obstacles2(new pcl::PointCloud); + pcl::copyPointCloud(*originalCloud_far, *obstacles, *obstacles2); + *obstaclesCloud += *obstacles2; + } } - if(cloud->size()) - { - rtabmap::util3d::segmentObstaclesFromGround(cloud, - ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_); - } - } - - pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); - if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) - { - pcl::copyPointCloud(*cloud, *ground, *groundCloud); - } - pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); - if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size()) - { - pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud); - } - - if(groundPub_.getNumSubscribers()) - { - sensor_msgs::PointCloud2 rosCloud; - pcl::toROSMsg(*groundCloud, rosCloud); - rosCloud.header.stamp = cloudMsg->header.stamp; - rosCloud.header.frame_id = frameId_; - - //publish the message - groundPub_.publish(rosCloud); - } - - if(obstaclesPub_.getNumSubscribers()) - { - sensor_msgs::PointCloud2 rosCloud; - pcl::toROSMsg(*obstaclesCloud, rosCloud); - rosCloud.header.stamp = cloudMsg->header.stamp; - rosCloud.header.frame_id = frameId_; - - //publish the message - obstaclesPub_.publish(rosCloud); } } + + if(groundPub_.getNumSubscribers()) + { + sensor_msgs::PointCloud2 rosCloud; + pcl::toROSMsg(*groundCloud, rosCloud); + rosCloud.header.stamp = cloudMsg->header.stamp; + rosCloud.header.frame_id = frameId_; + + //publish the message + groundPub_.publish(rosCloud); + } + + if(obstaclesPub_.getNumSubscribers()) + { + sensor_msgs::PointCloud2 rosCloud; + pcl::toROSMsg(*obstaclesCloud, rosCloud); + rosCloud.header.stamp = cloudMsg->header.stamp; + rosCloud.header.frame_id = frameId_; + + //publish the message + obstaclesPub_.publish(rosCloud); + } + + ROS_INFO("Obstacles segmentation time = %f s", (ros::Time::now() - time).toSec()); } private: @@ -185,6 +265,7 @@ private: int minClusterSize_; double maxObstaclesHeight_; bool waitForTransform_; + bool optimizeForCloseObjects_; tf::TransformListener tfListener_; diff --git a/src/nodelets/point_cloud_aggregator.cpp b/src/nodelets/point_cloud_aggregator.cpp new file mode 100644 index 00000000..a1fd1b1c --- /dev/null +++ b/src/nodelets/point_cloud_aggregator.cpp @@ -0,0 +1,88 @@ + +#include +#include +#include + +#include +#include +#include + +#include + +#include + +#include +#include + +#include +#include +#include + +#include + +namespace rtabmap_ros +{ + +class PointCloudAggregator : public nodelet::Nodelet +{ +public: + PointCloudAggregator() : sync(NULL) + {} + + virtual ~PointCloudAggregator() + { + if (sync!=NULL) delete sync; + } + +private: + void clouds_callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg_1, + const sensor_msgs::PointCloud2ConstPtr & cloudMsg_2, + const sensor_msgs::PointCloud2ConstPtr & cloudMsg_3) + { + if(cloudPub_.getNumSubscribers()) + { + pcl::fromROSMsg(*cloudMsg_1, cloud1); + pcl::fromROSMsg(*cloudMsg_2, cloud2); + pcl::fromROSMsg(*cloudMsg_3, cloud3); + pcl::PointCloud totalCloud; + totalCloud = cloud1 + cloud2; + totalCloud += cloud3; + sensor_msgs::PointCloud2 rosCloud; + pcl::toROSMsg(totalCloud, rosCloud); + rosCloud.header.stamp = cloudMsg_1->header.stamp; + rosCloud.header.frame_id = cloudMsg_1->header.frame_id; + cloudPub_.publish(rosCloud); + } + } + + typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; + virtual void onInit() + { + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + int queueSize = 5; + pnh.param("queue_size", queueSize, queueSize); + + cloudSub_1_.subscribe(nh, "cloud1", 1); + cloudSub_2_.subscribe(nh, "cloud2", 1); + cloudSub_3_.subscribe(nh, "cloud3", 1); + + sync = new message_filters::Synchronizer(MySyncPolicy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_); + sync->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds_callback, this, _1, _2, _3)); + + cloudPub_ = nh.advertise("combined_cloud", 1); + } + + message_filters::Synchronizer* sync; + message_filters::Subscriber cloudSub_1_; + message_filters::Subscriber cloudSub_2_; + message_filters::Subscriber cloudSub_3_; + pcl::PointCloud cloud1, cloud2, cloud3; + + ros::Publisher cloudPub_; +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudAggregator, nodelet::Nodelet); +} + diff --git a/src/nodelets/point_cloud_xyz.cpp b/src/nodelets/point_cloud_xyz.cpp index e944b9cf..6bed0638 100644 --- a/src/nodelets/point_cloud_xyz.cpp +++ b/src/nodelets/point_cloud_xyz.cpp @@ -66,6 +66,9 @@ public: decimation_(1), noiseFilterRadius_(0.0), noiseFilterMinNeighbors_(5), + cut_left_(0), + cut_right_(0), + create_close_obstacle_if_depth_is_missing_(false), approxSyncDepth_(0), approxSyncDisparity_(0), exactSyncDepth_(0), @@ -99,6 +102,10 @@ private: pnh.param("decimation", decimation_, decimation_); pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_); pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_); + pnh.param("cut_left", cut_left_, cut_left_); + pnh.param("cut_right", cut_right_, cut_right_); + pnh.param("special_filter_close_object", create_close_obstacle_if_depth_is_missing_, create_close_obstacle_if_depth_is_missing_); + ROS_INFO("Approximate time sync = %s", approxSync?"true":"false"); if(approxSync) @@ -147,6 +154,47 @@ private: if(cloudPub_.getNumSubscribers()) { cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth); + cv::Mat image=imageDepthPtr->image; + int rows = image.rows; + int cols = image.cols; + + //Cut left and cut right options to mask the image. + //If cut_left (resp. cut_right) is set to a positive value, we set the first (resp. last) columns + //of the depth image to 0, meaning that no depth reading has been received. + //Number of columns to be masked is equal to cut_left (resp. cut_right value) + if (cut_left_>0){ + cv::Mat pRoi = image(cv::Rect(0, 0, cut_left_, rows)); + pRoi.setTo(cv::Scalar(0.)); + } + if (cut_right_<0){ + cv::Mat pRoi = image(cv::Rect(cols-cut_right_, 0, cut_right_, rows)); + pRoi.setTo(cv::Scalar(0.)); + } + + //This option enables a filter for close object. + //Fist, we do a median blur on the image to get rid of potential noise + //Second, we set all false reading that are likely due to an object sitting in front of the camera + // to a short distance estimation (here, 40cm). + //This hence make the assumption that the depth camera is looking forward and sees the floor on + // the bottom rows of the depth image + //This option is highly experimental and should be used with extreme care. + if (create_close_obstacle_if_depth_is_missing_){ + cv::Mat pRoi = image(cv::Rect(int(0.05*(float(cols))),int(0.05*(float(rows))),int(0.9*(float(cols))),int(0.9*float(rows)))); + cv::medianBlur(pRoi, pRoi, 3); + + //Do filter of close objects + //If the depth is registered, there is usually a black frame around the depth image + //Hence, the ROI stops before the expected "frame" + pRoi = image(cv::Rect(int(cols/10),int(0.8*(float(rows))),int(0.8*(float(cols))),int(0.15*float(rows)))); + cv::Mat blurredImage=pRoi.clone(); + cv::GaussianBlur(pRoi, blurredImage, cv::Size(5, 5), 0, 0); + for(int y = 0; y < blurredImage.cols; y++) + for(int x = 0; x < blurredImage.rows; x++){ + if (blurredImage.at(x,y) == 0){ + pRoi.at(x,y) = 400; + } + } + } image_geometry::PinholeCameraModel model; model.fromCameraInfo(*cameraInfo); @@ -157,13 +205,12 @@ private: pcl::PointCloud::Ptr pclCloud; pclCloud = rtabmap::util3d::cloudFromDepth( - imageDepthPtr->image, + image, cx, cy, fx, fy, decimation_); - processAndPublish(pclCloud, depth->header); } } @@ -245,6 +292,9 @@ private: int decimation_; double noiseFilterRadius_; int noiseFilterMinNeighbors_; + int cut_left_; + int cut_right_; + bool create_close_obstacle_if_depth_is_missing_; ros::Publisher cloudPub_;