diff --git a/src/lidar_obstacle_detector_alg_node.cpp b/src/lidar_obstacle_detector_alg_node.cpp index 29ddcd6bd624c79429a30e225db74b6cae6da76a..f7f25270c7d2617c5ec822f2f8548ee1d06e1a34 100644 --- a/src/lidar_obstacle_detector_alg_node.cpp +++ b/src/lidar_obstacle_detector_alg_node.cpp @@ -46,7 +46,10 @@ bool LidarObstacleDetectorAlgNode::get_point(unsigned int ring, unsigned int col } else { - point=this->transformed_input->at(ring,col); + if (this->transformed_input->width > this->transformed_input->height) + point=this->transformed_input->at(col,ring); + else + point=this->transformed_input->at(ring,col); if(!isnan(point.x) && !isnan(point.y) && !isnan(point.z)) { this->lidar_data[col*this->config_.max_ring+ring].valid=true; @@ -96,6 +99,7 @@ double LidarObstacleDetectorAlgNode::get_max_inclination(double current_angle,pc void LidarObstacleDetectorAlgNode::mainNodeThread(void) { + bool ring_ordered = false; static bool success; static std::vector<float> farthest_ranges,virtual_ranges; static double farthest_range,virtual_range; @@ -151,6 +155,8 @@ void LidarObstacleDetectorAlgNode::mainNodeThread(void) 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) { ROS_WARN("Droping pointcloud because it is not organized"); @@ -178,13 +184,13 @@ void LidarObstacleDetectorAlgNode::mainNodeThread(void) 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_.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>this->transformed_input->height) - max_col=this->transformed_input->height; + 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); @@ -219,7 +225,10 @@ void LidarObstacleDetectorAlgNode::mainNodeThread(void) prev_ring=-1; prev_ring_point_valid=true; - point=this->transformed_input->at(0,col); + 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)) { old_angle=angle; @@ -249,7 +258,7 @@ void LidarObstacleDetectorAlgNode::mainNodeThread(void) 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; + this->lidar_data[col*this->config_.max_ring+ring].col_range=col_range;//next_col???????????????? } if(prev_ring==-1 || prev_ring_point_valid) {