24 #ifndef THORMANG3_FEET_FT_MODULE_FEET_FORCE_TORQUE_SENSOR_MODULE_H_ 25 #define THORMANG3_FEET_FT_MODULE_FEET_FORCE_TORQUE_SENSOR_MODULE_H_ 30 #include <std_msgs/String.h> 31 #include <boost/thread.hpp> 32 #include <eigen3/Eigen/Eigen> 33 #include <yaml-cpp/yaml.h> 35 #include "robotis_controller_msgs/StatusMsg.h" 36 #include "thormang3_feet_ft_module_msgs/BothWrench.h" 64 void process(std::map<std::string, robotis_framework::Dynamixel *> dxls, std::map<std::string, robotis_framework::Sensor *> sensors);
74 void publishBothFTData(
int type, Eigen::MatrixXd &ft_right, Eigen::MatrixXd &ft_left);
Eigen::MatrixXd r_foot_ft_gnd_
ros::Publisher thormang3_foot_ft_both_ft_pub_
double l_foot_fx_scaled_N_
void initialize(const int control_cycle_msec, robotis_framework::Robot *robot)
Eigen::MatrixXd r_foot_ft_air_
double l_foot_ft_current_voltage_[6]
double r_foot_tz_scaled_Nm_
double r_foot_ft_current_voltage_[6]
boost::mutex publish_mutex_
double r_foot_fy_scaled_N_
double r_foot_fx_scaled_N_
double l_foot_ty_scaled_Nm_
double l_foot_fy_scaled_N_
double r_foot_ty_scaled_Nm_
void process(std::map< std::string, robotis_framework::Dynamixel * > dxls, std::map< std::string, robotis_framework::Sensor * > sensors)
boost::thread queue_thread_
double l_foot_tx_scaled_Nm_
double r_foot_fz_scaled_N_
double l_foot_fz_scaled_N_
double l_foot_tz_scaled_Nm_
KinematicsDynamics * thormang3_kd_
void publishBothFTData(int type, Eigen::MatrixXd &ft_right, Eigen::MatrixXd &ft_left)
void ftSensorCalibrationCommandCallback(const std_msgs::String::ConstPtr &msg)
double r_foot_tx_scaled_Nm_
ATIForceTorqueSensorTWE r_foot_ft_sensor_
Eigen::MatrixXd l_foot_ft_air_
void publishStatusMsg(unsigned int type, std::string msg)
double l_foot_ft_scale_factor_
Eigen::MatrixXd l_foot_ft_gnd_
void saveFTCalibrationData(const std::string &path)
ATIForceTorqueSensorTWE l_foot_ft_sensor_
double r_foot_ft_scale_factor_
void initializeFeetForceTorqueSensor()
ros::Publisher thormang3_foot_ft_status_pub_