From 639c687a59f46b06588c2b9138b5959dac3fd6f4 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 11 Feb 2020 18:53:19 +0100
Subject: [PATCH] Removed the clearing laser scan. Added checks on the number
of subscribers to publish or not.
---
CMakeLists.txt | 8 +--
README.md | 66 ++++++++++++++++++++++
include/lidar_obstacle_detector_alg_node.h | 3 -
launch/nodelet.launch | 2 +-
src/lidar_obstacle_detector_alg_node.cpp | 24 +-------
5 files changed, 74 insertions(+), 29 deletions(-)
create mode 100644 README.md
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 34d1796..f351c7a 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 0000000..1791e9a
--- /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 7d7238b..7b36575 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 4c0a5e6..9926783 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 794195b..8069d8f 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_);
}
}
--
GitLab