型号和序列号等参数如下：
| 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 <arpa/inet.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netdb.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>#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 <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <string.h>
#include <fstream>
#include <sys/types.h>
#include <sys/socket.h>
#include <errno.h>
#include <netdb.h>
#include <arpa/inet.h>
#include <stdint.h>
#include <unistd.h>
#include <sstream>
#include <map>
#include <vector>#define MAX_XML_SIZE 35535
#define RDT_RECORD_SIZE 36namespace ati{
static const std::string default_ip = "192.168.1.1";
static const int current_calibration=-1;
// 数据相应结构体
typedef struct response_struct {uint32_t rdt_sequence;uint32_t ft_sequence;uint32_t status;int32_t Fx;int32_t Fy;int32_t Fz;int32_t Tx;int32_t Ty;int32_t Tz;uint32_t cpt;uint32_t cpf;
} response_s;// 发送指令
typedef struct command_struct{static const uint16_t command_header = 0x1234;uint16_t command;uint32_t sample_count;static const uint16_t STOP = 0x0000;static const uint16_t REALTIME = 0x0002;static const uint16_t BUFFERED = 0x0003;static const uint16_t MULTIUNIT = 0x0004;static const uint16_t RESET_THRESHOLD_LATCH = 0x0041;static const uint16_t SET_SOFWARE_BIAS = 0x0042;static const int DEFAULT_PORT = 49152;
} command_s;class FTSensor{
public:FTSensor();~FTSensor();enum settings_error_t{NO_SETTINGS_ERROR,SETTINGS_REQUEST_ERROR,CALIB_PARSE_ERROR,GAUGE_PARSE_ERROR,RDTRATE_PARSE_ERROR};// 初始化,从XML中读取配置参数bool init(std::string ip, int calibration_index = ati::current_calibration,uint16_t cmd = ati::command_s::REALTIME, int sample_count = -1);// 读取力和力矩counts数据值const double getCountsperForce(){return resp_.cpf;};const double getCountsperTorque(){return resp_.cpt;};// 读取真实数据值template<typename T>void getMeasurements(T measurements[6]){doComm();measurements[0]=static_cast<T>( resp_.Fx ) / static_cast<T>(resp_.cpf);measurements[1]=static_cast<T>( resp_.Fy ) / static_cast<T>(resp_.cpf);measurements[2]=static_cast<T>( resp_.Fz ) / static_cast<T>(resp_.cpf);measurements[3]=static_cast<T>( resp_.Tx ) / static_cast<T>(resp_.cpt);measurements[4]=static_cast<T>( resp_.Ty ) / static_cast<T>(resp_.cpt);measurements[5]=static_cast<T>( resp_.Tz ) / static_cast<T>(resp_.cpt);}template<typename T>void getMeasurements(T measurements[6],uint32_t& rdt_sequence){getMeasurements<T>(measurements);rdt_sequence = resp_.rdt_sequence;}template<typename T>void getMeasurements(T measurements[6],uint32_t& rdt_sequence,uint32_t& ft_sequence){getMeasurements<T>(measurements,rdt_sequence);ft_sequence = resp_.ft_sequence;}// 写入IP地址const std::string getIP(){return this->ip;}const uint16_t getPort(){return this->port;}std::string message_header(){std::stringstream ss;ss << "[ft_sensor " << this->ip << ":" << this->port << "] ";return ss.str();}// 数据传输频率const int getRDTRate(){return this->rdt_rate_;}// 归零void setBias();void setTimeout(float sec);bool isInitialized();bool getCalibrationData();settings_error_t getSettings();bool setRDTOutputRate(unsigned int rate);std::vector<int> getGaugeBias();bool setGaugeBias(unsigned int gauge_idx, int gauge_bias);bool setGaugeBias(std::map<unsigned int, int> &gauge_map);bool setGaugeBias(std::vector<int> &gauge_vect);protected:// 套接字信息bool startRealTimeStreaming(uint32_t sample_count=1);bool startBufferedStreaming(uint32_t sample_count=100);bool startMultiUnitStreaming(uint32_t sample_count=100);bool resetThresholdLatch();bool setSoftwareBias();bool stopStreaming();bool startStreaming();bool startStreaming(int nb_samples);bool openSockets();void openSocket(int& handle, const std::string ip, const uint16_t port, const int option);bool closeSockets();int closeSocket(const int& handle);void setCommand(uint16_t cmd);void setSampleCount(uint32_t sample_count);bool sendCommand();bool sendCommand(uint16_t cmd);bool getResponse();bool sendTCPrequest(std::string &request_cmd);void doComm();std::string ip;uint16_t port;int calibration_index;int rdt_rate_;int *setbias_;int socketHandle_;int socketHTTPHandle_;struct sockaddr_in addr_;socklen_t addr_len_;struct hostent *hePtr_;// 通信协议response_s resp_;command_s cmd_;unsigned char request_[8];unsigned char response_[RDT_RECORD_SIZE];bool initialized_;bool timeout_set_;struct timeval timeval_;int response_ret_;char xml_c_[MAX_XML_SIZE];std::string xml_s_;};
}
/* ft_sensor_node.cpp */#include "ft_sensor.h"
#include <stdexcept>
#include <sys/socket.h>// XML 相关的库
#include <libxml/parser.h>
#include <libxml/tree.h>
#include <libxml/encoding.h>
#include <libxml/xmlwriter.h>
#include <sstream>
#include <vector>
#include <string>#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_RCVTIMEOstatic std::string getStringInXml(const std::string& xml_s,const std::string& tag)
{const std::string tag_open = "<"+tag+">";const std::string tag_close = "</"+tag+">";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<typename T>
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<T>(r);
}
template<typename T>
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<T>(r);++idx;start = str.find_first_not_of(delim, end);}return (idx == len);
}
template<typename T>
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<T>(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_countreturn startStreaming();}else {// use given sample countuint32_t sample_count = static_cast<uint32_t>(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 parametersinitialized_ = 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 Socketif(!ip.empty() && openSockets()){if(!stopStreaming()) // if previously launchedstd::cerr << "\033[33m" << message_header() << "Could not stop streaming\033[0m" << std::endl;setCommand(cmd); // Setting cmd modeinitialized_ &= startStreaming(sample_count); // Starting streamingif (!initialized_){std::cerr << "\033[1;31m" << message_header() << "Could not start streaming\033[0m" << std::endl;return initialized_;}initialized_ &= getResponse();// Parse Calibration from web serverif(initialized_)getCalibrationData();}elseinitialized_ = 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 socketopenSocket(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 socketif (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);}elsehandle = 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 bindedrt_dev_setsockopt(handle, SOL_SOCKET, SO_REUSEADDR, 0, 0);// set the socket parametersstruct 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);// connectif (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 successfulreturn 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 "<<calibration_index<< std::endl;}elsestd::cout << message_header() << "Using current calibration" << std::endl;static const uint32_t chunkSize = 4; // Every chunk of data will be of this sizestatic const uint32_t maxSize = 65536; // The maximum file size to receive// The recv bufferstd::string filename = "/netftapi2.xml"+index; // the name of the file to reveicestd::string host = getIP();std::string request_s = "GET "+filename+" HTTP/1.1\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 " << getIP()<< ":80. Please make sure that RTnet TCP protocol is installed" << std::endl;return SETTINGS_REQUEST_ERROR;}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 0break;}xml_s_ = xml_c_;const uint32_t cfgcpf_r = getNumberInXml<uint32_t>(xml_s_,"cfgcpf");const uint32_t cfgcpt_r = getNumberInXml<uint32_t>(xml_s_,"cfgcpt");const int cfgcomrdtrate = getNumberInXml<int>(xml_s_,"comrdtrate");rdt_rate_ = cfgcomrdtrate;// 6 tokens separated by semi-colonif (!getArrayFromXml<int>(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 sizestatic const uint32_t maxSize = 65536; // The maximum file size to receivestd::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 resultint 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 0break;}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 backrdt_rate_ = rate;return true;}elsereturn 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<unsigned int, int> map;map[gauge_idx] = gauge_bias;return setGaugeBias(map);
}std::vector<int> FTSensor::getGaugeBias()
{FTSensor::settings_error_t err = getSettings();if(err!=SETTINGS_REQUEST_ERROR && err!=GAUGE_PARSE_ERROR){std::vector<int> bias(setbias_, setbias_ + 6);return bias;}else{std::cerr << message_header() << "Could not get gauge bias values" << std::endl;return std::vector<int>();}
}bool FTSensor::setGaugeBias(std::vector<int> &gauge_vect)
{std::map<unsigned int, int> map;for (size_t i=0; i < gauge_vect.size(); ++i){map[i] = gauge_vect[i];}return setGaugeBias(map);
}bool FTSensor::setGaugeBias(std::map<unsigned int, int> &gauge_map)
{std::stringstream setbias_ss;std::map<unsigned int, int>::iterator it;bool first_element = true;//prepare the queryfor (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<uint16_t*>(&request_[0]) = htons(command_s::command_header);*reinterpret_cast<uint16_t*>(&request_[2]) = htons(cmd);*reinterpret_cast<uint32_t*>(&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<uint32_t*>(&response_[0]));resp_.ft_sequence = ntohl(*reinterpret_cast<uint32_t*>(&response_[4]));resp_.status = ntohl(*reinterpret_cast<uint32_t*>(&response_[8]));resp_.Fx = static_cast<int32_t>(ntohl(*reinterpret_cast<int32_t*>(&response_[12 + 0 * 4])));resp_.Fy = static_cast<int32_t>(ntohl(*reinterpret_cast<int32_t*>(&response_[12 + 1 * 4])));resp_.Fz = static_cast<int32_t>(ntohl(*reinterpret_cast<int32_t*>(&response_[12 + 2 * 4])));resp_.Tx = static_cast<int32_t>(ntohl(*reinterpret_cast<int32_t*>(&response_[12 + 3 * 4])));resp_.Ty = static_cast<int32_t>(ntohl(*reinterpret_cast<int32_t*>(&response_[12 + 4 * 4])));resp_.Tz = static_cast<int32_t>(ntohl(*reinterpret_cast<int32_t*>(&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 " <<response_ret_ << " but should be "<< RDT_RECORD_SIZE << std::endl;return response_ret_==RDT_RECORD_SIZE;
}void FTSensor::doComm()
{if (isInitialized()) {if(cmd_.sample_count != 0) //do not repeat send if infinite samplesif(!sendCommand())std::cerr << message_header() << "Error while sending command" << std::endl;if(!getResponse())std::cerr << message_header() << "Error while getting response, command:" <<cmd_.command << std::endl;}
}void FTSensor::setBias()
{//std::cout << "Setting bias"<<std::endl;this->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<unsigned int>(sec);timeval_.tv_usec = static_cast<unsigned int>(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 <ros/ros.h>
#include <ros/message_operations.h>
#include <std_msgs/String.h>
#include <std_msgs/Bool.h>
#include <geometry_msgs/WrenchStamped.h>
#include <std_srvs/Empty.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <string>
#include <boost/shared_ptr.hpp>
#include "ft_sensor.h"namespace ftsensor {class FTSensorPublisher
{
private://! The node handleros::NodeHandle nh_;//! Node handle in the private namespaceros::NodeHandle priv_nh_;//! The sensorboost::shared_ptr<ati::FTSensor> ftsensor_;std::string ip_;std::string frame_ft_;//! Publisher for sensor readingsros::Publisher pub_sensor_readings_;//! Service for setting the biasros::ServiceServer srv_set_bias_;public://------------------ Callbacks -------------------// Callback for setting biasbool setBiasCallback(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response);// Publish the measurementsvoid publishMeasurements();//! Subscribes to and advertises topicsFTSensorPublisher(ros::NodeHandle nh) : nh_(nh), priv_nh_("~"){priv_nh_.param<std::string>("frame", frame_ft_, "/ati_ft_link");priv_nh_.param<std::string>("ip", ip_, "192.168.1.1");ROS_INFO_STREAM("ATISensor IP : "<< ip_);ROS_INFO_STREAM("ATISensor frame : "<< frame_ft_);// Create a new sensorftsensor_ = boost::shared_ptr<ati::FTSensor>(new ati::FTSensor());// Init FT Sensorif (ftsensor_->init(ip_)){// Set biasftsensor_->setBias();ROS_INFO_STREAM("ATISensor RDT Rate : "<< ftsensor_->getRDTRate());// Advertise topic where readings are publishedpub_sensor_readings_ = priv_nh_.advertise<geometry_msgs::WrenchStamped>("data", 10);// Advertise service for setting the biassrv_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 ftsensorint 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编译，运行节点即可。