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