diff --git a/src/lidar_obstacle_detector_alg_node.cpp b/src/lidar_obstacle_detector_alg_node.cpp
index 8069d8f0ca072af8cc13b834d59febe9503e16b0..e07fa8abf71b17662e3ed2442606397cc0d94d43 100644
--- a/src/lidar_obstacle_detector_alg_node.cpp
+++ b/src/lidar_obstacle_detector_alg_node.cpp
@@ -3,6 +3,8 @@
#include <math.h>
#include <opencv2/imgproc.hpp>
+#include <algorithm>
+
LidarObstacleDetectorAlgNode::LidarObstacleDetectorAlgNode(const ros::NodeHandle &nh) :
algorithm_base::IriBaseAlgorithm<LidarObstacleDetectorAlgorithm>(nh),
input(new pcl::PointCloud<pcl::PointXYZ>()),
@@ -19,9 +21,6 @@ LidarObstacleDetectorAlgNode::LidarObstacleDetectorAlgNode(const ros::NodeHandle
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_out_LaserScan_msg_.angle_min=-3.14159;
- this->scan_out_LaserScan_msg_.angle_max=3.14159;
-
this->new_point_cloud=false;
// [init services]
@@ -97,14 +96,17 @@ double LidarObstacleDetectorAlgNode::get_max_inclination(double current_angle,pc
void LidarObstacleDetectorAlgNode::mainNodeThread(void)
{
- static bool first=true,success;
+ 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 double current_angle;
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;
@@ -157,35 +159,55 @@ void LidarObstacleDetectorAlgNode::mainNodeThread(void)
}
else
{
- if(first)
+ point=this->transformed_input->at(0,0);
+ if(!isnan(point.x) && !isnan(point.y) && !isnan(point.z))
{
- first=false;
- this->inc_image.create(this->config_.max_ring,this->transformed_input->height,CV_8UC1);
- this->label_image.create(this->config_.max_ring,this->transformed_input->height,CV_32SC1);
+ 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
+ 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)/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>this->transformed_input->height)
+ max_col=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(this->transformed_input->height*this->config_.max_ring);
+ this->lidar_data.resize(max_col*this->config_.max_ring);
// mark all point as not initialized yet
- for(unsigned int i=0;i<this->transformed_input->height*this->config_.max_ring;i++)
+ for(unsigned int i=0;i<max_col*this->config_.max_ring;i++)
{
this->lidar_data[i].init=false;
this->lidar_data[i].valid=false;
}
farthest_ranges.clear();
- farthest_ranges.resize(this->transformed_input->height,0);
+ farthest_ranges.resize(max_col,0);
virtual_ranges.clear();
- virtual_ranges.resize(this->transformed_input->height,this->config_.max_range);
- for(unsigned int col=0;col<this->transformed_input->height;col++)
+ virtual_ranges.resize(max_col,this->config_.max_range);
+
+ for(unsigned int col=0;col<max_col;col++)
{
if(col==0)
- prev_col=this->transformed_input->height-1;
+ prev_col=max_col-1;
else
prev_col=col-1;
- if(col==this->transformed_input->height-1)
+ if(col==max_col-1)
next_col=0;
else
next_col=col+1;
- current_angle=col*(360.0/this->transformed_input->height);
// robot position
prev_ring_point.x=0.0;
prev_ring_point.y=0.0;
@@ -194,6 +216,18 @@ void LidarObstacleDetectorAlgNode::mainNodeThread(void)
prev_ring=-1;
prev_ring_point_valid=true;
+ point=this->transformed_input->at(0,col);
+ if(!isnan(point.x) && !isnan(point.y) && !isnan(point.z))
+ {
+ 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;
+ }
for(unsigned int ring=0;ring<this->config_.max_ring-1;ring++)
{
// current point
@@ -263,6 +297,7 @@ void LidarObstacleDetectorAlgNode::mainNodeThread(void)
prev_ring++;
prev_ring_point_valid=point_valid;
}
+ current_angle+=(2.0*3.14159)/max_col;
}
if(this->config_.virtual_enable)
@@ -275,7 +310,7 @@ void LidarObstacleDetectorAlgNode::mainNodeThread(void)
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->transformed_input->height;col++)
+ for(unsigned int col=0;col<max_col;col++)
{
if(this->config_.farthest_enable)
{
@@ -318,16 +353,10 @@ void LidarObstacleDetectorAlgNode::mainNodeThread(void)
}
}
- // initialize sensor messages
- 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();
- this->scan_out_LaserScan_msg_.range_min=this->config_.min_range;
- this->scan_out_LaserScan_msg_.range_max=this->config_.max_range;
if(this->config_.farthest_enable)
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_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++)
@@ -337,7 +366,7 @@ void LidarObstacleDetectorAlgNode::mainNodeThread(void)
}
}
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::morphologyEx(threshold,threshold,cv::MORPH_CLOSE,cv::getStructuringElement(cv::MORPH_RECT,cv::Size(11,3)));
cv::connectedComponentsWithStats(threshold,this->label_image,stats_image,centroid_image,8);
for(int i=1;i<stats_image.rows;i++)
{
@@ -365,6 +394,22 @@ 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(angle_inc<0.0)
+ {
+ 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++)
+ {
+ 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];
+ }
+ }
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);