Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef _ual_catec_alg_node_h_
00026 #define _ual_catec_alg_node_h_
00027
00028 #include <iri_base_algorithm/iri_base_algorithm.h>
00029 #include "ual_catec_alg.h"
00030
00031
00032 #include <ar_pose/ARMarkers.h>
00033 #include <catec_msgs/ControlReferenceRwStamped.h>
00034 #include <catec_msgs/UALStateStamped.h>
00035
00036 #include <tf/transform_datatypes.h>
00037
00038
00039
00040
00041 #include <catec_actions_msgs/TakeOffAction.h>
00042 #include <actionlib/client/simple_action_client.h>
00043 #include <actionlib/client/terminal_state.h>
00044 #include <catec_actions_msgs/LandAction.h>
00045
00046 #include <Eigen/Dense>
00047 #include <Eigen/Eigenvalues>
00048 #include <eigen_conversions/eigen_kdl.h>
00049
00050
00051 #include "rotateOP/Quaternion.h"
00052
00053 using namespace std;
00054
00059 class UalCatecAlgNode : public algorithm_base::IriBaseAlgorithm<UalCatecAlgorithm>
00060 {
00061 private:
00062
00063 bool LandGoalSended_,TakeOffGoalSended_,go_initial_tag_pose_,take_off_action_,land_action_,tag_detected_,land_position_ok_,land_position_fixed_;
00064 double seq_,quad_x_,quad_y_,quad_z_,quad_yaw_,tag_x_,tag_y_,tag_z_,tag_yaw_,goal_x_,goal_y_,goal_z_,goal_yaw_,dist_to_tag_,ini_pos_x_,ini_pos_y_,ini_pos_z_,ini_pos_yaw_,cruise_,marker_cruise_,goal_cruise_,yaw_cruise_;
00065 Eigen::MatrixXf land_pos_;
00066
00067 int fixed_to_id_;
00068
00069 btMatrix3x3 body_earth_rot_;
00070
00071 geometry_msgs::Pose pose_;
00072 ros::Time t_,last_t_tag_;
00073
00074
00075 ros::Publisher control_references_rw_publisher_;
00076 catec_msgs::ControlReferenceRwStamped ControlReferenceRwStamped_msg_;
00077
00078
00079 ros::Subscriber input_tag_subscriber_;
00080 void input_tag_callback(const ar_pose::ARMarkers::ConstPtr& msg);
00081 CMutex input_tag_mutex_;
00082 ros::Subscriber ual_state_subscriber_;
00083 void ual_state_callback(const catec_msgs::UALStateStamped::ConstPtr& msg);
00084 CMutex ual_state_mutex_;
00085
00086
00087
00088
00089
00090
00091
00092
00093 actionlib::SimpleActionClient<catec_actions_msgs::TakeOffAction> take_off_action_client_;
00094 catec_actions_msgs::TakeOffGoal take_off_action_goal_;
00095 void take_off_actionMakeActionRequest();
00096 void take_off_actionDone(const actionlib::SimpleClientGoalState& state, const catec_actions_msgs::TakeOffResultConstPtr& result);
00097 void take_off_actionActive();
00098 void take_off_actionFeedback(const catec_actions_msgs::TakeOffFeedbackConstPtr& feedback);
00099
00100 actionlib::SimpleActionClient<catec_actions_msgs::LandAction> land_action_client_;
00101 catec_actions_msgs::LandGoal land_action_goal_;
00102 void land_actionMakeActionRequest();
00103 void land_actionDone(const actionlib::SimpleClientGoalState& state, const catec_actions_msgs::LandResultConstPtr& result);
00104 void land_actionActive();
00105 void land_actionFeedback(const catec_actions_msgs::LandFeedbackConstPtr& feedback);
00106
00107 string takeOffMessage(const catec_actions_msgs::TakeOffResultConstPtr& result);
00108 string landMessage(const catec_actions_msgs::LandResultConstPtr& result);
00109
00110 public:
00117 UalCatecAlgNode(void);
00118
00125 ~UalCatecAlgNode(void);
00126
00127 protected:
00140 void mainNodeThread(void);
00141
00154 void node_config_update(Config &config, uint32_t level);
00155
00162 void addNodeDiagnostics(void);
00163
00164
00165
00166
00167 };
00168
00169 #endif