00001 00006 #ifndef TREE_FK_H 00007 #define TREE_FK_H 00008 00009 #include "robodyn_controllers/KdlTreeFk.h" 00010 #include "r2_msgs/PoseState.h" 00011 #include <sensor_msgs/JointState.h> 00012 #include "robodyn_utilities/RosMsgConverter.h" 00013 #include <tf_conversions/tf_kdl.h> 00014 00015 class RosMsgTreeFk : public KdlTreeFk 00016 { 00017 public: 00018 RosMsgTreeFk(); 00019 ~RosMsgTreeFk(); 00020 00021 void reset() 00022 { 00023 prevVel.clear(); 00024 } 00025 00026 bool getPoseState(const sensor_msgs::JointState& jointState, r2_msgs::PoseState& poseState); 00027 00028 virtual void initialize(); 00029 00030 protected: 00031 00032 00033 private: 00034 typedef std::map<std::string, std::pair<ros::Time, geometry_msgs::Twist> > prevVel_type; 00035 prevVel_type prevVel; 00036 std::vector<std::string> jointNames; 00037 KDL::JntArrayVel joints; 00038 std::map<std::string, KDL::FrameVel> poseMap; 00039 }; 00040 00041 #endif