00001 #ifndef ROS_MSG_GAIN_CALCULATOR_H 00002 #define ROS_MSG_GAIN_CALCULATOR_H 00003 00004 #include "robodyn_controllers/GainCalculator.h" 00005 #include "robodyn_controllers/JointGainsData.h" 00006 #include "robodyn_controllers/KdlTreeUtilities.h" 00007 #include "robodyn_utilities/RateLimiter.h" 00008 00009 #include <boost/function.hpp> 00010 #include <sensor_msgs/JointState.h> 00011 #include <r2_msgs/Gains.h> 00012 #include <r2_msgs/StringArray.h> 00013 #include "robot_instance/StringUtilities.h" 00014 #include "robodyn_mechanisms/JointCommandFactory.h" 00015 00016 class RosMsgGainCalculator : public KdlTreeUtilities 00017 { 00018 public: 00019 RosMsgGainCalculator(); 00020 ~RosMsgGainCalculator(); 00021 00022 void initialize(); 00023 00024 void storeGainInformation(const r2_msgs::Gains& desiredDyn); 00025 00026 void storeGainProperties(); 00027 00028 void findDesiredDynamics(const sensor_msgs::JointState& inertia, 00029 r2_msgs::Gains& desiredGains); 00030 00031 void setDefaultGains(const double& wn, const double& zeta, const double& windup); 00032 00033 bool getCompletionMessage(r2_msgs::StringArray& completionMsg); 00034 00035 RateLimiter yl, ylWindup; 00036 boost::function<float(std::string)> getBrainstemCoeff; 00037 boost::function<std::string(std::string)> getCommandFile; 00038 00039 std::map<std::string, JointGainsData> jointGainsData; 00040 00041 private: 00042 std::map<std::string, JointGainsData>::iterator jointGainsIt; 00043 bool initialized; 00044 00045 }; 00046 00047 #endif