int mwFileTransfer_close(struct mwFileTransfer *ft, guint32 code) {
struct mwServiceFileTransfer *srvc;
struct mwFileTransferHandler *handler;
int ret = 0;
g_return_val_if_fail(ft != NULL, -1);
if(mwFileTransfer_isOpen(ft))
ft_state(ft, mwFileTransfer_CANCEL_LOCAL);
if(ft->channel) {
ret = mwChannel_destroy(ft->channel, code, NULL);
ft->channel = NULL;
}
srvc = ft->service;
g_return_val_if_fail(srvc != NULL, ret);
handler = srvc->handler;
g_return_val_if_fail(handler != NULL, ret);
if(handler->ft_closed)
handler->ft_closed(ft, code);
return ret;
}
int mwFileTransfer_accept(struct mwFileTransfer *ft) {
struct mwServiceFileTransfer *srvc;
struct mwFileTransferHandler *handler;
int ret;
g_return_val_if_fail(ft != NULL, -1);
g_return_val_if_fail(ft->channel != NULL, -1);
g_return_val_if_fail(mwFileTransfer_isPending(ft), -1);
g_return_val_if_fail(mwChannel_isIncoming(ft->channel), -1);
g_return_val_if_fail(mwChannel_isState(ft->channel, mwChannel_WAIT), -1);
g_return_val_if_fail(ft->service != NULL, -1);
srvc = ft->service;
g_return_val_if_fail(srvc->handler != NULL, -1);
handler = srvc->handler;
ret = mwChannel_accept(ft->channel);
if(ret) {
mwFileTransfer_close(ft, ERR_FAILURE);
} else {
ft_state(ft, mwFileTransfer_OPEN);
if(handler->ft_opened)
handler->ft_opened(ft);
}
return ret;
}
static void recv_channelCreate(struct mwServiceFileTransfer *srvc,
struct mwChannel *chan,
struct mwMsgChannelCreate *msg) {
struct mwFileTransferHandler *handler;
struct mwGetBuffer *b;
char *fnm, *txt;
guint32 size, junk;
gboolean b_err;
g_return_if_fail(srvc->handler != NULL);
handler = srvc->handler;
b = mwGetBuffer_wrap(&msg->addtl);
guint32_get(b, &junk); /* unknown */
mwString_get(b, &fnm); /* offered filename */
mwString_get(b, &txt); /* offering message */
guint32_get(b, &size); /* size of offered file */
/// Miranda NG adaptation - start - http://www.lilotux.net/~mikael/pub/meanwhile/ft_fix.diff
/* guint32_get(b, &junk); */ /* unknown */
/// Miranda NG adaptation - end
/* and we just skip an unknown guint16 at the end */
b_err = mwGetBuffer_error(b);
mwGetBuffer_free(b);
if(b_err) {
g_warning("bad/malformed addtl in File Transfer service");
mwChannel_destroy(chan, ERR_FAILURE, NULL);
} else {
struct mwIdBlock idb;
struct mwFileTransfer *ft;
login_into_id(&idb, mwChannel_getUser(chan));
ft = mwFileTransfer_new(srvc, &idb, txt, fnm, size);
ft->channel = chan;
ft_state(ft, mwFileTransfer_PENDING);
mwChannel_setServiceData(chan, ft, NULL);
if(handler->ft_offered)
handler->ft_offered(ft);
}
g_free(fnm);
g_free(txt);
}
int mwFileTransfer_offer(struct mwFileTransfer *ft) {
struct mwServiceFileTransfer *srvc;
struct mwFileTransferHandler *handler;
g_return_val_if_fail(ft != NULL, -1);
g_return_val_if_fail(ft->channel == NULL, -1);
g_return_val_if_fail(mwFileTransfer_isNew(ft), -1);
g_return_val_if_fail(ft->service != NULL, -1);
srvc = ft->service;
g_return_val_if_fail(srvc->handler != NULL, -1);
handler = srvc->handler;
ft_create_chan(ft);
if(ft->channel) {
ft_state(ft, mwFileTransfer_PENDING);
} else {
ft_state(ft, mwFileTransfer_ERROR);
mwFileTransfer_close(ft, ERR_FAILURE);
}
return 0;
}
static void recv_RECEIVED(struct mwFileTransfer *ft,
struct mwOpaque *data) {
struct mwServiceFileTransfer *srvc;
struct mwFileTransferHandler *handler;
srvc = ft->service;
handler = srvc->handler;
if(! ft->remaining)
ft_state(ft, mwFileTransfer_DONE);
if(handler->ft_ack)
handler->ft_ack(ft);
if(! ft->remaining)
mwFileTransfer_close(ft, mwFileTransfer_SUCCESS);
}
static void recv_channelAccept(struct mwServiceFileTransfer *srvc,
struct mwChannel *chan,
struct mwMsgChannelAccept *msg) {
struct mwFileTransferHandler *handler;
struct mwFileTransfer *ft;
g_return_if_fail(srvc->handler != NULL);
handler = srvc->handler;
ft = mwChannel_getServiceData(chan);
g_return_if_fail(ft != NULL);
ft_state(ft, mwFileTransfer_OPEN);
if(handler->ft_opened)
handler->ft_opened(ft);
}
static void recv_channelDestroy(struct mwServiceFileTransfer *srvc,
struct mwChannel *chan,
struct mwMsgChannelDestroy *msg) {
struct mwFileTransferHandler *handler;
struct mwFileTransfer *ft;
guint32 code;
code = msg->reason;
g_return_if_fail(srvc->handler != NULL);
handler = srvc->handler;
ft = mwChannel_getServiceData(chan);
g_return_if_fail(ft != NULL);
ft->channel = NULL;
if(! mwFileTransfer_isDone(ft))
ft_state(ft, mwFileTransfer_CANCEL_REMOTE);
mwFileTransfer_close(ft, code);
}
struct mwFileTransfer *
mwFileTransfer_new(struct mwServiceFileTransfer *srvc,
const struct mwIdBlock *who, const char *msg,
const char *filename, guint32 filesize) {
struct mwFileTransfer *ft;
g_return_val_if_fail(srvc != NULL, NULL);
g_return_val_if_fail(who != NULL, NULL);
ft = g_new0(struct mwFileTransfer, 1);
ft->service = srvc;
mwIdBlock_clone(&ft->who, who);
ft->filename = g_strdup(filename);
ft->message = g_strdup(msg);
ft->size = ft->remaining = filesize;
ft_state(ft, mwFileTransfer_NEW);
/* stick a reference in the service */
srvc->transfers = g_list_prepend(srvc->transfers, ft);
return ft;
}
static void recv_TRANSFER(struct mwFileTransfer *ft,
struct mwOpaque *data) {
struct mwServiceFileTransfer *srvc;
struct mwFileTransferHandler *handler;
srvc = ft->service;
handler = srvc->handler;
g_return_if_fail(mwFileTransfer_isOpen(ft));
if(data->len > ft->remaining) {
/* @todo handle error */
} else {
ft->remaining -= data->len;
if(! ft->remaining)
ft_state(ft, mwFileTransfer_DONE);
if(handler->ft_recv)
handler->ft_recv(ft, data);
}
}
/*!
* \brief Unpack force/torque ADC samples from realtime data.
*
* \return True, if there are no problems, false if there is something wrong with the data.
*/
bool WG06::unpackFT(WG06StatusWithAccelAndFT *status, WG06StatusWithAccelAndFT *last_status)
{
pr2_hardware_interface::ForceTorqueState &ft_state(force_torque_.state_);
ros::Time current_time(ros::Time::now());
// Fill in raw analog output with most recent data sample, (might become deprecated?)
{
ft_raw_analog_in_.state_.state_.resize(6);
const FTDataSample &sample(status->ft_samples_[0]);
for (unsigned i=0; i<6; ++i)
{
int raw_data = sample.data_[i];
ft_raw_analog_in_.state_.state_[i] = double(raw_data);
}
}
unsigned new_samples = (unsigned(status->ft_sample_count_) - unsigned(last_status->ft_sample_count_)) & 0xFF;
ft_sample_count_ += new_samples;
int missed_samples = std::max(int(0), int(new_samples) - 4);
ft_missed_samples_ += missed_samples;
unsigned usable_samples = min(new_samples, MAX_FT_SAMPLES);
// Also, if number of new_samples is ever 0, then there is also an error
if (usable_samples == 0)
{
ft_sampling_rate_error_ = true;
}
// Make room in data structure for more f/t samples
ft_state.samples_.resize(usable_samples);
// If any f/t channel is overload or the sampling rate is bad, there is an error.
ft_state.good_ = ( (!ft_sampling_rate_error_) &&
(ft_overload_flags_ == 0) &&
(!ft_disconnected_) &&
(!ft_vhalf_error_) );
for (unsigned sample_index=0; sample_index<usable_samples; ++sample_index)
{
// samples are stored in status data, so that newest sample is at index 0.
// this is the reverse of the order data is stored in hardware_interface::ForceTorque buffer.
unsigned status_sample_index = usable_samples-sample_index-1;
const FTDataSample &sample(status->ft_samples_[status_sample_index]);
geometry_msgs::Wrench &wrench(ft_state.samples_[sample_index]);
convertFTDataSampleToWrench(sample, wrench);
}
// Put newest sample into analog vector for controllers (deprecated)
if (usable_samples > 0)
{
const geometry_msgs::Wrench &wrench(ft_state.samples_[usable_samples-1]);
ft_analog_in_.state_.state_[0] = wrench.force.x;
ft_analog_in_.state_.state_[1] = wrench.force.y;
ft_analog_in_.state_.state_[2] = wrench.force.z;
ft_analog_in_.state_.state_[3] = wrench.torque.x;
ft_analog_in_.state_.state_[4] = wrench.torque.y;
ft_analog_in_.state_.state_[5] = wrench.torque.z;
}
// Put all new samples in buffer and publish it
if ((raw_ft_publisher_ != NULL) && (raw_ft_publisher_->trylock()))
{
raw_ft_publisher_->msg_.samples.resize(usable_samples);
raw_ft_publisher_->msg_.sample_count = ft_sample_count_;
raw_ft_publisher_->msg_.missed_samples = ft_missed_samples_;
for (unsigned sample_num=0; sample_num<usable_samples; ++sample_num)
{
// put data into message so oldest data is first element
const FTDataSample &sample(status->ft_samples_[sample_num]);
ethercat_hardware::RawFTDataSample &msg_sample(raw_ft_publisher_->msg_.samples[usable_samples-sample_num-1]);
msg_sample.sample_count = ft_sample_count_ - sample_num;
msg_sample.data.resize(NUM_FT_CHANNELS);
for (unsigned ch_num=0; ch_num<NUM_FT_CHANNELS; ++ch_num)
{
msg_sample.data[ch_num] = sample.data_[ch_num];
}
msg_sample.vhalf = sample.vhalf_;
}
raw_ft_publisher_->msg_.sample_count = ft_sample_count_;
raw_ft_publisher_->unlockAndPublish();
}
// Put newest sample in realtime publisher
if ( (usable_samples > 0) && (ft_publisher_ != NULL) && (ft_publisher_->trylock()) )
{
ft_publisher_->msg_.header.stamp = current_time;
ft_publisher_->msg_.wrench = ft_state.samples_[usable_samples-1];
ft_publisher_->unlockAndPublish();
}
// If this returns false, it will cause motors to halt.
// Return "good_" state of sensor, unless halt_on_error is false.
return ft_state.good_ || !force_torque_.command_.halt_on_error_;
}
