Removed the clearing laser scan.

Added checks on the number of subscribers to publish or not.

CMakeLists.txt

@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
 # ******************************************************************** 
 #                 Add catkin additional components here
 # ******************************************************************** 
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm cv_bridge image_transport camera_info_manager sensor_msgs nodelet pcl_conversions pcl_ros)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm cv_bridge sensor_msgs nodelet pcl_conversions pcl_ros)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -62,7 +62,7 @@ catkin_package(
 # ******************************************************************** 
 #            Add ROS and IRI ROS run time dependencies
 # ******************************************************************** 
- CATKIN_DEPENDS iri_base_algorithm cv_bridge image_transport camera_info_manager sensor_msgs nodelet pcl_conversions pcl_ros
+ CATKIN_DEPENDS iri_base_algorithm cv_bridge sensor_msgs nodelet pcl_conversions pcl_ros
 # ******************************************************************** 
 #      Add system and labrobotica run time dependencies here
 # ******************************************************************** 
@@ -105,9 +105,9 @@ target_link_libraries(${PROJECT_NAME}_nodelet ${OpenCV_LIBS})
 # ******************************************************************** 
 # add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME} cv_bridge_generate_messages_cpp)
-add_dependencies(${PROJECT_NAME} image_transport_generate_messages_cpp)
-add_dependencies(${PROJECT_NAME} camera_info_manager_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME} sensor_msgs_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME}_nodelet cv_bridge_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME}_nodelet sensor_msgs_generate_messages_cpp)
 # ******************************************************************** 
 #               Add dynamic reconfigure dependencies 
 # ******************************************************************** 

README.md

+# iri_nav_lidar_obstacles
+
+IRI package created to detect obstacles around the robot using a lidar with 16 rings. It also creates a ring of fictional obstacles at maximum range to clearing operations.
+
+## Getting Started
+
+It uses a lidar sensor to detect several types of obstacles. It detects positive and negative obstacles (holes) and ramps with a too high slope which are also obstacles since they are not passables.
+
+These instructions will get you a copy of the project up and running on your local machine for development and testing purposes.
+
+### Prerequisites
+
+This package is part of the *3d_navigation* project, but you do not need to install all the packages in the project to see it running.
+More specifically, with the *iri_nav_lidar_filter* package, is possible to see the capacities of this sensor discarting wrong points due to the robot's structure.
+
+In addition, for compilation, several packages are necessary: *iri_base_algorithm*, *sensor_msgs*, *std_msgs* and *velodyne_pointcloud*.
+
+### Installing
+
+Create a new folder for the package and clone the repository using
+```
+mkdir iri_nav_lidar_obstacles
+cd iri_nav_lidar_obstacles
+git clone https://gitlab.iri.upc.edu/mobile_robotics/colrobtransp/3d_navigation/iri_hole_detector2.git
+```
+Compile the package with
+```
+roscd
+cd ..
+catkin_make
+```
+
+## Node inputs, outputs and params
+
+### Subscribers
+
+* **sensor_cloud_in**<sensor_msgs::PointCloud2> - It receives a pointcloud (typically the output of the the *iri_nav_lidar_filter*) and it makes all the calculations in order to detect all the obstacles arround the robot.
+
+### Publishers
+
+* **hole_scan_out**<sensor_msgs::LaserScan> - It generates a pointcloud with virtual obstacles at every position the algorithm has detected an obstacle or at the maximum valid range of the sensor for clearing purposes. This pointcloud is sent to the navigation stack to be aware of the presence of an obstacle at the position of each red point.
+* **test_cloud_out**<sensor_msgs::PointCloud2> - It outputs the same pointcloud as the input but ...
+
+### Params
+
+* **x_min**, **x_max**, **y_min**, **y_max** - Filter limit.
+* **n_rings_lidar** - Number of laser rings (typically 16).
+* **subdivisions** - Downsampling param. The bigger, more space between rows.
+* **range_filter** - Downsampling param. It reduces the number of rows.
+* **hole_gap_size** - Minimum gap for considering a hole.
+* **max_incline** - Maximum slope inclination.
+* **angle_min** - LaserScan minimum angle.
+* **angle_max** - LaserScan maximum angle.
+* **range_min** - Laser scan's minimum range.
+* **range_max** - Laser scan's maximum range.
+* **num_readings** - Number of LaserScan points.
+* **base_frame** - Name of the robot's base frame.
+* **debug_mode** - Allows extra topics publication for testing.
+
+## Authors
+
+* **Joan Grimalt Oliver** - *Initial work Velodyne sensor*
+
+## Acknowledgments
+
+* Hat tip to anyone whose code was used.

include/lidar_obstacle_detector_alg_node.h

@@ -68,9 +68,6 @@ class LidarObstacleDetectorAlgNode : public algorithm_base::IriBaseAlgorithm<Lid
     ros::Publisher obstacles_img_publisher_;
     sensor_msgs::Image obstacles_img_Image_msg_;
 
-    ros::Publisher scan_clearing_out_publisher_;
-    sensor_msgs::LaserScan scan_clearing_out_LaserScan_msg_;
-
     ros::Publisher scan_out_publisher_;
     sensor_msgs::LaserScan scan_out_LaserScan_msg_;
 

launch/nodelet.launch

@@ -15,7 +15,7 @@
           pkg="nodelet"
           type="nodelet"
           args="load iri_lidar_obstacle_detector/LidarObstacleDetectorAlgNodelet $(arg lidar_nodelet_manager)"
-          output="screen">
+          output="$(arg output)">
       <remap from="~pointcloud_in" to="$(arg cloud_in)"/>
       <rosparam file="$(arg config_file)" command="load"/>
     </node>

src/lidar_obstacle_detector_alg_node.cpp

