00001 00008 #include <actionlib/server/simple_action_server.h> 00009 #include <costmap_2d/costmap_2d.h> 00010 #include <geometry_msgs/Twist.h> 00011 #include <navfn/navfn.h> 00012 #include <nav_msgs/OccupancyGrid.h> 00013 #include <ros/ros.h> 00014 #include <sensor_msgs/Image.h> 00015 #include <sys/time.h> 00016 00017 #include <youbot_overhead_localization/CostmapCost.h> 00018 #include <youbot_overhead_localization/GetCost.h> 00019 #include <youbot_overhead_localization/Path.h> 00020 #include <youbot_overhead_localization/PlanPathAction.h> 00021 #include <youbot_overhead_localization/PlanPathGoal.h> 00022 #include <youbot_overhead_localization/Pose2d.h> 00023 #include <youbot_overhead_vision/CalibCameraImg.h> 00024 #include <youbot_overhead_vision/CoordConversion.h> 00025 00026 //Filter constants 00027 #define B1 0.1 00028 #define B2 0.1 00029 #define A -0.8 00030 #define SIG_THRESH -.995 00031 00032 //Control constants 00033 #define KP_ANG 4 00034 #define KI_ANG .0001 00035 #define KD_ANG .01 00036 #define KP_LIN 2.3 00037 #define KI_LIN .002 00038 #define KD_LIN .008 00039 00040 #define SIM_TOP_CAMERA_OFFSET 539 00041 #define REAL_TOP_CAMERA_OFFSET 476 00042 00043 #define PATH_TIMEOUT 120.0 00044 00045 #define MAX_SPEED .5; 00046 00047 #define PI 3.14159 00048 00057 class pathPlanner 00058 { 00059 public: 00060 //publishers and subscribers 00061 ros::NodeHandle n; 00062 ros::Publisher baseCommandPublisher; 00063 ros::Publisher costPublisher; 00064 ros::Publisher pointLogPublisher; 00065 ros::Subscriber localizationSubscriber; 00066 ros::Subscriber mapSubscriber; 00067 ros::ServiceClient coordConversionClient; 00068 ros::ServiceServer costServer; 00069 00070 //action lib 00071 actionlib::SimpleActionServer<youbot_overhead_localization::PlanPathAction> as; 00072 std::string actionName; 00073 youbot_overhead_localization::PlanPathFeedback asFeedback; 00074 youbot_overhead_localization::PlanPathResult asResult; 00075 00076 //localization 00077 bool positionInitialized; 00078 youbot_overhead_localization::Pose2d currentPose; 00079 bool useSim; 00080 00081 //map 00082 std::string mapFileName; 00083 std::string mapDebugLocation; 00084 00085 //path 00086 float goalx; 00087 float goaly; 00088 youbot_overhead_localization::Path plan; 00089 00090 //costmap_2d 00091 costmap_2d::Costmap2D *cmObj; 00092 00093 //navfn 00094 navfn::NavFn *navFnObj; 00095 00096 //initialization 00097 bool cmInitialized; 00098 bool navFnInitialized; 00099 00100 //costmap parameters 00101 int lethalCostThreshold; 00102 int unknownCostValue; 00103 unsigned int mapHeight; 00104 unsigned int mapWidth; 00105 double circumRadius; 00106 double costScalingFactor; 00107 double inflationRadius; 00108 double inRadius; 00109 double mapOriginx; 00110 double mapOriginy; 00111 double mapResolution; 00112 double maxObstacleHeight; 00113 double obstacleRange; 00114 double raytraceRange; 00115 bool trackUnknownSpace; 00116 std::vector<unsigned char> mapData; 00117 00118 //NavFn parameters 00119 bool allowUnknown; 00120 00121 //Move command 00122 geometry_msgs::Twist baseCommand; 00123 00124 //Time at which previous path point was reached 00125 timespec prevPointTime; 00126 00127 //trajectory error tracking 00128 float prevError; 00129 float prevTurnError; 00130 float totalError; 00131 float totalTurnError; 00132 00137 void executePathPlanning(const youbot_overhead_localization::PlanPathGoalConstPtr& goal); 00138 00144 void mapserverCallback(const nav_msgs::OccupancyGridConstPtr& newMap); 00145 00150 void localizationCallback(const youbot_overhead_localization::Pose2d& newPose); 00151 00155 void followPath(); 00156 00161 int makePlanAstar(); 00162 00169 bool getCost(youbot_overhead_localization::GetCost::Request &req, 00170 youbot_overhead_localization::GetCost::Response &res); 00171 00175 void printOccupancyGrid(); 00176 00180 void printCostmap(); 00181 00186 pathPlanner(std::string name); 00187 00191 ~pathPlanner(); 00192 }; 00193