00001 #ifndef __WRENCH_MANAGER__ 00002 #define __WRENCH_MANAGER__ 00003 00004 #include <eigen_conversions/eigen_kdl.h> 00005 #include <eigen_conversions/eigen_msg.h> 00006 #include <geometry_msgs/WrenchStamped.h> 00007 #include <kdl_conversions/kdl_msg.h> 00008 #include <ros/ros.h> 00009 #include <tf/transform_broadcaster.h> 00010 #include <tf/transform_listener.h> 00011 #include <Eigen/Dense> 00012 #include <generic_control_toolbox/manager_base.hpp> 00013 #include <generic_control_toolbox/matrix_parser.hpp> 00014 00015 namespace generic_control_toolbox 00016 { 00021 class WrenchManager : public ManagerBase 00022 { 00023 public: 00024 WrenchManager(ros::NodeHandle nh_ = ros::NodeHandle("~")); 00025 ~WrenchManager(); 00026 00038 bool initializeWrenchComm(const std::string &end_effector, 00039 const std::string &sensor_frame, 00040 const std::string &gripping_point_frame, 00041 const std::string &sensor_topic, 00042 const std::string &calib_matrix_param); 00043 00052 bool wrenchAtGrippingPoint(const std::string &end_effector, 00053 Eigen::Matrix<double, 6, 1> &wrench) const; 00054 00062 bool wrenchAtSensorPoint(const std::string &end_effector, 00063 Eigen::Matrix<double, 6, 1> &wrench) const; 00064 00065 private: 00066 int max_tf_attempts_; 00067 std::map<std::string, std::string> sensor_frame_; 00068 std::map<std::string, KDL::Frame> sensor_to_gripping_point_; 00069 std::map<std::string, KDL::Wrench> measured_wrench_; 00070 std::map<std::string, ros::Subscriber> ft_sub_; 00071 std::map<std::string, ros::Publisher> processed_ft_pub_; 00072 std::map<std::string, std::string> gripping_frame_; 00073 std::map<std::string, Eigen::Matrix<double, 6, 6> > calibration_matrix_; 00074 tf::TransformListener listener_; 00075 MatrixParser parser_; 00076 ros::NodeHandle nh_; 00077 00083 void forceTorqueCB(const geometry_msgs::WrenchStamped::ConstPtr &msg); 00084 }; 00085 00094 bool setWrenchManager(const ArmInfo &arm_info, WrenchManager &manager); 00095 bool setWrenchManager(const ArmInfo &arm_info, 00096 std::shared_ptr<WrenchManager> &manager); 00097 } // namespace generic_control_toolbox 00098 #endif