kinton_vs_control_sim_alg_node.h
Go to the documentation of this file.
00001 // Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
00002 // Author 
00003 // All rights reserved.
00004 //
00005 // This file is part of iri-ros-pkg
00006 // iri-ros-pkg is free software: you can redistribute it and/or modify
00007 // it under the terms of the GNU Lesser General Public License as published by
00008 // the Free Software Foundation, either version 3 of the License, or
00009 // at your option) any later version.
00010 //
00011 // This program is distributed in the hope that it will be useful,
00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 // GNU Lesser General Public License for more details.
00015 //
00016 // You should have received a copy of the GNU Lesser General Public License
00017 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
00018 // 
00019 // IMPORTANT NOTE: This code has been generated through a script from the 
00020 // iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
00021 // of the scripts. ROS topics can be easly add by using those scripts. Please
00022 // refer to the IRI wiki page for more information:
00023 // http://wikiri.upc.es/index.php/Robotics_Lab
00024 
00025 #ifndef _kinton_vs_control_alg_node_h_
00026 #define _kinton_vs_control_alg_node_h_
00027 
00028 #include <iri_base_algorithm/iri_base_algorithm.h>
00029 #include "kinton_vs_control_alg.h"
00030 
00031 #include <Eigen/Dense>
00032 #include <Eigen/Eigenvalues>
00033 #include <Eigen/SVD>
00034 
00035 #include <tf/transform_listener.h>
00036 
00037 // [publisher subscriber headers]
00038 #include <geometry_msgs/TwistWithCovariance.h>
00039 #include <geometry_msgs/Twist.h>
00040 #include <nav_msgs/Odometry.h>
00041 
00042 
00043 // [service client headers]
00044 
00045 // [action server client headers]
00046 
00051 class KintonVsControlAlgNode : public algorithm_base::IriBaseAlgorithm<KintonVsControlAlgorithm>
00052 {
00053   private:
00054     // [publisher attributes]
00055     ros::Publisher cmd_vel_publisher_;
00056     geometry_msgs::Twist cmd_vel_;
00057 
00058     // [subscriber attributes]
00059     ros::Subscriber cam_vel_subscriber_;
00060     void cam_vel_callback(const geometry_msgs::TwistWithCovariance::ConstPtr& msg);
00061     CMutex cam_vel_mutex_;
00062     ros::Subscriber odom_subscriber_;
00063     void odom_callback(const nav_msgs::Odometry::ConstPtr& msg);
00064     CMutex odom_mutex_;
00065 
00066     bool activate_; //When to activate the servoing
00067 
00068     Eigen::Matrix4d T_quad_to_cam_; //Homogenous transform between Quadrotor and camera frames
00069 
00070     Eigen::MatrixXd quad_twist_; //Actual velocity of the quadrotor from the odometry
00071     Eigen::MatrixXd cam_twist_; //Actual velocity of the camera from the visual servo
00072 
00073     double roll_, pitch_; //Quadrotor angles r.t. inertial frame
00074 
00075     // [service attributes]
00076 
00077     // [client attributes]
00078 
00079     // [action server attributes]
00080 
00081     // [action client attributes]
00082 
00083   public:
00090     KintonVsControlAlgNode(void);
00091 
00098     ~KintonVsControlAlgNode(void);
00099 
00100   protected:
00113     void mainNodeThread(void);
00114 
00127     void node_config_update(Config &config, uint32_t level);
00128 
00135     void addNodeDiagnostics(void);
00136 
00137     // [diagnostic functions]
00138     
00139     // [test functions]
00140 
00146     Eigen::MatrixXd cam_to_quad_vel();
00147 
00153     Eigen::Matrix4d getTransform(const tf::StampedTransform& transform);
00154 };
00155 
00156 #endif


kinton_vs_control
Author(s): asantamaria
autogenerated on Fri Dec 6 2013 23:24:20