From 6df51dcc53e3889287e67337cc94ad0d1bac8e9b Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 2 Jun 2021 10:22:12 +0200
Subject: [PATCH] Used a PointCloud2 instead of a scan. Added a second point
cloud to do clearing in free space. Created some functions to make the code
easier to maintain. Used the angle filter configuration file to know the dark
areas of the sensor.
---
cfg/LidarObstacleDetector.cfg | 1 +
config/default_config.yaml | 2 +-
include/lidar_obstacle_detector_alg.h | 1 +
include/lidar_obstacle_detector_alg_node.h | 19 +-
launch/node.launch | 4 +-
launch/nodelet.launch | 4 +-
src/lidar_obstacle_detector_alg.cpp | 27 +
src/lidar_obstacle_detector_alg_node.cpp | 687 ++++++++++++---------
8 files changed, 440 insertions(+), 305 deletions(-)
diff --git a/cfg/LidarObstacleDetector.cfg b/cfg/LidarObstacleDetector.cfg
index 37d4227..4085c6b 100755
--- a/cfg/LidarObstacleDetector.cfg
+++ b/cfg/LidarObstacleDetector.cfg
@@ -50,6 +50,7 @@ gen.add("max_inc", double_t, 0, "
gen.add("min_inc", double_t, 0, "minimum inclination to take into account",0.05,0,0.5)
gen.add("inc_area_filter", int_t, 0, "Minimum inclination area to take into account",10,1,1000)
gen.add("max_ring", int_t, 0, "Maximum ring to process" , 10, 0, 15)
+gen.add("angle_resolution", double_t, 0, "Angle resolution for the free space pointcloud",0.001,0.0001,0.1)
# farthest point computation
gen.add("farthest_enable", bool_t, 0, "Enable the computation of the farthest point",True)
gen.add("farthest_num_rings", int_t, 0, "Number of rings to use in the farthest point computation",9,1,16)
diff --git a/config/default_config.yaml b/config/default_config.yaml
index bf059a1..857af7a 100755
--- a/config/default_config.yaml
+++ b/config/default_config.yaml
@@ -1,5 +1,5 @@
# sensor information
-#base_frame_id: "ana/base_footprint"
+base_frame_id: "base_footprint"
min_range: 0.1
max_range: 50.0
sensor_height: 0.3
diff --git a/include/lidar_obstacle_detector_alg.h b/include/lidar_obstacle_detector_alg.h
index 57d624a..70507f3 100644
--- a/include/lidar_obstacle_detector_alg.h
+++ b/include/lidar_obstacle_detector_alg.h
@@ -131,6 +131,7 @@ class LidarObstacleDetectorAlgorithm
// here define all lidar_obstacle_detector_alg interface methods to retrieve and set
// the driver parameters
+ std::vector<std::string> get_strings(const ros::NodeHandle& nh, const std::string& param_name);
/**
* \brief Destructor
diff --git a/include/lidar_obstacle_detector_alg_node.h b/include/lidar_obstacle_detector_alg_node.h
index 7b36575..0140d7c 100644
--- a/include/lidar_obstacle_detector_alg_node.h
+++ b/include/lidar_obstacle_detector_alg_node.h
@@ -55,6 +55,11 @@ typedef struct
bool init;
}TLidarData;
+typedef struct{
+ double max;
+ double min;
+}TFilterRange;
+
/**
* \brief IRI ROS Specific Algorithm Class
*
@@ -68,8 +73,10 @@ class LidarObstacleDetectorAlgNode : public algorithm_base::IriBaseAlgorithm<Lid
ros::Publisher obstacles_img_publisher_;
sensor_msgs::Image obstacles_img_Image_msg_;
- ros::Publisher scan_out_publisher_;
- sensor_msgs::LaserScan scan_out_LaserScan_msg_;
+ ros::Publisher obstacles_publisher_;
+ sensor_msgs::PointCloud2 obstacles_PointCloud2_msg_;
+ ros::Publisher free_space_publisher_;
+ sensor_msgs::PointCloud2 free_space_PointCloud2_msg_;
// [subscriber attributes]
ros::Subscriber pointcloud_in_subscriber_;
@@ -82,6 +89,7 @@ class LidarObstacleDetectorAlgNode : public algorithm_base::IriBaseAlgorithm<Lid
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_input;
bool new_point_cloud;
+
// [service attributes]
// [client attributes]
@@ -92,9 +100,11 @@ class LidarObstacleDetectorAlgNode : public algorithm_base::IriBaseAlgorithm<Lid
tf::TransformListener listener;
+ unsigned int max_col_;
cv::Mat inc_image;
cv::Mat label_image;
std::vector<TLidarData> lidar_data;
+ std::vector<TFilterRange> ranges;
/**
* \brief config variable
@@ -157,8 +167,13 @@ class LidarObstacleDetectorAlgNode : public algorithm_base::IriBaseAlgorithm<Lid
*/
void addNodeDiagnostics(void);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr transform_pointcloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &input);
bool get_point(unsigned int ring, unsigned int col,pcl::PointXYZ &point, double &range);
double get_max_inclination(double current_angle,pcl::PointXYZ &point1,double range1,pcl::PointXYZ &point2,double range2);
+ void init_free_space(std::vector<bool> &free_space);
+ void compute_inclination(void);
+ void virtual_obstacles(std::vector<pcl::PointXYZ> &points,std::vector<bool> &free_space);
+ void farthest_obstacles(std::vector<pcl::PointXYZ> &points,std::vector<bool> &free_space);
// [diagnostic functions]
diff --git a/launch/node.launch b/launch/node.launch
index 52a0653..85165f9 100644
--- a/launch/node.launch
+++ b/launch/node.launch
@@ -4,10 +4,10 @@
<arg name="ns" default="ana"/>
<arg name="node_name" default="lidar_obstacle_detector"/>
<arg name="config_file" default="$(find iri_lidar_obstacle_detector)/config/default_config.yaml"/>
+ <arg name="ranges_file" default="$(find iri_lidar_obstacle_detector)/config/default_ranges.yaml"/>
<arg name="output" default="screen"/>
<arg name="launch_prefix" default=""/>
<arg name="cloud_in" default="~pointcloud_in"/>
- <arg name="base_frame_id" default="$(arg ns)/base_footprint"/>
<group ns="$(arg ns)">
@@ -18,7 +18,7 @@
launch-prefix="$(arg launch_prefix)">
<remap from="~pointcloud_in" to="$(arg cloud_in)"/>
<rosparam file="$(arg config_file)" command="load"/>
- <param name="base_frame_id" value="$(arg base_frame_id)"/>
+ <rosparam file="$(arg ranges_file)" command="load"/>
</node>
</group>
diff --git a/launch/nodelet.launch b/launch/nodelet.launch
index dbdc16d..260193e 100644
--- a/launch/nodelet.launch
+++ b/launch/nodelet.launch
@@ -5,10 +5,10 @@
<arg name="node_name" default="lidar_obstacle_detector"/>
<arg name="lidar_nodelet_manager" default="lidar_nodelet_manager"/>
<arg name="config_file" default="$(find iri_lidar_obstacle_detector)/config/default_config.yaml"/>
+ <arg name="ranges_file" default="$(find iri_lidar_obstacle_detector)/config/default_ranges.yaml"/>
<arg name="output" default="screen"/>
<arg name="launch_prefix" default=""/>
<arg name="cloud_in" default="~pointcloud_in"/>
- <arg name="base_frame_id" default="$(arg ns)/base_footprint"/>
<group ns="$(arg ns)">
@@ -19,7 +19,7 @@
output="$(arg output)">
<remap from="~pointcloud_in" to="$(arg cloud_in)"/>
<rosparam file="$(arg config_file)" command="load"/>
- <param name="base_frame_id" value="$(arg base_frame_id)"/>
+ <rosparam file="$(arg ranges_file)" command="load"/>
</node>
</group>
diff --git a/src/lidar_obstacle_detector_alg.cpp b/src/lidar_obstacle_detector_alg.cpp
index ae5be7e..4db9103 100644
--- a/src/lidar_obstacle_detector_alg.cpp
+++ b/src/lidar_obstacle_detector_alg.cpp
@@ -10,6 +10,33 @@ LidarObstacleDetectorAlgorithm::~LidarObstacleDetectorAlgorithm(void)
pthread_mutex_destroy(&this->access_);
}
+std::vector<std::string> LidarObstacleDetectorAlgorithm::get_strings(const ros::NodeHandle& nh, const std::string& param_name)
+{
+ XmlRpc::XmlRpcValue xml_array;
+ if(!nh.getParam(param_name,xml_array))
+ {
+ ROS_ERROR_STREAM("PointCloudAngleFilterAlg::get_strings: Could not find '" << param_name << "' parameter (namespace: " << nh.getNamespace() << ").");
+ return std::vector<std::string>();
+ }
+ if(xml_array.getType()!=XmlRpc::XmlRpcValue::TypeArray)
+ {
+ ROS_ERROR_STREAM("PointCloudAngleFilterAlg::get_strings: The '" << param_name << "' parameter is not an array (namespace: " << nh.getNamespace() << ").");
+ return std::vector<std::string>();
+ }
+
+ std::vector<std::string> out;
+ for(int i=0;i<xml_array.size();++i)
+ {
+ if(xml_array[i].getType()!=XmlRpc::XmlRpcValue::TypeString)
+ {
+ ROS_ERROR_STREAM("PointCloudAngleFilterAlg::get_strings: The '" << param_name << "' parameter contains a non-string element (namespace: " << nh.getNamespace() << ").");
+ return std::vector<std::string>();
+ }
+ out.push_back(static_cast<std::string>(xml_array[i]));
+ }
+ return out;
+}
+
void LidarObstacleDetectorAlgorithm::config_update(Config& config, uint32_t level)
{
this->lock();
diff --git a/src/lidar_obstacle_detector_alg_node.cpp b/src/lidar_obstacle_detector_alg_node.cpp
index f7f2527..e1a7173 100644
--- a/src/lidar_obstacle_detector_alg_node.cpp
+++ b/src/lidar_obstacle_detector_alg_node.cpp
@@ -15,8 +15,43 @@ LidarObstacleDetectorAlgNode::LidarObstacleDetectorAlgNode(const ros::NodeHandle
// [init publishers]
this->obstacles_img_publisher_ = this->public_node_handle_.advertise<sensor_msgs::Image>("obstacles_img/image_raw", 1);
- this->scan_out_publisher_ = this->public_node_handle_.advertise<sensor_msgs::LaserScan>("scan_out", 1);
-
+ this->obstacles_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("obstacles", 1);
+ this->obstacles_PointCloud2_msg_.height=1;
+ this->obstacles_PointCloud2_msg_.fields.resize(3);
+ this->obstacles_PointCloud2_msg_.fields[0].name="x";
+ this->obstacles_PointCloud2_msg_.fields[0].offset=0;
+ this->obstacles_PointCloud2_msg_.fields[0].datatype=7;
+ this->obstacles_PointCloud2_msg_.fields[0].count=1;
+ this->obstacles_PointCloud2_msg_.fields[1].name="y";
+ this->obstacles_PointCloud2_msg_.fields[1].offset=sizeof(float);
+ this->obstacles_PointCloud2_msg_.fields[1].datatype=7;
+ this->obstacles_PointCloud2_msg_.fields[1].count=1;
+ this->obstacles_PointCloud2_msg_.fields[2].name="z";
+ this->obstacles_PointCloud2_msg_.fields[2].offset=2*sizeof(float);
+ this->obstacles_PointCloud2_msg_.fields[2].datatype=7;
+ this->obstacles_PointCloud2_msg_.fields[2].count=1;
+ this->obstacles_PointCloud2_msg_.is_bigendian=true;
+ this->obstacles_PointCloud2_msg_.point_step=3*sizeof(float);
+ this->obstacles_PointCloud2_msg_.is_dense=true;
+ this->free_space_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("free_space", 1);
+ this->free_space_PointCloud2_msg_.height=1;
+ this->free_space_PointCloud2_msg_.fields.resize(3);
+ this->free_space_PointCloud2_msg_.fields[0].name="x";
+ this->free_space_PointCloud2_msg_.fields[0].offset=0;
+ this->free_space_PointCloud2_msg_.fields[0].datatype=7;
+ this->free_space_PointCloud2_msg_.fields[0].count=1;
+ this->free_space_PointCloud2_msg_.fields[1].name="y";
+ this->free_space_PointCloud2_msg_.fields[1].offset=sizeof(float);
+ this->free_space_PointCloud2_msg_.fields[1].datatype=7;
+ this->free_space_PointCloud2_msg_.fields[1].count=1;
+ this->free_space_PointCloud2_msg_.fields[2].name="z";
+ this->free_space_PointCloud2_msg_.fields[2].offset=2*sizeof(float);
+ this->free_space_PointCloud2_msg_.fields[2].datatype=7;
+ this->free_space_PointCloud2_msg_.fields[2].count=1;
+ this->free_space_PointCloud2_msg_.is_bigendian=true;
+ this->free_space_PointCloud2_msg_.point_step=3*sizeof(float);
+ this->free_space_PointCloud2_msg_.is_dense=false;
+
// [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);
@@ -37,6 +72,33 @@ LidarObstacleDetectorAlgNode::~LidarObstacleDetectorAlgNode(void)
pthread_mutex_destroy(&this->pointcloud_in_mutex_);
}
+pcl::PointCloud<pcl::PointXYZ>::Ptr LidarObstacleDetectorAlgNode::transform_pointcloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &input)
+{
+ bool success;
+
+ try{
+ if(this->input_header.frame_id!=this->config_.base_frame_id)
+ {
+ this->alg_.unlock();
+ success=this->listener.waitForTransform(this->input_header.frame_id,this->config_.base_frame_id,this->input_header.stamp,ros::Duration(3));
+ this->alg_.lock();
+ if(success)
+ pcl_ros::transformPointCloud(this->config_.base_frame_id,*this->input,*this->transformed_input,this->listener);
+ else
+ {
+ ROS_ERROR_STREAM("Impossible to transform pointcloud to " << this->config_.base_frame_id);
+ return NULL;
+ }
+ return this->transformed_input;
+ }
+ else
+ return this->input;
+ }catch (tf::TransformException ex){
+ ROS_ERROR("%s",ex.what());
+ return NULL;
+ }
+}
+
bool LidarObstacleDetectorAlgNode::get_point(unsigned int ring, unsigned int col,pcl::PointXYZ &point, double &range)
{
if(this->lidar_data[col*this->config_.max_ring+ring].init)
@@ -97,339 +159,347 @@ double LidarObstacleDetectorAlgNode::get_max_inclination(double current_angle,pc
return inc;
}
-void LidarObstacleDetectorAlgNode::mainNodeThread(void)
+void LidarObstacleDetectorAlgNode::init_free_space(std::vector<bool> &free_space)
{
- bool ring_ordered = false;
- static bool success;
- static std::vector<float> farthest_ranges,virtual_ranges;
- static double farthest_range,virtual_range;
- static double new_x,new_y;
- static unsigned int prev_col,next_col,prev_ring;
- static unsigned int last_valid_ring,last_valid_col;
- static int top,left,bottom,right;
- static double start_angle=0.0,old_start_angle=0.0,start_angle_inc=0.0;
- double angle=0.0,old_angle=0.0,angle_inc=0.0;
- double current_angle;
- unsigned int max_col;
- // current ring points
- static pcl::PointXYZ prev_ring_point,next_ring_point,point;
- static double prev_ring_range,next_ring_range,range;
- static bool point_valid,prev_ring_point_valid,next_ring_point_valid;
- static double ring_inc1,ring_inc2;
- // current col points
- static pcl::PointXYZ prev_col_point,next_col_point;
- static double prev_col_range,next_col_range,col_range;
- static bool prev_col_point_valid,next_col_point_valid;
- static double col_inc1,col_inc2;
- // image variables
- static cv::Mat traversable_space;
- static cv::Mat positive_slope;
- static cv::Mat stats_image;
- static cv::Mat centroid_image;
- static cv::Mat threshold;
- static std::vector<std::vector<cv::Point> > contours;
- static std::vector<cv::Vec4i> hierarchy;
-
- this->alg_.lock();
- if(this->new_point_cloud && this->scan_out_publisher_.getNumSubscribers()>0)
+ free_space.resize((2.0*3.14159)/this->config_.angle_resolution,true);
+ for(unsigned int i=0;i<this->ranges.size();i++)
{
- this->new_point_cloud=false;
- //tf transform from sensor frame to robot base frame
- try{
- if(this->input_header.frame_id!=this->config_.base_frame_id)
- {
- this->alg_.unlock();
- success=this->listener.waitForTransform(this->input_header.frame_id,this->config_.base_frame_id,this->input_header.stamp,ros::Duration(3));
- this->alg_.lock();
- if(success)
- pcl_ros::transformPointCloud(this->config_.base_frame_id,*this->input,*this->transformed_input,this->listener);
- else
- {
- ROS_ERROR_STREAM("Impossible to transform pointcloud to " << this->config_.base_frame_id);
- this->alg_.unlock();
- return;
- }
- }
- }catch (tf::TransformException ex){
- ROS_ERROR("%s",ex.what());
- this->alg_.unlock();
- return;
- }
- if (this->transformed_input->width > this->transformed_input->height)
- ring_ordered = true;
- if(this->transformed_input->height==1 || this->transformed_input->width==1)
+ for(unsigned int j=0;j<(2.0*3.14159)/this->config_.angle_resolution;j++)
{
- ROS_WARN("Droping pointcloud because it is not organized");
- this->alg_.unlock();
- return;
+ if((j*this->config_.angle_resolution-3.14159)<this->ranges[i].max && (j*this->config_.angle_resolution-3.14159)>this->ranges[i].min)
+ free_space[j]=false;
}
+ }
+}
+
+void LidarObstacleDetectorAlgNode::compute_inclination(void)
+{
+ unsigned int prev_col,prev_ring,next_col;
+ double current_angle;
+ // previous point data
+ pcl::PointXYZ prev_ring_point,prev_col_point;
+ double prev_ring_range,prev_col_range;
+ bool prev_ring_point_valid,prev_col_point_valid;
+ // current point data
+ pcl::PointXYZ point;
+ double range,col_range;
+ bool point_valid;
+ double ring_inc1,ring_inc2;
+ double col_inc1,col_inc2;
+ // next point data
+ bool next_ring_point_valid,next_col_point_valid;
+ pcl::PointXYZ next_ring_point,next_col_point;
+ double next_ring_range,next_col_range;
+
+ for(unsigned int col=0;col<this->max_col_;col++)
+ {
+ if(col==0)
+ prev_col=this->max_col_-1;
+ else
+ prev_col=col-1;
+ if(col==this->max_col_-1)
+ next_col=0;
else
+ next_col=col+1;
+ // robot position
+ prev_ring_point.x=0.0;
+ prev_ring_point.y=0.0;
+ prev_ring_point.z=0.0;
+ prev_ring_range=0.0;
+ prev_ring=-1;
+ prev_ring_point_valid=true;
+
+ for(unsigned int ring=0;ring<this->config_.max_ring-1;ring++)
{
- point=this->transformed_input->at(0,0);
- if(!isnan(point.x) && !isnan(point.y) && !isnan(point.z))
- {
- old_start_angle=start_angle;
- start_angle=atan2(point.y,point.x);
- start_angle_inc=start_angle-old_start_angle;
- if(start_angle_inc>3.14159)
- start_angle_inc-=2.0*3.14159;
- }
- else
- {
- old_start_angle=start_angle;
- start_angle+=start_angle_inc;
- }
- current_angle=start_angle;
- // initialize sensor messages
- this->scan_out_LaserScan_msg_.header=this->input_header;
- this->scan_out_LaserScan_msg_.angle_min=-3.14159;
- this->scan_out_LaserScan_msg_.angle_max=3.14159;
- this->scan_out_LaserScan_msg_.angle_increment=(2.0*3.14159-start_angle_inc)/(ring_ordered? this->transformed_input->width: this->transformed_input->height);
- this->scan_out_LaserScan_msg_.ranges.clear();
- this->scan_out_LaserScan_msg_.range_min=this->config_.min_range;
- this->scan_out_LaserScan_msg_.range_max=this->config_.max_range;
- max_col=(unsigned int)((2.0*3.14159)/this->scan_out_LaserScan_msg_.angle_increment);
- if(max_col>(ring_ordered? this->transformed_input->width: this->transformed_input->height))
- max_col=(ring_ordered? this->transformed_input->width: this->transformed_input->height);
-
- this->inc_image.create(this->config_.max_ring,max_col,CV_8UC1);
- this->label_image.create(this->config_.max_ring,max_col,CV_32SC1);
- this->lidar_data.clear();
- this->lidar_data.resize(max_col*this->config_.max_ring);
- // mark all point as not initialized yet
- for(unsigned int i=0;i<max_col*this->config_.max_ring;i++)
+ // current point
+ point_valid=this->get_point(ring,col,point,range);
+ // next ring
+ next_ring_point_valid=this->get_point(ring+1,col,next_ring_point,next_ring_range);
+ if(point_valid)
{
- this->lidar_data[i].init=false;
- this->lidar_data[i].valid=false;
- }
- farthest_ranges.clear();
- farthest_ranges.resize(max_col,this->config_.max_range);
- virtual_ranges.clear();
- virtual_ranges.resize(max_col,this->config_.max_range);
-
- for(unsigned int col=0;col<max_col;col++)
- {
- if(col==0)
- prev_col=max_col-1;
- else
- prev_col=col-1;
- if(col==max_col-1)
- next_col=0;
- else
- next_col=col+1;
- // robot position
- prev_ring_point.x=0.0;
- prev_ring_point.y=0.0;
- prev_ring_point.z=0.0;
- prev_ring_range=0.0;
- prev_ring=-1;
- prev_ring_point_valid=true;
-
- if (ring_ordered)
- point=this->transformed_input->at(col,0);
- else
- point=this->transformed_input->at(0,col);
- if(!isnan(point.x) && !isnan(point.y) && !isnan(point.z))
+ current_angle=atan2(point.y,point.x);
+ // previous column
+ if((prev_col_point_valid=this->get_point(ring,prev_col,prev_col_point,prev_col_range))==true)
{
- old_angle=angle;
- angle=atan2(point.y,point.x);
- if((angle-old_angle)>3.14159)
- angle_inc+=(angle-old_angle)-2.0*3.14159;
- else if((angle-old_angle)<-3.14159)
- angle_inc+=(angle-old_angle)+2.0*3.14159;
- else
- angle_inc+=angle-old_angle;
+ col_range=sqrt(pow(prev_col_point.x-point.x,2.0)+pow(prev_col_point.y-point.y,2.0));
+ this->lidar_data[prev_col*this->config_.max_ring+ring].col_range=col_range;
+ }
+ // next column
+ if((next_col_point_valid=this->get_point(ring,next_col,next_col_point,next_col_range))==true)
+ {
+ col_range=sqrt(pow(point.x-next_col_point.x,2.0)+pow(point.y-next_col_point.y,2.0));
+ this->lidar_data[next_col*this->config_.max_ring+ring].col_range=col_range;
}
- for(unsigned int ring=0;ring<this->config_.max_ring-1;ring++)
+ if(prev_ring==-1 || prev_ring_point_valid)
{
- // current point
- point_valid=this->get_point(ring,col,point,range);
- // next ring
- next_ring_point_valid=this->get_point(ring+1,col,next_ring_point,next_ring_range);
- if(this->lidar_data[col*this->config_.max_ring+ring].valid)
+ ring_inc1=this->get_max_inclination(current_angle,point,range,prev_ring_point,prev_ring_range);
+ if(next_ring_point_valid)
{
- // previous column
- if((prev_col_point_valid=this->get_point(ring,prev_col,prev_col_point,prev_col_range))==true)
- {
- col_range=sqrt(pow(prev_col_point.x-point.x,2.0)+pow(prev_col_point.y-point.y,2.0));
- this->lidar_data[prev_col*this->config_.max_ring+ring].col_range=col_range;
- }
- // next column
- if((next_col_point_valid=this->get_point(ring,next_col,next_col_point,next_col_range))==true)
- {
- col_range=sqrt(pow(point.x-next_col_point.x,2.0)+pow(point.y-next_col_point.y,2.0));
- this->lidar_data[col*this->config_.max_ring+ring].col_range=col_range;//next_col????????????????
- }
- if(prev_ring==-1 || prev_ring_point_valid)
- {
- ring_inc1=this->get_max_inclination(current_angle,point,range,prev_ring_point,prev_ring_range);
- if(next_ring_point_valid)
- {
- ring_inc2=this->get_max_inclination(current_angle,next_ring_point,next_ring_range,point,range);
- if(fabs(ring_inc1)<fabs(ring_inc2))
- ring_inc1=ring_inc2;
- }
- }
- else
- {
- if(next_ring_point_valid)
- ring_inc1=this->get_max_inclination(current_angle,next_ring_point,next_ring_range,point,range);
- else
- ring_inc1=0.0;
- }
- if(prev_col_point_valid)
- {
- col_inc1=atan2(point.z-prev_col_point.z,prev_col_range)*180/3.14159;
- if(next_col_point_valid)
- {
- col_inc2=atan2(next_col_point.z-point.z,col_range)*180/3.14159;
- if(fabs(col_inc1)<fabs(col_inc2))
- col_inc1=col_inc2;
- }
- }
- else
- {
- if(next_col_point_valid)
- col_inc1=atan2(next_col_point.z-point.z,col_range)*180/3.14159;
- else
- col_inc1=0.0;
- }
- this->lidar_data[col*this->config_.max_ring+ring].col_inc=col_inc1;
- this->lidar_data[col*this->config_.max_ring+ring].ring_inc=ring_inc1;
- if(fabs(col_inc1)>fabs(ring_inc1))
- this->inc_image.at<unsigned char>(ring,col,0)=((signed char)col_inc1)+128;
- else
- this->inc_image.at<unsigned char>(ring,col,0)=((signed char)ring_inc1)+128;
+ ring_inc2=this->get_max_inclination(current_angle,next_ring_point,next_ring_range,point,range);
+ if(fabs(ring_inc1)<fabs(ring_inc2))
+ ring_inc1=ring_inc2;
}
- else
- this->inc_image.at<unsigned char>(ring,col,0)=0;
- // next iteration
- prev_ring_point=point;
- prev_ring_range=range;
- prev_ring++;
- prev_ring_point_valid=point_valid;
}
- current_angle+=(2.0*3.14159)/max_col;
- }
-
- if(this->config_.virtual_enable)
- {
- cv::inRange(this->inc_image,cv::Scalar(128-((unsigned char)(this->config_.max_inc*180.0/3.14159))),cv::Scalar(128+((unsigned char)(this->config_.max_inc*180.0/3.14159))),traversable_space);
- cv::morphologyEx(traversable_space,traversable_space,cv::MORPH_CLOSE,cv::getStructuringElement(cv::MORPH_RECT,cv::Size(3,3)));
- }
- if(this->config_.farthest_enable)
- {
- cv::inRange(this->inc_image,cv::Scalar(131),cv::Scalar(255),positive_slope);
- cv::morphologyEx(positive_slope,positive_slope,cv::MORPH_OPEN,cv::getStructuringElement(cv::MORPH_RECT,cv::Size(3,3)));
- }
- for(unsigned int col=0;col<max_col;col++)
- {
- last_valid_col=col;
- if(this->config_.farthest_enable)
+ else
{
- farthest_range=0.0;
- for(unsigned int ring=0;ring<this->config_.farthest_num_rings;ring++)
- {
- if(this->lidar_data[col*this->config_.max_ring+ring].valid)
- {
- farthest_range=this->lidar_data[col*this->config_.max_ring+ring].range;
- last_valid_ring=ring;
- }
- }
- if(farthest_range==0.0)
- farthest_ranges[col]=this->config_.max_range;
- else if(positive_slope.at<unsigned int>(last_valid_ring,last_valid_col,0)!=0)
- farthest_ranges[col]=this->config_.max_range;
+ if(next_ring_point_valid)
+ ring_inc1=this->get_max_inclination(current_angle,next_ring_point,next_ring_range,point,range);
else
- farthest_ranges[col]=farthest_range;
+ ring_inc1=0.0;
}
- if(this->config_.virtual_enable)
+ if(prev_col_point_valid)
{
- for(unsigned int ring=0;ring<this->config_.max_ring-1;ring++)
+ col_inc1=atan2(point.z-prev_col_point.z,prev_col_range)*180/3.14159;
+ if(next_col_point_valid)
{
- if(this->lidar_data[col*this->config_.max_ring+ring].valid)
- {
- if(this->lidar_data[col*this->config_.max_ring+ring].point.z<-this->config_.virtual_floor_threshold)
- {
- if(traversable_space.at<unsigned int>(ring,col,0)==0)
- {
- new_x=this->lidar_data[col*this->config_.max_ring+ring].point.x*this->config_.sensor_height/(this->config_.sensor_height-this->lidar_data[col*this->config_.max_ring+ring].point.z);
- new_y=this->lidar_data[col*this->config_.max_ring+ring].point.y*new_x/this->lidar_data[col*this->config_.max_ring+ring].point.x;
- virtual_range=sqrt(pow(new_x,2.0)+pow(new_y,2.0));
- if(virtual_range<virtual_ranges[col])
- virtual_ranges[col]=virtual_range;
- }
- }
- }
+ col_inc2=atan2(next_col_point.z-point.z,col_range)*180/3.14159;
+ if(fabs(col_inc1)<fabs(col_inc2))
+ col_inc1=col_inc2;
}
- }
- }
-
- if(this->config_.farthest_enable)
- this->scan_out_LaserScan_msg_.ranges=farthest_ranges;
- else
- this->scan_out_LaserScan_msg_.ranges.resize(max_col,this->config_.max_range);
- if(this->config_.virtual_enable)
- {
- for(unsigned int i=0;i<this->scan_out_LaserScan_msg_.ranges.size();i++)
+ }
+ else
{
- if(this->scan_out_LaserScan_msg_.ranges[i]>virtual_ranges[i])
- this->scan_out_LaserScan_msg_.ranges[i]=virtual_ranges[i];
+ if(next_col_point_valid)
+ col_inc1=atan2(next_col_point.z-point.z,col_range)*180/3.14159;
+ else
+ col_inc1=0.0;
}
+ this->lidar_data[col*this->config_.max_ring+ring].col_inc=col_inc1;
+ this->lidar_data[col*this->config_.max_ring+ring].ring_inc=ring_inc1;
+ if(fabs(col_inc1)>fabs(ring_inc1))
+ this->inc_image.at<unsigned char>(ring,col,0)=((signed char)col_inc1)+128;
+ else
+ this->inc_image.at<unsigned char>(ring,col,0)=((signed char)ring_inc1)+128;
}
- cv::inRange(this->inc_image,cv::Scalar(128+((unsigned char)(this->config_.max_inc*180.0/3.14159))),cv::Scalar(255),threshold);
- cv::morphologyEx(threshold,threshold,cv::MORPH_CLOSE,cv::getStructuringElement(cv::MORPH_RECT,cv::Size(3,3)));
- cv::connectedComponentsWithStats(threshold,this->label_image,stats_image,centroid_image,8);
- for(int i=1;i<stats_image.rows;i++)
+ else
+ this->inc_image.at<unsigned char>(ring,col,0)=0;
+ // next iteration
+ prev_ring_point=point;
+ prev_ring_range=range;
+ prev_ring++;
+ prev_ring_point_valid=point_valid;
+ }
+ }
+}
+
+void LidarObstacleDetectorAlgNode::virtual_obstacles(std::vector<pcl::PointXYZ> &points,std::vector<bool> &free_space)
+{
+ cv::Mat traversable_space;
+ unsigned int last_valid_col;
+
+ cv::inRange(this->inc_image,cv::Scalar(128-((unsigned char)(this->config_.min_inc*180.0/3.14159))),cv::Scalar(128+((unsigned char)(this->config_.min_inc*180.0/3.14159))),traversable_space);
+ cv::morphologyEx(traversable_space,traversable_space,cv::MORPH_CLOSE,cv::getStructuringElement(cv::MORPH_RECT,cv::Size(3,3)));
+
+ for(unsigned int col=0;col<this->max_col_;col++)
+ {
+ last_valid_col=col;
+ for(unsigned int ring=0;ring<this->config_.max_ring-1;ring++)
+ {
+ if(this->lidar_data[col*this->config_.max_ring+ring].valid)
{
- if(stats_image.at<int>(i,cv::CC_STAT_AREA)>this->config_.inc_area_filter)
+ if(this->lidar_data[col*this->config_.max_ring+ring].point.z<-this->config_.virtual_floor_threshold)
{
- top=stats_image.at<int>(i,cv::CC_STAT_TOP);
- left=stats_image.at<int>(i,cv::CC_STAT_LEFT);
- bottom=top+stats_image.at<int>(i,cv::CC_STAT_HEIGHT);
- right=left+stats_image.at<int>(i,cv::CC_STAT_WIDTH);
- cv::findContours(threshold(cv::Range(top,bottom),cv::Range(left,right)),contours,hierarchy,CV_RETR_TREE,CV_CHAIN_APPROX_NONE);
- for(unsigned int j=0;j<contours.size();j++)
+ if(traversable_space.at<unsigned int>(ring,col,0)!=0)
{
- for(unsigned int k=0;k<contours[j].size();k++)
- {
- 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;
- }
+ pcl::PointXYZ point;
+ point.x=this->lidar_data[col*this->config_.max_ring+ring].point.x*this->config_.sensor_height/(this->config_.sensor_height-this->lidar_data[col*this->config_.max_ring+ring].point.z);
+ point.y=this->lidar_data[col*this->config_.max_ring+ring].point.y*point.x/this->lidar_data[col*this->config_.max_ring+ring].point.x;
+ point.z=this->config_.sensor_height;
+ points.push_back(point);
+ break;
}
}
}
- for(unsigned int i=0;i<this->scan_out_LaserScan_msg_.ranges.size();i++)
+ }
+ }
+ if(this->config_.debug && this->obstacles_img_publisher_.getNumSubscribers()>0)
+ {
+ this->cv_image_=cv_bridge::CvImage(this->input_header, sensor_msgs::image_encodings::MONO8,traversable_space);
+ 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_);
+ }
+}
+
+void LidarObstacleDetectorAlgNode::farthest_obstacles(std::vector<pcl::PointXYZ> &points,std::vector<bool> &free_space)
+{
+ cv::Mat positive_slope;
+ unsigned int last_valid_col,last_valid_ring;
+ double farthest_range;
+
+ cv::inRange(this->inc_image,cv::Scalar(131),cv::Scalar(255),positive_slope);
+ cv::morphologyEx(positive_slope,positive_slope,cv::MORPH_OPEN,cv::getStructuringElement(cv::MORPH_RECT,cv::Size(3,3)));
+
+ for(unsigned int col=0;col<this->max_col_;col++)
+ {
+ last_valid_col=col;
+ farthest_range=0.0;
+ for(unsigned int ring=0;ring<this->config_.farthest_num_rings;ring++)
+ {
+ if(this->lidar_data[col*this->config_.max_ring+ring].valid)
{
- if(this->scan_out_LaserScan_msg_.ranges[i]>this->config_.max_range)
- this->scan_out_LaserScan_msg_.ranges[i]=this->config_.max_range;
+ farthest_range=this->lidar_data[col*this->config_.max_ring+ring].range;
+ last_valid_ring=ring;
}
- if(angle_inc<0.0)
+ }
+ if(farthest_range!=0.0 && positive_slope.at<unsigned int>(last_valid_ring,last_valid_col,0)==0)
+ points.push_back(this->lidar_data[last_valid_col*this->config_.max_ring+last_valid_ring].point);
+ // TO DO: clear free space accordingly
+ }
+ /*
+ 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_);
+ }
+ */
+}
+
+void LidarObstacleDetectorAlgNode::mainNodeThread(void)
+{
+ bool ring_ordered = false;
+ int top,left,bottom,right;
+ double current_angle;
+ // image variables
+ cv::Mat stats_image;
+ cv::Mat centroid_image;
+ cv::Mat threshold;
+ cv::Mat label_image;
+ std::vector<std::vector<cv::Point> > contours;
+ std::vector<cv::Vec4i> hierarchy;
+
+ this->alg_.lock();
+ if(this->new_point_cloud && this->obstacles_publisher_.getNumSubscribers()>0)
+ {
+ this->new_point_cloud=false;
+ //tf transform from sensor frame to robot base frame
+ if((this->transformed_input=this->transform_pointcloud(this->input))==NULL)
+ {
+ this->alg_.unlock();
+ return;
+ }
+ // check input format
+ if(this->transformed_input->height==1 || this->transformed_input->width==1)
+ {
+ ROS_WARN("Droping pointcloud because it is not organized");
+ this->alg_.unlock();
+ return;
+ }
+ if(this->transformed_input->width > this->transformed_input->height)
+ ring_ordered = true;
+ // initialize local variables
+ if(ring_ordered)
+ this->max_col_=this->transformed_input->width;
+ else
+ this->max_col_=this->transformed_input->height;
+ this->inc_image.create(this->config_.max_ring,this->max_col_,CV_8UC1);
+ this->label_image.create(this->config_.max_ring,this->max_col_,CV_32SC1);
+ this->lidar_data.clear();
+ this->lidar_data.resize(this->max_col_*this->config_.max_ring);
+ // mark all point as not initialized yet
+ for(unsigned int i=0;i<this->max_col_*this->config_.max_ring;i++)
+ {
+ this->lidar_data[i].init=false;
+ this->lidar_data[i].valid=false;
+ }
+ /* initialize free space */
+ std::vector<bool> free_space;
+ this->init_free_space(free_space);
+
+ // compute inclinations
+ this->compute_inclination();
+
+ cv::inRange(this->inc_image,cv::Scalar(128+((unsigned char)(this->config_.max_inc*180.0/3.14159))),cv::Scalar(255),threshold);
+ cv::morphologyEx(threshold,threshold,cv::MORPH_CLOSE,cv::getStructuringElement(cv::MORPH_RECT,cv::Size(3,3)));
+ cv::connectedComponentsWithStats(threshold,this->label_image,stats_image,centroid_image,8);
+
+ /*
+ if(this->config_.debug && this->obstacles_img_publisher_.getNumSubscribers()>0)
+ {
+ this->cv_image_=cv_bridge::CvImage(this->input_header, sensor_msgs::image_encodings::MONO8,threshold);
+ 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_);
+ }
+ */
+ std::vector<pcl::PointXYZ> points;
+ for(int i=1;i<stats_image.rows;i++)
+ {
+ if(stats_image.at<int>(i,cv::CC_STAT_AREA)>this->config_.inc_area_filter)
{
- this->scan_out_LaserScan_msg_.angle_min=0.0;
- this->scan_out_LaserScan_msg_.angle_max=2.0*3.14159;
- std::vector<float> ranges=this->scan_out_LaserScan_msg_.ranges;
- this->scan_out_LaserScan_msg_.ranges.clear();
- this->scan_out_LaserScan_msg_.ranges.resize(max_col);
- unsigned int num_swap_samples=((start_angle+3.14159)/this->scan_out_LaserScan_msg_.angle_increment);
- for(unsigned int i=0;i<ranges.size();i++)
+ top=stats_image.at<int>(i,cv::CC_STAT_TOP);
+ left=stats_image.at<int>(i,cv::CC_STAT_LEFT);
+ bottom=top+stats_image.at<int>(i,cv::CC_STAT_HEIGHT);
+ right=left+stats_image.at<int>(i,cv::CC_STAT_WIDTH);
+ cv::findContours(threshold(cv::Range(top,bottom),cv::Range(left,right)),contours,hierarchy,CV_RETR_TREE,CV_CHAIN_APPROX_NONE);
+ double max_angle=-std::numeric_limits<double>::max();
+ double min_angle=std::numeric_limits<double>::max();
+ for(unsigned int j=0;j<contours.size();j++)
{
- if(i<=num_swap_samples)
- this->scan_out_LaserScan_msg_.ranges[i]=ranges[num_swap_samples-i];
- else
- this->scan_out_LaserScan_msg_.ranges[i]=ranges[max_col-i+num_swap_samples];
+ for(unsigned int k=0;k<contours[j].size();k++)
+ {
+ contours[j][k].x+=left;
+ contours[j][k].y+=top;
+ points.push_back(this->lidar_data[contours[j][k].x*this->config_.max_ring+contours[j][k].y].point);
+ current_angle=atan2(this->lidar_data[contours[j][k].x*this->config_.max_ring+contours[j][k].y].point.y,this->lidar_data[contours[j][k].x*this->config_.max_ring+contours[j][k].y].point.x);
+ if(current_angle>max_angle)
+ max_angle=current_angle;
+ if(current_angle<min_angle)
+ min_angle=current_angle;
+ }
}
+ for(unsigned int i=((min_angle+3.14159)/this->config_.angle_resolution);i<((max_angle+3.14159)/this->config_.angle_resolution);i++)
+ free_space[i]=false;
}
- if(this->config_.debug && this->obstacles_img_publisher_.getNumSubscribers()>0)
+ }
+ if(this->config_.virtual_enable)
+ this->virtual_obstacles(points,free_space);
+ if(this->config_.farthest_enable)
+ this->farthest_obstacles(points,free_space);
+ this->obstacles_PointCloud2_msg_.header.frame_id=this->config_.base_frame_id;
+ this->obstacles_PointCloud2_msg_.header.stamp=this->input_header.stamp;
+ this->obstacles_PointCloud2_msg_.width=points.size();
+ this->obstacles_PointCloud2_msg_.row_step=this->obstacles_PointCloud2_msg_.width*this->obstacles_PointCloud2_msg_.point_step;
+ this->obstacles_PointCloud2_msg_.data.resize(this->obstacles_PointCloud2_msg_.row_step);
+ for(unsigned int i=0;i<points.size();i++)
+ {
+ memcpy(&(this->obstacles_PointCloud2_msg_.data.data()[i*this->obstacles_PointCloud2_msg_.point_step]),(unsigned char *)&points[i].x,sizeof(float));
+ memcpy(&(this->obstacles_PointCloud2_msg_.data.data()[i*this->obstacles_PointCloud2_msg_.point_step+sizeof(float)]),(unsigned char *)&points[i].y,sizeof(float));
+ memcpy(&(this->obstacles_PointCloud2_msg_.data.data()[i*this->obstacles_PointCloud2_msg_.point_step+2*sizeof(float)]),(unsigned char *)&points[i].z,sizeof(float));
+ }
+
+ points.clear();
+ for(unsigned int i=0;i<(2.0*3.14159)/this->config_.angle_resolution;i++)
+ {
+ if(free_space[i])
{
- 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_);
+ pcl::PointXYZ point;
+ point.x=this->config_.max_range*cos(i*this->config_.angle_resolution-3.14159);
+ point.y=this->config_.max_range*sin(i*this->config_.angle_resolution-3.14159);
+ point.z=this->config_.sensor_height;
+ points.push_back(point);
}
- this->scan_out_publisher_.publish(this->scan_out_LaserScan_msg_);
}
+ this->free_space_PointCloud2_msg_.header.frame_id=this->config_.base_frame_id;
+ this->free_space_PointCloud2_msg_.header.stamp=this->input_header.stamp;
+ this->free_space_PointCloud2_msg_.width=points.size();
+ this->free_space_PointCloud2_msg_.row_step=this->free_space_PointCloud2_msg_.width*this->free_space_PointCloud2_msg_.point_step;
+ this->free_space_PointCloud2_msg_.data.resize(this->free_space_PointCloud2_msg_.row_step);
+ for(unsigned int i=0;i<points.size();i++)
+ {
+ memcpy(&(this->free_space_PointCloud2_msg_.data.data()[i*this->free_space_PointCloud2_msg_.point_step]),(unsigned char *)&points[i].x,sizeof(float));
+ memcpy(&(this->free_space_PointCloud2_msg_.data.data()[i*this->free_space_PointCloud2_msg_.point_step+sizeof(float)]),(unsigned char *)&points[i].y,sizeof(float));
+ memcpy(&(this->free_space_PointCloud2_msg_.data.data()[i*this->free_space_PointCloud2_msg_.point_step+2*sizeof(float)]),(unsigned char *)&points[i].z,sizeof(float));
+ }
+
+ /*
+ for(unsigned int i=0;i<this->scan_out_LaserScan_msg_.ranges.size();i++)
+ {
+ if(this->scan_out_LaserScan_msg_.ranges[i]>this->config_.max_range)
+ this->scan_out_LaserScan_msg_.ranges[i]=this->config_.max_range;
+ }
+ */
+ this->obstacles_publisher_.publish(this->obstacles_PointCloud2_msg_);
+ this->free_space_publisher_.publish(this->free_space_PointCloud2_msg_);
}
this->alg_.unlock();
@@ -477,7 +547,28 @@ void LidarObstacleDetectorAlgNode::pointcloud_in_mutex_exit(void)
void LidarObstacleDetectorAlgNode::node_config_update(Config &config, uint32_t level)
{
+ std::vector<std::string> range_names;
+ TFilterRange new_range;
+ static bool first=true;
+ double max,min;
+
this->alg_.lock();
+ if(first)
+ {
+ first=false;
+ this->ranges.clear();
+ range_names=this->alg_.get_strings(this->public_node_handle_,"ranges");
+ for(unsigned int i=0;i<range_names.size();i++)
+ {
+ ros::NodeHandle range_nh(this->public_node_handle_,range_names[i]);
+ new_range.max=0;
+ new_range.min=0;
+ range_nh.getParam("max",new_range.max);
+ range_nh.getParam("min",new_range.min);
+ this->ranges.push_back(new_range);
+ }
+ }
+
this->config_=config;
this->alg_.unlock();
}
--
GitLab