17 #ifndef ATI_FT_SENSOR_ATI_FORCE_TORQUE_SENSOR_TWE_H_ 18 #define ATI_FT_SENSOR_ATI_FORCE_TORQUE_SENSOR_TWE_H_ 22 #include <geometry_msgs/WrenchStamped.h> 23 #include <boost/thread.hpp> 24 #include <eigen3/Eigen/Eigen> 25 #include <yaml-cpp/yaml.h> 38 bool initialize(
const std::string& ft_data_path,
const std::string& ft_data_key,
39 const std::string& ft_frame_id,
40 const std::string& ft_raw_publish_name,
const std::string& ft_scaled_publish_name);
44 void setScaleParam(
double ft_scale_factor, Eigen::MatrixXd ft_null);
48 double voltage3,
double voltage4,
double voltage5);
55 double *torque_x_Nm,
double *torque_y_Nm,
double *torque_z_Nm);
57 double *torque_x_Nm,
double *torque_y_Nm,
double *torque_z_Nm);
60 double voltage3,
double voltage4,
double voltage5);
64 bool parseFTData(
const std::string& ft_data_path,
const std::string& ft_data_key);
ros::Publisher ft_raw_pub_
Eigen::MatrixXd getCurrentForceTorqueRaw()
void setCurrentVoltageOutput(double voltage0, double voltage1, double voltage2, double voltage3, double voltage4, double voltage5)
bool is_ft_scaled_published_
ATIForceTorqueSensorTWE()
std::string ft_scaled_publish_name_
Eigen::MatrixXd ft_scaled_
boost::mutex ft_scale_param_mutex_
bool is_ft_raw_published_
void setScaleParam(double ft_scale_factor, Eigen::MatrixXd ft_null)
void setCurrentVoltageOutputPublish(double voltage0, double voltage1, double voltage2, double voltage3, double voltage4, double voltage5)
void setScaleFactror(double ft_scale_factor)
Eigen::MatrixXd ft_current_volatge_
bool parseFTData(const std::string &ft_data_path, const std::string &ft_data_key)
bool initialize(const std::string &ft_data_path, const std::string &ft_data_key, const std::string &ft_frame_id, const std::string &ft_raw_publish_name, const std::string &ft_scaled_publish_name)
~ATIForceTorqueSensorTWE()
geometry_msgs::WrenchStamped ft_raw_msg_
void setNullForceTorque(Eigen::MatrixXd _ft_null)
Eigen::MatrixXd ft_coeff_mat_
std::string ft_raw_publish_name_
Eigen::MatrixXd ft_unload_volatge_
Eigen::MatrixXd getCurrentForceTorqueScaled()
geometry_msgs::WrenchStamped ft_scaled_msg_
ros::Publisher ft_scaled_pub_