@@ -9,20 +9,16 @@ LidarObstacleDetectorAlgNode::LidarObstacleDetectorAlgNode(const ros::NodeHandle
   transformed_input(new pcl::PointCloud<pcl::PointXYZ>())
 {
   //init class attributes if necessary
-  //this->loop_rate_ = 2;//in [Hz]
+  this->setRate(100.0);//in [Hz]
 
   // [init publishers]
   this->obstacles_img_publisher_ = this->public_node_handle_.advertise<sensor_msgs::Image>("obstacles_img/image_raw", 1);
-  this->scan_clearing_out_publisher_ = this->public_node_handle_.advertise<sensor_msgs::LaserScan>("scan_clearing_out", 1);
   this->scan_out_publisher_ = this->public_node_handle_.advertise<sensor_msgs::LaserScan>("scan_out", 1);
   
   // [init subscribers]
   this->pointcloud_in_subscriber_ = this->public_node_handle_.subscribe("pointcloud_in", 1, &LidarObstacleDetectorAlgNode::pointcloud_in_callback, this);
   pthread_mutex_init(&this->pointcloud_in_mutex_,NULL);
 
-  this->scan_clearing_out_LaserScan_msg_.angle_min=-3.14159;
-  this->scan_clearing_out_LaserScan_msg_.angle_max=3.14159;
-
   this->scan_out_LaserScan_msg_.angle_min=-3.14159;
   this->scan_out_LaserScan_msg_.angle_max=3.14159;
   
@@ -129,7 +125,7 @@ void LidarObstacleDetectorAlgNode::mainNodeThread(void)
   static std::vector<cv::Vec4i> hierarchy;
 
   this->alg_.lock();
-  if(this->new_point_cloud)
+  if(this->new_point_cloud && this->scan_out_publisher_.getNumSubscribers()>0)
   {
     this->new_point_cloud=false;
     //tf transform from sensor frame to robot base frame
@@ -323,12 +319,6 @@ void LidarObstacleDetectorAlgNode::mainNodeThread(void)
       }
 
       // initialize sensor messages
-//    this->scan_clearing_out_LaserScan_msg_.header=msg->header;
-//    this->scan_clearing_out_LaserScan_msg_.angle_increment=(3.14159*2.0)/transformed_cloud->height;
-//    this->scan_clearing_out_LaserScan_msg_.ranges.clear();
-//    this->scan_clearing_out_LaserScan_msg_.ranges.resize(transformed_cloud->height,this->config_.max_range);
-//    this->scan_clearing_out_LaserScan_msg_.range_min=this->config_.min_range;
-//    this->scan_clearing_out_LaserScan_msg_.range_max=this->config_.max_range;
       this->scan_out_LaserScan_msg_.header=this->input_header;
       this->scan_out_LaserScan_msg_.angle_increment=(3.14159*2.0)/this->transformed_input->height;
       this->scan_out_LaserScan_msg_.ranges.clear();
@@ -338,16 +328,12 @@ void LidarObstacleDetectorAlgNode::mainNodeThread(void)
         this->scan_out_LaserScan_msg_.ranges=farthest_ranges;
       else
         this->scan_out_LaserScan_msg_.ranges.resize(this->transformed_input->height,this->config_.max_range);
-//    this->scan_clearing_out_LaserScan_msg_.ranges=farthest_ranges;
       if(this->config_.virtual_enable)
       {
         for(unsigned int i=0;i<this->scan_out_LaserScan_msg_.ranges.size();i++)
         {
           if(this->scan_out_LaserScan_msg_.ranges[i]>virtual_ranges[i])
-          {
             this->scan_out_LaserScan_msg_.ranges[i]=virtual_ranges[i];
-//            this->scan_clearing_out_LaserScan_msg_.ranges[i]=virtual_ranges[i]-0.2;
-          }
         }
       }
       cv::inRange(this->inc_image,cv::Scalar(128+((unsigned char)(this->config_.max_inc*180.0/3.14159))),cv::Scalar(255),threshold);
@@ -369,10 +355,7 @@ void LidarObstacleDetectorAlgNode::mainNodeThread(void)
               contours[j][k].x+=left;
               contours[j][k].y+=top;
               if(this->scan_out_LaserScan_msg_.ranges[contours[j][k].x]>this->lidar_data[contours[j][k].x*this->config_.max_ring+contours[j][k].y].range)
-              {
                 this->scan_out_LaserScan_msg_.ranges[contours[j][k].x]=this->lidar_data[contours[j][k].x*this->config_.max_ring+contours[j][k].y].range;
-//                this->scan_clearing_out_LaserScan_msg_.ranges[contours[j][k].x]=this->lidar_data[contours[j][k].x*this->config_.max_ring+contours[j][k].y].range-0.2;
-              }
             }
           }
         }
@@ -382,14 +365,13 @@ void LidarObstacleDetectorAlgNode::mainNodeThread(void)
         if(this->scan_out_LaserScan_msg_.ranges[i]>this->config_.max_range)
           this->scan_out_LaserScan_msg_.ranges[i]=this->config_.max_range;
       }
-      if(this->config_.debug)
+      if(this->config_.debug && this->obstacles_img_publisher_.getNumSubscribers()>0)
       {
         this->cv_image_=cv_bridge::CvImage(this->input_header, sensor_msgs::image_encodings::MONO8,positive_slope);
         this->cv_image_.toImageMsg(this->obstacles_img_Image_msg_); // from cv_bridge to sensor_msgs::Image
         this->obstacles_img_publisher_.publish(this->obstacles_img_Image_msg_);
       }
       this->scan_out_publisher_.publish(this->scan_out_LaserScan_msg_);
-//    this->scan_clearing_out_publisher_.publish(this->scan_clearing_out_LaserScan_msg_);
     }
   }