Go to the documentation of this file.00001 #include "kinton_arm_tag_tracker_alg_node.h"
00002
00003 using namespace KDL;
00004 using namespace Eigen;
00005 using namespace std;
00006
00007 KintonArmTagTrackerAlgNode::KintonArmTagTrackerAlgNode(void) :
00008 algorithm_base::IriBaseAlgorithm<KintonArmTagTrackerAlgorithm>()
00009 {
00010
00011 this->loop_rate_ = 2;
00012
00013 this->time_ = ros::Time::now();
00014
00015 this->cam_vel_ = MatrixXd::Zero(6,1);
00016
00017 this->num_joints_ = 0;
00018 this->cam_vel_ok_ = false;
00019
00020 this->T_endeff_ = Matrix4d::Zero();
00021 this->T_endeff_.block(0,0,3,3) = Matrix3d::Identity();
00022 this->T_endeff_(3,3) = 1.0;
00023
00024 ros::NodeHandle nh;
00025
00026 string tip_name, root_name;
00027 string xml_string;
00028 Tree tree;
00029 string urdf_xml,full_urdf_xml;
00030 nh.param("urdf_xml",urdf_xml,string("robot_description"));
00031 nh.searchParam(urdf_xml,full_urdf_xml);
00032 ROS_DEBUG("Reading xml file from parameter server\n");
00033 if (!nh.getParam(full_urdf_xml, xml_string)){
00034 ROS_ERROR("Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
00035 }
00036 if (!nh.getParam("root_name", root_name)){
00037 ROS_ERROR("No root name found on parameter server");
00038 }
00039 if (!nh.getParam("tip_name", tip_name)){
00040 ROS_ERROR("No tip name found on parameter server");
00041 }
00042 if (!kdl_parser::treeFromString(xml_string, tree)){
00043 ROS_ERROR("Could not initialize tree object");
00044 }
00045 if (!tree.getChain(root_name, tip_name, this->kdl_chain_)){
00046 ROS_ERROR("Could not initialize kdl_chain object");
00047 }
00048
00049
00050 this->output_publisher_ = this->public_node_handle_.advertise<sensor_msgs::JointState>("output", 10);
00051
00052
00053 this->input_subscriber_ = this->public_node_handle_.subscribe("input", 10, &KintonArmTagTrackerAlgNode::input_callback, this);
00054
00055
00056
00057
00058
00059
00060
00061
00062 }
00063
00064 KintonArmTagTrackerAlgNode::~KintonArmTagTrackerAlgNode(void)
00065 {
00066
00067 }
00068
00069 void KintonArmTagTrackerAlgNode::mainNodeThread(void)
00070 {
00071
00072
00073 if (this->init_)
00074 {
00075 this->time_last_=this->time_;
00076 init_arm();
00077 this->init_=false;
00078 }
00079
00080 this->dt_=(this->time_-this->time_last_).toSec();
00081
00082 if (this->cam_vel_ok_ && this->activate_){
00083
00084 this->alg_.lock();
00085 this->tag_tracker_.tag_tracker(this->cam_vel_,this->kdl_chain_,this->dt_,this->q_);
00086 this->alg_.unlock();
00087 }
00088 else
00089 {
00090 this->init_ = true;
00091 }
00092
00093
00094
00095
00096
00097 for (int s=0; s<this->num_joints_; ++s){
00098 this->JointState_msg_.position[s]=this->q_(s,0);
00099 this->JointState_msg_.velocity[s]=0.0;
00100 this->JointState_msg_.effort[s]=10;
00101 }
00102
00103 this->time_last_ = this->time_;
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113 this->output_publisher_.publish(this->JointState_msg_);
00114 }
00115
00116
00117 void KintonArmTagTrackerAlgNode::input_callback(const geometry_msgs::TwistWithCovariance::ConstPtr& msg)
00118 {
00119
00120
00121
00122
00123 this->input_mutex_.enter();
00124
00125 this->cam_vel_ = MatrixXd::Zero(6,1);
00126
00127 if (msg->covariance[0]<1000 && this->activate_)
00128 {
00129
00130
00131 this->cam_vel_(0,0) = msg->twist.linear.x;
00132 this->cam_vel_(1,0) = msg->twist.linear.y;
00133 this->cam_vel_(2,0) = msg->twist.linear.z;
00134 this->cam_vel_(3,0) = msg->twist.angular.x;
00135 this->cam_vel_(4,0) = msg->twist.angular.y;
00136 this->cam_vel_(5,0) = msg->twist.angular.z;
00137
00138 this->cam_vel_ok_ = true;
00139 }
00140 else
00141 {
00142 this->cam_vel_ok_ = false;
00143 }
00144
00145 this->time_ = ros::Time::now();
00146
00147
00148
00149
00150
00151 this->input_mutex_.exit();
00152 }
00153
00154 void KintonArmTagTrackerAlgNode::init_arm()
00155 {
00156
00157 this->num_joints_=5;
00158 this->JointState_msg_.header.stamp=ros::Time::now();
00159 this->JointState_msg_.header.frame_id="";
00160 this->JointState_msg_.name.resize(this->num_joints_);
00161 this->JointState_msg_.position.resize(this->num_joints_);
00162 this->JointState_msg_.velocity.resize(this->num_joints_);
00163 this->JointState_msg_.effort.resize(this->num_joints_);
00164 this->JointState_msg_.name[0]="joint1";
00165 this->JointState_msg_.name[1]="joint2";
00166 this->JointState_msg_.name[2]="joint3";
00167 this->JointState_msg_.name[3]="joint4";
00168 this->JointState_msg_.name[4]="joint5";
00169 this->q_ = MatrixXd::Zero(this->num_joints_,1);
00170 this->q_ << 90, 160, 180, 20, 90;
00171 }
00172
00173
00174
00175
00176
00177
00178
00179
00180 void KintonArmTagTrackerAlgNode::node_config_update(Config &config, uint32_t level)
00181 {
00182 this->alg_.lock();
00183 this->activate_=config.activate;
00184 this->alg_.unlock();
00185 }
00186
00187 void KintonArmTagTrackerAlgNode::addNodeDiagnostics(void)
00188 {
00189 }
00190
00191
00192 int main(int argc,char *argv[])
00193 {
00194 return algorithm_base::main<KintonArmTagTrackerAlgNode>(argc, argv, "kinton_arm_tag_tracker_alg_node");
00195 }