00001 #ifndef PURE_PURSUIT_CONTROLLER_NODE_H 00002 #define PURE_PURSUIT_CONTROLLER_NODE_H 00003 00004 #include <string> 00005 #include <vector> 00006 00007 #include <ros/ros.h> 00008 #include "std_msgs/String.h" 00009 #include <nav_msgs/Path.h> 00010 #include <nav_msgs/Odometry.h> 00011 00012 #include <geometry_msgs/PoseStamped.h> 00013 #include <geometry_msgs/PointStamped.h> 00014 00015 #include <tf/transform_listener.h> 00016 #include <math.h> 00017 #include <std_msgs/Int32.h> 00018 #include <pure_pursuit_controller/executePath.h> 00019 #include <pure_pursuit_controller/cancelPath.h> 00020 #include <pure_pursuit_controller/cutPath.h> 00021 #include <pure_pursuit_controller/recoverPath.h> 00022 00023 using namespace std; 00027 class PurePursuitControllerNode 00028 { 00029 public: 00033 00034 PurePursuitControllerNode(const ros::NodeHandle &nh); 00035 00037 virtual ~PurePursuitControllerNode(); 00044 00045 void spin(); 00047 bool step(geometry_msgs::Twist &twist); 00049 geometry_msgs::PoseStamped getCurrentPose() const; 00051 double getLookAheadThreshold() const; 00053 int getNextWayPoint(int wayPoint); 00054 void resetParam(); 00057 void angle2quat(vector<float> &angle, vector<float> &quaternion); 00058 00059 protected: 00063 // /// Retrieves parameters 00064 // void getParameters(); 00066 bool pathExecute(pure_pursuit_controller::executePath::Request &req,pure_pursuit_controller::executePath::Response &res); 00067 bool goTriExecute(pure_pursuit_controller::executePath::Request &req,pure_pursuit_controller::executePath::Response &res); 00068 bool pathCancel(pure_pursuit_controller::cancelPath::Request &req, pure_pursuit_controller::cancelPath::Response &res); 00069 bool pathCut(pure_pursuit_controller::cutPath::Request &req, pure_pursuit_controller::cutPath::Response &res); 00070 00072 void odometryCallback(const nav_msgs::Odometry &msg); 00074 void timerCallback(const ros::TimerEvent &event); 00075 void getParameters(); 00076 00083 00084 ros::NodeHandle _nodeHandle; 00087 ros::ServiceServer _goTriExecute; 00088 ros::ServiceServer _pathExecute; 00089 ros::ServiceServer _pathCancel; 00090 ros::ServiceServer _pathCut; 00092 ros::Publisher _finishGoTri; 00094 std::string _pathExecuteServerName; 00095 00097 std::string _odometryTopicName; 00099 std::string _poseFrameId; 00100 00102 int _queueDepth; 00103 bool isFinish; 00104 int safeWayNum; 00105 bool ifGoTrisector; 00106 bool ifFirstPoint; 00107 bool ifCutPath; 00108 00110 nav_msgs::Path _currentReferencePath; 00112 geometry_msgs::Twist _currentVelocity; 00114 double _frequency; 00116 int _nextWayPoint; 00118 ros::Publisher _cmdVelocityPublisher; 00120 std::string _cmdVelocityTopicName; 00121 00123 int _initialWayPoint; 00125 double _velocity; 00127 double _lookAheadRatio; 00129 double _epsilon; 00131 tf::TransformListener _tfListener; 00133 ros::Timer _timer; 00134 }; 00135 #endif // PURE_PURSUIT_CONTROLLER_NODE_H