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__