ATI F/T Gamma sensor( 力和力矩传感器 ) 开箱测评 + 使用说明

型号和序列号等参数如下：

Description: Six-Axis Force/Torque Sensor Manufacturer: ATI Industrial Automation Serial Number: FT29352 Model: Gamma Calibration: SI-130-10 Electronics: Net F/T Gain Multiplier: 100%

F/T Tramsducer 将力和力矩转换成电信号并发送到teansducer cable中，这个信号是数字量的信号。其余的一些型号例如Nana和Mini发送的信号是模拟量信号。传感器和盒子接线方式如下，可以通过CAN总线或者网线的方式通信。

 我们采用的方式是通过网线连接，所以Can的端口是被占用的，实际接的是24V直流电源。

上一张实际的接线图，24V电源通过220V转24V的开关电源提供，注意开关电源的电流不要过大。

接下来按照使用手册的说明，应该将网口接到电脑上，电脑的操作系统要求是win7，并且设置电脑的IP地址为192.168.1.100，子网掩码为255.255.255.0，然后在电脑上通过浏览器访问192.168.1.1即可进入传感器的使用和配置界面。我们实际使用的场景是在ROS中的机械臂上，所以操作系统是Linux，由于手册并没有提到linux，所以实测了一下，发现是可以使用的，和windows一模一样。将网线连接到工控机(内置系统为ubuntu14.04+ROS)，设置IP地址为静态IP，192.168.1.xxx即可。设置方法参考：https://blog.csdn.net/qq_34935373/article/details/96474491

网页访问如下，注意system status为healthy，当初测试的时候由于线缆接口的地方有点接触不良，导致system status一直报错，而且盒子的指示灯也没有按照指定的方式闪烁。

大致简单的对传感器进行如下设置，不具有参考性，需要按照实际工作环境和要求设置。

 Setting界面，是否使能低通滤波器，是否对峰值进行监控（每个轴的最高和最低力和力矩值会被保存为最大峰值和最小峰值），设置软件偏移矢量，初始值为0。在Snapshot界面点击bias按钮，即可设置将传感器数据当前值作为零点，此时bias vector数值就是相差的偏移量，可以在setting界面再将bias vector数据都还原成0并apply，实际应用中，每次启动软件会自动将静止状态的传感器数值设置成bias vector，用来消除运动时候的偏差。

Thresholding界面，设置各个轴的阈值，单位可以是N，也可以是counts，其中Counts = Desired Loading Level × Counts per Force  =  N × 1000000 counts/N。

本产品提供了16组配置，方便用户应用于不同场景，设置一组参数然后保存，注意此处需要产品序列号。

注意此处设置的单位，后面要统一。 Tool trasnform全都是0表示的是力矩传感器的原点在底面的中心处。

通信方式，网络通信,由于用CAN接口供电，所以注意Protocol要设成CAN Bus(图示设置会报错DeviceNet Protocol erroIF)。 其中RDT表示通信的方式为UDP的方式，频率为7000HZ.同理也可以使用下面的TCP方式，UDP相比较TCP，速度更快，但是可能会丢包。

设置完成之后，从官网下载java的demo可视化程序，进行简单测试。windows环境下需要事先安装JAVA的运行环境，linux环境下同理。

 

---

F/T传感器的配置是通过发送URL来对硬件进行设置，其实我也可以通过软件编程的方式来对传感器进行设置，而不需要访问网页，而且通过网络编程的方法，可以直接通过TCP或者UDP获取到数据并进行处理，而不需要想上面那样需要demo来显示。

/* 通过UDP读取一次传感器的数据 */

#include 
#include 
#include 
#include 
#include 
#include 
#include 

#define PORT 49152 /* 传感器使用的端口 */
#define COMMAND 2    
#define NUM_SAMPLES 1 /* 采集一次数据 */

/* 定义数据格式和UDP数据类型 */
typedef unsigned int uint32;
typedef int int32;
typedef unsigned short uint16;
typedef short int16;
typedef unsigned char byte;
typedef struct response_struct {
	uint32 rdt_sequence;
	uint32 ft_sequence;
	uint32 status;
	int32 FTData[6];
} RESPONSE;

