kurt-freespace.cc
Go to the documentation of this file.
00001 #include <math.h>
00002 #include <stdio.h>
00003 #include <ros/ros.h>
00004 #include <geometry_msgs/Twist.h>
00005 #include <sensor_msgs/LaserScan.h>
00006 
00007 #define INVPIDIV180          57.29578               /*   180 / pi   */
00008 #define EPSILON              0.000001               // vergleich von 2 doubles
00009 #define min(a,b) ((a)<(b)?(a):(b))
00010 
00011 double max_vel_x, max_rotational_vel;
00012 ros::Publisher vel_pub;
00013 
00014 double calc_freespace(const sensor_msgs::LaserScan::ConstPtr &laser)
00015 {
00016   unsigned int i;
00017   double phi;
00018   double sinsum = 0.0, cossum = 0.0;
00019   double orientation_weight;
00020   double alpha = 0.0;
00021 
00022   //testen
00023 
00024   // printf("*****************\n");
00025   for (i = 0; i < laser->ranges.size(); i++) {
00026     // -M_PI/2.0 fuer die verschiebung auf -90 bis 90
00027     // macht in der summe +M_PI/2.0#
00028 
00029     // phi = (double)i / (laser->ranges.size() - 1) * M_PI - M_PI/2.0;
00030     phi = laser->angle_min + i * laser->angle_increment;
00031 
00032     //printf("%d %f \n", i, phi);
00033     // cos(phi/2) gewichtet die werte nach vorne maximal und am
00034     // rand entsprechend weniger (hier 0)
00035     // exp term fuer grosse distance ungefaehr 0 d.h. keine einfluss
00036 
00037     orientation_weight = cos(phi/1.2) * 1.0/(1.0+exp(-(laser->ranges[i]-3.0)/0.3));
00038     // kann aber nicht oben berechnet werden wegen scalierung von obigen wert
00039     // printf("%d %lf %f %f\n",i,laser->ranges[i], INVPIDIV180 * phi, orientation_weight);
00040     sinsum += sin(phi) * orientation_weight;
00041     cossum += cos(phi) * orientation_weight;
00042   }
00043   // printf("*****************\n");
00044 
00045   if (fabs(sinsum) > EPSILON) alpha = atan2(sinsum, cossum);
00046 
00047   // printf("a %f \n",INVPIDIV180 * alpha);
00048   // hier noch checken ob was null wird
00049   // if (fabs(Back_sinsum) > EPSILON) alpha += atan2(Back_sinsum,Back_cossum);
00050   /*
00051   printf("alpha %f sinsum %f cossum %f \n",
00052       INVPIDIV180 * alpha,
00053       sinsum,
00054       cossum);
00055   */
00056   return alpha;
00057 }
00058 
00059 int SICK_Check_range(const sensor_msgs::LaserScan::ConstPtr &laser,
00060     double xregion, double yregion,
00061     int *index_to_obstacle, double *distance_to_obstacle)
00062 {
00063   unsigned int i;
00064   double general_distance = 65536.0;
00065   *distance_to_obstacle = 65536.0;
00066   *index_to_obstacle = -1;
00067   //for (i = 0; i < laser->ranges.size(); i++) {
00068   for (i = 10; i < (laser->ranges.size() - 10); i++) {
00069     // new wegen der aufhaengung muss ein minimal wert ueberschritten sein
00070     // cout<<"sickscanner check range laser->ranges.size(): "<<laser->ranges.size()<<" distance "<<*distance<<" x["<<i<<"] "<<x[i]<<" y["<<i<<"] "<<y[i] <<" xregion "<<xregion<<" yregion "<<yregion<<endl;
00071     double x = laser->ranges[i] * sin(laser->angle_min + i * laser->angle_increment);
00072     double y = laser->ranges[i] * cos(laser->angle_min + i * laser->angle_increment);
00073     if (laser->ranges[i] > 0.1) { // damit wir auch nicht nur uns sehen
00074       if ((fabs(x) < xregion) && (y < yregion)) { // hier muessen wir uns etwas merken
00075         if (*distance_to_obstacle > y) {
00076           *distance_to_obstacle = y;
00077           *index_to_obstacle = i;
00078         }
00079       }
00080       if ((fabs(x) < xregion)) { // hier muessen wir uns etwas merken
00081         if (general_distance > y) {
00082           general_distance = y;
00083         }
00084       }
00085     }
00086   }
00087   if (*index_to_obstacle == -1) *distance_to_obstacle = general_distance;
00088   return 0;
00089 }
00090 
00091 void autonomous_behave(const sensor_msgs::LaserScan::ConstPtr &laser)
00092 {
00093 
00094   int state_turn = 0;
00095   double XRegion = 0.25; //25.0
00096   double ZRegion = 0.56; // 56.0
00097   double DTOStopTurning = 0.60; //60.0
00098   double DTOStartTurning = 0.50; //50.0
00099   int index_to_obstacle = -1;
00100   double Front_Max_Distance = -1; //maximaler laserstrahl
00101   double turn_omega = 0.0;
00102   double u_turning = 0.2 * max_vel_x;
00103   double distance_to_obstacle = INFINITY;
00104   double omega = calc_freespace(laser);
00105   double u = max_vel_x;
00106   for(unsigned int i = 0; i < laser->ranges.size(); i++) {
00107     if(laser->ranges[i] > Front_Max_Distance) {
00108       Front_Max_Distance = laser->ranges[i];
00109     }
00110   }
00111   //cout<<" autonomous behave omega: "<<omega<<" max_vel_x "<<max_vel_x<<endl;
00112   // check if obstacle in front (xregion: width, yregion: length)
00113   // virtual roadway
00114   SICK_Check_range(laser,
00115       XRegion,
00116       ZRegion,
00117       &index_to_obstacle,
00118       &distance_to_obstacle);
00119 
00120   // slow down if obstacle
00121   if (index_to_obstacle != -1) {
00122     //cout<<" autonomous obstacle"<<endl;
00123     u = distance_to_obstacle / ZRegion * max_vel_x;
00124   }
00125 
00126   // handle turning mode
00127   if (state_turn == 1) {
00128     //cout<<" autonomous state turn"<<endl;
00129     if ( (distance_to_obstacle > DTOStopTurning)
00130         // && (fabs(omega) < M_PI*10.0/180.0) )
00131       ) {
00132         state_turn = 0;                 // end turning
00133         //status->AntiWindup = 0.0;               // reset pid integration
00134       } else {
00135         omega = turn_omega;             // turn
00136         u = min(u, u_turning);
00137       }
00138   }
00139   // detect end of corridor use StopTurn val
00140   if (((Front_Max_Distance < DTOStopTurning)
00141         || (distance_to_obstacle < DTOStartTurning))
00142       && (state_turn == 0) ) {
00143     //cout<<" autonomous end of corridor FMD "<<Front_Max_Distance<<endl;
00144     // start turning
00145     // turn away from obstacle
00146     if (omega > 0.0) {
00147       omega = 0.999 * M_PI;
00148     }
00149     if (omega < 0.0) {
00150       omega = -0.999 * M_PI;
00151     }
00152     state_turn = 1;
00153     turn_omega = omega;
00154     u = min(u, u_turning);
00155     //status->AntiWindup = 0.0;                 // reset pid integration
00156   }
00157 
00158   //cout << u << " " << omega << " " << state_turn << " " << distance_to_obstacle << endl;
00159 
00160   geometry_msgs::Twist vel;
00161   vel.linear.x = u;
00162   vel.angular.z = omega;
00163   vel_pub.publish(vel);
00164 }
00165 
00166 int main(int argc, char** argv)
00167 {
00168   ros::init(argc, argv, "kurt_freespace");
00169 
00170   ros::NodeHandle nh;
00171   ros::NodeHandle nh_ns("~");
00172 
00173   nh_ns.param("max_vel_x", max_vel_x, 0.9);
00174   nh_ns.param("max_rotational_vel", max_rotational_vel, 1.0);
00175 
00176   vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
00177   ros::Subscriber laser_sub = nh.subscribe("scan", 10, autonomous_behave);
00178 
00179   ros::spin();
00180 }


kurt_freespace
Author(s): Jochen Sprickerhof
autogenerated on Wed Apr 2 2014 22:19:51