00001 #ifndef TREE_ID_H 00002 #define TREE_ID_H 00003 00004 #include "robodyn_controllers/KdlTreeId.h" 00005 #include "robodyn_controllers/JointDynamicsData.h" 00006 #include "robodyn_utilities/RateLimiter.h" 00007 00008 #include <r2_msgs/StringArray.h> 00009 #include <sensor_msgs/JointState.h> 00010 #include "r2_msgs/WrenchState.h" 00011 00012 class RosMsgTreeId 00013 { 00014 public: 00015 RosMsgTreeId(); 00016 ~RosMsgTreeId(); 00017 00018 int setBaseFrame(const r2_msgs::StringArray& baseMsg, std::string& baseFrame); 00019 void setJointData(const sensor_msgs::JointState& actualState, 00020 const sensor_msgs::JointState& desiredStates, 00021 const r2_msgs::WrenchState& wrenchState, 00022 JointDynamicsData& jd); 00023 00024 void getJointCommand(const JointDynamicsData& jd, sensor_msgs::JointState& tauFF); 00025 void getJointInertias(const JointDynamicsData& jd, sensor_msgs::JointState& Hv); 00026 void getSegmentForces(const JointDynamicsData& jd, r2_msgs::WrenchState& segForceMsg); 00027 void setEmptyTorqueMsg(const JointDynamicsData& jd, sensor_msgs::JointState& tauFF); 00028 bool GetCompletionMessage(RateLimiter& yl, r2_msgs::StringArray& completionMsg); 00029 00030 }; 00031 00032 #endif