00001 00028 #ifndef COB_LOOKAT_CONTROLLER_H 00029 #define COB_LOOKAT_CONTROLLER_H 00030 00031 #include <ros/ros.h> 00032 00033 #include <std_msgs/Float64.h> 00034 #include <sensor_msgs/JointState.h> 00035 #include <geometry_msgs/Twist.h> 00036 #include <brics_actuator/JointVelocities.h> 00037 00038 #include <kdl_parser/kdl_parser.hpp> 00039 #include <kdl/chainiksolvervel_pinv.hpp> 00040 #include <kdl/chainiksolvervel_wdls.hpp> 00041 #include <kdl/jntarray.hpp> 00042 #include <kdl/jntarrayvel.hpp> 00043 #include <kdl/frames.hpp> 00044 00045 #include <tf/transform_listener.h> 00046 00047 00048 class CobLookatController 00049 { 00050 private: 00051 ros::NodeHandle nh_; 00052 tf::TransformListener tf_listener_; 00053 00054 ros::Subscriber jointstate_sub; 00055 ros::Subscriber lookatstate_sub; 00056 ros::Subscriber twist_sub; 00057 ros::Publisher chain_vel_pub; 00058 ros::Publisher lookat_vel_pub; 00059 std::string chain_vel_pub_topic_; 00060 00061 KDL::Chain chain_; 00062 std::string chain_base_; 00063 std::string chain_tip_; 00064 00065 KDL::ChainIkSolverVel_pinv* p_iksolver_vel_; 00066 KDL::ChainIkSolverVel_wdls* p_iksolver_vel_wdls_; 00067 00068 std::vector<std::string> chain_joints_; 00069 std::vector<std::string> lookat_joints_; 00070 std::vector<std::string> total_joints_; 00071 unsigned int chain_dof_; 00072 unsigned int lookat_dof_; 00073 unsigned int total_dof_; 00074 std::vector<float> chain_limits_vel_; 00075 00076 KDL::JntArray last_q_; 00077 KDL::JntArray last_q_dot_; 00078 00079 00080 public: 00081 CobLookatController() {;} 00082 ~CobLookatController(); 00083 00084 void initialize(); 00085 void run(); 00086 00087 void jointstate_cb(const sensor_msgs::JointState::ConstPtr& msg); 00088 void lookatstate_cb(const sensor_msgs::JointState::ConstPtr& msg); 00089 void twist_cb(const geometry_msgs::Twist::ConstPtr& msg); 00090 00091 void safety_stop_tracking(); 00092 }; 00093 00094 #endif 00095