navigator_internal.h
Go to the documentation of this file.
00001 /* -*- mode: C++ -*-
00002  *
00003  *  Navigator class interface
00004  *
00005  *  Copyright (C) 2007, 2010, Austin Robot Technology
00006  *
00007  *  License: Modified BSD Software License Agreement
00008  *
00009  *  $Id: navigator_internal.h 1629 2011-08-10 01:58:40Z jack.oquin $
00010  */
00011 
00012 #ifndef __NAVIGATOR_INTERNAL_H__
00013 #define __NAVIGATOR_INTERNAL_H__
00014 
00015 #include <cstdlib>
00016 #include <iostream>
00017 #include <fstream>
00018 #include <string>
00019 #include <vector>
00020 #include <math.h>
00021 
00022 #include <ros/ros.h>
00023 
00024 #include <art/epsilon.h>
00025 #include <art/error.h>
00026 #include <art/infinity.h>
00027 
00028 #include <art_msgs/ArtHertz.h>
00029 #include <art_map/euclidean_distance.h>
00030 #include <art_map/PolyOps.h>
00031 
00032 #include <art_msgs/Behavior.h>
00033 #include <art_msgs/NavigatorCommand.h>
00034 #include <art_msgs/NavigatorState.h>
00035 #include <art_msgs/ObservationArray.h>
00036 #include <nav_msgs/Odometry.h>
00037 
00038 #include <art_nav/NavBehavior.h>
00039 
00040 #include "art_nav/NavigatorConfig.h"
00041 typedef art_nav::NavigatorConfig Config;
00042 
00043 // Provide short names for some messages so they can more easily be
00044 // packaged differently.
00045 using art_msgs::Behavior;
00046 using art_msgs::NavigatorCommand;
00047 using art_msgs::NavigatorState;
00048 using art_msgs::Observation;
00049 using art_msgs::ObservationArray;
00050 using nav_msgs::Odometry;
00051 using art_msgs::Order;
00052 
00053 typedef struct
00054 {
00055   float velocity;
00056   float yawRate;
00057 } pilot_command_t;
00058 
00059 // forward class declarations
00060 class Estop;
00061 class Course;
00062 class Obstacle;
00063 
00064 class Navigator
00065 {
00066 public:
00067 
00068   // helper classes
00069   PolyOps* pops;                        // polygon operations class
00070   Course* course;                       // course planning class
00071   Obstacle* obstacle;                   // obstacle class
00072 
00073   // subordinate controllers
00074   Estop *estop;
00075 
00076   // public data used by controllers
00077   art_msgs::Order order;               // current commander order
00078   art_msgs::NavigatorState navdata;    // current navigator state data
00079   nav_msgs::Odometry estimate;         // estimated control position
00080   nav_msgs::Odometry *odometry;
00081 
00082   // public methods
00083   Navigator(nav_msgs::Odometry *odom_msg);
00084   ~Navigator();
00085 
00086   // configure parameters
00087   void configure();
00088 
00089   Config config_;                       // configuration parameters
00090 
00091   // decrease pilot command velocity, obeying min_speed
00092   void reduce_speed_with_min(pilot_command_t &pcmd, float new_speed)
00093   {
00094     pcmd.velocity = fminf(pcmd.velocity,
00095                           fmaxf(order.min_speed, new_speed));
00096   }
00097 
00098   // main navigator entry point -- called once every cycle
00099   pilot_command_t navigate(void);
00100 
00101   // trace controller state
00102   void trace_controller(const char *name, pilot_command_t &pcmd)
00103   {
00104     if (verbose >= 4)
00105       ART_MSG(7, "%s: pcmd = (%.3f, %.3f) ",
00106               name, pcmd.velocity, pcmd.yawRate);
00107   }
00108 
00109 private:
00110   int verbose;                          // log message verbosity
00111 };
00112 
00113 #endif // __NAVIGATOR_INTERNAL_H__


art_nav
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:08:43