00001 #include "kinton_vs_control_sim_alg_node.h"
00002
00003 KintonVsControlAlgNode::KintonVsControlAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<KintonVsControlAlgorithm>()
00005 {
00006
00007 this->loop_rate_ = 100;
00008
00009 this->quad_twist_ = Eigen::MatrixXd::Zero(6,1);
00010 this->cam_twist_ = Eigen::MatrixXd::Zero(6,1);
00011
00012
00013 tf::TransformListener listener;
00014 tf::StampedTransform tf_quad_to_cam;
00015
00016
00017 sleep(5.0);
00018
00019 try {
00020 listener.waitForTransform("/base_link","/front_cam_optical_frame", ros::Time::now(), ros::Duration(1.0));
00021 listener.lookupTransform("/base_link","/front_cam_optical_frame", ros::Time::now(), tf_quad_to_cam);
00022 }
00023 catch (tf::TransformException ex) {
00024 ROS_ERROR("Quad_to_cam Transform error: %s",ex.what());
00025 }
00026
00027
00028 this->T_quad_to_cam_ = getTransform(tf_quad_to_cam);
00029
00030
00031 this->cmd_vel_publisher_ = this->public_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
00032
00033 this->cam_vel_subscriber_ = this->public_node_handle_.subscribe("cam_vel", 100, &KintonVsControlAlgNode::cam_vel_callback, this);
00034 this->odom_subscriber_ = this->public_node_handle_.subscribe("odom", 100, &KintonVsControlAlgNode::odom_callback, this);
00035
00036
00037
00038
00039
00040
00041
00042
00043 }
00044
00045 KintonVsControlAlgNode::~KintonVsControlAlgNode(void)
00046 {
00047
00048 }
00049
00050 void KintonVsControlAlgNode::mainNodeThread(void)
00051 {
00052
00053 Eigen::MatrixXd quad_vel;
00054 quad_vel = cam_to_quad_vel();
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064 this->cmd_vel_.linear.x = quad_vel(0,0);
00065 this->cmd_vel_.linear.y = quad_vel(1,0);
00066 this->cmd_vel_.linear.z = quad_vel(2,0);
00067
00068
00069 this->cmd_vel_.angular.x = 0.0;
00070 this->cmd_vel_.angular.y = 0.0;
00071 this->cmd_vel_.angular.z = quad_vel(5,0);
00072
00073 this->cmd_vel_publisher_.publish(this->cmd_vel_);
00074 }
00075
00076
00077 void KintonVsControlAlgNode::cam_vel_callback(const geometry_msgs::TwistWithCovariance::ConstPtr& msg)
00078 {
00079
00080
00081
00082
00083 this->cam_vel_mutex_.enter();
00084
00085
00086 if (msg->covariance[0]<1000 && this->activate_)
00087 {
00088
00089
00090 this->cam_twist_(0,0) = msg->twist.linear.x;
00091 this->cam_twist_(1,0) = msg->twist.linear.y;
00092 this->cam_twist_(2,0) = msg->twist.linear.z;
00093 this->cam_twist_(3,0) = msg->twist.angular.x;
00094 this->cam_twist_(4,0) = msg->twist.angular.y;
00095 this->cam_twist_(5,0) = msg->twist.angular.z;
00096 }
00097 else this->cam_twist_ = Eigen::MatrixXd::Zero(6,1);
00098
00099
00100
00101
00102
00103 this->cam_vel_mutex_.exit();
00104 }
00105 void KintonVsControlAlgNode::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
00106 {
00107
00108
00109
00110
00111 this->odom_mutex_.enter();
00112
00113 this->roll_ = msg->pose.pose.orientation.x;
00114 this->pitch_ = msg->pose.pose.orientation.y;
00115
00116 this->quad_twist_(0,0) = msg->twist.twist.linear.x;
00117 this->quad_twist_(1,0) = msg->twist.twist.linear.y;
00118 this->quad_twist_(2,0) = msg->twist.twist.linear.z;
00119 this->quad_twist_(3,0) = msg->twist.twist.angular.x;
00120 this->quad_twist_(4,0) = msg->twist.twist.angular.y;
00121 this->quad_twist_(5,0) = msg->twist.twist.angular.z;
00122
00123
00124
00125 this->odom_mutex_.exit();
00126 }
00127
00128 Eigen::MatrixXd KintonVsControlAlgNode::cam_to_quad_vel()
00129 {
00130
00131 Eigen::MatrixXd cam_lin = Eigen::MatrixXd::Zero(3,1);
00132 Eigen::MatrixXd cam_ang = Eigen::MatrixXd::Zero(3,1);
00133
00134
00135 this->cam_vel_mutex_.enter();
00136 cam_lin = this->cam_twist_.block(0,0,3,1);
00137 cam_ang = this->cam_twist_.block(3,0,3,1);
00138 this->cam_vel_mutex_.exit();
00139
00140
00141
00142 Eigen::MatrixXd quad_ang = Eigen::MatrixXd::Zero(3,1);
00143 quad_ang = this->T_quad_to_cam_.block(0,0,3,3) * cam_ang;
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163 Eigen::MatrixXd quad_lin = Eigen::MatrixXd::Zero(3,1);
00164 quad_lin = this->T_quad_to_cam_.block(0,0,3,3) * cam_lin;
00165
00166
00167 Eigen::MatrixXd quad_vel = Eigen::MatrixXd::Zero(6,1);
00168 quad_vel.block(0,0,3,1) = quad_lin;
00169 quad_vel.block(3,0,3,1) = quad_ang;
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184 return quad_vel;
00185 }
00186
00187 Eigen::Matrix4d KintonVsControlAlgNode::getTransform(const tf::StampedTransform& transform)
00188 {
00189
00190 double roll,pitch,yaw;
00191 tf::Quaternion q = transform.getRotation();
00192 tf::Matrix3x3(q).getRPY(roll,pitch,yaw);
00193
00194 Eigen::Matrix3d Rot;
00195 Rot = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) \
00196 * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) \
00197 * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
00198 Eigen::Matrix4d T = Eigen::Matrix4d::Zero();
00199
00200 T.block(0,0,3,3) = Rot;
00201 T(0,3) = transform.getOrigin().x();
00202 T(1,3) = transform.getOrigin().y();
00203 T(2,3) = transform.getOrigin().z();
00204 T(3,3) = 1.0;
00205
00206
00207
00208 return T;
00209 }
00210
00211
00212
00213
00214
00215
00216
00217
00218 void KintonVsControlAlgNode::node_config_update(Config &config, uint32_t level)
00219 {
00220 this->alg_.lock();
00221 this->activate_=config.sim_activate;
00222 this->alg_.unlock();
00223 }
00224
00225 void KintonVsControlAlgNode::addNodeDiagnostics(void)
00226 {
00227 }
00228
00229
00230 int main(int argc,char *argv[])
00231 {
00232 return algorithm_base::main<KintonVsControlAlgNode>(argc, argv, "kinton_vs_control_alg_node");
00233 }