型号和序列号等参数如下：
| 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 36
namespace 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_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 = "</"+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_count
return startStreaming();
}
else {
// use given sample count
uint32_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 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 "<<calibration_index<< std::endl;
}else
std::cout << message_header() << "Using current calibration" << std::endl;
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
// The recv buffer
std::string filename = "/netftapi2.xml"+index; // the name of the file to reveice
std::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 0
break;
}
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-colon
if (!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 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<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 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<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 samples
if(!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 handle
ros::NodeHandle nh_;
//! Node handle in the private namespace
ros::NodeHandle priv_nh_;
//! The sensor
boost::shared_ptr<ati::FTSensor> 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<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 sensor
ftsensor_ = boost::shared_ptr<ati::FTSensor>(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<geometry_msgs::WrenchStamped>("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编译，运行节点即可。
7179
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
