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_arm_task_priority_control_alg_node_h_ 00026 #define _kinton_arm_task_priority_control_alg_node_h_ 00027 00028 #include <iri_base_algorithm/iri_base_algorithm.h> 00029 #include "kinton_arm_task_priority_control_alg.h" 00030 #include <urdf/model.h> 00031 00032 // [publisher subscriber headers] 00033 #include <sensor_msgs/JointState.h> 00034 #include <geometry_msgs/Twist.h> 00035 #include <geometry_msgs/TwistWithCovariance.h> 00036 #include <nav_msgs/Odometry.h> 00037 #include <ar_pose/ARMarkers.h> 00038 00039 // [service client headers] 00040 00041 // [action server client headers] 00042 00043 using namespace KDL; 00044 using namespace Eigen; 00045 using namespace std; 00046 00047 00052 class KintonArmTaskPriorityControlAlgNode : public algorithm_base::IriBaseAlgorithm<KintonArmTaskPriorityControlAlgorithm> 00053 { 00054 private: 00055 // [publisher attributes] 00056 ros::Publisher joints_pub_publisher_; 00057 sensor_msgs::JointState JointState_msg_; 00058 ros::Publisher quad_vel_publisher_; 00059 geometry_msgs::Twist Twist_msg_; 00060 00061 // [subscriber attributes] 00062 ros::Subscriber cam_vel_subscriber_; 00063 void cam_vel_callback(const geometry_msgs::TwistWithCovariance::ConstPtr& msg); 00064 CMutex cam_vel_mutex_; 00065 ros::Subscriber odom_subscriber_; 00066 void odom_callback(const nav_msgs::Odometry::ConstPtr& msg); 00067 CMutex odom_mutex_; 00068 void input_tag_callback(const ar_pose::ARMarkers::ConstPtr& msg); 00069 CMutex input_tag_mutex_; 00070 ros::Subscriber input_tag_subscriber_; 00071 00072 // [service attributes] 00073 00074 // [client attributes] 00075 00076 // [action server attributes] 00077 00078 // [action client attributes] 00079 00080 bool activate_; //When to activate the servoing. 00081 00082 ros::Time time_,time_last_; //Time variables. 00083 double dt_; // Time diferential. 00084 00085 Matrix4d Tcam_in_tip_; // Homogenous transform of camera in arm tip frames. 00086 Matrix4d Ttag_in_cam_; // Homogenous Transform of tag in camera frames. 00087 00088 // Arm parameters 00089 Chain kdl_chain_; // Arm chain. 00090 int num_joints_; // Number of joints. 00091 bool init_; //Initialize timers and arm parameters. 00092 bool arm_unina_; // Arm designed by Unina. 00093 bool arm_catec_; // Arm designed by Fada-Catec. 00094 bool enable_sec_task_; // Enable secondary task. 00095 bool cam_vel_ok_; // Check the camera velocities covariance. 00096 00097 00098 geometry_msgs::Pose pose_; // Artag pose to compute Quadrotor position r.t. marker 00099 00100 MatrixXd q_; // DOF variables. 00101 MatrixXd lambda_robot_; // Proportional gain of Robot DOFs. 00102 MatrixXd gain_sec_task_; // Gains of secondary tasks. 00103 00104 MatrixXd v_rollpitch_; // Current roll and pitch velocity of the quadrotor from the odometry. 00105 MatrixXd q_rollpitch_; // Curent quadrotor roll and pitch angles from the odometry. 00106 MatrixXd cam_vel_; // Current velocity of the camera from the visual servo. 00107 MatrixXd vel_; // Quadrotor velocity. 00108 00109 vector<arm_joint> joint_info_; // Arm joints info 00110 00111 KintonArmTaskPriorityControlAlgorithm task_priority_ctrl_; 00112 00113 public: 00120 KintonArmTaskPriorityControlAlgNode(void); 00121 00128 ~KintonArmTaskPriorityControlAlgNode(void); 00129 00130 protected: 00143 void mainNodeThread(void); 00144 00157 void node_config_update(Config &config, uint32_t level); 00158 00165 void addNodeDiagnostics(void); 00166 00167 // [diagnostic functions] 00168 00169 // [test functions] 00170 00176 Matrix4d getTransform(const string& frame1, const string& frame2); 00177 00183 void init_arm(); 00184 00190 bool readJoints(urdf::Model &robot_model,const string& root_name,const string& tip_name); 00191 00197 Matrix4d get_HT_cam_in_tip(const string& cam_frame, const string& tip_frame); 00198 00199 }; 00200 00201 #endif