diff --git a/CMakeLists.txt b/CMakeLists.txt
index 34d17962057514bc279479fc1947d88c091ae50b..f351c7a40ad6758dcec341fed77df8d4a135a643 100644
--- a/CMakeLists.txt
+++ b/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
# ********************************************************************
diff --git a/README.md b/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..1791e9a2032066cceb4d4c8e111a709ee6522007
--- /dev/null
+++ b/README.md
@@ -0,0 +1,66 @@
+# 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.
diff --git a/include/lidar_obstacle_detector_alg_node.h b/include/lidar_obstacle_detector_alg_node.h
index 7d7238b0cb32c6a7809cfa3bdeb871cc8b87cf5d..7b36575ca4903797dc2345a9cef41bd2b0dbeda8 100644
--- a/include/lidar_obstacle_detector_alg_node.h
+++ b/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_;
diff --git a/launch/nodelet.launch b/launch/nodelet.launch
index 4c0a5e6b1bbaf588d028eb560285c55dda79d3c5..9926783b94d5cd41a29cb504c25cf8addae50106 100644
--- a/launch/nodelet.launch
+++ b/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>
diff --git a/src/lidar_obstacle_detector_alg_node.cpp b/src/lidar_obstacle_detector_alg_node.cpp
index 794195baaf9158f7db481219ec3ab4a50d6c11d2..8069d8f0ca072af8cc13b834d59febe9503e16b0 100644
--- a/src/lidar_obstacle_detector_alg_node.cpp
+++ b/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_);
}
}