slow_for_curves.cc
Go to the documentation of this file.
00001 /*
00002  *  Navigator "slow down for curves" controller
00003  *
00004  *  Copyright (C) 2007, Mickey Ristroph
00005  *  License: Modified BSD Software License Agreement
00006  *
00007  *  $Id: slow_for_curves.cc 872 2010-11-28 13:31:07Z jack.oquin $
00008  */
00009 
00010 #include <float.h>
00011 
00012 #include <art/epsilon.h>
00013 #include <art/steering.h>
00014 #include <art_map/coordinates.h>
00015 
00016 #include "navigator_internal.h"
00017 #include "Controller.h"
00018 #include "course.h"
00019 #include "slow_for_curves.h"
00020 
00021 Controller::result_t SlowForCurves::control(pilot_command_t &pcmd)
00022 {
00023   
00024   if (pcmd.velocity < config_->min_speed_for_curves)
00025     {
00026       ROS_DEBUG("Already going slow: %.3f", pcmd.velocity);
00027       return OK;
00028     }
00029   
00030 
00031   // These indices are checked in max_safe_speed
00032   int start_index = pops->getClosestPoly(course->plan,
00033                                          MapPose(estimate->pose.pose));
00034 
00035   // TODO: lookahead_distance should probably be time in seconds.
00036   int stop_index = pops->index_of_downstream_poly(course->plan,
00037                                                   start_index,
00038                                                   config_->lookahead_distance);
00039         
00040   float max_speed = max_safe_speed(course->plan,
00041                                    start_index,
00042                                    stop_index,
00043                                    pcmd.velocity);
00044 
00045   // Never slow down below min_speed... if the commanded velocity is larger.
00046   // This prevents a bogusly sharp turn bring the car to a complete stop.
00047   max_speed = fmaxf(config_->min_speed_for_curves, max_speed);
00048         
00049   // XXX: Scale the yawRate. Is this the right thing to do? Test.
00050   // PFB: Not needed because heading slowdown will be taken care of
00051   // later/
00052 
00053   //pcmd.yawRate = pcmd.yawRate * (max_speed/pcmd.velocity);
00054 
00055   pcmd.velocity =  max_speed;
00056         
00057   trace("slow_for_curves controller", pcmd);
00058 
00059   return OK;                            // always successful
00060 }
00061 
00062 float SlowForCurves::max_safe_speed(const std::vector<poly>& polygons,
00063                                     const int& start_index,
00064                                     const int& stop_index,
00065                                     const float& max) {
00066   //Check for bogus input
00067   if (start_index < 0
00068       || start_index >= (int)polygons.size()
00069       || stop_index < 0
00070       || stop_index >= (int)polygons.size()
00071       || start_index >= stop_index)
00072     {
00073       ROS_INFO("bogus input: start_index %d and stop_index %d",
00074                start_index, stop_index);
00075       return -1;
00076     }
00077 
00078   // MGR: I am not totally sure the logic in the function is what we
00079   // want. It is an attempt to find the highest speed we could be
00080   // travelling right now, and still be able to slow down enough
00081   // to satisfy the speed constraints for every curve ahead of us.
00082 
00083   // The dheading and corrosponding distance may be mismatched.
00084 
00085   // TODO: Try using to indexs into the array, and keeping them X meters
00086   // apart. Call the other functions on that distance and that change in
00087   // heading.
00088 
00089   if (verbose >= 4)
00090     ART_MSG(8, "slow_for_curves: searching over polygons %d(%s) to %d(%s)",
00091             polygons.at(start_index).poly_id,
00092             polygons.at(start_index).start_way.name().str,
00093             polygons.at(stop_index).poly_id,
00094             polygons.at(stop_index).start_way.name().str);
00095 
00096   float distance = 0;
00097   float max_speed = 100;
00098 
00099   char limiting_string[512] = "";
00100   int limiting_id = 0;
00101 
00102   for(int begin = start_index; begin < stop_index; begin++) {
00103     distance += (polygons.at(begin).length +
00104                  polygons.at(begin+1).length)/2.0;
00105 
00106     float length = 0;
00107     int end = begin;
00108     while(end < stop_index && length < config_->min_curve_length) {
00109       length += (polygons.at(end).length +
00110                  polygons.at(end+1).length)/2.0;
00111       end++;
00112     }
00113                 
00114     float dheading =
00115       Coordinates::normalize(polygons[end].heading - polygons[begin].heading);
00116                 
00117     float max_then =
00118       fmaxf(config_->min_speed_for_curves,
00119             course->max_speed_for_change_in_heading(dheading, length, 
00120                                                     max_speed, config_->max_yaw_rate));
00121                 
00122     float max_now =
00123       course->max_speed_for_slow_down(max_then, distance, 
00124                                       max_speed, config_->max_deceleration);
00125  
00126     if (max_now < max_speed) {
00127       limiting_id = polygons.at(begin).poly_id;
00128       sprintf(limiting_string, "Polygons: %d(%s)->%d(%s), (dheading: %.3f, length: %.3f, max_then: %.3f)   Max speed now is %.3f for curve in %.3f meters.",
00129               polygons.at(begin).poly_id,
00130               polygons.at(begin).start_way.name().str,
00131               polygons.at(end).poly_id,
00132               polygons.at(end).start_way.name().str,
00133               dheading,
00134               length,
00135               max_then,
00136               max_now,
00137               distance);
00138     }
00139 
00140     max_speed = fminf(max_speed, max_now);
00141     if (Epsilon::equal(max_speed,0.0))
00142       return 0.0;
00143   }
00144         
00145   if(verbose >= 4 && current_limiting_id != limiting_id) {
00146     ART_MSG(2, "new limiting_factor: %s", limiting_string);
00147     current_limiting_id = limiting_id;
00148   }
00149 
00150   max_speed = fminf(max_speed, max);
00151 
00152   return max_speed;
00153 }
00154 
00155 
00156 void SlowForCurves::reset(void) {
00157   trace_reset("SlowForCurves");
00158   current_limiting_id = 0;
00159 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


art_nav
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Tue Sep 24 2013 10:42:11