int main ( int argc, char ** argv ) {
	int socketHandle;			/* Handle to UDP socket used to communicate with Net F/T. */
	struct sockaddr_in addr;	/* Address of Net F/T. */
	struct hostent *he;			/* Host entry for Net F/T. */
	byte request[8];			/* The request data sent to the Net F/T. */
	RESPONSE resp;				/* The structured response received from the Net F/T. */
	byte response[36];			/* The raw response data received from the Net F/T. */
	int i;						/* Generic loop/array index. */
	int err;					/* Error status of operations. */
	char * AXES[] = { "Fx", "Fy", "Fz", "Tx", "Ty", "Tz" };	

    /* 运行程序需要加上目的地址192.168.1.1 */
	if ( 2 > argc )
	{
		fprintf( stderr, "Usage: %s IPADDRESS\n", argv[0] );
		return -1;
	}
	
	/* 开始UDP通信 */
	socketHandle = socket(AF_INET, SOCK_DGRAM, 0);
	if (socketHandle == -1) {
		exit(1);
	}
	
    /* 具体参考 9.1 in Net F/T user manual. */
	*(uint16*)&request[0] = htons(0x1234); 
	*(uint16*)&request[2] = htons(COMMAND); 
	*(uint32*)&request[4] = htonl(NUM_SAMPLES); 
	
	/* 发送请求 */
	he = gethostbyname(argv[1]);
	memcpy(&addr.sin_addr, he->h_addr_list[0], he->h_length);
	addr.sin_family = AF_INET;
	addr.sin_port = htons(PORT);
	
	err = connect( socketHandle, (struct sockaddr *)&addr, sizeof(addr) );
	if (err == -1) {
		exit(2);
	}
	send( socketHandle, request, 8, 0 );

	/* 接收响应 */
	recv( socketHandle, response, 36, 0 );
	resp.rdt_sequence = ntohl(*(uint32*)&response[0]);
	resp.ft_sequence = ntohl(*(uint32*)&response[4]);
	resp.status = ntohl(*(uint32*)&response[8]);
	for( i = 0; i < 6; i++ ) {
		resp.FTData[i] = ntohl(*(int32*)&response[12 + i * 4]);
	}

	/* 打印显示数据 */
	printf( "Status: 0x%08x\n", resp.status );
	for (i =0;i < 6;i++) {
		printf("%s: %d\n", AXES[i], resp.FTData[i]);
	}

	return 0;
}

 通过gcc netft.c -o netft  生成文件运行./netft 192.168.1.1即可。

---

---

---

---

接下来通过ROS对传感器进行设置并读取数据，首先要对传感器的标定矩阵有所了解，知道传感器是如何通过计算得到最终的六轴数据。

 

 

/* 头文件 ft_sensor.h */

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

/* ft_sensor_node.cpp */

#include "ft_sensor.h"
#include 
#include 

// XML 相关的库
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#define rt_dev_socket       socket
#define rt_dev_setsockopt   setsockopt
#define rt_dev_bind         bind
#define rt_dev_recvfrom     recvfrom
#define rt_dev_sendto       sendto
#define rt_dev_close        close
#define rt_dev_connect      connect
#define rt_dev_recv         recv
#define rt_dev_send         send
#define RT_SO_TIMEOUT       SO_RCVTIMEO

static std::string getStringInXml(const std::string& xml_s,const std::string& tag)
{
    const std::string tag_open = "<"+tag+">";
    const std::string tag_close = "";
    const std::size_t n_start = xml_s.find(tag_open);
    const std::size_t n_end = xml_s.find(tag_close);
    return xml_s.substr(n_start+tag_open.length(),n_end);
}
template
static T getNumberInXml(const std::string& xml_s,const std::string& tag)
{
    const std::string num = getStringInXml(xml_s,tag);
    double r = ::atof(num.c_str());
    return static_cast(r);
}
template
static bool getArrayFromString(const std::string& str,const char delim,T *data,size_t len)
{
    size_t start = str.find_first_not_of(delim), end=start;
    size_t idx = 0;
    while (start != std::string::npos && idx < len){
        end = str.find(delim, start);
        std::string token = str.substr(start, end-start);
        if (token.empty())
          token = "0.0";
        double r = ::atof(token.c_str());
        data[idx] = static_cast(r);
        ++idx;
        start = str.find_first_not_of(delim, end);
    }
    return (idx == len);
}
template
static bool getArrayFromXml(const std::string& xml_s,const std::string& tag,const char delim,T *data,size_t len)
{
    const std::string str = getStringInXml(xml_s,tag);
    return getArrayFromString(str,delim,data,len);
}


