navigator.cc
Go to the documentation of this file.
00001 /*
00002  *  Navigator class interface
00003  *
00004  *  Copyright (C) 2007, 2010, Austin Robot Technology
00005  *
00006  *  License: Modified BSD Software License Agreement
00007  *
00008  *  $Id: navigator.cc 872 2010-11-28 13:31:07Z jack.oquin $
00009  */
00010 
00011 #include <assert.h>
00012 //#include <art/DARPA_rules.h>
00013 
00014 #include "navigator_internal.h"
00015 #include "course.h"
00016 
00017 #include "obstacle.h"
00018 
00019 // subordinate controller classes
00020 #include "estop.h"
00021 
00031 Navigator::Navigator(nav_msgs::Odometry *odom_msg)
00032 {
00033   odometry = odom_msg;
00034 
00035   order.behavior.value = art_msgs::Behavior::Pause; // initial order
00036 
00037   // set navigator state flags to false, current polygon none
00038   navdata.cur_poly = -1;
00039   navdata.lane_blocked = false;
00040   navdata.stopped = false;
00041   navdata.road_blocked = false;
00042   navdata.reverse = false;
00043   navdata.have_zones = false;
00044   art_msgs::MapID null_waypt = ElementID().toMapID();
00045   navdata.last_waypt = null_waypt;
00046   navdata.replan_waypt = null_waypt;
00047 
00048   // allocate helper classes
00049   pops = new PolyOps();
00050   course = new Course(this, verbose);
00051   obstacle = new Obstacle(this, verbose);
00052 
00053   // allocate controller classes
00054   estop = new Estop(this, verbose);
00055 };
00056 
00057 Navigator::~Navigator()
00058 {
00059   // controller classes
00060   delete estop;
00061 
00062   // free helper classes
00063   delete obstacle;
00064   delete course; 
00065   delete pops;
00066 };
00067 
00075 pilot_command_t Navigator::navigate(void)
00076 {
00077   pilot_command_t pcmd;                 // pilot command to return
00078 
00079   // report whether odometry reports vehicle currently stopped
00080   navdata.stopped = (fabsf(odometry->twist.twist.linear.x)
00081                      < Epsilon::speed);
00082 
00083   // run top-level (E-stop) state machine controller
00084   estop->control(pcmd);
00085 
00086   // copy last commander order to navigator state message -- it may
00087   // have been updated due to way-points passed
00088   navdata.last_order = order;
00089 
00090   return pcmd;
00091 }
00092 
00094 void Navigator::configure()
00095 {
00096   course->configure();
00097 }
00098 
 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