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 _artag_pose_based_vs_alg_h_
00026 #define _artag_pose_based_vs_alg_h_
00027
00028 #include <iri_artag_pose_based_vs/ArtagPoseBasedVsConfig.h>
00029 #include "mutex.h"
00030
00031 #include <iostream>
00032
00033 #include <tf/transform_listener.h>
00034 #include <tf/transform_datatypes.h>
00035
00036 #include <Eigen/Dense>
00037
00038
00039 #include <pose_based_vs.h>
00040
00041
00042
00043 using namespace Eigen;
00044
00050 class ArtagPoseBasedVsAlgorithm
00051 {
00052 protected:
00059 CMutex alg_mutex_;
00060
00061
00062
00063 MatrixXd current_pose_;
00064 MatrixXd desired_pose_;
00065 MatrixXd cam_vel_;
00066
00067 double dt_;
00068 double i_lim_;
00069
00070
00071 MatrixXd p_error_last_;
00072 MatrixXd p_error_;
00073 MatrixXd d_error_;
00074 MatrixXd i_error_;
00075 MatrixXd kp_;
00076 MatrixXd kd_;
00077 MatrixXd ki_;
00078 double cmd_;
00079
00080 public:
00087 typedef iri_artag_pose_based_vs::ArtagPoseBasedVsConfig Config;
00088
00095 Config config_;
00096
00105 ArtagPoseBasedVsAlgorithm(void);
00106
00112 void lock(void) { alg_mutex_.enter(); };
00113
00119 void unlock(void) { alg_mutex_.exit(); };
00120
00128 bool try_enter(void) { return alg_mutex_.try_enter(); };
00129
00141 void config_update(Config& new_cfg, uint32_t level=0);
00142
00143
00144
00145
00152 ~ArtagPoseBasedVsAlgorithm(void);
00153
00160 void pose_based_vs(const MatrixXd& desired_pose,
00161 const MatrixXd& current_pose,
00162 const double& dt,
00163 const MatrixXd& kp,
00164 const MatrixXd& kd,
00165 const MatrixXd& ki,
00166 const double& i_lim,
00167 MatrixXd& cam_vel);
00168
00169 CPose_Based_Vs pbvs;
00170
00177 double updatePid(double& error,const double& dt,const double& i);
00178 };
00179
00180 #endif