00001
00002
00003
00004
00005
00006
00007
00008
00009
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/Observers.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
00044
00045 using art_msgs::Behavior;
00046 using art_msgs::NavigatorCommand;
00047 using art_msgs::NavigatorState;
00048 using art_msgs::Observers;
00049 using art_msgs::Observation;
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
00060 class Estop;
00061 class Course;
00062 class Obstacle;
00063
00064 class Navigator
00065 {
00066 public:
00067
00068
00069 PolyOps* pops;
00070 Course* course;
00071 Obstacle* obstacle;
00072
00073
00074 Estop *estop;
00075
00076
00077 art_msgs::Order order;
00078 art_msgs::NavigatorState navdata;
00079 nav_msgs::Odometry estimate;
00080 nav_msgs::Odometry *odometry;
00081
00082
00083 Navigator(nav_msgs::Odometry *odom_msg);
00084 ~Navigator();
00085
00086
00087 void configure();
00088
00089 Config config_;
00090
00091
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
00099 pilot_command_t navigate(void);
00100
00101
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;
00111 };
00112
00113 #endif // __NAVIGATOR_INTERNAL_H__