art_nav: art_nav: zone.cc Source File

00001 /*
00002  *  Navigator zone controller
00003  *
00004  *  Copyright (C) 2007, 2010, Austin Robot Technology
00005  *
00006  *  License: Modified BSD Software License Agreement
00007  *
00008  *  $Id: zone.cc 479 2010-08-27 01:16:11Z jack.oquin $
00009  */
00010 
00011 #include "navigator_internal.h"
00012 #include "Controller.h"
00013 #include "course.h"
00014 #include "safety.h"
00015 #include "zone.h"
00016 
00017 Zone::Zone(Navigator *navptr, int _verbose):
00018   Controller(navptr, _verbose)
00019 {
00020   safety = new Safety(navptr, _verbose);
00021 }
00022 
00023 Zone::~Zone()
00024 {
00025   delete safety;
00026 }
00027 
00028 // configuration method
00029 void Zone::configure()
00030 {
00031   // maximum speed when inside a zone
00032   zone_speed_limit = cf->ReadFloat(section, "zone_speed_limit", 3.0);
00033   ART_MSG(2, "\tspeed limit in a zone is %.1f m/s", zone_speed_limit);
00034 
00035   safety->configure(cf, section);
00036 }
00037 
00038 // go to next zone way-point
00039 //
00040 // returns: OK.
00041 //
00042 Controller::result_t Zone::control(pilot_command_t &pcmd)
00043 {
00044   result_t result = OK;
00045 
00046   if (verbose >= 2)
00047     {
00048       ART_MSG(5, "Go to zone waypoint %s",
00049               order->waypt[1].id.name().str);
00050     }
00051 
00052   // limit speed inside zones
00053   pcmd.velocity = fminf(pcmd.velocity, zone_speed_limit);
00054 
00055   // return desired heading for this cycle
00056   set_heading(pcmd);
00057 
00058   // check off way-point, if reached
00059   course->zone_waypoint_reached();
00060 
00061   // must always run this on the final command
00062   result = safety->control(pcmd);
00063 
00064   // TODO: implement obstacle evasion if Unsafe or Blocked
00065 
00066   trace("zone controller", pcmd, result);
00067   return result;
00068 }
00069 
00070 // reset all subordinate controllers
00071 void Zone::reset(void)
00072 {
00073   trace_reset("Zone");
00074   safety->reset();
00075 }
00076 
00077 // set heading to next way-point
00078 void Zone::set_heading(pilot_command_t &pcmd)
00079 {
00080   // STUB: this is just a place-holder that steers directly for the
00081   // next way-point.  The real version will use parking controller.
00082 
00083   // egocentric polar aim point
00084   Polar aim_polar =
00085     Coordinates::MapXY_to_Polar(order->waypt[1].map, estimate->pos);
00086 
00087   if (verbose >= 3)
00088     {
00089       ART_MSG(8, "target, current positions: (%.3f, %.3f), (%.3f, %.3f)",
00090               order->waypt[1].map.x, order->waypt[1].map.y,
00091               estimate->pos.px, estimate->pos.py);
00092       ART_MSG(8, "relative heading: %.3f radians, distance: %.3f meters",
00093               aim_polar.heading, aim_polar.range);
00094     }
00095 
00096   pcmd.yawRate = aim_polar.heading/fmax(1.0,estimate->vel.px/2);
00097   trace("zone set_heading", pcmd);
00098 }