follow_safely.cc
Go to the documentation of this file.
00001 /*
00002  *  Navigator safe following distance controller
00003  *
00004  *  Copyright (C) 2007, 2010, Austin Robot Technology
00005  *  License: Modified BSD Software License Agreement
00006  *
00007  *  $Id: follow_safely.cc 1910 2011-12-05 23:39:28Z jack.oquin $
00008  */
00009 
00010 #include "navigator_internal.h"
00011 #include "Controller.h"
00012 #include "obstacle.h"
00013 #include "follow_safely.h"
00014 
00015 #include <art/DARPA_rules.h>
00016 
00017 FollowSafely::FollowSafely(Navigator *navptr, int _verbose):
00018   Controller(navptr, _verbose) {};
00019 
00020 FollowSafely::~FollowSafely() {};
00021 
00022 // Set desired speed for avoiding obstacles within a lane.
00023 //
00024 //  returns:
00025 //      Blocked if lane blocked;
00026 //      Collision if car approaching from ahead in this lane;
00027 //      OK otherwise.
00028 //  exit:
00029 //      sets obstacle->last_obst as a side-effect
00030 //
00031 // We may be following a moving car that could stop at any time or
00032 // already be stopped.  If it stops, stop at a safe distance and set
00033 // lane_blocked.  Higher level controllers will decide if we need to
00034 // pass.  Until told to pass, wait for the obstacle to move.
00035 //
00036 // When stopping, the front bumper must be at least one vehicle length
00037 // (DARPA_rules::min_forw_sep_travel) from the obstacle.
00038 //
00039 // Based on pseudo-code from Dr. Peter Stone.
00040 //
00041 Controller::result_t FollowSafely::control(pilot_command_t &pcmd)
00042 {
00043   bool was_blocked = navdata->lane_blocked;
00044   navdata->lane_blocked = false;
00045 
00046   result_t result = OK;
00047   art_msgs::Observation fobs =
00048     obstacle->observation(art_msgs::Observation::Nearest_forward);
00049 
00050   ROS_DEBUG("Nearest_forward: C%d A%d, dist %.3f, time %.3f, vel %.3f",
00051             fobs.clear, fobs.applicable,
00052             fobs.distance, fobs.time, fobs.velocity);
00053 
00054   if (!fobs.applicable
00055       || fobs.distance >= obstacle->maximum_range())
00056     {
00057       // no obstacle that matters, leave pcmd unmodified
00058       ROS_DEBUG("no obstacle ahead");
00059       return result;
00060     }
00061 
00062   ROS_DEBUG("obstacle is %.3f sec ahead, closing at %.3f m/s",
00063             fobs.time, fobs.velocity);
00064 
00065   // A 2 sec minimum following time at 10mph will cause us to brake
00066   // hard when still about two car lengths away (~9m).  One length is
00067   // the minimum distance allowed by the DARPA rules.
00068   if ((fobs.time <= config_->min_following_time)
00069       || (fobs.distance <= config_->close_stopping_distance))
00070     {
00071       // be safe, request immediate stop
00072       pcmd.velocity = 0.0;
00073       ROS_DEBUG("Obstacle avoidance requesting immediate halt");
00074 
00075       // when fully stopped, initiate blocked lane behavior
00076       // (may already be doing it)
00077       if (navdata->stopped)
00078         {
00079           navdata->lane_blocked = true;
00080           if (!was_blocked)
00081             {
00082               // flag not already set, new obstacle
00083               ROS_INFO("New obstacle detected!");
00084             }
00085           result = Blocked;
00086         }
00087     }
00088   else if (fobs.time < config_->desired_following_time)
00089     {
00090       adjust_speed(pcmd, fobs.distance); // speed up a bit
00091     }
00092   else if (nav->navdata.stopped
00093            || (fobs.time > config_->desired_following_time))
00094     {
00095       adjust_speed(pcmd, fobs.distance); // slow down a bit
00096     }
00097 
00098   // The multiple calls to adjust_speed() above could be tightened
00099   // up some, but they help make the logic clearer.
00100 
00101   // See if there is anyone coming towards us ahead in this lane.
00102   if (obstacle->car_approaching())
00103     {
00104       ROS_ERROR("Possible collision ahead!");
00105       result = Collision;
00106     }
00107 
00108   trace("follow_safely controller", pcmd, result);
00109   return result;
00110 };
00111 
00112 
00113 // private methods
00114 
00115 // adjust speed to maintain a safe following distance
00116 void FollowSafely::adjust_speed(pilot_command_t &pcmd, float obs_dist)
00117 {
00118   // try to adjust speed to achieve desired following time, obeying
00119   // the speed limit (ignore order->min_speed when following)
00120   float adjusted_speed = fminf(obs_dist / config_->desired_following_time,
00121                                order->max_speed);
00122 
00123   // at any rate, do not go faster than the desired speed already set
00124   pcmd.velocity = fminf(pcmd.velocity, adjusted_speed);
00125 }
 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