avoid.cc
Go to the documentation of this file.
00001 /*
00002  *  Navigator avoid obstacles in path controller
00003  *
00004  *  Copyright (C) 2007, 2010, Austin Robot Technology
00005  *
00006  *  License: Modified BSD Software License Agreement
00007  *
00008  *  $Id: avoid.cc 479 2010-08-27 01:16:11Z jack.oquin $
00009  */
00010 
00011 #include "navigator_internal.h"
00012 #include "Controller.h"
00013 #include "avoid.h"
00014 
00015 #include "lane_edge.h"
00016 #include "safety.h"
00017 
00018 Avoid::Avoid(Navigator *navptr, int _verbose):
00019   Controller(navptr, _verbose)
00020 {
00021   lane_edge = new LaneEdge(navptr, _verbose);
00022   safety = new Safety(navptr, _verbose);
00023   reset_me();
00024 };
00025 
00026 Avoid::~Avoid()
00027 {
00028   delete lane_edge;
00029   delete safety;
00030 };
00031 
00032 void Avoid::configure()
00033 {
00034   // left and right lane offsets for lane_edge controller
00035   left_offset_ratio = cf->ReadFloat(section, "left_offset_ratio", 0.8);
00036   ART_MSG(2, "\toffset ratio to left of lane is %.3f", left_offset_ratio);
00037   right_offset_ratio = cf->ReadFloat(section, "right_offset_ratio", -0.8);
00038   ART_MSG(2, "\toffset ratio to right of lane is %.3f", right_offset_ratio);
00039 
00040   lane_edge->configure(cf, section);
00041   safety->configure(cf, section);
00042 }
00043 
00044 // avoid obstacles in travel lane
00045 //
00046 // entry:
00047 //      pcmd is planned output from primary lane following controller
00048 //      incmd is a copy of the original primary controller input
00049 // exit:
00050 //      pcmd updated based on safety considerations
00051 // result:
00052 //      from safety controller
00053 //
00054 Controller::result_t Avoid::control(pilot_command_t &pcmd,
00055                                      pilot_command_t incmd)
00056 {
00057   // make copies of primary controller output
00058   pilot_command_t right_cmd = pcmd;
00059   pilot_command_t left_cmd = pcmd;
00060   result_t result = safety->control(pcmd);
00061   if (result != OK)                     // safety modified pcmd?
00062     {
00063       // Try some avoidance maneuvers.  Each reruns the safety
00064       // controller on its output before returning.
00065       result_t edge_right = lane_edge->control(right_cmd, right_offset_ratio);
00066       result_t edge_left = lane_edge->control(left_cmd, left_offset_ratio);
00067 
00068       if (std::min(edge_right, edge_left) < result)
00069         {
00070           if (edge_left < edge_right)   // left side is clearer?
00071             {
00072               if (verbose >= 2)
00073                 ART_MSG(5, "avoiding to left (%s < %s)",
00074                         result_name(edge_left), result_name(edge_right));
00075               pcmd = left_cmd;
00076               result = edge_left;
00077             }
00078           else                          // right side at least as good?
00079             {
00080               if (verbose >= 2)
00081                 ART_MSG(5, "avoiding to right (%s >= %s)",
00082                         result_name(edge_left), result_name(edge_right));
00083               pcmd = right_cmd;
00084               result = edge_right;
00085             }
00086         }
00087     }
00088 
00089   trace("avoid controller", pcmd, result);
00090   return result;
00091 };
00092 
00093 // reset all subordinate controllers
00094 void Avoid::reset(void)
00095 {
00096   trace_reset("Avoid");
00097   reset_me();
00098   lane_edge->reset();
00099   safety->reset();
00100 }
00101 
00102 // reset this controller only
00103 void Avoid::reset_me(void)
00104 {
00105 }
 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