00001 #pragma once 00002 00003 #include "ros/ros.h" 00004 #include <sensor_msgs/JointState.h> 00005 #include <actionlib/server/simple_action_server.h> 00006 #include <asr_flir_ptu_controller/PTUMovementAction.h> 00007 #include "asr_flir_ptu_driver/Validate.h" 00008 #include <std_srvs/Empty.h> 00009 #include "asr_flir_ptu_driver/State.h" 00010 namespace asr_flir_ptu_controller 00011 { 00012 class PTUController 00013 { 00014 public: 00015 PTUController(ros::NodeHandle& n, std::string name); 00016 ~PTUController(); 00017 double getMaximumPan(); 00018 double getMinimumPan(); 00019 00020 double getMaximumTilt(); 00021 double getMinimumTilt(); 00022 00023 double getCurrentPan(); 00024 double getCurrentTilt(); 00025 00026 std::string getDefaultStateCmdTopicName(); 00027 std::string getDefaultStateTopicName(); 00028 double getTimeToWait(); 00029 double getToleranceValue(); 00030 int getMaxSteps(); 00031 static double getRadianToDegree() {return RAD_TO_DEG;} 00032 00033 double getMargin() const; 00034 void setMargin(double value); 00035 00036 private: 00037 void goalCB(); 00038 void preemptCB(); 00039 void setSettings(); 00040 void currentStateArrived(const asr_flir_ptu_driver::State::ConstPtr& msg); 00041 bool validate(double pan, double tilt); 00042 int count; 00043 double startDistance; 00044 00045 sensor_msgs::JointState target_joint; 00046 ros::NodeHandle nodeHandle; 00047 ros::Time lastStateTime; 00048 00049 ros::Publisher stateCommandPublisher; 00050 ros::Subscriber stateSubscriber; 00051 ros::ServiceClient validate_client; 00052 ros::ServiceClient alive; 00053 sensor_msgs::JointState stateCommandMessage; 00054 00055 static const double RAD_TO_DEG; 00056 00057 double current_pan; 00058 double desired_pan; 00059 double max_pan; 00060 double min_pan; 00061 00062 double current_tilt; 00063 double desired_tilt; 00064 double max_tilt; 00065 double min_tilt; 00066 int seq_num; 00067 00068 int maxSteps; 00069 double timeToWait; 00070 double tolerance; 00071 double margin; 00072 std::vector< std::map< std::string, double> > forbiddenAreas; 00073 std::string topicName; 00074 std::string commandTopicName; 00075 std::string validation_service; 00076 std::string alive_service; 00077 actionlib::SimpleActionServer<asr_flir_ptu_controller::PTUMovementAction> simpleActionServer; 00078 asr_flir_ptu_controller::PTUMovementFeedback simpleActionServerFeedback; 00079 asr_flir_ptu_controller::PTUMovementResult simpleActionServerResult; 00080 }; 00081 } 00082