diff --git a/cfg/LidarObstacleDetector.cfg b/cfg/LidarObstacleDetector.cfg
index 37d422711edc3608c48ae2466ebc810c9da63835..4085c6b784454b7c95b245f4dafc34893a1123bf 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 bf059a19747afa22406cbf7f717bae5843d8869b..857af7acc2ff67e0bb14a9222beaa118bb57a071 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 57d624a66b251d70b1c4ca7b4729ce6d565f5b22..70507f31438d1479deb179c05503ceef6ae350e5 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 7b36575ca4903797dc2345a9cef41bd2b0dbeda8..0140d7c65373cf92bc672993400b630e89cbedd2 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 52a0653e55f2111e226d5e38f37f606e844fde98..85165f9dbca67ca3e55f89b5f5f5318e6ac13e4a 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 dbdc16d2dc3afa178e1b1e47b4a3d70db9348e36..260193ed8076386b42b2a851806d988aad37fc20 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 ae5be7e2542ff0ee7bb9854cd53b99147aafb2a0..4db9103ea9189412bb4f4fa2aa47cd810025975f 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 f7f25270c7d2617c5ec822f2f8548ee1d06e1a34..e1a7173ad2bd0ea2cb7c77bbd338ff503a0f16d8 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();
}