using namespace ati;

FTSensor::FTSensor()
{
    // 初始化参数
    initialized_                = false;
    ip                          = ati::default_ip;
    port                        = command_s::DEFAULT_PORT;
    cmd_.command                = command_s::STOP;
    cmd_.sample_count           = 1;
    calibration_index           = ati::current_calibration;
    socketHandle_               = -1;
    resp_.cpf                   = 1000000;
    resp_.cpt                   = 1000000;
    rdt_rate_                   = 0;
    timeval_.tv_sec             = 2;
    timeval_.tv_usec            = 0;
    xml_s_.reserve(MAX_XML_SIZE);
    setbias_ = new int[6];
}

FTSensor::~FTSensor()
{
  stopStreaming();
  if(!closeSockets())
    std::cerr << message_header() << "Sensor did not shutdown correctly" << std::endl;
  delete setbias_;
}

bool FTSensor::startStreaming(int nb_samples)
{
  if (nb_samples < 0) {
    // use default sample_count
    return startStreaming();
  }
  else {
    // use given sample count
    uint32_t sample_count = static_cast(nb_samples);
      switch(cmd_.command){
        case command_s::REALTIME:
    //std::cout << "Starting realtime streaming" << std::endl;
    return startRealTimeStreaming(sample_count);
        case command_s::BUFFERED:
    //std::cout << "Starting buffered streaming" << std::endl;
    return startBufferedStreaming(sample_count);
        case command_s::MULTIUNIT:
    //std::cout << "Starting multi-unit streaming" << std::endl;
    return startMultiUnitStreaming(sample_count);
        default:
    std::cout <<  message_header() << cmd_.command << ": command mode not allowed" << std::endl;
    return false;
      }
  }
}

// Initialization read from XML file
bool FTSensor::startStreaming()
{
    switch(cmd_.command){
      case command_s::REALTIME:
	//std::cout << "Starting realtime streaming" << std::endl;
	return startRealTimeStreaming();
      case command_s::BUFFERED:
	//std::cout << "Starting buffered streaming" << std::endl;
	return startBufferedStreaming();
      case command_s::MULTIUNIT:
	//std::cout << "Starting multi-unit streaming" << std::endl;
	return startMultiUnitStreaming();
      default:
	std::cout << message_header() << cmd_.command<< ": command mode not allowed" << std::endl;
	return false;
    }
}

