Go to the documentation of this file.00001 #include "pose_based_vs_alg_node.h"
00002
00003 PoseBasedVsAlgNode::PoseBasedVsAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<PoseBasedVsAlgorithm>()
00005 {
00006
00007 this->loop_rate_ = 100;
00008
00009
00010 this->current_pose_.resize(6,0.0);
00011 this->desired_pose_.resize(6,0.0);
00012 this->kp_.resize(6,0.0);
00013 this->kd_.resize(6,0.0);
00014 this->ki_.resize(6,0.0);
00015 this->cam_vel_.resize(6,0.0);
00016
00017
00018
00019 this->cmd_vel_cov_publisher_ = this->public_node_handle_.advertise<geometry_msgs::TwistWithCovariance>("cmd_vel_cov", 1);
00020
00021
00022 this->input_ARtag_subscriber_ = this->public_node_handle_.subscribe("input_ARtag", 1, &PoseBasedVsAlgNode::input_ARtag_callback, this);
00023
00024
00025
00026
00027
00028
00029
00030
00031 }
00032
00033 PoseBasedVsAlgNode::~PoseBasedVsAlgNode(void)
00034 {
00035
00036 }
00037
00038 void PoseBasedVsAlgNode::mainNodeThread(void)
00039 {
00040
00041
00042
00043
00044
00045
00046
00047
00048 this->cmd_vel_cov_publisher_.publish(this->TwistWC_msg_);
00049 }
00050
00051
00052 void PoseBasedVsAlgNode::input_ARtag_callback(const ar_pose::ARMarkers::ConstPtr& msg)
00053 {
00054
00055
00056
00057
00058 this->input_ARtag_mutex_.enter();
00059
00060 this->time_ = ros::Time::now();
00061 this->dt_ = this->time_.toSec()-this->time_last_.toSec();
00062
00063 if (msg->markers.empty()){
00064 this->cam_vel_.resize(6,0.0);
00065
00066
00067 this->TwistWC_msg_.twist.linear.x=this->cam_vel_[0];
00068 this->TwistWC_msg_.twist.linear.y=this->cam_vel_[1];
00069 this->TwistWC_msg_.twist.linear.z=this->cam_vel_[2];
00070 this->TwistWC_msg_.twist.angular.x=this->cam_vel_[3];
00071 this->TwistWC_msg_.twist.angular.y=this->cam_vel_[4];
00072 this->TwistWC_msg_.twist.angular.z=this->cam_vel_[5];
00073
00074
00075 this->TwistWC_msg_.covariance[0] =
00076 this->TwistWC_msg_.covariance[7] =
00077 this->TwistWC_msg_.covariance[14] =
00078 this->TwistWC_msg_.covariance[21] =
00079 this->TwistWC_msg_.covariance[28] =
00080 this->TwistWC_msg_.covariance[35] = 99999;
00081 }
00082 else if (!msg->markers.empty()){
00083
00084
00085 geometry_msgs::Pose pose;
00086 pose = msg->markers.back().pose.pose;
00087
00088 tf::Transform tf_pose;
00089 double roll,pitch,yaw;
00090 tf::poseMsgToTF(pose, tf_pose);
00091
00092 tf_pose.getBasis().getRPY(roll,pitch,yaw);
00093
00094 this->current_pose_.at(0)=pose.position.x;
00095 this->current_pose_.at(1)=pose.position.y;
00096 this->current_pose_.at(2)=pose.position.z;
00097 this->current_pose_.at(3)=roll;
00098 this->current_pose_.at(4)=pitch;
00099 this->current_pose_.at(5)=yaw;
00100
00101
00102 this->pose_based_vs_alg_.pose_vs(this->current_pose_,this->desired_pose_,this->kp_,this->kd_,this->ki_,this->i_lim_,this->dt_,this->cam_vel_);
00103
00104
00105 this->TwistWC_msg_.twist.linear.x=this->cam_vel_[0];
00106 this->TwistWC_msg_.twist.linear.y=this->cam_vel_[1];
00107 this->TwistWC_msg_.twist.linear.z=this->cam_vel_[2];
00108 this->TwistWC_msg_.twist.angular.x=this->cam_vel_[3];
00109 this->TwistWC_msg_.twist.angular.y=this->cam_vel_[4];
00110 this->TwistWC_msg_.twist.angular.z=this->cam_vel_[5];
00111
00112
00113 double cov = double(100-msg->markers.back().confidence)/100;
00114 this->TwistWC_msg_.covariance[0] =
00115 this->TwistWC_msg_.covariance[7] =
00116 this->TwistWC_msg_.covariance[14] =
00117 this->TwistWC_msg_.covariance[21] =
00118 this->TwistWC_msg_.covariance[28] =
00119 this->TwistWC_msg_.covariance[35] = cov;
00120 }
00121
00122
00123 this->time_last_ = this->time_;
00124
00125
00126
00127
00128 this->input_ARtag_mutex_.exit();
00129 }
00130
00131
00132
00133
00134
00135
00136
00137 void PoseBasedVsAlgNode::node_config_update(Config &config, uint32_t level)
00138 {
00139 this->alg_.lock();
00140
00141
00142 this->desired_pose_.at(0)=config.desired_x;
00143 this->desired_pose_.at(1)=config.desired_y;
00144 this->desired_pose_.at(2)=config.desired_z;
00145 this->desired_pose_.at(3)=config.desired_roll;
00146 this->desired_pose_.at(4)=config.desired_pitch;
00147 this->desired_pose_.at(5)=config.desired_yaw;
00148 this->kp_.at(0)=config.kp_x;
00149 this->kd_.at(0)=config.kd_x;
00150 this->ki_.at(0)=config.ki_x;
00151 this->kp_.at(1)=config.kp_y;
00152 this->kd_.at(1)=config.kd_y;
00153 this->ki_.at(1)=config.ki_y;
00154 this->kp_.at(2)=config.kp_z;
00155 this->kd_.at(2)=config.kd_z;
00156 this->ki_.at(2)=config.ki_z;
00157 this->kp_.at(3)=config.kp_roll;
00158 this->kd_.at(3)=config.kd_roll;
00159 this->ki_.at(3)=config.ki_roll;
00160 this->kp_.at(4)=config.kp_pitch;
00161 this->kd_.at(4)=config.kd_pitch;
00162 this->ki_.at(4)=config.ki_pitch;
00163 this->kp_.at(5)=config.kp_yaw;
00164 this->kd_.at(5)=config.kd_yaw;
00165 this->ki_.at(5)=config.ki_yaw;
00166 this->i_lim_=config.i_lim;
00167
00168 this->alg_.unlock();
00169 }
00170
00171 void PoseBasedVsAlgNode::addNodeDiagnostics(void)
00172 {
00173 }
00174
00175
00176 int main(int argc,char *argv[])
00177 {
00178 return algorithm_base::main<PoseBasedVsAlgNode>(argc, argv, "pose_based_vs_alg_node");
00179 }