gapter: LaserScanner.cpp Source File

Go to the documentation of this file.
00001 #include <stdio.h>
00002 #include <stdlib.h>
00003 #include <unistd.h>
00004 #include <string.h>
00005 #include <fstream>
00006 #include <iostream>
00007 #include <iomanip>
00008 #include "LaserScanner.h"
00009 
00010 using namespace std;
00011 
00012 
00013 double LaserScanner::Index2Angle(sensor_msgs::LaserScan laser_scan_msg, int index){
00014         return (laser_scan_msg.angle_min + (index*laser_scan_msg.angle_increment));
00015         //return -(LaserScanMsg.angle_min+(LaserScanMsg.angle_increment*getIndexOfMaximumRange(LaserScanMsg)));
00016 }
00017 
00018 int LaserScanner::Angle2Index(sensor_msgs::LaserScan laser_scan_msg, double angle){
00019         return (int)((angle-laser_scan_msg.angle_min)/laser_scan_msg.angle_increment);
00020 }
00021 
00022 double LaserScanner::getAverageRange(sensor_msgs::LaserScan & laser_scan_msg, int start_index, int end_index){
00023   double avg = 0;
00024   
00025   for(int i = start_index; i < end_index;i++){
00026     if(!std::isnan(laser_scan_msg.ranges[i]))
00027       avg = avg + laser_scan_msg.ranges[i];
00028   }
00029     
00030   avg = avg / (end_index - start_index + 1);
00031   
00032   return avg; 
00033   
00034 }
00035 
00036 double LaserScanner::getAverageRangeRight(sensor_msgs::LaserScan & laser_scan_msg, double degree){
00037   int start_index = 0;
00038   int end_index = (int)(degree/radian2degree(laser_scan_msg.angle_increment));
00039   //cout<<"start_index RIGHT "<<start_index<<endl;
00040   //cout<<"end_index RIGHT"<<end_index<<endl;
00041   
00042   return getAverageRange(laser_scan_msg, start_index, end_index);
00043   
00044   
00045 }
00046 
00047 double LaserScanner::getAverageRangeLeft(sensor_msgs::LaserScan & laser_scan_msg, double degree){
00048   int end_index = laser_scan_msg.ranges.size();
00049   int start_index = end_index- (int)(degree/radian2degree(laser_scan_msg.angle_increment));
00050   //cout<<"start_index LEFT "<<start_index<<endl;
00051   //cout<<"end_index LEFT"<<end_index<<endl;
00052   return getAverageRange(laser_scan_msg, start_index, end_index);
00053   
00054   
00055 }
00056 
00057 double LaserScanner::getAverageRangeStraight(sensor_msgs::LaserScan & laser_scan_msg, double degree){
00058   int start_index =(int)(laser_scan_msg.ranges.size()/2)-(int)((degree/radian2degree(laser_scan_msg.angle_increment))/2);
00059   int end_index = (int)(laser_scan_msg.ranges.size()/2)+(int)((degree/radian2degree(laser_scan_msg.angle_increment))/2);
00060   //cout<<"start_index "<<start_index<<endl;
00061   //cout<<"end_index "<<end_index<<endl;
00062   return getAverageRange(laser_scan_msg, start_index, end_index);
00063   
00064   
00065 }
00066 
00067 
00068 
00069 double LaserScanner::getMinimumRange(sensor_msgs::LaserScan & LaserScanMsg, int start_index, int end_index){
00070   //get initial value of max_index for the first valid range
00071   int min_index = 0;
00072   for(int i = start_index; i < end_index ; i++){
00073 if(!std::isnan(LaserScanMsg.ranges[i])){
00074     if((LaserScanMsg.ranges[i]>= LaserScanMsg.range_min) && (LaserScanMsg.ranges[i]<= LaserScanMsg.range_max)){
00075       min_index = i;
00076       break;
00077     }
00078 }
00079   }
00080   //look for max_index
00081   for(int i = min_index+1; i < end_index ; i++){
00082 if(!std::isnan(LaserScanMsg.ranges[i])){
00083     if((LaserScanMsg.ranges[i]>= LaserScanMsg.range_min) && (LaserScanMsg.ranges[i]<= LaserScanMsg.range_max)){
00084       if (LaserScanMsg.ranges[min_index]>LaserScanMsg.ranges[i]){
00085         //std::cout<<"max_index changed "<<i<<" new range: "<<LaserScanMsg.ranges[i]<<std::endl;  
00086         min_index=i;
00087       }
00088       
00089     }
00090 }
00091   } 
00092   return LaserScanMsg.ranges[min_index]; 
00093   
00094 }
00095 
00096 
00097 double LaserScanner::getMinimumRangeRight(sensor_msgs::LaserScan & laser_scan_msg, double degree){
00098   int start_index = 0;
00099   int end_index = (int)(degree/radian2degree(laser_scan_msg.angle_increment));
00100   //cout<<"start_index RIGHT "<<start_index<<endl;
00101   //cout<<"end_index RIGHT"<<end_index<<endl;
00102   
00103   return getMinimumRange(laser_scan_msg, start_index, end_index);
00104   
00105 }
00106 
00107 
00108 double LaserScanner::getMinimumRangeLeft(sensor_msgs::LaserScan & laser_scan_msg, double degree){
00109   int end_index = laser_scan_msg.ranges.size();
00110   int start_index = end_index- (int)(degree/radian2degree(laser_scan_msg.angle_increment));
00111   //cout<<"start_index LEFT "<<start_index<<endl;
00112   //cout<<"end_index LEFT"<<end_index<<endl;
00113   return getMinimumRange(laser_scan_msg, start_index, end_index);
00114   
00115   
00116 }
00117 
00118 
00119 double LaserScanner::getMaximumRange(sensor_msgs::LaserScan & LaserScanMsg, int start_index, int end_index){
00120   //get initial value of max_index for the first valid range
00121   int max_index = 0;
00122   for(int i = start_index; i < end_index ; i++){
00123     if((LaserScanMsg.ranges[i]>= LaserScanMsg.range_min) && (LaserScanMsg.ranges[i]<= LaserScanMsg.range_max)){
00124       max_index = i;
00125       break;
00126     }
00127   }
00128   //look for max_index
00129   for(int i = max_index+1; i < end_index ; i++){
00130     if((LaserScanMsg.ranges[i]>= LaserScanMsg.range_min) && (LaserScanMsg.ranges[i]<= LaserScanMsg.range_max)){
00131       if (LaserScanMsg.ranges[max_index]<LaserScanMsg.ranges[i]){
00132         //std::cout<<"max_index changed "<<i<<" new range: "<<LaserScanMsg.ranges[i]<<std::endl;  
00133         max_index=i;
00134       }
00135       
00136     }
00137   } 
00138   return LaserScanMsg.ranges[max_index]; 
00139   
00140 }
00141 
00142 double LaserScanner::getMaximumRangeRight(sensor_msgs::LaserScan & laser_scan_msg, double degree){
00143   int start_index = 0;
00144   int end_index = (int)(degree/radian2degree(laser_scan_msg.angle_increment));
00145   //cout<<"start_index RIGHT "<<start_index<<endl;
00146   //cout<<"end_index RIGHT"<<end_index<<endl;
00147   
00148   return getMaximumRange(laser_scan_msg, start_index, end_index);
00149   
00150 }
00151 
00152 
00153 double LaserScanner::getMaximumRangeLeft(sensor_msgs::LaserScan & laser_scan_msg, double degree){
00154   int end_index = laser_scan_msg.ranges.size();
00155   int start_index = end_index- (int)(degree/radian2degree(laser_scan_msg.angle_increment));
00156   //cout<<"start_index LEFT "<<start_index<<endl;
00157   //cout<<"end_index LEFT"<<end_index<<endl;
00158   return getMaximumRange(laser_scan_msg, start_index, end_index);
00159   
00160   
00161 }
00162 
00163 
00164 
00165 
00171 int LaserScanner::getIndexOfMaximumRange(sensor_msgs::LaserScan & LaserScanMsg){
00172   
00173   //get initial value of max_index for the first valid range
00174   int max_index = 0;
00175   for(int i = 0; i < LaserScanMsg.ranges.size() ; i++){
00176     if((LaserScanMsg.ranges[i]>= LaserScanMsg.range_min) && (LaserScanMsg.ranges[i]<= LaserScanMsg.range_max)){
00177       max_index = i;
00178       break;
00179     }
00180   }
00181   //look for max_index
00182   for(int i = max_index+1; i < LaserScanMsg.ranges.size() ; i++){
00183     if((LaserScanMsg.ranges[i]>= LaserScanMsg.range_min) && (LaserScanMsg.ranges[i]<= LaserScanMsg.range_max)){
00184       if (LaserScanMsg.ranges[max_index]<LaserScanMsg.ranges[i]){
00185         //std::cout<<"max_index changed "<<i<<" new range: "<<LaserScanMsg.ranges[i]<<std::endl;  
00186         max_index=i;
00187       }
00188       
00189     }
00190   } 
00191   return max_index;
00192 }
00193 
00199 int LaserScanner::getIndexOfMinimumRange(sensor_msgs::LaserScan & LaserScanMsg){
00200   
00201   //get initial value of max_index for the first valid range
00202   int min_index = 0;
00203   for(int i = 0; i < LaserScanMsg.ranges.size() ; i++){
00204     if((LaserScanMsg.ranges[i]>= LaserScanMsg.range_min) && (LaserScanMsg.ranges[i]<= LaserScanMsg.range_max)){
00205       min_index = i;
00206       break;
00207     }
00208   }
00209   //look for max_index
00210   for(int i = min_index+1; i < LaserScanMsg.ranges.size() ; i++){
00211     if((LaserScanMsg.ranges[i]>= LaserScanMsg.range_min) && (LaserScanMsg.ranges[i]<= LaserScanMsg.range_max)){
00212       if (LaserScanMsg.ranges[min_index]>LaserScanMsg.ranges[i]){
00213         //std::cout<<"max_index changed "<<i<<" new range: "<<LaserScanMsg.ranges[i]<<std::endl;  
00214         min_index=i;
00215       }
00216       
00217     }
00218   } 
00219   return min_index;
00220 }
00221 
00228 double LaserScanner::getMaximumRange(sensor_msgs::LaserScan & LaserScanMsg){
00229   
00230   return LaserScanMsg.ranges[getIndexOfMaximumRange(LaserScanMsg)]; 
00231 }
00232 
00239 double LaserScanner::getMinimumRange(sensor_msgs::LaserScan & LaserScanMsg){
00240   
00241   return LaserScanMsg.ranges[getIndexOfMinimumRange(LaserScanMsg)];
00242 }
00243 
00244 double  LaserScanner::getFrontRange(sensor_msgs::LaserScan & LaserScanMsg){
00245   
00246   return LaserScanMsg.ranges[LaserScanMsg.ranges.size()/2];
00247 }
00248 
00249 
00255 double LaserScanner::getRelativeAngleOfMaximumRange(sensor_msgs::LaserScan & LaserScanMsg){
00256   //we add minus to follow the rule of clockwise direction
00257   return -(LaserScanMsg.angle_min+(LaserScanMsg.angle_increment*getIndexOfMaximumRange(LaserScanMsg)));
00258 }
00259 
00265 double LaserScanner::getRelativeAngleOfMinimumRange(sensor_msgs::LaserScan & LaserScanMsg){
00266   //we add minus to follow the rule of clockwise direction
00267   return -(LaserScanMsg.angle_min+(LaserScanMsg.angle_increment*getIndexOfMinimumRange(LaserScanMsg)));
00268 }
00269 
00275 bool LaserScanner::isObstacleTooClose(sensor_msgs::LaserScan & LaserMsg, int start_index, int end_index, double DistanceThreshold){
00276   bool result=false;
00277   //for front obstacle choose start_index = 260 and end_index=380
00278   if (getMinimumRange(LaserMsg, start_index, end_index)< DistanceThreshold)
00279     result=true;
00280   
00281   return result;
00282 }
00283 
00290 void LaserScanner::printLaserScanRanges(sensor_msgs::LaserScan::ConstPtr LaserScanMsg)
00291 {
00293   cout<<endl;
00294   cout << "-- LASER SCAN INFO --"<<endl; 
00295   cout << "Header Info (std_msgs/Header.msg) "<<endl; 
00296   cout << setw(20)<< "Sequence: " << LaserScanMsg->header.seq <<endl; //sequence ID: consecutively increasing ID
00297   //Two-integer timestamp that is expressed as:
00298   // * stamp.sec: seconds (stamp_secs) since epoch, 
00299   // * stamp.nsec: nanoseconds since stamp_secs
00300   // * time-handling sugar is provided by the client library
00301   cout << setw(20)<< "Stamp: " << LaserScanMsg->header.stamp <<endl;
00302   //Frame this data is associated with 0: no frame, 1: global frame
00303   cout << setw(20)<< "Frame_ID: " << LaserScanMsg->header.frame_id <<endl<<endl;
00304   
00306   cout << "LaserScan Info (sensor_msgs/LaserScan.msg) "<<endl; 
00307   cout << setw(20)<<"Size: " << LaserScanMsg->ranges.size() <<endl;
00308   cout << setw(20)<< "Angle increment: "<<"[" <<LaserScanMsg->angle_increment <<" rad, " <<radian2degree(LaserScanMsg->angle_increment) <<" deg]"<<endl; // angular distance between measurements [rad]
00309   cout << setw(20)<< "Minimum angle: " <<"["<<LaserScanMsg->angle_min <<" rad, " <<radian2degree(LaserScanMsg->angle_min) <<" deg]"<<endl; //start angle of the scan [rad]
00310   cout << setw(20)<< "Maximum angle: " <<"["<<LaserScanMsg->angle_max<<" rad, " <<radian2degree(LaserScanMsg->angle_max) <<" deg]" <<endl; //end angle of the scan [rad]
00311   cout << setw(20)<< "Minumum range: " <<LaserScanMsg->range_min <<" m"<<endl; //minimum range value [m]
00312   cout << setw(20)<< "Maximum range: " <<LaserScanMsg->range_max <<" m"<<endl; //maximum range value [m]
00313   cout << setw(20)<< "Scan time: " <<LaserScanMsg->scan_time <<" seconds"<<endl; //time between scans [seconds]
00314   cout << setw(20)<< "Time increment: " <<LaserScanMsg->scan_time <<" seconds"<<endl<<endl; //time between measurements [seconds] - if your scanner is moving, this will be used in interpolating position of 3d points
00315   
00317   cout << setw(20)<< "LaserScan Ranges"<<endl; 
00318   //cout << setw(20)<< "Minimum range [range: "<<getMinimumRange(LaserScanMsg)<<", angle: " <<radian2degree(getRelativeAngleOfMinimumRange(LaserScanMsg))<<", index: " <<getIndexOfMinimumRange(LaserScanMsg)<<"]" <<endl;
00319   //cout << setw(20)<< "Maximum range [range: "<<getMaximumRange(LaserScanMsg)<<", angle: " <<radian2degree(getRelativeAngleOfMaximumRange(LaserScanMsg))<<" , index: " <<getIndexOfMaximumRange(LaserScanMsg)<<"]" <<endl;
00320   //cout << setw(20)<< "Front range: " << getFrontRange(LaserScanMsg) <<endl;
00321   cout << "---"<<endl<<endl; 
00322   
00323 }
00324 
00325 void LaserScanner::printLaserScanRanges(sensor_msgs::LaserScan LaserScanMsg)
00326 {
00328   cout<<endl;
00329   cout << "-- LASER SCAN INFO --"<<endl;
00330   cout << "Header Info (std_msgs/Header.msg) "<<endl;
00331   cout << setw(20)<< "Sequence: " << LaserScanMsg.header.seq <<endl; //sequence ID: consecutively increasing ID
00332   //Two-integer timestamp that is expressed as:
00333   // * stamp.sec: seconds (stamp_secs) since epoch,
00334   // * stamp.nsec: nanoseconds since stamp_secs
00335   // * time-handling sugar is provided by the client library
00336   cout << setw(20)<< "Stamp: " << LaserScanMsg.header.stamp <<endl;
00337   //Frame this data is associated with 0: no frame, 1: global frame
00338   cout << setw(20)<< "Frame_ID: " << LaserScanMsg.header.frame_id <<endl<<endl;
00339 
00341   cout << "LaserScan Info (sensor_msgs/LaserScan.msg) "<<endl;
00342   cout << setw(20)<<"Size: " << LaserScanMsg.ranges.size() <<endl;
00343   cout << setw(20)<< "Angle increment: "<<"[" <<LaserScanMsg.angle_increment <<" rad, " <<radian2degree(LaserScanMsg.angle_increment) <<" deg]"<<endl; // angular distance between measurements [rad]
00344   cout << setw(20)<< "Minimum angle: " <<"["<<LaserScanMsg.angle_min <<" rad, " <<radian2degree(LaserScanMsg.angle_min) <<" deg]"<<endl; //start angle of the scan [rad]
00345   cout << setw(20)<< "Maximum angle: " <<"["<<LaserScanMsg.angle_max<<" rad, " <<radian2degree(LaserScanMsg.angle_max) <<" deg]" <<endl; //end angle of the scan [rad]
00346   cout << setw(20)<< "Minumum range: " <<LaserScanMsg.range_min <<" m"<<endl; //minimum range value [m]
00347   cout << setw(20)<< "Maximum range: " <<LaserScanMsg.range_max <<" m"<<endl; //maximum range value [m]
00348   cout << setw(20)<< "Scan time: " <<LaserScanMsg.scan_time <<" seconds"<<endl; //time between scans [seconds]
00349   cout << setw(20)<< "Time increment: " <<LaserScanMsg.scan_time <<" seconds"<<endl<<endl; //time between measurements [seconds] - if your scanner is moving, this will be used in interpolating position of 3d points
00350 
00352   cout << setw(20)<< "LaserScan Ranges"<<endl;
00353   cout << setw(20)<< "Minimum range [range: "<<getMinimumRange(LaserScanMsg)<<", angle: " <<radian2degree(getRelativeAngleOfMinimumRange(LaserScanMsg))<<", index: " <<getIndexOfMinimumRange(LaserScanMsg)<<"]" <<endl;
00354   cout << setw(20)<< "Maximum range [range: "<<getMaximumRange(LaserScanMsg)<<", angle: " <<radian2degree(getRelativeAngleOfMaximumRange(LaserScanMsg))<<" , index: " <<getIndexOfMaximumRange(LaserScanMsg)<<"]" <<endl;
00355   cout << setw(20)<< "Front range: " << getFrontRange(LaserScanMsg) <<endl;
00356   cout << "---"<<endl<<endl;
00357 
00358 }