bool FTSensor::init(std::string ip, int calibration_index, uint16_t cmd, int sample_count)
{
  //  Re-Initialize parameters
  initialized_ = true;
  this->ip = ip;
  this->port = command_s::DEFAULT_PORT;
  cmd_.command = command_s::STOP;
  cmd_.sample_count = 1;
  this->calibration_index = calibration_index;

  //  Open Socket
  if(!ip.empty() && openSockets())
  {
    if(!stopStreaming()) // if previously launched
        std::cerr << "\033[33m" << message_header() << "Could not stop streaming\033[0m" << std::endl;
    setCommand(cmd); // Setting cmd mode

    initialized_ &= startStreaming(sample_count);            // Starting streaming

    if (!initialized_)
    {
        std::cerr << "\033[1;31m" << message_header() << "Could not start streaming\033[0m" << std::endl;
        return initialized_;
    }
    initialized_ &= getResponse();

    // Parse Calibration from web server
    if(initialized_)
        getCalibrationData();
  }else
    initialized_ = false;

  if (!initialized_)
    std::cerr << "\033[1;31m" << message_header() << "Error during initialization, FT sensor NOT started\033[0m" << std::endl;

  return initialized_;
}
bool FTSensor::openSockets()
{
  try{
    // To get the online configuration (need to build rtnet with TCP option)
    openSocket(socketHTTPHandle_,getIP(),80,IPPROTO_TCP);
    // The data socket
    openSocket(socketHandle_,getIP(),getPort(),IPPROTO_UDP);
  }
  catch (std::exception &ex) {
    std::cerr << "\033[1;31m" << message_header() <<  "openSockets error: " << ex.what()  <<"\033[0m" << std::endl;
    return false;
  }
  return true;
}
void FTSensor::openSocket(int& handle,const std::string ip,const uint16_t port,const int option)
{
  // create the socket
    if (handle != -1)
        rt_dev_close(handle);
    std::string proto = "";
    if(option == IPPROTO_UDP)
    {
        proto = "UDP";
        handle = rt_dev_socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
    }
    else if(option == IPPROTO_TCP)
    {
        proto = "TCP";
        handle = rt_dev_socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
    }
    else
        handle = rt_dev_socket(AF_INET, SOCK_DGRAM, 0);

    if (handle < 0) {
        std::cerr << "\033[31m" << message_header() << "Could not init sensor socket for proto [" << proto 
                  << "], please make sure your can ping the sensor\033[0m" << std::endl;
        throw std::runtime_error("failed to init sensor socket");
    }

    // re-use address in case it's still binded
    rt_dev_setsockopt(handle, SOL_SOCKET, SO_REUSEADDR, 0, 0);

    // set the socket parameters
    struct sockaddr_in addr = {0};
    hostent * hePtr = NULL;
    hePtr = gethostbyname(ip.c_str());
    memcpy(&addr.sin_addr, hePtr->h_addr_list[0], hePtr->h_length);

    //addr_.sin_addr.s_addr = INADDR_ANY;
    addr.sin_family = AF_INET;
    addr.sin_port = htons(port);

    // connect
    if (rt_dev_connect(handle, (struct sockaddr*) &addr, sizeof(addr)) < 0)
    {
        std::cerr << "\033[31m" << message_header() << "Could not connect to " << ip << "\033[0m" << std::endl ;
        throw std::runtime_error("failed to connect to socket" );
    }
    return;
}
bool FTSensor::closeSockets()
{
  return closeSocket(socketHandle_) == 0 && closeSocket(socketHTTPHandle_) == 0;
}
int FTSensor::closeSocket(const int& handle)
{
  if(handle < 0)
    return 0;  // closing a non-open socket is considered successful
  return rt_dev_close(handle);
}

bool FTSensor::getCalibrationData()
{
  FTSensor::settings_error_t err = getSettings();
  if(err!=SETTINGS_REQUEST_ERROR && err!=CALIB_PARSE_ERROR)
  {
      std::cout << message_header() << "Sucessfully retrieved counts per force : " << resp_.cpf << std::endl;
      std::cout << message_header() << "Sucessfully retrieved counts per torque : " << resp_.cpt << std::endl;
  }
  else
  {
      std::cerr << message_header() << "Using default counts per force : " << resp_.cpf << std::endl;
      std::cerr << message_header() << "Using default counts per torque : " << resp_.cpt << std::endl;
  }
}

FTSensor::settings_error_t FTSensor::getSettings()
{
  std::string index("");
  if(calibration_index != ati::current_calibration)
  {
    std::stringstream ss;
    ss << calibration_index;
    index = "?index=" + ss.str();
    std::cout << message_header() << "Using calibration index "<(xml_s_,"cfgcpf");
    const uint32_t cfgcpt_r = getNumberInXml(xml_s_,"cfgcpt");
    const int cfgcomrdtrate = getNumberInXml(xml_s_,"comrdtrate");
    rdt_rate_ = cfgcomrdtrate;

    // 6 tokens separated by semi-colon
    if (!getArrayFromXml(xml_s_,"setbias",';',setbias_, 6))
    {
        return GAUGE_PARSE_ERROR;
    }

    if(cfgcpf_r && cfgcpt_r)
    {
        resp_.cpf = cfgcpf_r;
        resp_.cpt = cfgcpt_r;
        return NO_SETTINGS_ERROR;
    }
  std::cerr << message_header() << "Could not parse file " << filename << std::endl;
  return SETTINGS_REQUEST_ERROR;
}

bool FTSensor::sendTCPrequest(std::string &request_cmd)
{
  if (request_cmd.empty() )
  {
    std::cerr << message_header() << "Empty TCP command, not sending" << std::endl;
    return false;
  }
  else
  {
    static const uint32_t chunkSize = 4;        // Every chunk of data will be of this size
    static const uint32_t maxSize = 65536;      // The maximum file size to receive
    std::string host = getIP();

    std::string request_s = "GET "+request_cmd+" HTTP/1.0\r\nHost: "+host+"\r\n\r\n";

    if (rt_dev_send(socketHTTPHandle_, request_s.c_str(),request_s.length(), 0) < 0)
    {
        std::cerr << message_header() << "Could not send GET request to " << host
                  << ":80. Please make sure that RTnet TCP protocol is installed" << std::endl;
        return false;
    }

    //empty the buffer but we don't care about the result
    int recvLength=0;
    int posBuff = 0;
    while(posBuff < maxSize) // Just a security to avoid infinity loop
    {
        recvLength = rt_dev_recv(socketHTTPHandle_, &xml_c_[posBuff],chunkSize, 0);
        posBuff += recvLength;
        if(recvLength <= 0) // The last chunk returns 0
            break;
    }
    if (posBuff > 4)
    {
        const char *awaited_response = "HTTP/1.0 302 Found";
        if (strncmp(xml_c_, awaited_response, 18 )==0)
        {
            return true;
        }
        else
        {
            std::cerr << message_header() << "Bad response from set command. Response is :" <<  xml_c_ << std::endl;
            return false;
        }
    }
    else
    {
        std::cerr << message_header() << "Bad response from set command. Response is :" <<  xml_c_  << std::endl;
        return false;
    }
  }
}


bool FTSensor::setRDTOutputRate(unsigned int rate)
{
  if (rate > 0 && rate <= 7000)
  {
      std::stringstream cfgcomrdtrate_ss;
      cfgcomrdtrate_ss << rate;
      std::string cmd = "/comm.cgi?comrdtrate=" + cfgcomrdtrate_ss.str();

      if(sendTCPrequest(cmd))
      {
          // we consider the rate was set and don't read it back
          rdt_rate_ = rate;
          return true;
      }
      else
          return false;
  }
  else
  {
      std::cerr << message_header() << "RDT rate must be in range [1-7000]" << std::endl;
      return false;
  }
}


bool FTSensor::setGaugeBias(unsigned int gauge_idx, int gauge_bias)
{
  std::map map;
  map[gauge_idx] = gauge_bias;
  return setGaugeBias(map);
}

std::vector FTSensor::getGaugeBias()
{
  FTSensor::settings_error_t err = getSettings();
  if(err!=SETTINGS_REQUEST_ERROR && err!=GAUGE_PARSE_ERROR)
  {
     std::vector bias(setbias_, setbias_ + 6);
     return bias;
  }
  else
  {
     std::cerr << message_header() << "Could not get gauge bias values" << std::endl;
     return std::vector();
  }
}


bool FTSensor::setGaugeBias(std::vector &gauge_vect)
{
  std::map map;
  for (size_t i=0; i < gauge_vect.size(); ++i)
  {
    map[i] = gauge_vect[i];
  }
  return setGaugeBias(map);
}

bool FTSensor::setGaugeBias(std::map &gauge_map)
{
  std::stringstream setbias_ss;
  std::map::iterator it;
  bool first_element = true;
  //prepare the query
  for (it=gauge_map.begin(); it!=gauge_map.end(); ++it)
  {
    if( it->first < 6)
    {
      if(first_element)
      {
        setbias_ss << "?";
        first_element = false;
      }
      else
      {
        setbias_ss << "&";
      }
      setbias_ss << "setbias" << it->first << "=" << it->second;
    }
    else
    {
      std::cerr << message_header() << "Invalid gauge number "<< it->first << std::endl;
      return false;
    }
  }

  std::string host = getIP();
  std::string cmd = "/setting.cgi" + setbias_ss.str();
  return sendTCPrequest(cmd);
}

bool FTSensor::sendCommand()
{
  return sendCommand(cmd_.command);
}

bool FTSensor::sendCommand(uint16_t cmd)
{
  *reinterpret_cast(&request_[0]) = htons(command_s::command_header);
  *reinterpret_cast(&request_[2]) = htons(cmd);
  *reinterpret_cast(&request_[4]) = htonl(cmd_.sample_count);
  //return rt_dev_sendto(socketHandle_, (void*) &request_, sizeof(request_), 0, (sockaddr*) &addr_, addr_len_ ) == 8;
  return rt_dev_send(socketHandle_, (void*) &request_, sizeof(request_), 0) == sizeof(request_);//, (sockaddr*) &addr_, addr_len_ ) == 8;
}

bool FTSensor::getResponse()
{

  //response_ret_ = rt_dev_recvfrom(socketHandle_, (void*) &response_, sizeof(response_), 0, (sockaddr*) &addr_, &addr_len_ );
  response_ret_ = rt_dev_recv(socketHandle_, (void*) &response_, sizeof(response_), 0);//, (sockaddr*) &addr_, &addr_len_ );
  resp_.rdt_sequence = ntohl(*reinterpret_cast(&response_[0]));
  resp_.ft_sequence = ntohl(*reinterpret_cast(&response_[4]));
  resp_.status = ntohl(*reinterpret_cast(&response_[8]));
  resp_.Fx = static_cast(ntohl(*reinterpret_cast(&response_[12 + 0 * 4])));
  resp_.Fy = static_cast(ntohl(*reinterpret_cast(&response_[12 + 1 * 4])));
  resp_.Fz = static_cast(ntohl(*reinterpret_cast(&response_[12 + 2 * 4])));
  resp_.Tx = static_cast(ntohl(*reinterpret_cast(&response_[12 + 3 * 4])));
  resp_.Ty = static_cast(ntohl(*reinterpret_cast(&response_[12 + 4 * 4])));
  resp_.Tz = static_cast(ntohl(*reinterpret_cast(&response_[12 + 5 * 4])));
  if (response_ret_ < 0)
  {
    std::cerr << "\033[1;31m" << message_header() << "Error while receiving: " << strerror(errno) << "\033[0m" << std::endl;
  }
  if (response_ret_!=RDT_RECORD_SIZE)
    std::cerr << message_header() <<  "Error of package size " <setSoftwareBias();
}
bool FTSensor::isInitialized()
{
    return initialized_;
}

void FTSensor::setTimeout(float sec)
{
    if (sec <= 0) {
        std::cerr << message_header() << "Can't set timeout <= 0 sec" << std::endl;
        return;
    }

    if (isInitialized()) {
        std::cerr << message_header() << "Can't set timeout if socket is initialized, call this before init()." << std::endl;
        return;
    }
  timeval_.tv_sec = static_cast(sec);
  timeval_.tv_usec = static_cast(sec/1.e6);
}

bool FTSensor::resetThresholdLatch()
{
  if(! sendCommand(command_s::RESET_THRESHOLD_LATCH)){
    std::cerr << message_header() << "Could not start reset threshold latch" << std::endl;
      return false;
  }
  return true;
}
bool FTSensor::setSoftwareBias()
{
  //if(!stopStreaming())
      //std::cerr << "Could not stop streaming" << std::endl;
  if(! sendCommand(command_s::SET_SOFWARE_BIAS)){
    ;//std::cerr << "Could not set software bias" << std::endl;
      return false;
  }
  //if(!startStreaming())
      //std::cerr << "Could not restart streaming" << std::endl;
  return true;
}
bool FTSensor::stopStreaming()
{
  return sendCommand(command_s::STOP);
}

bool FTSensor::startBufferedStreaming(uint32_t sample_count)
{
  setSampleCount(sample_count);
  setCommand(command_s::BUFFERED);
  if(! sendCommand()){
    std::cerr << message_header() << "Could not start buffered streaming" << std::endl;
      return false;
  }
  return true;
}
bool FTSensor::startMultiUnitStreaming(uint32_t sample_count)
{
  setSampleCount(sample_count);
  setCommand(command_s::MULTIUNIT);
  if(! sendCommand()){
    std::cerr << message_header() << "Could not start multi-unit streaming" << std::endl;
      return false;
  }
  return true;
}
bool FTSensor::startRealTimeStreaming(uint32_t sample_count)
{
  setSampleCount(sample_count);
  setCommand(command_s::REALTIME);
  if(! sendCommand()){
    std::cerr << message_header() << "Could not start realtime streaming" << std::endl;
    return false;
  }
  std::cout << message_header() << "Start realtime streaming with " << sample_count <<" samples " << std::endl;
  return true;
}

void FTSensor::setCommand(uint16_t cmd)
{
  this->cmd_.command = cmd;
}

void FTSensor::setSampleCount(uint32_t sample_count)
{
  this->cmd_.sample_count = sample_count;
}

/* ft_sensor_node.cpp */

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include "ft_sensor.h"

namespace ftsensor {

class FTSensorPublisher
{
private:
    //! The node handle
    ros::NodeHandle nh_;
    //! Node handle in the private namespace
    ros::NodeHandle priv_nh_;

    //! The sensor
    boost::shared_ptr ftsensor_;
    std::string ip_;
    std::string frame_ft_;

    //! Publisher for sensor readings
    ros::Publisher pub_sensor_readings_;

    //! Service for setting the bias
    ros::ServiceServer srv_set_bias_;

public:
    //------------------ Callbacks -------------------
    // Callback for setting bias
    bool setBiasCallback(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response);

    // Publish the measurements
    void publishMeasurements();

    //! Subscribes to and advertises topics
    FTSensorPublisher(ros::NodeHandle nh) : nh_(nh), priv_nh_("~")
    {
        priv_nh_.param("frame", frame_ft_, "/ati_ft_link");
        priv_nh_.param("ip", ip_, "192.168.1.1");

        ROS_INFO_STREAM("ATISensor IP : "<< ip_);
        ROS_INFO_STREAM("ATISensor frame : "<< frame_ft_);

        // Create a new sensor
        ftsensor_ = boost::shared_ptr(new ati::FTSensor());

        // Init FT Sensor
        if (ftsensor_->init(ip_))
        {
            // Set bias
            ftsensor_->setBias();

            ROS_INFO_STREAM("ATISensor RDT Rate : "<< ftsensor_->getRDTRate());

            // Advertise topic where readings are published
            pub_sensor_readings_ = priv_nh_.advertise("data", 10);

            // Advertise service for setting the bias
            srv_set_bias_ = priv_nh_.advertiseService("set_bias", &FTSensorPublisher::setBiasCallback, this);
        }
        else
        {
            throw std::runtime_error("FT sensor could not initialize");
        }
    }

    //! Empty stub
    ~FTSensorPublisher() {}

};

/* 每次启动自动设置bias vetor,用来消除运动时候的偏差 */
bool FTSensorPublisher::setBiasCallback(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
{
    ftsensor_->setBias();
}

void FTSensorPublisher::publishMeasurements()
    {
    // Recall that this has to be transformed using the stewart platform
    //tf_broadcaster_.sendTransform(tf::StampedTransform(nano_top_frame_, ros::Time::now(), "/world", "/nano_top_frame"));
    geometry_msgs::WrenchStamped ftreadings;
    float measurements[6];
    ftsensor_->getMeasurements(measurements);

    ftreadings.wrench.force.x = measurements[0];
    ftreadings.wrench.force.y = measurements[1];
    ftreadings.wrench.force.z = measurements[2];
    ftreadings.wrench.torque.x = measurements[3];
    ftreadings.wrench.torque.y = measurements[4];
    ftreadings.wrench.torque.z = measurements[5];

    ftreadings.header.stamp = ros::Time::now();
    ftreadings.header.frame_id = frame_ft_;

    pub_sensor_readings_.publish(ftreadings);

    ROS_INFO("Measured Force: %f %f %f Measured Torque: %f %f %f", measurements[0], measurements[1], measurements[2], measurements[3], measurements[4], measurements[5]);
}

} // namespace ftsensor

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ft_sensor");
    ros::NodeHandle nh;
    ros::Rate loop(100);
    try
    {
        ftsensor::FTSensorPublisher node(nh);
        while(ros::ok())
        {
            node.publishMeasurements();
            ros::spinOnce();

            loop.sleep();
        }
    }
    catch (std::exception &ex) {
        ROS_FATAL_STREAM("Failed with error : " << ex.what());
        return -1;
    }
    return 0;
}

编写成ROS节点，修改 CMakeLists.txt和package.xml，通过catkin_make编译，运行节点即可。